Method and system for pose optimization of mobile robot chassis based on observation quality driving

Abstract

The application discloses a mobile robot chassis pose optimization method and system based on observation quality driving and belongs to the technical field of mobile robot autonomous navigation and control. In the existing unified QP framework, the chassis can receive the gradient of "moving to which can reduce the pixel difference", but cannot receive the gradient of "moving to which can improve the observation condition". In the application, the camera cost in QP is replaced by the observation quality function, which is composed of three derivable sub-terms of visibility, perspective quality and depth appropriateness, so that the chassis receives the observation quality gradient signal for the first time. In the optimization, the movement direction of the chassis is uniformly weighed by the arm tracking gradient and the observation quality gradient, and manual coordination is not needed. The application realizes the functional upgrade of the chassis from passive arrival at the preset position to active improvement of the observation condition, and effectively improves the operation precision and autonomy of the mobile robot in a complex environment.

Description

Technical Field

[0001] This invention belongs to the field of autonomous navigation and control technology for mobile robots, and particularly relates to a method and system for optimizing the chassis pose of a mobile robot based on observation quality. This invention is applicable to mobile robots equipped with robotic arms and vision systems, and is especially suitable for scenarios requiring high-precision operation tasks in dynamic, unstructured environments, including but not limited to power line inspection, industrial assembly, logistics handling, emergency rescue, and service robots. Background Technology

[0002] Mobile robots consist of a wheeled or tracked chassis and a multi-degree-of-freedom robotic arm mounted on it. They possess both wide-range mobility and precise manipulation capabilities, and have broad application prospects in fields such as industrial manufacturing, warehousing and logistics, power line inspection, and disaster relief. Accurate pose estimation and environmental perception are fundamental to the autonomous navigation and precise operation of mobile robots. The vision system, as the primary sensor for robot environmental perception, directly impacts the accuracy of subsequent pose estimation and manipulation. In recent years, with the widespread application of vision sensors and inertial measurement units (IMUs), simultaneous localization and mapping (SLAM) technology based on vision-inertial fusion has become the mainstream solution in the field of mobile robot perception. However, how to achieve autonomous optimization of chassis pose under visual observation conditions remains one of the key bottlenecks restricting the accuracy of mobile robots operating in complex environments.

[0003] Existing motion planning and control methods for mobile robots can be divided into three categories based on the coupling method between the chassis and the robotic arm.

[0004] The first category is the sequential planning method. In this method, the chassis first navigates to and locks at a preset workstation, and then the robotic arm performs the operation task. This approach has a simple architecture and decoupled control, making it widely used in industrial scenarios. However, once the chassis is locked, the camera's field of view is fixed, and the observation conditions of the vision system depend entirely on the selection of the preset workstation, which cannot be adjusted online regardless of the quality of the current observation conditions. This fundamentally limits the robotic arm's operational accuracy under poor observation conditions, and the method cannot adapt to changes in the environment or the target's pose.

[0005] The second category is the loosely coupled parallel method. In this type of method, the chassis and the robotic arm move simultaneously, but are driven by their respective independent controllers. The chassis tracks a predetermined path, while the robotic arm performs end-effector control based on visual signals. There is only state sharing between the two, without joint gradient optimization. The camera moves with the chassis, but this movement is aimed at navigating the path, not at improving observation conditions. In some schemes, the system coordinates the movement of the chassis and arm through predictive repositioning or dynamic weight allocation, but the visual servoing process and chassis motion planning remain relatively independent, lacking proactive awareness of the chassis's observation quality.

[0006] The third category is the unified optimization method. This type of method incorporates the degrees of freedom of the chassis and the robotic arm into the same optimization problem framework, employing quadratic programming (QP) or model predictive control (MPC) for whole-body control. In recent years, this type of method has received widespread attention. Representative schemes integrate the chassis, robotic arm, and vision sensors into a unified optimization framework, achieving coordinated control of the chassis and arm by constructing a unified optimization problem that includes a camera cost function. However, the camera cost function in existing unified optimization methods has fundamental limitations.

[0007] Specifically, existing solutions typically use visual servoing error as the camera cost function, i.e., minimizing the deviation between the current image feature point coordinates and the desired coordinates. While mathematically the gradient of the camera cost function with respect to the chassis pose is non-zero—because chassis movement does indeed change the camera pose and thus the image feature coordinates—the information carried by this gradient is essentially "which direction of movement reduces the image coordinate deviation," rather than "which direction of movement improves the observation conditions." There is a fundamental difference between the two.

[0008] In some scenarios, existing methods attempt to construct ground feature residuals by using feature point depth observations under the assumption of a flat ground plane, thereby imposing additional depth constraints on pose estimation; or they use instance segmentation techniques to identify dynamic objects and remove feature points from them, in order to improve the robustness of visual localization in dynamic environments. However, the optimization goal of these techniques is still to improve the accuracy of pose estimation itself, that is, to make pose estimation "more accurate," rather than to allow the chassis to actively adjust its pose so that the camera can "see more clearly." In other words, the utilization of visual information is still limited to the scope of "passive perception" and has not achieved the function of "actively optimizing observation conditions."

[0009] Some studies utilize multi-camera systems or visual servoing frameworks to achieve closed-loop pose control, driving mobile robots to the desired pose through visual feature feedback. However, in these schemes, the desired pose is usually pre-set or determined by the visual tracking target, and the control objective is "to reach the desired pose" rather than "to optimize observation conditions." When visual features degrade due to changes in viewpoint, the system lacks a corresponding mechanism to actively adjust the pose to improve observation quality.

[0010] In recent years, observation-quality-based active perception has received increasing attention in the field of robotics. However, existing research mainly focuses on sensor planning or data acquisition strategies, such as Next-Best-View and Active SLAM. These methods typically select the next observation position at the decision level, rather than enabling the chassis to continuously fine-tune based on the observation quality gradient within a unified underlying optimization framework. Combining this with real-time full-body control of mobile robots, enabling them to autonomously perform centimeter-level / degree-level pose fine-tuning based on the observation quality gradient in each control cycle, is a technical approach that has not been publicly disclosed or inspired in existing technologies.

