A mobile manipulator robot model predictive control method and system

Abstract

This invention discloses a model predictive control method and system for a mobile manipulator, relating to the field of robot intelligent control technology. The method includes: S1, constructing a full-body kinematic model of the mobile chassis and robotic arm, using the chassis pose and robotic arm joint angles as state variables, and the chassis linear velocity, steering angular velocity, and joint angular velocities as control variables, while considering nonholonomic constraints on the chassis. The proposed model predictive control method and system for a mobile manipulator constructs an Euclidean symbolic distance field by caching voxel distance gradients, simplifying distance and gradient query operations to local linear calculations. This significantly reduces the computational overhead of the online perception stage, enabling stable operation of real-time collision avoidance within the high-frequency control loop. Furthermore, by introducing a relaxation obstacle function within the unified model predictive control framework, it solves the problem of dispersed constraint processing in traditional methods, thereby improving the overall motion coordination capability.

Description

Technical Field

[0001] This invention relates to the field of robot intelligent control technology, specifically to a model predictive control method and system for a mobile operating robot. Background Technology

[0002] Mobile manipulators combine the wide operating range of a mobile platform with the precision manipulating capabilities of a robotic arm, demonstrating significant application value in unstructured environments such as construction, industrial inspection, and logistics handling. To achieve autonomous operation, these robots need to perceive their environment online and generate control commands in real time, simultaneously satisfying multiple objectives such as end-effector trajectory tracking, dynamic obstacle avoidance, and interactive force control. Model predictive control, due to its ability to explicitly handle multiple constraints and perform rolling optimization within a finite time domain, has become an important technological approach in the field of mobile manipulator control. Building upon this, integrating visual and force perception information to coordinate the full-body motion of the chassis and robotic arm within a unified framework is a key technological direction for enabling robots to operate safely, accurately, and stably in unknown dynamic environments.

[0003] However, existing technologies still face several challenges in practical applications. In environmental perception and motion planning, sampling-based global planning methods are computationally time-consuming and lack timely response to dynamic obstacles. While grid-map-based online planning can improve real-time performance, repeated interpolation queries of distance information in high-dimensional state spaces incur significant computational overhead. Regarding force-position coordination control, admittance control or force-position hybrid control typically operate as independent modules, lacking deep integration with the motion planner, resulting in limited stability and accuracy of force tracking in continuous contact operations. In terms of whole-body motion integration, while joint modeling of the mobile chassis and multi-degree-of-freedom robotic arm improves system redundancy, it also presents various heterogeneous constraints such as joint limits, overall stability, real-time obstacle avoidance, and interactive force response. Traditional methods often handle these constraints separately, making efficient collaboration difficult within a unified optimization framework. To address these challenges, we propose a model predictive control method and system for mobile manipulators. Summary of the Invention

[0004] To address the aforementioned technical problems, a model predictive control method and system for mobile operating robots is provided. This technical solution solves the problems of insufficient real-time performance of the sampling-based planning method, which is difficult to cope with dynamic environments; high computational overhead for online querying of environmental distance information; insufficient integration of force-position coordination control and motion planning; limited force tracking accuracy and stability under continuous contact operations; and the fact that various heterogeneous constraints introduced by the joint control of the mobile chassis and the robotic arm are often handled separately, lacking a unified whole-body collaborative optimization framework.

[0005] To achieve the above objectives, the technical solution adopted by the present invention is as follows: A model predictive control method for a mobile robot includes: S1. Construct a full-body kinematic model of the mobile chassis and robotic arm, taking the chassis pose and robotic arm joint angles as state variables, and the chassis linear velocity, steering angular velocity and joint angular velocities as control variables, while considering the chassis nonholonomic constraints. S2. Acquire environmental data through depth sensors, generate a three-dimensional Euclidean symbol distance field online, and synchronously calculate and store the distance gradient information of each voxel during map building and updating; S3. Configure a set of collision detection spheres on the robot's kinematic chain, estimate the distance from the center of the sphere to the nearest obstacle using the cached gradient, and construct collision avoidance constraints; construct mechanical stability constraints based on the relationship between the zero moment point and the inscribed circle of the supporting polygon; at the same time, introduce joint limit constraints, and use a relaxation obstacle function to unify the above constraints into a soft constraint penalty term in the cost function. S4. Construct the cost function of model predictive control, which includes the end effector pose tracking error term, the control input weighted penalty term, and the soft constraint penalty term; S5. Collect the deviation between the measured force spinner of the end effector and the desired force spinner, generate the displacement compensation amount through admittance control, and superimpose it with the original desired trajectory to form an augmented reference trajectory. S6. Sequential linear quadratic model predictive control is adopted. In the prediction time domain, the kinematic model is linearized and the cost function is approximated twice. The algebraic Riccati equation is solved to obtain the control sequence containing chassis speed command and robotic arm joint speed command.