[0011] In summary, in existing mobile robot systems, the chassis's function is consistently limited to extending the workspace of the robotic arm's end effector—that is, acting as a mobile platform to "transport the robotic arm to a preset position"—and has never been explicitly modeled as a degree of freedom capable of actively improving visual observation conditions. Within the unified optimization framework, while the chassis can receive gradient information from the robotic arm's tracking task through joint Jacobians, it cannot receive gradient information related to observation quality. This results in the system lacking a fundamental mechanism to autonomously improve observations when conditions deteriorate, fundamentally limiting the mobile robot's operational accuracy and autonomy in complex environments.

[0012] Therefore, there is an urgent need for a technical solution that enables the chassis to directly receive the observation quality gradient signal within a unified optimization framework, thereby achieving active chassis pose optimization driven by observation quality. Summary of the Invention

[0013] In view of this, the purpose of this invention is to provide a method and system for optimizing the chassis pose of a mobile robot based on observation quality. This method replaces the camera cost in the unified QP framework with the servo error and the observation quality function, so that the chassis receives gradient signals indicating "where to move to improve observation conditions". This enables active fine-tuning of the pose to optimize visual observation and improve operational accuracy.

[0014] To achieve the above objectives, the present invention provides the following technical solution: This invention first proposes a method for optimizing the chassis pose of a mobile robot based on observation quality, comprising the following steps: S1: Establish the generalized coordinate system of the mobile robot. ,in: Indicates the position and orientation of a wheeled chassis; Represent the joint angle vectors of the robotic arm; establish a coordinate system from the world coordinate system. via chassis coordinate system To each camera coordinate system The complete coordinate transformation chain; S2: Constructing the observation quality function The observation quality function This is used to quantify the current observation conditions of at least one camera on the target object, and its value decreases monotonically as the observation conditions improve. right Differentiable; S3: Calculate the observation quality function within each control cycle. chassis position gradient This gradient is then incorporated as a cost term into the unified quadratic programming (QP) optimization problem. S4: Solve the unified quadratic programming optimization problem to obtain the joint velocity decision variables of the entire system. The joint velocity decision variables of the entire system Includes chassis speed With the joint speed of the robotic arm ; S5: Set the chassis speed The command is sent to the chassis drive, which moves the chassis to adjust the camera's pose in a direction that optimizes the observation conditions.

[0015] Furthermore, in step S2, the observed mass function It consists of a weighted sum of the observation quality from M cameras:

[0016] in: This is a visibility sub-item used to quantify the target object in the [missing information - likely a specific location or event]. The degree to which the view of the camera is obstructed; For the perspective quality sub-item, used to quantify the first The degree of deviation between the camera's line of sight and the target surface's normal direction; The depth-of-field suitability sub-item is used to quantize the target object to the [missing information - likely a specific depth-of-field value]. The degree to which the distance between the cameras deviates from the optimal working distance; For the first The fusion weights of multiple cameras; , and These are the weighting coefficients for the three sub-items; .

[0017] Furthermore, the visibility sub-item Represented as:

[0018] in: Preset to small positive numbers; For the target object in the first The probability that a field of view of a camera is not obstructed is calculated using a directed distance field (SDF) and a sigmoid smoothing mapping:

[0019] in: It represents the directed distance from the spatial sampling point to the surface of the nearest obstacle along the camera's line of sight; This is the preset smoothing factor; The target object in the first The probability that the field of view of the camera is not obstructed. Camera pose The gradient is obtained through the chain rule. Analytical calculation, where: It is given by the gradient field of the directed distance field.

[0020] Furthermore, the aforementioned perspective quality sub-item Represented as:

[0021] in: This is the unit normal vector of the target object's surface at the target point; For from the first The optical center of the camera is pointed in the unit line-of-sight direction to the target point; A preset small positive number is used to ensure that the function... Differentiability at =0; Unit normal vector The acquisition method is as follows: for target objects with CAD models, the surface normal vector is read from the aligned model after ICP point cloud registration; for unknown target objects, the 3D point cloud is taken in the neighborhood of the target area of ​​the depth image for local principal component analysis, and the feature direction corresponding to the minimum eigenvalue is taken as the normal vector.

[0022] Furthermore, the depth-of-field suitability sub-item Represented as:

[0023] in: For from the first The distance measured from the camera to the target object; For the first The optimal working distance for a single camera; For the first The effective working distance range of the camera, and .

[0024] Furthermore, in step S3, the gradient The calculation method is as follows:

[0025] in: , and By visibility sub-items Perspective Quality Sub-item and depth of field suitability sub-item The parsing expression is calculated directly; The coordinate transformation chain is given analytically.

[0026] Furthermore, in step S3, the cost function for the unified quadratic programming optimization problem is:

[0027] in: For the joint velocity decision variables of the entire system; This is the spatial trajectory tracking error term for the end-stage task; Penalty item for base movement; The observation quality cost function; For the potential field of the entire system, it represents the obstacle avoidance or control function term. For full-joint velocity smoothing and energy minimization regularization; The Jacobian matrix for all system tasks; The desired task space velocity; The weighted penalty coefficient for each optimization objective; Nonlinear terms and In the current state Perform a first-order Taylor expansion at the given point, and incorporate the linear terms into the gradient vector of the unified quadratic programming.

[0028] Furthermore, the unified quadratic programming optimization problem also includes ZMP stability constraints:

[0029] in: This is the horizontal projection of the system's zero-moment point into the world frame. A linear inequality description of the convex hull of the supporting polygon formed by the grounding points of the chassis wheel assembly; To control the cycle, They are slack variables; and The expression is:

[0030] in: Let be the horizontal projection of the system's center of mass into the world system; The height of the center of mass above the ground; It is the acceleration due to gravity; This is the horizontal acceleration of the center of mass.

[0031] Furthermore, the mobile robot includes: Wheel-driven chassis; At least one multi-degree-of-freedom serial robotic arm is mounted on the chassis; At least one camera, mounted on the chassis or robotic arm, is used to acquire images and / or depth information of the target object; The chassis, the robotic arm, and the camera share the same set of generalized coordinates in the unified quadratic programming optimization problem. The chassis speed With the joint speed of the robotic arm As the same decision variable The components are solved uniformly by the unified quadratic programming optimization problem.

[0032] This invention also proposes a mobile robot chassis pose optimization system based on observation quality, comprising: Wheel-driven chassis; At least one multi-degree-of-freedom serial robotic arm is mounted on the chassis; At least one camera, mounted on the chassis or the robotic arm, is used to acquire images and / or depth information of the target object; The processor is configured to execute the observation quality-driven mobile robot chassis pose optimization method described above; A memory for storing programs and data required by the processor to execute the method; The processor calculates the observation quality function in each control cycle. and its gradient with respect to chassis pose The cost term containing the gradient is incorporated into a unified quadratic programming optimization problem for unified solution, and the chassis speed command obtained from the solution is sent to the chassis drive.

[0033] The beneficial effects of this invention are as follows: This invention presents a mobile robot chassis pose optimization method driven by observation quality. By replacing the traditional visual servo error with the observation quality function as the camera cost term in a unified quadratic programming framework, the following technical effects are achieved.

[0034] (1) A fundamental shift in the function of the chassis. In existing solutions, the function of the chassis has always been limited to "extending the workspace of the robotic arm's end effector," that is, as a mobile platform that transports the robotic arm to a preset position, and has never been explicitly modeled as a degree of freedom that can actively improve visual observation conditions. This invention enables the chassis to receive gradient signals of "which direction to move to improve observation conditions" for the first time in a unified quadratic programming framework, realizing a functional upgrade from "passively reaching the preset position" to "actively and continuously optimizing observation conditions," fundamentally changing the role of the chassis in the mobile robot system.

[0035] (2) Quantifiability and Optimability of Observation Quality. This invention transforms the originally qualitative description of "good or bad observation conditions" into a quantifiable and differentiable continuous scalar function, enabling observation quality to be incorporated as an optimization objective into a unified quadratic programming framework for mathematical solution. When observation conditions deteriorate, the system no longer needs to rely on human presets or external decision-making modules to determine whether adjustments are needed. Instead, it automatically drives the chassis to respond through the observation quality gradient, realizing the transformation from "open-loop execution" to "closed-loop proactive perception".

[0036] (3) Simplicity and uniformity of system architecture. This invention does not require the introduction of an additional decision layer or planning layer to achieve active optimization of observation conditions. Instead, it seamlessly embeds the observation quality function into the existing unified quadratic programming framework, and performs unified trade-offs and collaborative solutions with the robotic arm tracking gradient in the same optimization problem, avoiding the delays and inconsistencies caused by multi-level decision-making.

[0037] (4) Autonomous response capability to degradation of observation conditions. In traditional schemes, when camera tilt, obstruction, or improper distance leads to a decline in observation quality, existing systems lack an underlying mechanism to proactively improve observation. This invention, through the combined action of three differentiable sub-terms of the observation quality function, enables the system to autonomously identify and compensate for degradation of observation conditions through chassis fine-tuning, without human intervention, significantly improving the system's environmental adaptability and operational autonomy.

[0038] In summary, this invention realizes a paradigm shift from "passive perception" to "active observation and optimization," enabling the chassis to transcend its traditional function of "expanding the workspace" and become an intelligent degree of freedom for actively optimizing observation conditions. Attached Figure Description

[0039] To make the objectives, technical solutions, and beneficial effects of this invention clearer, the following figures are provided for illustration: Figure 1 This is a structural diagram of a mobile robot; Figure 2 This is a link diagram of the coordinate transformation chain; Figure 3These are the three sub-items in the observation quality function: (a) visibility sub-item; (b) viewpoint quality sub-item; (c) depth of field suitability sub-item. Figure 4 The flowchart is for the control cycle algorithm; Figure 5 This is a flowchart of the observation quality-driven mobile robot chassis pose optimization method of the present invention; Figure 6 This is a diagram illustrating the grasping of multiple objects.

[0040] Explanation of reference numerals in the attached figures: 10-Mobile robot; 11-Bearing base; 12-Left robotic arm; 13-Right robotic arm; 14-Gripper; 15-Torso camera; 16-Wrist camera; 17-LiDAR. Detailed Implementation

[0041] The present invention will be further described below with reference to the accompanying drawings and specific embodiments, so that those skilled in the art can better understand and implement the present invention. However, the embodiments described are not intended to limit the present invention.

[0042] This invention provides a mobile robot chassis pose optimization method based on observation quality. By replacing the camera cost function in the unified quadratic programming (QP) framework with the observation quality function instead of the visual servo error, the chassis can receive gradient signals indicating "which direction to move in to improve observation conditions", thereby achieving active chassis pose optimization based on observation quality.

[0043] I. System Composition.

[0044] like Figure 1 As shown, in this embodiment, the mobile robot 10 includes a support base 11, a robotic arm, a vision system, and an environmental perception system.

[0045] In this embodiment, the supporting base 11 is a wheeled drive chassis, and its generalized coordinates are as follows: ;in: For the chassis in the world coordinate system The two-dimensional position below; This is the chassis heading angle; Represents a two-dimensional real number space, corresponding to the chassis position. , which is the amount of translation of the chassis along the X and Y axes on the horizontal plane, and its value is any real number; This is a special orthogonal group in 2D, representing the set of rotation matrices in a two-dimensional plane, corresponding to the chassis's heading angle. (i.e., rotation about the Z-axis). The chassis has nonholonomic constraints, and its motion satisfies the kinematic equations of the differential drive model.

[0046] The robotic arm is a multi-degree-of-freedom serial robotic arm mounted on a chassis. In a preferred embodiment, the robotic arm consists of two four-degree-of-freedom or six-degree-of-freedom serial robotic arms, denoted as left robotic arm 12 and right robotic arm 13, with joint angles denoted as follows: and ,in and These represent the degrees of freedom of the left robotic arm 12 and the right robotic arm 13, respectively. Force / torque sensors are installed at the end effectors of the robotic arms to sense the contact force between the end effectors and the environment. As an optional implementation of this embodiment, a gripper 14 is installed at the end effector of the robotic arm, and force / torque sensors are mounted on the gripper 14 for gripping confirmation and contact status feedback; their output signals are not directly involved in the calculation of the observed mass gradient.

[0047] The vision system includes at least one camera. In a preferred embodiment, the vision system includes a depth camera or RGB camera (i.e., torso camera 15) mounted directly in front of the chassis, with its optical axis pointing in the robot's forward direction. In other optional embodiments, the vision system may also include a wrist camera 16 mounted on the robotic arm's wrist and / or an external camera fixed to the environment to enable multi-view observation.

[0048] The environmental perception system includes a lidar sensor 17 or a depth camera to acquire information about obstacles in the environment and provide data support for collision avoidance.

[0049] The generalized coordinates of the entire system are uniformly denoted as The total dimension is Task Jacobian Matrix Map the overall system speed to the end-task space speed, where This represents the dimension of the task space.

[0050] II. Unified kinematic modeling.

[0051] To achieve the differentiability of the observation quality function with respect to the chassis pose, this embodiment first establishes a world coordinate system. The complete coordinate transformation chain to each end and camera, such as Figure 2 As shown.

[0052] Specifically, the coordinate system transformation chain includes the following paths: (1) From the world coordinate system Chassis coordinate system This transformation is based on the chassis pose. Defined as a planar rigid body transformation, its homogeneous transformation matrix is:

[0053] (2) From the chassis coordinate system To the coordinate system of the left robotic arm 12 base This transformation is a fixed transformation, determined by the installation position of the robotic arm, and is denoted as... .

[0054] (3) From the coordinate system of the left robotic arm 12 base To the coordinate system of the end of the left robotic arm 12 This transformation is achieved by the angles of each of the 12 joints of the left robotic arm. Determined through forward kinematics, denoted as: .

[0055] (4) From the chassis coordinate system To the coordinate system of the right robotic arm 13 base This transformation is a fixed transformation, determined by the installation position of the robotic arm, and is denoted as... .

[0056] (5) From the coordinate system of the right robotic arm 13 base To the coordinate system of the end of the right robotic arm 13 This transformation is achieved by the joint angles of the right robotic arm 13. Determined through positive kinematics, denoted as .

[0057] (6) From the chassis coordinate system To the torso camera 15 coordinate system This transformation is a fixed transformation, determined by the camera's mounting position on the chassis, denoted as . .

[0058] The key property of the above coordinate transformation chain lies in: chassis pose Any change will simultaneously alter the chassis coordinate system. and all its sub-coordinate systems (including the arm base coordinate system and the camera coordinate system) in the world coordinate system The pose of each camera in the world coordinate system. yes The function can be analytically expressed through the aforementioned fixed transformation and kinematic chain. Specifically, the pose of the torso camera 15 in the world coordinate system is:

[0059] This analytical expression constitutes the observation quality function pair. Differentiable geometric foundations enable the observation quality gradient to be transferred from image space to chassis configuration space via the chain rule.

[0060] III. Construction of the observation quality function.

[0061] The observation quality function constructed in this embodiment The quality of visual observation conditions is quantified as a continuously differentiable scalar function, the value of which decreases monotonically as the observation conditions improve.

[0062] Specifically, for For a given number of cameras, the observation quality function is defined as a weighted sum of the observation qualities of each camera:

[0063] in: This is a visibility sub-item used to quantify the target object in the [missing information - likely a specific location or event]. The degree to which the view of the camera is obstructed; For the perspective quality sub-item, used to quantify the first The degree of deviation between the camera's line of sight and the target surface's normal direction; The depth-of-field suitability sub-item is used to quantize the target object to the [missing information - likely a specific depth-of-field value]. The degree to which the distance between the cameras deviates from the optimal working distance; For the first The fusion weights of the cameras satisfy , ; , and These are the weighting coefficients for the three sub-items, all of which are positive and satisfy the following conditions: ; , where represents the number of cameras.

[0064] Figure 3 The diagram below shows the three sub-items of the observation quality function provided in the embodiment of the present invention. The specific construction methods of the three sub-items are described in detail below.

[0065] (1) Visibility sub-item .

[0066] Visibility sub-item Used to quantize the target object in the first The degree to which the view of a camera is obstructed. It is defined as:

[0067] in: Preset small positive numbers (preferably, ), used to prevent the logarithmic function from Strange occurrences sometimes occur; For the target object in the first The probability that the view of a camera is not obstructed.

[0068] probability It is calculated using a Signed Distance Field (SDF) and a sigmoid smoothing map. Specifically, a Euclidean SDF is pre-constructed for the environment, and the directed distance from each spatial point to the nearest obstacle surface is stored. The interior of the agreed obstacle ,external Sampling is performed along the camera's line of sight from the target point towards the camera's optical center, obtaining the SDF value of each sampling point, and mapping it to the visibility probability using the sigmoid function:

[0069] in: The preset smoothing factor is preferably... When the sampling point is in the direction of the line of sight. A large positive value indicates that the line of sight is moving away from the obstacle. Approaching 1, the cost It tends to 0; when the line of sight passes through an obstacle ( When the value is negative, Approaching zero, cost It tends towards infinity.

[0070] The differentiability analysis for this form is as follows: right The gradient is obtained through the chain rule. Analytical calculation; where: Given by the derivative of the sigmoid function, The gradient field is given by SDF. The results are given analytically from the aforementioned kinematic transformation chain.

[0071] As a preferred embodiment of this example, the SDF of static obstacles is pre-computed offline and reused throughout the task to reduce computational costs; dynamic obstacles are locally incrementally updated using real-time point clouds from the LiDAR 17.

[0072] (2) Perspective quality sub-item .

[0073] Perspective quality sub-item Used to quantify the The degree of deviation between the camera's line of sight and the target surface's normal direction. It is defined as:

[0074] in: This is the unit normal vector of the target object's surface at the target point; For from the first The optical center of the camera is pointed in the unit line-of-sight direction to the target point. A preset small positive number is used to ensure that the function... Differentiation at =0, preferably .

[0075] The meaning of this function value is: when the camera is facing the target surface... and The direction is parallel (or anti-parallel). , The cost is minimal; when the camera observes the target surface at a grazing angle, , The cost is the greatest.

[0076] Regarding the normal vector of the target surface In one preferred embodiment of this invention, for a target object with a CAD model, the actual observed point cloud is aligned with the CAD model through ICP point cloud registration, and the surface normal vector is directly read from the aligned model. In another preferred embodiment of this invention, for an unknown target object, a 3D point cloud is taken in the neighborhood of the target region of the depth image and local principal component analysis (PCA) is performed, and the feature direction corresponding to the minimum eigenvalue is taken as the normal vector.

[0077] (3) Depth of field suitability sub-item .

[0078] Depth of field suitability sub-item Used to quantize the target object to the number The degree to which the distance between the cameras deviates from the optimal working distance. It is defined as:

[0079] in: For from the first The distance measured from the camera to the target object; For the first The optimal working distance for a single camera; For the first The effective working distance range of the camera, and satisfying This quadratic function causes the penalty for deviating from the optimal distance to increase symmetrically.

[0080] All three sub-items mentioned above are correct. Differentiable: Changes in chassis pose alter the camera's pose in the world coordinate system, thereby changing the SDF sampling position (affecting...). ), line of sight (affects) ) and target distance (affect) This enables continuous quantification of observation quality. This forms the mathematical basis for the chassis receiving the observation quality gradient.

[0081] IV. Observation quality gradient and mixed gradient.

[0082] This embodiment will observe the mass function. chassis position The gradient is used as a new optimization driver and is jointly optimized with the gradient of the robotic arm tracking task within a unified QP framework. To facilitate understanding of the innovative essence of this invention, the gradient characteristics of the camera cost function in the prior art are first analyzed, and then the method for calculating the observation quality gradient in this invention is explained.

[0083] (1) Gradient analysis of existing technologies.

[0084] In the existing unified QP scheme, the camera cost is calculated using visual servo error. ;in: The coordinates of the current image features. The coordinates are the desired image features. This cost is related to the chassis pose. The gradient is:

[0085] This gradient is mathematically nonzero—chassis movement does indeed change the camera pose and thus the image feature coordinates. However, the information carried by this gradient is "which direction to move in to reduce pixel differences," not "which direction to move in to improve observation conditions." Specifically, when the camera is angled at the target surface at a large deflection angle, as long as the feature points are not out of the field of view, the servo error... The gradient is still small, close to zero, and QP lacks the power to output chassis fine-tuning commands. Information such as skew, occlusion, and depth-of-field inaccuracies measured by the camera cannot be transmitted to the chassis at this cost.

[0086] (2) The observation quality gradient of the present invention.

[0087] In this embodiment, the camera cost is replaced with... Afterwards, it was to The gradient is:

[0088] Among them, each sub-item pairs partial derivatives , and Each is calculated directly using its respective analytical expression; This is given analytically by the aforementioned coordinate transformation chain. The information carried by this gradient is "which direction to move in to reduce occlusion, face the surface directly, and optimize the distance".

[0089] Specifically, under camera tilt observation conditions, the observation quality gradient can generate a significant chassis drive signal, while the traditional servo error gradient is close to zero.

[0090] In QP, the final direction of chassis motion is determined by the superposition of two gradients:

[0091] Both are weighed uniformly by QP, requiring no manual coordination, thus achieving autonomous collaboration between the arm, eye, and chassis within a unified optimization framework.

[0092] in: To unify the total cost function in the QP optimization problem, it is expressed as:

[0093] In the formula This indicates the sensitivity of the total cost to the direction of chassis speed, i.e., which direction the chassis should move to most effectively reduce the total cost.

[0094] The cost terms related to the task performed by the robotic arm's end effector mainly include the pose tracking error of the robotic arm's end effector: ; The gradient generated for the robotic arm's end effector tracking task (transmitted to the chassis via the robotic arm's Jacobian matrix) represents how chassis movement affects the quality of the robotic arm's end effector task completion. Its physical meaning is that even without moving the robotic arm joints, translation or rotation of the chassis alone can change the position and orientation of the end effector—this coupling effect is transmitted to the chassis via the robotic arm's Jacobian matrix. In existing technologies, this is the only gradient information from the task level received by the chassis.

[0095] The cost term related to the quality of visual observation is represented as:

[0096] in: For the observed quality function, The newly added observation quality gradient in this embodiment indicates how chassis movement improves or worsens the current visual observation conditions. The information carried by this gradient is "which direction to move the camera to reduce occlusion, face the surface directly, and optimize the observation distance"—this is a brand-new gradient signal introduced for the first time in this invention using a unified QP framework.

[0097] V. Problem Construction and Solution of QP Optimization.