[0006] Preferably, a system state vector for the mobile robot is defined, which consists of the pose of the mobile chassis and the angles of each joint of the robotic arm; the pose of the mobile chassis includes a three-dimensional position vector and an attitude quaternion in the world coordinate system. Define the control input vector of the mobile robot, which consists of the forward linear velocity of the mobile chassis, the turning angular velocity of the mobile chassis, and the angular velocities of each joint of the robotic arm; The type of mobile chassis is determined to be differential drive, skid steering, or tracked, and the mobile chassis has non-holonomic constraint characteristics.

[0007] Preferably, the whole-body kinematic equations of the mobile robot are constructed, and the whole-body kinematic equations are in block matrix form; The first row of the block matrix corresponds to the attitude update of the mobile chassis, which is calculated by the Hamiltonian product of the attitude quaternion and the steering angular velocity. The second row of the block matrix corresponds to the position update of the moving chassis, which is calculated by rotating the forward linear velocity vector using attitude quaternions. The third row of the block matrix corresponds to the update of the robot arm joint positions, which is directly obtained by integrating the angular velocities of each joint of the robot arm.

[0008] Preferably, three-dimensional depth data of the environment is acquired through a depth camera or LiDAR; An incremental mapping framework is used to generate a three-dimensional Euclidean symbolic distance field, which is composed of multiple voxels; Each voxel stores the signed distance value from the voxel center to the nearest obstacle; During the mapping and updating phase, the distance gradient information of each voxel is calculated synchronously and stored in the corresponding voxel.

[0009] Preferably, for any query point in space, the voxel containing the query point is located; The distance from the query point to the nearest obstacle is estimated using a local linear model. The local linear model is calculated by adding the cached distance value of the voxel to the dot product of the cached gradient vector and the difference between the coordinates of the query point and the voxel center. Distance gradient information of voxels is recalculated only within the local map update area; global gradient updates are not performed.

[0010] Preferably, a sphere is used to construct a collision avoidance constraint; the collision avoidance constraint is defined as: the difference between the signed distance from the center of the i-th collision detection sphere to the nearest obstacle and the radius of the sphere is greater than or equal to zero; Calculate the robot's zero-moment point and construct mechanical stability constraints; the mechanical stability constraints are defined as: the difference between the square of the radius of the inscribed circle of the supporting polygon and the square of the distance from the zero-moment point to the center of the supporting surface is greater than or equal to zero; Construct joint limiting constraints, which include position and speed limits for each joint of the robotic arm; The collision avoidance constraint, mechanical stability constraint, and joint limit constraint are uniformly transformed into a soft constraint penalty term by using a relaxation obstacle function. The relaxation obstacle function is a piecewise function. When the constraint function value is greater than the threshold, it adopts a logarithmic form, and when the constraint function value is less than or equal to the threshold, it adopts a quadratic function form that is continuously differentiable.

[0011] Preferably, a cost function for model predictive control is constructed, which consists of an end effector pose tracking error term, a soft constraint penalty term, and a control input weighted penalty term; The end effector pose tracking error term includes translation error and rotation error. The translation error is calculated using the L2 norm of the difference between the actual position and the desired position vector, and the rotation error is calculated using the quaternion error formula. The weighted penalty term for the control input is calculated by multiplying the transpose of the control input vector by the weight matrix.

[0012] Preferably, the measured force rotation of the end effector is collected, and the measured force rotation includes three-dimensional interactive force and three-dimensional interactive torque; Calculate the force deviation between the measured force spinor and the expected force spinor; Task space admittance control is used to map force deviation into displacement compensation of the end effector, which includes translational compensation and rotational compensation. The task space admittance control calculates the displacement compensation by multiplying the proportional gain matrix by the force deviation and adding the integral gain matrix by the integral of the force deviation over time. Set an anti-saturation limit for the integral term of the mission space admittance control; The displacement compensation amount is superimposed on the original desired trajectory to generate an augmented reference trajectory.