[0098] Quadratic programming (QP) is a general mathematical framework for the whole-body control of a mobile robot. Its basic structure includes quadratic cost plus linear constraints, Taylor expansion at the operating point, and standard form solution. The innovation of this embodiment lies in introducing a new component into the cost function. This enables it to carry observation quality semantics and generate a driving gradient for the chassis. .

[0099] (1) Cost function of QP optimization problem.

[0100] In this embodiment, the cost function for the unified quadratic programming optimization problem is:

[0101] in: For the joint speed decision variables of the entire system, including the base speed With the joint speed of the robotic arm ; This is the spatial trajectory tracking error term for the end-effector, used to ensure the tracking accuracy of the robotic arm's end effector on the desired trajectory; This is a base motion penalty measure used to suppress meaningless violent vibrations or movements of the chassis; The observation quality cost function consists of three sub-terms: visibility, viewpoint quality, and depth of field suitability. This is a potential field collision avoidance or obstacle control function term for the entire system, used to ensure a safe distance between the robot body and environmental obstacles; Regularization terms for smoothing the velocity of all joints and minimizing energy are used to ensure the smoothness of control commands; The task Jacobian matrix for the entire system maps the velocity of the entire system to the velocity in the task space. The desired task space velocity; The weighted penalty coefficients for each optimization objective are all positive numbers; Nonlinear terms and In the current state Perform a first-order Taylor expansion at the given point, and incorporate the linear terms into the gradient vector of the unified quadratic programming.

[0102] (2) Constraints.

[0103] The optimization problem described above is subject to the following constraints: 1) Joint limitation (hard constraint): This ensures that each joint does not exceed its physical range of motion. 2) End-effector collision avoidance (hard constraint): The robotic arm's end effector maintains a safe distance from obstacles; 3) Nonholonomic constraints (hard constraints): The chassis motion satisfies the nonholonomic constraints of the differential driving model: ; 4) ZMP stability constraints (soft constraints): ensure the overall stability of the robot and prevent tipping over; 5) Target within field of view constraint (soft constraint): Ensures that the target object is within the camera's field of view; 6) Speed ​​smoothing constraint (soft constraint): ensures the timing smoothness of control commands.

[0104] The constraints 1)-6) above are general safety measures for the 10QP control of mobile robots, and those skilled in the art can select and configure them according to specific application scenarios. The following provides a detailed explanation of the specific implementation of the ZMP stability constraints.

[0105] (3) Specific implementation of ZMP stability constraints.

[0106] The zero-moment point (ZMP) is an important indicator for judging the dynamic stability of a wheeled mobile robot. Let the total mass of the system be... The horizontal projection of the centroid in the world coordinate system is The acceleration of the center of mass is Under the quasi-static approximation, the position of ZMP Estimated by the following formula:

[0107] Where: h is the height of the center of mass above the ground; Let be the acceleration due to gravity. This equation shows that when the center of mass accelerates, the ZMP shifts in the opposite direction of acceleration—the stronger the acceleration, the closer the ZMP gets to the support boundary.

[0108] The four contact points of the chassis wheel assembly form a supporting polygon (convex hull), the boundary of which is determined by a system of linear inequalities. Description. Wherein: The coefficient matrix supporting the polygon; This is the corresponding boundary value vector; The number of sides supporting the polygon.

[0109] To incorporate the ZMP constraint into the QP optimization problem, a first-order Taylor expansion is performed on the ZMP position of the next cycle at the current operating point:

[0110] Substitute the supporting polygon constraints and introduce non-negative relaxation variables. Preventing QP optimization problems from becoming unsolvable:

[0111] in: This is the horizontal projection of the system's zero-moment point into the world frame. A linear inequality description of the convex hull of the supporting polygon formed by the grounding points of the chassis wheel assembly; To control the cycle, These are slack variables.

[0112] The relaxation amount is subject to a quadratic penalty in the cost function. Under normal circumstances ZMP strictly guards the interior of the boundary; when the robot arm moves violently and ZMP inevitably approaches the boundary, the relaxation absorbs the impact, and the cost function forces the chassis to adjust in the opposite direction to restore stability.

[0113] (4) Linearization and solution of nonlinear terms.

[0114] Solving the QP optimization problem requires that the cost function be the decision variable. The quadratic form therefore requires the nonlinear term to be... and At the current work point Perform a first-order Taylor expansion at this point:

[0115]

[0116] constant term and In optimization, linear terms are discarded (without affecting the optimal solution). and Incorporate gradient vector The QP optimization problem is transformed into a standard quadratic form:

[0117] Among them, the Hessian matrix It consists of the weighted sum of the coefficients of each quadratic form; Let be the gradient vector. Due to the positive definiteness of the robotic arm's inertia matrix and the introduction of regularization terms, Always righteous, the solution is unique.

[0118] As a preferred embodiment of this invention, the QP solver can be qpOASES (suitable for small-scale problems, with a short solution time). ) or OSQP (suitable for large-scale sparse problems).

[0119] VI. Control cycle algorithm flow.

[0120] like Figure 4 As shown, the following steps are executed sequentially in each control cycle.

[0121] 1) Sensor Data Acquisition. Acquire data from various sensors, including but not limited to: joint encoder readings (to obtain the current angles of each joint). Force sensor readings (to acquire end contact force), camera images (to acquire current visual observations), and lidar 17 point cloud (to acquire environmental obstacle information).

[0122] 2) State estimation. The current system state, including the current generalized coordinates, is estimated based on sensor data. and its speed And the poses of each end and camera in the world coordinate system. , .

[0123] 3) Calculate the observation quality function and its gradient. Based on the current camera pose... Calculate the observation quality function based on the target object information. and its gradient with respect to the generalized coordinates of the entire system Specifically, calculate the three sub-items separately. , , and its impact The partial derivatives, and then through the chain rule Transmitted to the configuration space.

[0124] 4) Merge gradients. Merge the gradients of the task tracking by the robotic arm. With observation quality gradient Merge to form the gradient vector of QP .