[0013] Preferably, the nonlinear system is forward-rolled using the current control strategy to obtain the state trajectory and input trajectory in the prediction time domain; The whole-body kinematics model is linearized along the state trajectory and input trajectory, and the cost function is approximated twice. Solve the algebraic Riccati equation to obtain an affine control strategy that includes time-varying state feedback gain and feedforward term; The control frequency is dynamically switched according to the task type. The conventional trajectory tracking task is executed at the first frequency, and the admittance control task is executed at a second frequency higher than the first frequency. Generate a control sequence that includes speed commands for the moving chassis and speed commands for each joint of the robotic arm.

[0014] A predictive control system for a mobile robot model, comprising: The system comprises a perception mapping module, a state estimation module, a trajectory planning module, an admittance adjustment module, a model predictive control module, and an execution module. The perception mapping module is used to acquire environmental data in real time through a depth sensor, generate a three-dimensional Euclidean symbolic distance field online, and synchronously calculate and cache the distance gradient information of each voxel during the mapping update stage. The state estimation module is used to obtain the pose and velocity of the mobile chassis, the angles and angular velocities of each joint of the robotic arm, and the measured force rotation of the end effector. The trajectory planning module is used to receive the desired trajectory in the task space and generate a time-scaled reference trajectory that satisfies the velocity and acceleration constraints. The admittance adjustment module is connected to the state estimation module and the trajectory planning module. It is used to generate a displacement compensation amount by task space admittance control based on the deviation between the measured force spin and the expected force spin, and to superimpose it with the reference trajectory to form an augmented reference trajectory. The model predictive control module is connected to the perception mapping module and the admittance adjustment module. It is used to construct a whole-body kinematic model of the mobile chassis and the robotic arm. The cost function is constructed with the end effector pose tracking error term, the control input weighted penalty term, and the relaxed obstacle function soft constraint penalty term including collision avoidance constraint, zero torque point stability constraint and joint limit constraint. The rolling time domain optimization solution is performed by sequential linear quadratic model predictive control to generate a control sequence containing chassis speed command and robotic arm joint speed command. The execution module is used to send control sequences to the mobile chassis driver and the robotic arm joint controller.

[0015] Compared with the prior art, the beneficial effects of the present invention are as follows: The proposed model predictive control method and system for mobile robots constructs an Euclidean symbolic distance field by caching voxel distance gradients, simplifying distance and gradient lookup operations to local linear calculations. This significantly reduces the computational overhead of online perception, enabling stable operation of real-time collision avoidance within high-frequency control loops. Furthermore, by introducing a relaxation barrier function within a unified model predictive control framework, it transforms various heterogeneous constraints, such as collision avoidance, joint limiting, and overall stability based on zero-moment points, into soft constraint penalty terms. This solves the problem of dispersed constraint processing in traditional methods, ensuring successful solution to the optimization problem while improving overall robot stability. The system's motion coordination capability deeply integrates task space admittance control and model predictive control. By generating augmented reference trajectories online through measured force deviations, position tracking and interactive force adjustment are executed collaboratively in a single optimization process, improving force tracking accuracy and compliance under continuous contact operations. The system employs sequential linear quadratic model predictive control for rolling time-domain solutions, balancing computational efficiency and control frequency requirements, enabling the system to respond quickly in dynamic and unknown environments. Furthermore, by utilizing admittance control to indirectly achieve force control through position commands, the requirements for the joint torque control capability of the robotic arm are reduced, facilitating deployment and application on industrial-grade high-load platforms. Attached Figure Description

[0016] Figure 1 This is a flowchart of the mobile operation robot model predictive control method of the present invention; Figure 2 This is a schematic diagram illustrating the construction and querying of an ESDF with cached gradients according to the present invention; Figure 3 This is a schematic diagram of the collision detection sphere arranged along the robot's motion chain according to the present invention. Figure 4 This is a schematic diagram of the mechanical stability constraint based on the zero torque point of the present invention; Figure 5 This is a schematic diagram of the mission space admittance control principle of the present invention; Figure 6This is a diagram showing the overall architecture of the mobile operation robot model prediction control system of the present invention. Detailed Implementation

[0017] The following description is intended to disclose the invention and enable those skilled in the art to implement it. The preferred embodiments described below are merely examples, and other obvious variations will occur to those skilled in the art.

[0018] Reference Figure 1 As shown, a model predictive control method for a mobile operating robot includes the following steps: S1. Construct a full-body kinematic model of the mobile chassis and robotic arm, taking the chassis pose and robotic arm joint angles as state variables, and the chassis linear velocity, steering angular velocity and joint angular velocities as control variables, while considering the chassis nonholonomic constraints. S1 includes: Define a system state vector for a mobile robot, which consists of the pose of the mobile chassis and the angles of each joint of the robotic arm; the pose of the mobile chassis includes a three-dimensional position vector and an attitude quaternion in the world coordinate system. Define the control input vector of the mobile robot, which consists of the forward linear velocity of the mobile chassis, the turning angular velocity of the mobile chassis, and the angular velocities of each joint of the robotic arm; The type of mobile chassis is determined to be differential drive, skid steering, or tracked, and the mobile chassis has non-holonomic constraint characteristics.

[0019] S1 further includes: Construct the whole-body kinematic equations of the mobile robot, wherein the whole-body kinematic equations are in block matrix form; The first row of the block matrix corresponds to the attitude update of the mobile chassis, which is calculated by the Hamiltonian product of the attitude quaternion and the steering angular velocity. The second row of the block matrix corresponds to the position update of the moving chassis, which is calculated by rotating the forward linear velocity vector using attitude quaternions. The third row of the block matrix corresponds to the update of the robot arm joint positions, which is directly obtained by integrating the angular velocities of each joint of the robot arm.

[0020] The specific form of the whole-body kinematic equation is:

[0021] In the formula, It is the derivative of the system state vector. The attitude quaternion of the moving chassis. This represents the Hamiltonian product of quaternions. This represents the chassis steering angular velocity. This indicates that a quaternion is used to rotate a vector. This represents the forward speed of the chassis. This represents the angular velocity vector of each joint of the robotic arm.

[0022] S2. Acquire environmental data through depth sensors, generate a three-dimensional Euclidean symbol distance field online, and synchronously calculate and store the distance gradient information of each voxel during map building and updating; S2 includes: Acquire 3D depth data of the environment using depth cameras or LiDAR; An incremental mapping framework is used to generate a three-dimensional Euclidean symbolic distance field, which is composed of multiple voxels; Each voxel stores the signed distance value from the voxel center to the nearest obstacle; During the mapping and updating phase, the distance gradient information of each voxel is calculated synchronously and stored in the corresponding voxel.

[0023] S2 further includes: For any query point in space, locate the voxel where the query point is located; The distance from the query point to the nearest obstacle is estimated using a local linear model. The local linear model is calculated by adding the cached distance value of the voxel to the dot product of the cached gradient vector and the difference between the coordinates of the query point and the voxel center. Distance gradient information of voxels is recalculated only within the local map update area; global gradient updates are not performed.

[0024] Reference Figure 2 As shown, each voxel in the 3D Euclidean signed distance field stores the coordinates of its center, the signed distance to the nearest obstacle, and the distance gradient vector. The specific form of the local linear model is: In the formula, For query point Estimated distance to the nearest obstacle. This is the index of the voxel where the query point is located. The coordinates of the center of the voxel are given. This is the signed distance value from the voxel center to the nearest obstacle. This is the cached voxel distance gradient vector. Taking a map with 1 million voxels and 5 cm side length as an example, gradient updates are only performed on voxels in newly added or changed areas, and the online computational overhead is negligible. The distance and gradient query speed is more than 50 times faster than traditional interpolation methods, which can meet the real-time requirements of high-frequency control loops.

[0025] S3. Configure a set of collision detection spheres on the robot's kinematic chain, estimate the distance from the center of the sphere to the nearest obstacle using the cached gradient, and construct collision avoidance constraints; construct mechanical stability constraints based on the relationship between the zero moment point and the inscribed circle of the supporting polygon; at the same time, introduce joint limit constraints, and use a relaxation obstacle function to unify the above constraints into a soft constraint penalty term in the cost function. S3 includes: A set of collision detection spheres are arranged along the robot's kinematic chain to construct a collision avoidance constraint; the collision avoidance constraint is defined as: the difference between the signed distance from the center of the i-th collision detection sphere to the nearest obstacle and the radius of the sphere is greater than or equal to zero; Calculate the robot's zero-moment point and construct mechanical stability constraints; the mechanical stability constraints are defined as: the difference between the square of the radius of the inscribed circle of the supporting polygon and the square of the distance from the zero-moment point to the center of the supporting surface is greater than or equal to zero; Construct joint limiting constraints, which include position and speed limits for each joint of the robotic arm; The collision avoidance constraint, mechanical stability constraint, and joint limit constraint are uniformly transformed into a soft constraint penalty term by using a relaxation obstacle function. The relaxation obstacle function is a piecewise function. When the constraint function value is greater than the threshold, it adopts a logarithmic form, and when the constraint function value is less than or equal to the threshold, it adopts a quadratic function form that is continuously differentiable.