[0125] 5) Construct the QP optimization problem. Based on the cost function and constraints described above, construct the complete QP optimization problem, including the Hessian matrix. gradient vector Linear constraint matrix and boundary values.

[0126] 6) Solve the QP optimization problem. Call the QP solver to find the optimal control quantity. .

[0127] 7) Issue control commands. Chassis speed in the middle and the joint speed of the robotic arm The data is distributed to the chassis driver and the robotic arm joint driver, respectively.

[0128] 8) Proceed to the next cycle. Wait for the next control cycle to be triggered, then return to step 1), and repeat until the task ends.

[0129] VII. Methods and systems for optimizing the posture of mobile robot chassis.

[0130] In summary, such as Figure 5 As shown in the figure, the mobile robot chassis pose optimization method based on observation quality driven in this embodiment includes the following steps.

[0131] S1: Establish the generalized coordinate system of mobile robot 10. ,in: Indicates the position and orientation of a wheeled chassis; Represent the joint angle vectors of the robotic arm; establish a coordinate system from the world coordinate system. via chassis coordinate system To each camera coordinate system The complete coordinate transformation chain.

[0132] S2: Constructing the observation quality function The observation quality function This is used to quantify the current observation conditions of at least one camera on the target object, and its value decreases monotonically as the observation conditions improve. right Differentiable.

[0133] S3: Calculate the observation quality function within each control cycle. chassis position gradient The gradient is then incorporated as a cost term into the unified quadratic programming (QP) optimization problem.

[0134] S4: Solve the unified quadratic programming optimization problem to obtain the joint velocity decision variables of the entire system. The joint velocity decision variables of the entire system Includes chassis speed With the joint speed of the robotic arm .

[0135] S5: Set the chassis speed The command is sent to the chassis drive, which moves the chassis to adjust the camera's pose in a direction that optimizes the observation conditions.

[0136] This embodiment also proposes a mobile robot chassis pose optimization system based on observation quality, comprising: a wheeled drive chassis; at least one multi-degree-of-freedom tandem robotic arm mounted on the chassis; at least one camera mounted on the chassis or the robotic arm for acquiring images and / or depth information of a target object; a processor configured to execute the mobile robot chassis pose optimization method based on observation quality as described in this embodiment; and a memory for storing the program and data required by the processor to execute the method; the processor calculates the observation quality function in each control cycle. and its gradient with respect to chassis pose The cost term containing the gradient is incorporated into a unified quadratic programming optimization problem for unified solution, and the chassis speed command obtained from the solution is sent to the chassis drive.

[0137] VIII. Examples

[0138] The technical solution and its effects of the present invention will be described in detail below through a specific example.

[0139] (1) Multi-object grasping.

[0140] In this embodiment, a wheeled dual-arm robot is parked in front of a workbench, on which target objects A, B, and C are placed, arranged in an arc shape. Figure 6 As shown, in the initial pose of the robot's torso camera 15: the viewing angle for object A is approximately 5° (close to facing directly), the viewing angle for object B is approximately 15°, and the viewing angle for object C is approximately 35°. The robot needs to grasp these three objects in sequence, with the grasping order being C, A, and B.

[0141] The robot's control system adopts the observation quality-driven chassis pose optimization method provided in this embodiment. Each control cycle ( ) Perform a complete perception-decision-execution closed loop.

[0142] (2) The grabbing process.

[0143] When the robot prepares to grasp object C, the vision system detects the current spectral quality of object C. Too high. QP was detected during the solution process. Gradient of chassis pose The instruction "Panning to the right can improve the view" is also available. The instruction reads, "Turn left to get closer to the target surface."

[0144] After jointly optimizing various costs, the QP system outputs chassis control commands: During the grasping process, the chassis autonomously translates to the right by approximately 5 cm and rotates to the left by approximately 3°. This fine-tuning improves the camera's viewing angle for object C from 35° to approximately 8°. Simultaneously, the viewing angle for object A, which was initially better, slightly deteriorates from 5° to approximately 12°. However, since 12° is still within the good observation range, the absolute value corresponding to A in the cost function remains very low. The system balances the optimization direction based on the net improvement of global observation conditions. Throughout the fine-tuning process, the chassis has no preset target pose and is entirely driven autonomously by the observation quality gradient.