[0026] Reference Figure 3 As shown, multiple collision detection spheres are arranged sequentially along the robot's kinematic chain from the chassis base to the end effector. All spheres share intermediate results from kinematic calculations, and increasing the number of spheres does not significantly increase the computational burden. The specific form of the collision avoidance constraint is as follows:

[0027] In the formula, For the first The constraint function corresponding to the collision detection sphere. (This is the first...) The position of the center of the collision detection sphere in the world coordinate system. Let be the radius of the sphere. This is the distance to the nearest obstacle obtained through fast querying of cached gradients.

[0028] Reference Figure 4 As shown, mechanical stability constraints are constructed with the zero-moment point as the core. The formula for calculating the zero-moment point is:

[0029] In the formula, The projection coordinates of the zero-moment point on the support surface. Let be the unit normal vector of the supporting surface. Let be the position of the center of mass of the robot system. This is the gravity vector. This indicates the position of the end effector. and These represent the interaction force and torque acting on the end effector, respectively. The specific form of the mechanical stability constraint is:

[0030] In the formula, The radius of the inscribed circle supporting the polygon. This involves L2 norm operations. The specific form of the relaxation barrier function is:

[0031] In the formula, For the first The relaxation barrier function value corresponding to each constraint. For constraint functions. It is a quadratic function that is continuously differentiable. and These are adjustable parameters that control the transition range of penalty intensity and soft constraint, respectively.

[0032] S4. Construct the cost function of model predictive control, which includes the end effector pose tracking error term, the control input weighted penalty term, and the soft constraint penalty term; S4 includes: A cost function for model predictive control is constructed, which consists of an end-effector pose tracking error term, a soft constraint penalty term, and a control input weighted penalty term. The end effector pose tracking error term includes translation error and rotation error. The translation error is calculated using the L2 norm of the difference between the actual position and the desired position vector, and the rotation error is calculated using the quaternion error formula. The weighted penalty term for the control input is calculated by multiplying the transpose of the control input vector by the weight matrix.

[0033] The specific form of the cost function is:

[0034]

[0035] In the formula, To predict the total cost in the time domain. To predict the length of the time domain. This is the cost per step. This is the end effector pose tracking error term. This is the sum of the penalty terms corresponding to all soft constraints. To control the input weighting penalty term. To control the input weight matrix, the rotational error of the end effector is calculated using a quaternion error formula, specifically:

[0036] In the formula, is the rotation error vector. The actual attitude quaternion of the end effector. Let be the desired orientation quaternion of the end effector. This represents the vector cross product operation. The Gaussian-Newton approximation is used for the Hessian matrix of the tracking error term to ensure its positive semi-definiteness and avoid numerical instability during the optimization process.

[0037] S5. Collect the deviation between the measured force spinner of the end effector and the desired force spinner, generate the displacement compensation amount through admittance control, and superimpose it with the original desired trajectory to form an augmented reference trajectory. S5 includes: The measured force spin of the end effector is collected, and the measured force spin includes three-dimensional interactive force and three-dimensional interactive torque. Calculate the force deviation between the measured force spinor and the expected force spinor; Task space admittance control is used to map force deviation into displacement compensation of the end effector, which includes translational compensation and rotational compensation. The task space admittance control calculates the displacement compensation by multiplying the proportional gain matrix by the force deviation and adding the integral gain matrix by the integral of the force deviation over time. Set an anti-saturation limit for the integral term of the mission space admittance control; The displacement compensation amount is superimposed on the original desired trajectory to generate an augmented reference trajectory.

[0038] Reference Figure 5 As shown, mission space admittance control converts force deviation into trajectory displacement compensation, forming a closed-loop force control loop. The formula for calculating the displacement compensation is:

[0039] In the formula, For the first The displacement compensation amount corresponding to each trajectory point. This is the amount of translation compensation. This is the rotational compensation amount. This is the proportional gain matrix. This is the integral gain matrix. This represents the deviation between the measured force curl and the desired force curl. The augmented reference trajectory is obtained by superimposing the original desired trajectory with the displacement compensation, and its specific form is as follows:

[0040] In the formula, To broaden the reference trajectory One point. The first of the original expected trajectories One point. This represents the superposition calculation of the task space pose. The integral term is set with an anti-saturation limit; when the displacement compensation reaches a preset threshold, integral accumulation stops to prevent integral saturation. In practical applications, the ratio and integral gain can be adjusted according to different tasks. For example, in grinding operations, appropriately increasing the integral gain in the force control direction improves the steady-state accuracy of force tracking.