[0145] In contrast, under the same initial pose (skewed 35° to object C), if the camera cost in QP adopts the common form of servo error ( Since the feature points are still within the field of view and the servo error is small, the gradient... When the angle is close to zero, QP does not generate chassis fine-tuning commands. The 35° skewed view will be maintained until the end of the grasping process. At this view, depth estimation noise increases and the point cloud becomes sparse, thus significantly reducing grasping accuracy.

[0146] The difference between the two results stems from the different optimization semantics carried by the camera cost function: servo error does not reflect observation quality, while the observation quality function does. This embodiment replaces servo error with the observation quality function, fundamentally changing the role of the chassis in the unified optimization framework: upgrading it from a "passive mobile platform reaching a preset position" to a "degree of freedom for continuously and actively improving observation conditions."

[0147] Those skilled in the art should understand that the above embodiments are merely one specific implementation of the present invention, used to illustrate the technical solution of the present invention and not to limit it. In practical applications, parameters (such as the weights of each sub-item) can be adjusted according to specific task requirements and hardware configuration. QP weight Control cycle Adjustments and optimizations (such as those made to the present invention) are all within the scope of protection of this invention.

[0148] The embodiments described above are merely preferred embodiments for fully illustrating the present invention, and the scope of protection of the present invention is not limited thereto. Equivalent substitutions or modifications made by those skilled in the art based on the present invention are all within the scope of protection of the present invention. The scope of protection of the present invention is defined by the claims.

Claims

1. A method for optimizing the chassis pose of a mobile robot based on observation quality, characterized in that: Includes the following steps: S1: Establish the generalized coordinate system of the mobile robot. ,in: Indicates the position and orientation of the wheeled chassis. For the chassis in the world coordinate system The two-dimensional position below, This is the chassis heading angle; Represent the joint angle vectors of the robotic arm; establish a coordinate system from the world coordinate system. via chassis coordinate system To each camera coordinate system The complete coordinate transformation chain; S2: Constructing the observation quality function The observation quality function This is used to quantify the current observation conditions of at least one camera on the target object, and its value decreases monotonically as the observation conditions improve. right Differentiable; Observational mass function It consists of a weighted sum of the observation quality from M cameras: in: This is a visibility sub-item used to quantify the target object in the [missing information - likely a specific location or event]. The degree to which the view of the camera is obstructed; For the perspective quality sub-item, used to quantify the first The degree of deviation between the camera's line of sight and the target surface's normal direction; The depth-of-field suitability sub-item is used to quantize the target object to the [missing information - likely a specific depth-of-field value]. The degree to which the distance between the cameras deviates from the optimal working distance; For the first The fusion weights of multiple cameras; , and These are the weighting coefficients for the three sub-items; ; S3: Calculate the observation quality function within each control cycle. chassis position gradient The gradient is then incorporated as a cost term into the unified quadratic programming optimization problem. The cost function for the unified quadratic programming optimization problem is: in: For the joint velocity decision variables of the entire system; This is the spatial trajectory tracking error term for the end-stage mission. Penalty item for base movement; The observation quality cost function; For the potential field of the entire system, it represents the obstacle avoidance or control function term. For full-joint velocity smoothing and energy minimization regularization; The Jacobian matrix for all system tasks; The desired task space velocity; The weighted penalty coefficient for each optimization objective; Nonlinear terms and In the current state Perform a first-order Taylor expansion at the given point, and incorporate the linear terms into the gradient vector of the unified quadratic programming; S4: Solve the unified quadratic programming optimization problem to obtain the joint velocity decision variables of the entire system. The joint velocity decision variables of the entire system Includes chassis speed With the joint speed of the robotic arm ; S5: Set the chassis speed The command is sent to the chassis drive, which moves the chassis to adjust the camera's pose in a direction that optimizes the observation conditions.

2. The mobile robot chassis pose optimization method based on observation quality driven according to claim 1, characterized in that: The visibility sub-item Represented as: in: Preset to small positive numbers; For the target object in the first The probability that the field of view of a camera is not obstructed is calculated using a directed range field and a sigmoid smooth mapping: in: It represents the directed distance from the spatial sampling point to the surface of the nearest obstacle along the camera's line of sight; This is the preset smoothing factor; The target object in the first The probability that the field of view of the camera is not obstructed. Camera pose The gradient is obtained through the chain rule. Analytical calculation, where: It is given by the gradient field of the directed distance field.

3. The mobile robot chassis pose optimization method based on observation quality driven according to claim 1, characterized in that: The perspective quality sub-item Represented as: in: This is the unit normal vector of the target object's surface at the target point; For from the first The optical center of the camera is pointed in the unit line-of-sight direction to the target point; A preset small positive number is used to ensure that the function... Differentiability at =0; Unit normal vector The acquisition method is as follows: for target objects with CAD models, the surface normal vector is read from the aligned model after ICP point cloud registration; for unknown target objects, the 3D point cloud is taken in the neighborhood of the target area of ​​the depth image for local principal component analysis, and the feature direction corresponding to the minimum eigenvalue is taken as the normal vector.

4. The mobile robot chassis pose optimization method based on observation quality driven according to claim 1, characterized in that: The depth of field suitability sub-item Represented as: in: For from the first The distance measured from the camera to the target object; For the first The optimal working distance for a single camera; For the first The effective working distance range of the camera, and .

5. The observation quality-driven mobile robot chassis pose optimization method according to any one of claims 1-4, characterized in that: In step S3, the gradient The calculation method is as follows: in: , and By visibility sub-items Perspective Quality Sub-item and depth of field suitability sub-item The parsing expression is calculated directly; The results are given analytically from the aforementioned coordinate transformation chain.

6. The mobile robot chassis pose optimization method based on observation quality driven according to claim 1, characterized in that: The unified quadratic programming optimization problem also includes ZMP stability constraints: in: This is the horizontal projection of the system's zero-moment point into the world system. A linear inequality description of the convex hull of the supporting polygon formed by the grounding points of the chassis wheel assembly; To control the cycle, They are slack variables; and The expression is: in: Let be the horizontal projection of the system's center of mass into the world system; The height of the center of mass above the ground; It is the acceleration due to gravity; This is the horizontal acceleration of the center of mass.

7. The mobile robot chassis pose optimization method based on observation quality driven according to claim 1, characterized in that: The mobile robot includes: Wheel-driven chassis; At least one multi-degree-of-freedom serial robotic arm is mounted on the chassis; At least one camera, mounted on the chassis or robotic arm, is used to acquire images and / or depth information of the target object; The chassis, the robotic arm, and the camera share the same set of generalized coordinates in the unified quadratic programming optimization problem. The chassis speed With the joint speed of the robotic arm As the same decision variable The components are solved uniformly by the unified quadratic programming optimization problem.

8. A mobile robot chassis pose optimization system based on observation quality, characterized in that: include: Wheel-driven chassis; At least one multi-degree-of-freedom serial robotic arm is mounted on the chassis; At least one camera, mounted on the chassis or the robotic arm, is used to acquire images and / or depth information of the target object; The processor is configured to execute the observation quality-driven mobile robot chassis pose optimization method as described in any one of claims 1-7; A memory for storing programs and data required by the processor to execute the method; The processor calculates the observation quality function in each control cycle. and its gradient with respect to chassis pose The cost term containing the gradient is incorporated into a unified quadratic programming optimization problem for unified solution, and the chassis speed command obtained from the solution is sent to the chassis drive.

Images