[0041] S6. Sequential linear quadratic model predictive control is adopted. In the prediction time domain, the kinematic model is linearized and the cost function is approximated twice. The algebraic Riccati equation is solved to obtain the control sequence containing chassis speed command and robotic arm joint speed command.

[0042] S6 includes: The nonlinear system is forward-rolled using the current control strategy to obtain the state trajectory and input trajectory in the predicted time domain. The whole-body kinematics model is linearized along the state trajectory and input trajectory, and the cost function is approximated twice. Solve the algebraic Riccati equation to obtain an affine control strategy that includes time-varying state feedback gain and feedforward term; The control frequency is dynamically switched according to the task type. The conventional trajectory tracking task is executed at the first frequency, and the admittance control task is executed at a second frequency higher than the first frequency. Generate a control sequence that includes speed commands for the moving chassis and speed commands for each joint of the robotic arm.

[0043] The sequential linear quadratic model predictive control performs rolling optimization within each control cycle. First, the nonlinear system is forward rolled using the current control strategy to obtain the state trajectory and input trajectory in the prediction time domain.

[0044] Then, a first-order Taylor expansion is performed on the whole-body kinematic model along this trajectory to achieve linearization, and a second-order Taylor expansion is performed on the cost function to achieve a quadratic approximation. Next, the algebraic Riccati equation is solved to obtain an affine control strategy that includes time-varying state feedback gain and feedforward terms.

[0045] The control frequency is dynamically switched according to the task type. A control frequency of 20 Hz is used for routine trajectory tracking tasks, while a control frequency of 100 Hz is used for admittance force control tasks, ensuring rapid response capability for force tracking. In the hardware implementation, control commands are sent to the actuator at a frequency of no less than 100 Hz, forming a closed-loop feedback control. For example, in spraying operations, the model prediction controller operates at a frequency of 20 Hz, and the average position error for tracking a 1.5-meter-long reference trajectory is 15 mm to 21 mm, with an average rotation error of 2.3 degrees to 3.1 degrees. In wiping operations, the controller operates at a frequency of 100 Hz, and when the desired force is 55 N, the measured average force is 54.995 N, with a standard deviation of 0.369 N, achieving high-precision force tracking.

[0046] Reference Figure 6 As shown, a mobile operation robot model prediction control system includes: The system comprises a perception mapping module, a state estimation module, a trajectory planning module, an admittance adjustment module, a model predictive control module, and an execution module. The perception mapping module is used to acquire environmental data in real time through a depth sensor, generate a three-dimensional Euclidean symbolic distance field online, and synchronously calculate and cache the distance gradient information of each voxel during the mapping update stage. The state estimation module is used to obtain the pose and velocity of the mobile chassis, the angles and angular velocities of each joint of the robotic arm, and the measured force rotation of the end effector. The trajectory planning module is used to receive the desired trajectory in the task space and generate a time-scaled reference trajectory that satisfies the velocity and acceleration constraints. The admittance adjustment module is connected to the state estimation module and the trajectory planning module. It is used to generate a displacement compensation amount by task space admittance control based on the deviation between the measured force spin and the expected force spin, and to superimpose it with the reference trajectory to form an augmented reference trajectory. The model predictive control module is connected to the perception mapping module and the admittance adjustment module. It is used to construct a whole-body kinematic model of the mobile chassis and the robotic arm. The cost function is constructed with the end effector pose tracking error term, the control input weighted penalty term, and the relaxed obstacle function soft constraint penalty term including collision avoidance constraint, zero torque point stability constraint and joint limit constraint. The rolling time domain optimization solution is performed by sequential linear quadratic model predictive control to generate a control sequence containing chassis speed command and robotic arm joint speed command. The execution module is used to send control sequences to the mobile chassis driver and the robotic arm joint controller.

[0047] The fusion-sensor mobile robot model predictive control system adopts a modular architecture, with modules interacting via standardized interfaces. The perception mapping module supports multiple sensors such as depth cameras and LiDAR, generating a 3D Euclidean symbolic distance field that can be updated in real time to meet dynamic environment perception requirements. The state estimation module integrates data from wheel odometry, IMU, and joint encoders to output high-precision robot state information. The trajectory planning module receives discrete path points from the upper-level task planner and generates a smooth time-scaled reference trajectory that satisfies velocity and acceleration constraints. The admittance adjustment module operates independently, publishing augmented reference trajectories at a frequency of 100 Hz to ensure real-time force control. The model predictive control module, as the system core, integrates kinematic modeling, constraint construction, and optimization solving functions, using the SLQ solver in the OCS2 toolbox for rapid optimization. The derivatives of the system model and cost function are automatically generated using the CodeGen module of the CppAD tool, improving computational efficiency. The execution module converts control commands into signals recognizable by the actuators, driving the mobile chassis and robotic arm to complete predetermined tasks.

[0048] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The embodiments and descriptions in the specification are merely principles of the invention. Various changes and modifications can be made to the invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the claimed invention. The scope of protection claimed by the appended claims and their equivalents is defined.

Claims

1. A model predictive control method for a mobile operating robot, characterized in that, include: S1. Construct a full-body kinematic model of the mobile chassis and robotic arm, taking the chassis pose and robotic arm joint angles as state variables, and the chassis linear velocity, steering angular velocity and joint angular velocities as control variables, while considering the chassis nonholonomic constraints. S2. Acquire environmental data through depth sensors, generate a three-dimensional Euclidean symbol distance field online, and synchronously calculate and store the distance gradient information of each voxel during map building and updating; S3. Configure a set of collision detection spheres on the robot's kinematic chain, estimate the distance from the center of the sphere to the nearest obstacle using the cached gradient, and construct collision avoidance constraints; construct mechanical stability constraints based on the relationship between the zero moment point and the inscribed circle of the supporting polygon; at the same time, introduce joint limit constraints, and use a relaxation obstacle function to unify the above constraints into a soft constraint penalty term in the cost function. S4. Construct the cost function of model predictive control, which includes the end effector pose tracking error term, the control input weighted penalty term, and the soft constraint penalty term; S5. Collect the deviation between the measured force spinner of the end effector and the desired force spinner, generate the displacement compensation amount through admittance control, and superimpose it with the original desired trajectory to form an augmented reference trajectory. S6. Sequential linear quadratic model predictive control is adopted. In the prediction time domain, the kinematic model is linearized and the cost function is approximated twice. The algebraic Riccati equation is solved to obtain the control sequence containing chassis speed command and robotic arm joint speed command.

2. The predictive control method for a mobile operating robot according to claim 1, characterized in that, S1 includes: Define a system state vector for a mobile robot, which consists of the pose of the mobile chassis and the angles of each joint of the robotic arm; the pose of the mobile chassis includes a three-dimensional position vector and an attitude quaternion in the world coordinate system. Define the control input vector of the mobile robot, which consists of the forward linear velocity of the mobile chassis, the turning angular velocity of the mobile chassis, and the angular velocities of each joint of the robotic arm; The type of mobile chassis is determined to be differential drive, skid steering, or tracked, and the mobile chassis has non-holonomic constraint characteristics.

3. The predictive control method for a mobile operating robot according to claim 2, characterized in that, S1 further includes: Construct the whole-body kinematic equations of the mobile robot, wherein the whole-body kinematic equations are in block matrix form; The first row of the block matrix corresponds to the attitude update of the mobile chassis, which is calculated by the Hamiltonian product of the attitude quaternion and the steering angular velocity. The second row of the block matrix corresponds to the position update of the moving chassis, which is calculated by rotating the forward linear velocity vector using attitude quaternions. The third row of the block matrix corresponds to the update of the robot arm joint positions, which is directly obtained by integrating the angular velocities of each joint of the robot arm.

4. The predictive control method for a mobile operating robot according to claim 3, characterized in that, S2 includes: Acquire 3D depth data of the environment using depth cameras or LiDAR; An incremental mapping framework is used to generate a three-dimensional Euclidean symbolic distance field, which is composed of multiple voxels; Each voxel stores the signed distance value from the voxel center to the nearest obstacle; During the mapping and updating phase, the distance gradient information of each voxel is calculated synchronously and stored in the corresponding voxel.

5. The predictive control method for a mobile operating robot according to claim 4, characterized in that, S2 further includes: For any query point in space, locate the voxel where the query point is located; The distance from the query point to the nearest obstacle is estimated using a local linear model. The local linear model is calculated by adding the cached distance value of the voxel to the dot product of the cached gradient vector and the difference between the coordinates of the query point and the voxel center. Distance gradient information of voxels is recalculated only within the local map update area; global gradient updates are not performed.

6. The predictive control method for a mobile operating robot according to claim 5, characterized in that, S3 includes: A set of collision detection spheres are arranged along the robot's kinematic chain to construct a collision avoidance constraint; the collision avoidance constraint is defined as: the difference between the signed distance from the center of the i-th collision detection sphere to the nearest obstacle and the radius of the sphere is greater than or equal to zero; Calculate the robot's zero-moment point and construct mechanical stability constraints; the mechanical stability constraints are defined as: the difference between the square of the radius of the inscribed circle of the supporting polygon and the square of the distance from the zero-moment point to the center of the supporting surface is greater than or equal to zero; Construct joint limiting constraints, which include position and speed limits for each joint of the robotic arm; The collision avoidance constraint, mechanical stability constraint, and joint limit constraint are uniformly transformed into a soft constraint penalty term by using a relaxation obstacle function. The relaxation obstacle function is a piecewise function. When the constraint function value is greater than the threshold, it adopts a logarithmic form, and when the constraint function value is less than or equal to the threshold, it adopts a quadratic function form that is continuously differentiable.

7. The predictive control method for a mobile operating robot according to claim 6, characterized in that, S4 includes: A cost function for model predictive control is constructed, which consists of an end-effector pose tracking error term, a soft constraint penalty term, and a control input weighted penalty term. The end effector pose tracking error term includes translation error and rotation error. The translation error is calculated using the L2 norm of the difference between the actual position and the desired position vector, and the rotation error is calculated using the quaternion error formula. The weighted penalty term for the control input is calculated by multiplying the transpose of the control input vector by the weight matrix.

8. The predictive control method for a mobile operating robot according to claim 7, characterized in that, S5 includes: The measured force spin of the end effector is collected, and the measured force spin includes three-dimensional interactive force and three-dimensional interactive torque. Calculate the force deviation between the measured force spinor and the expected force spinor; Task space admittance control is used to map force deviation into displacement compensation of the end effector, which includes translational compensation and rotational compensation. The task space admittance control calculates the displacement compensation by multiplying the proportional gain matrix by the force deviation and adding the integral gain matrix by the integral of the force deviation over time. Set an anti-saturation limit for the integral term of the mission space admittance control; The displacement compensation amount is superimposed on the original desired trajectory to generate an augmented reference trajectory.

9. A predictive control method for a mobile operating robot according to claim 8, characterized in that, S6 includes: The nonlinear system is forward-rolled using the current control strategy to obtain the state trajectory and input trajectory in the predicted time domain. The whole-body kinematics model is linearized along the state trajectory and input trajectory, and the cost function is approximated twice. Solve the algebraic Riccati equation to obtain an affine control strategy that includes time-varying state feedback gain and feedforward term; The control frequency is dynamically switched according to the task type. The conventional trajectory tracking task is executed at the first frequency, and the admittance control task is executed at a second frequency higher than the first frequency. Generate a control sequence that includes speed commands for the moving chassis and speed commands for each joint of the robotic arm.

10. A predictive control system for a mobile operating robot model, characterized in that, include: The system comprises a perception mapping module, a state estimation module, a trajectory planning module, an admittance adjustment module, a model predictive control module, and an execution module. The perception mapping module is used to acquire environmental data in real time through a depth sensor, generate a three-dimensional Euclidean symbolic distance field online, and synchronously calculate and cache the distance gradient information of each voxel during the mapping update stage. The state estimation module is used to obtain the pose and velocity of the mobile chassis, the angles and angular velocities of each joint of the robotic arm, and the measured force rotation of the end effector. The trajectory planning module is used to receive the desired trajectory in the task space and generate a time-scaled reference trajectory that satisfies the velocity and acceleration constraints. The admittance adjustment module is connected to the state estimation module and the trajectory planning module. It is used to generate a displacement compensation amount by task space admittance control based on the deviation between the measured force spin and the expected force spin, and to superimpose it with the reference trajectory to form an augmented reference trajectory. The model predictive control module is connected to the perception mapping module and the admittance adjustment module. It is used to construct a whole-body kinematic model of the mobile chassis and the robotic arm. The cost function is constructed with the end effector pose tracking error term, the control input weighted penalty term, and the relaxed obstacle function soft constraint penalty term including collision avoidance constraint, zero torque point stability constraint and joint limit constraint. The rolling time domain optimization solution is performed by sequential linear quadratic model predictive control to generate a control sequence containing chassis speed command and robotic arm joint speed command. The execution module is used to send control sequences to the mobile chassis driver and the robotic arm joint controller.

Images