Robot motion control method and system based on centroid dynamics and hierarchical optimization

Abstract

The invention relates to a robot motion control method and system based on centroid dynamics and hierarchical optimization, and the method comprises the steps: building a centroid dynamics model of a four-wheel-foot robot, and constructing a nonlinear model prediction control optimization problem according to the centroid dynamics model in combination with centroid-joint coupling dynamics constraint, wheel-ground rolling constraint and auxiliary constraint; solving by adopting a real-time solving strategy based on discretization and iterative optimization to obtain an optimal state variable and an optimal input variable; and based on the optimal state variable and the optimal input variable, the optimal torque and the wheel angular velocity of the robot are solved through a layered optimized whole-body control strategy. According to the method, control errors caused by model simplification are avoided, and the overall motion performance and robustness of the system in a complex scene are remarkably improved by fusing a high-precision dynamic model and hierarchical optimization control.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, and in particular to a robot motion control method and system based on center-of-mass dynamics and hierarchical optimization. Background Technology

[0002] The statements in this section are merely background information related to the present invention and do not necessarily constitute prior art.

[0003] Quadruped robots combine the high-speed mobility of wheeled robots with the complex terrain adaptability of legged robots, making them promising for a wide range of applications. However, the efficiency, stability, and robustness of their motion control remain key issues that urgently need to be addressed. Quadruped robots are robots that combine the structural characteristics of both wheels and legs, achieving movement through the coordinated motion of wheels and legs. They need to ensure stable rolling when in contact with the ground while also handling complex motion requirements such as leg swinging and posture adjustment, demanding high accuracy in dynamic modeling and strong multi-task coordination capabilities.

[0004] In existing wheeled robot control technologies, dynamic modeling often adopts simplified methods, ignoring the influence of leg mass and leg joint torque on the center of mass momentum; multi-task processing adopts an equal weighted optimization strategy, without prioritizing tasks such as wheel-ground rolling constraints and swing leg trajectory tracking; and optimization frameworks or static task priority allocation are needed to cope with dynamic environments that rely on fixed time steps.

[0005] In summary, the existing control technologies for wheeled robots have the following problems: (1) Existing dynamic modeling methods are simplified. Existing dynamic modeling methods for wheeled robots are significantly simplified and cannot accurately describe the system characteristics. Traditional methods ignore the mass of the legs, which is not applicable to wheeled robots equipped with heavy wheels. They do not consider the influence of leg joint torque on the momentum of the center of mass, and cannot accurately reflect the coupling effect between leg motion and overall dynamics. This modeling defect is particularly obvious during high-speed motion or large-range posture adjustments, which restricts the improvement of motion control accuracy.

[0006] (2) The lack of a multi-task coordination mechanism and the absence of a hierarchical optimization-based whole-body control in existing methods result in a lack of priority classification for various tasks in motion planning (such as wheel-ground rolling constraints, swing leg trajectory tracking, joint torque constraints, etc.). Traditional methods adopt an equal weighted optimization strategy. When there are conflicts between tasks (such as simultaneously satisfying pure wheel rolling and fuselage attitude adjustment), control failure or performance compromise is easily caused by constraint competition.

[0007] (3) Insufficient adaptability to dynamic environments: Existing control methods lack the ability to dynamically adjust in real time when dealing with sudden environmental disturbances (such as instantaneous slippage or sudden changes in ground stiffness). Traditional methods rely on optimization frameworks with fixed time steps or static task priority allocation, which makes it difficult to respond to high-frequency environmental feedback (such as sudden changes in contact force or a surge in wheel-end slip ratio) in a timely manner, resulting in delayed control commands. For example, during sudden slippage, the system cannot quickly reconstruct the plantar force distribution and joint torque distribution strategy, resulting in instantaneous motion instability and a decrease in trajectory tracking accuracy. Summary of the Invention

[0008] This invention proposes a robot motion control method and system based on center-of-mass dynamics and hierarchical optimization, with the aim of solving the technical problems mentioned in the background section.

[0009] To achieve the above objectives, the present invention adopts the following technical solution: In a first aspect, the present invention provides a robot motion control method based on center-of-mass dynamics and hierarchical optimization, comprising: Establish a center-of-mass dynamic model for the four-wheeled legged robot, and set the state variables and input variables; Based on the aforementioned centroid dynamics model, and combined with centroid-joint coupling dynamic constraints, wheel-ground rolling constraints, and auxiliary constraints, a nonlinear model predictive control optimization problem including constraints and cost functions is constructed. A real-time solution strategy based on discretization and iterative optimization is adopted to solve the problem and obtain the optimal state variables and optimal input variables. Based on the optimal state variables and optimal input variables, the optimal torque and wheel angular velocity of the robot are solved through a hierarchical optimization whole-body control strategy.

[0010] A further technical solution is that the center-of-mass dynamic model of the four-wheeled robot is expressed as follows: ;in, Represents state variables, Indicates input variables, Indicates time; The state variables include the center of mass momentum, base coordinates, and joint angle vector, and the input variables include the joint angular velocity vector and the contact force applied to the contact point by the environment.

[0011] In a further technical solution, the auxiliary constraints include friction cone constraints, oscillation trajectory constraints, and initial condition constraints; The nonlinear model predictive control optimization problem is expressed as: ; in, Indicates the terminal cost, Indicates operating costs, This represents the projection of a vector in the vertical direction. Indicates the first The velocity of one leg at the point of contact between the wheel and the ground in the world coordinate system. This represents the ground normal vector.

[0012] A further technical solution is that the cost function of the nonlinear model predictive control optimization problem is designed as a weighted quadratic form of tracking error and control input, specifically expressed as: ; in, The weight matrix representing the terminal cost. The weight matrix represents the cost of state deviation. This represents the weight matrix that controls the input cost.

[0013] A further technical solution involves defining optimization variables, dividing the whole-body controller tasks into high-priority and low-priority tasks according to their priority, constructing linear equality and inequality constraints of the whole-body controller tasks with respect to the optimization variables, solving the problems through hierarchical optimization to obtain the optimal solution for the optimization variables, and thus obtaining the optimal torque and wheel angular velocity.

[0014] In a further technical solution, the optimization variables include generalized acceleration, contact force applied by the environment at the contact point, and generalized torque; The linear equality and inequality constraints for the whole-body controller task with respect to the optimization variables are constructed as follows: ;in, and This represents the slack variable that needs to be minimized. This indicates the task of the full-body controller. This represents the optimization variable.

[0015] A further technical solution is that the optimal solution of the optimization variables includes the optimal torque of each robot leg joint. The method to obtain the wheel angular velocity is as follows: integrate the generalized acceleration in the optimal solution of the optimization variables to obtain the generalized velocity, then obtain the foot velocity from the generalized contact Jacobian matrix, and then project it onto the rolling direction to obtain the wheel speed after whole-body control, and thus obtain the wheel angular velocity.

[0016] Secondly, the present invention provides a robot motion control system based on center-of-mass dynamics and hierarchical optimization, comprising: The center of mass dynamics model building module is configured to: establish the center of mass dynamics model of the four-wheeled robot and set the state variables and input variables; The control optimization problem construction and solution module is configured to: construct a nonlinear model including constraints and cost functions to predict the control optimization problem based on the centroid dynamics model, combined with centroid-joint coupling dynamic constraints, wheel-ground rolling constraints and auxiliary constraints; and solve the problem using a real-time solution strategy based on discretization and iterative optimization to obtain the optimal state variables and optimal input variables. The hierarchical optimization module is configured to solve for the robot's optimal torque and wheel angular velocity based on the optimal state variables and optimal input variables, using a hierarchical optimization whole-body control strategy.

[0017] Compared with the prior art, the present invention has the following advantages and beneficial effects: 1. This invention significantly improves the modeling accuracy in complex motion scenarios by establishing a center-of-mass dynamic model that includes the torque coupling effect of leg joints. Compared with traditional models, this invention fully considers the influence of wheel-leg coordinated motion on the center-of-mass momentum, dynamically correlates the center-of-mass momentum with the rolling motion of the wheel ends, and realizes pure rolling conditions through local coordinate system projection. This allows for a more accurate description of the dynamic coupling effect between the torso and leg joints, effectively avoiding control errors caused by model simplification.

[0018] 2. This invention employs a hierarchical optimized whole-body control strategy, prioritizing wheel-to-ground rolling constraints to ensure stability and anti-slip capability in wheeled motion. Based on this, the remaining degrees of freedom are used to optimize low-priority tasks (such as swing leg trajectory tracking and joint torque constraints), achieving efficient coordination and execution of multiple tasks. By integrating a high-precision dynamic model with hierarchical optimized control, the overall motion performance and robustness of the system in complex scenarios are significantly improved.

[0019] 3. This invention systematically integrates high-precision dynamic modeling, nonlinear model predictive control, and hierarchical optimization control to form a highly efficient and collaborative overall solution, overcoming the limitations of single-module optimization in traditional methods. The method adopts a modular design, supporting flexible expansion and adjustment. It can dynamically add or optimize sub-tasks according to actual needs without reconstructing the overall architecture, significantly improving the system's practicality and adaptability, and providing a universal control solution for diverse application scenarios. Attached Figure Description

[0020] The accompanying drawings, which form part of this invention, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute a limitation thereof.

[0021] Figure 1 This invention relates to a control framework for a four-wheeled legged robot based on nonlinear model predictive control. Figure 2 This is a schematic diagram of the wheel-to-ground contact point constraint of the present invention; Figure 3 This is the connection of the four-wheeled legged robot module in this invention; Figure 4 This is a flowchart of the method in this invention; Figure 5 This is the task priority for whole-body control in this invention. Detailed Implementation

[0022] Example 1 It should be noted that the following detailed description is illustrative and intended to provide further explanation of the invention. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains.

[0023] like Figure 2 As shown, in this embodiment, the four-wheeled legged robot has four degrees of freedom for each leg, and four motors are mounted on a real-time control bus. These motors are connected to the main control computer via a bus protocol converter, allowing for real-time control of the motors and feedback of position, speed, and torque data from sensors. The inertial measurement unit (IMU) is connected to the main control computer via a device interconnect interface; specific module connections are detailed in [link to IMU documentation]. Figure 3 .

[0024] like Figure 4 As shown, this embodiment provides a robot motion control method based on center-of-mass dynamics and hierarchical optimization, and adopts the following technical solution: S1: Establish the center of mass dynamics model of the four-wheeled legged robot and set the state variables and input variables.

[0025] In step S1, it is known that the joint control capability of the four-wheeled robot is sufficient, that is, the joint motor torque is large enough. The joint torque required in various application scenarios does not reach the maximum torque limit of the joint motor. Taking into account the principle of momentum conservation at the robot's center of mass, the kinematic characteristics of the base coordinates, the joint dynamics, and taking into account the environmental forces and torques, gravity effects, and joint driving characteristics at each contact point, state variables are set. Input variables ,in, The mass momentum is defined in the center-of-mass coordinate system, including linear momentum and angular momentum; The base coordinates are the position and posture of the torso; The joint angle vector; This is the joint angular velocity vector; Indicates the environment applied at the contact point Contact force.

[0026] Based on the above description, the center of mass dynamics model of the four-wheeled robot is expressed as follows: ;in, Represents state variables, Indicates input variables, Indicates time.

[0027] S2: Based on the aforementioned centroid dynamics model, and combined with centroid-joint coupling dynamic constraints, wheel-ground rolling constraints, and auxiliary constraints, a nonlinear model predictive control optimization problem including constraints and cost functions is constructed; a real-time solution strategy based on discretization and iterative optimization is adopted to solve the problem and obtain the optimal state variables and optimal input variables.

[0028] In step S2, considering the centroid-joint coupling dynamic constraints of the aforementioned high-precision centroid dynamic model, as well as the wheel-ground rolling constraints dynamically associated with it, and auxiliary constraints such as friction cone constraints, swing trajectory constraints, and initial condition constraints, a nonlinear model predictive control optimization problem is constructed, including constraints and cost functions. A complete control framework for a four-wheeled legged robot based on nonlinear model predictive control optimization is as follows: Figure 1 As shown.

[0029] Specifically: within the prediction interval The previous complete framework for constructing a nonlinear model predictive control optimization problem is as follows: Figure 1 As shown, it is represented as: ; in, Indicates the terminal cost, Indicates operating costs, This represents the constraint of the system's center-of-mass dynamic equation. This represents the initial condition constraints.

[0030] like Figure 2 As shown, and This represents the friction cone inequality constraint, ensuring that the frictional force when the wheel end contacts the ground is within an allowable range, preventing slippage. Among these constraints, Indicates the coefficient of friction between wheels and the ground. , , They represent the contact forces at... The directional component.

[0031] and For wheel-to-ground rolling constraints, where, This represents the projection of a vector in the vertical direction. Indicates the first The velocity of one leg at the point of contact between the wheel and the ground in the world coordinate system. This represents the ground normal vector. Wheel-ground roll constraint means that the velocity at the wheel-ground contact point in the rolling direction is unrestricted, but the velocity in the direction of the ground normal vector and in the direction perpendicular to the plane formed by the rolling direction and the ground normal vector is still constrained to 0. The velocity in the rolling direction can be converted from the wheel radius to the wheel angular velocity.

[0032] The swing leg trajectory constraint means that during the swing phase, the wheel-end speed needs to meet the trajectory requirements of gait generation to ensure that the robot's motion conforms to the expected gait planning.

[0033] For a four-wheeled legged robot, the cost function of the nonlinear model predictive control optimization problem is designed as a weighted quadratic form of tracking error and control input, specifically expressed as: ; in, The weight matrix representing the terminal cost. The weight matrix represents the cost of state deviation. This represents the weight matrix that controls the input cost.

[0034] By employing a real-time solution strategy based on discretization and iterative optimization, the optimal state variables can be obtained. and optimal input variables .

[0035] S3: Based on the optimal state variables and optimal input variables, the optimal torque and wheel angular velocity of the robot are solved through a hierarchical optimization whole-body control strategy.

[0036] In step S3, the hierarchical optimization whole-body control strategy is as follows: Let the whole-body controller task be T, and define the optimization variables as generalized acceleration, contact force applied by the environment at the contact point, and generalized torque, specifically expressed as follows: ,in The acceleration is defined in a generalized sense, including 12-dimensional joint acceleration and 6-dimensional base acceleration. The torque is a generalized torque, including 12-dimensional joint torque and 6-dimensional virtual base torque (which is 0).

[0037] The linear equality and inequality constraints for the whole-body controller task with respect to the optimization variables are constructed as follows: ;in, and This represents the slack variable that needs to be minimized. This indicates the task of the full-body controller. Let represent the optimization variable. For those with high priority The optimal solution for each task, to ensure a strict priority order, includes tasks with lower priority. Solution Need to be in the null space of high-priority tasks In the calculation, among which Ensure that high-priority tasks are completed, and then complete low-priority tasks in the remaining degrees of freedom.

[0038] Based on the principle of hierarchical optimization, such as Figure 5 As shown, in this embodiment, the whole-body controller task of the four-wheeled robot is divided into sub-tasks with multiple priorities, which are divided into high-priority tasks and low-priority tasks according to priority.

[0039] By dividing the wheel-ground rolling constraint into a separate task and assigning it a higher priority, stable motion of the wheeled system can be achieved while ensuring system stability, preventing wheel slippage and detachment during rolling. For the wheel-ground rolling constraint, the optimal state variables are obtained by solving a nonlinear model predictive control optimization problem. and optimal input variables Then, the velocity of the wheel end in the global coordinate system can be easily obtained from the forward kinematics. The optimal speed in the rolling direction can be obtained through projection. ,in Where is the radius of the wheel. This is the angular velocity of the wheel.

[0040] Three additional direction vectors are set. , , They are used to calculate the velocity in the global coordinate system. The three directions projected onto the local coordinate system of the contact point are: the rolling direction of the wheel, the direction perpendicular to the plane of the thigh and calf, and the direction of the ground normal vector. The wheel-ground rolling constraint is expressed by the following formula: ; Differentiation yields the acceleration level constraint, expressed as: ; For the first The Jacobian matrix of the contact point of each leg. The optimization variables for whole-body control are known to be... Then the wheel-ground rolling constraint can be written in the form of the following equation: ; The optimal solution for the optimization variables is obtained by solving the problem through hierarchical optimization. It includes the optimal torque for each robot leg joint and ensures a strict priority order among various tasks.

[0041] The method for obtaining the wheel angular velocity is: finding the optimal solution for the optimization variables. Generalized acceleration Integrating to obtain the generalized velocity Then, from the generalized contact Jacobian matrix Obtain the foot velocity Then, by projecting the result onto the rolling direction, the wheel speed after full-body control can be obtained. This allows us to obtain the wheel's angular velocity. .

[0042] For a four-wheeled robot, the optimal torque is sent to the joint motors, and the wheel angular velocity is... The speed loop is sent to the hub motor for control, ultimately enabling coordinated and stable control of the leg joints and wheels.

[0043] Example 2 This embodiment provides a robot motion control system based on center-of-mass dynamics and hierarchical optimization, which specifically includes the following modules: The center of mass dynamics model building module is configured to: establish the center of mass dynamics model of the four-wheeled robot and set the state variables and input variables; The control optimization problem construction and solution module is configured to: construct a nonlinear model including constraints and cost functions to predict the control optimization problem based on the centroid dynamics model, combined with centroid-joint coupling dynamic constraints, wheel-ground rolling constraints and auxiliary constraints; and solve the problem using a real-time solution strategy based on discretization and iterative optimization to obtain the optimal state variables and optimal input variables. The hierarchical optimization module is configured to solve for the robot's optimal torque and wheel angular velocity based on the optimal state variables and optimal input variables, using a hierarchical optimization whole-body control strategy.

[0044] The implementation of specific modules in this embodiment refers to the steps of the robot motion control method based on centroid dynamics and hierarchical optimization described in Embodiment 1, and will not be described in detail here.

[0045] The embodiments described above are merely illustrative of several implementations of the present invention, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of the invention. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these modifications and improvements all fall within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the appended claims.

Claims

1. A robot motion control method based on center-of-mass dynamics and hierarchical optimization, characterized in that, include: Establish a center-of-mass dynamic model for the four-wheeled legged robot, and set the state variables and input variables; Based on the aforementioned centroid dynamics model, and combined with centroid-joint coupling dynamic constraints, wheel-ground rolling constraints, and auxiliary constraints, a nonlinear model predictive control optimization problem including constraints and cost functions is constructed. A real-time solution strategy based on discretization and iterative optimization is adopted to solve the problem and obtain the optimal state variables and optimal input variables. Based on the optimal state variables and optimal input variables, the optimal torque and wheel angular velocity of the robot are solved through a hierarchical optimization whole-body control strategy.

2. The robot motion control method based on center-of-mass dynamics and hierarchical optimization as described in claim 1, characterized in that, The center-of-mass dynamics model of the four-wheeled robot is expressed as follows: ;in, Represents state variables, Indicates input variables, Indicates time; The state variables include the center of mass momentum, base coordinates, and joint angle vector, and the input variables include the joint angular velocity vector and the contact force applied to the contact point by the environment.

3. The robot motion control method based on center-of-mass dynamics and hierarchical optimization as described in claim 1, characterized in that, The auxiliary constraints include friction cone constraints, oscillation trajectory constraints, and initial condition constraints; The nonlinear model predictive control optimization problem is expressed as: ； in, Indicates the terminal cost, Indicates operating costs, This represents the projection of a vector in the vertical direction. Indicates the first The velocity of one leg at the point of contact between the wheel and the ground in the world coordinate system. This represents the ground normal vector.

4. The robot motion control method based on center-of-mass dynamics and hierarchical optimization as described in claim 1, characterized in that, The cost function for the nonlinear model predictive control optimization problem is designed as a weighted quadratic form of tracking error and control input, specifically expressed as: ； in, The weight matrix representing the terminal cost. The weight matrix represents the cost of state deviation. This represents the weight matrix that controls the input cost.

5. The robot motion control method based on center-of-mass dynamics and hierarchical optimization as described in claim 1, characterized in that, The hierarchical optimization whole-body control strategy involves defining optimization variables, dividing the whole-body controller tasks into high-priority and low-priority tasks according to their priority, constructing linear equality and inequality constraints of the whole-body controller tasks with respect to the optimization variables, solving the problem through hierarchical optimization, obtaining the optimal solution for the optimization variables, and thus obtaining the optimal torque and wheel angular velocity.

6. The robot motion control method based on center-of-mass dynamics and hierarchical optimization as described in claim 1, characterized in that, The optimization variables include generalized acceleration, contact force applied by the environment at the contact point, and generalized torque; The linear equality and inequality constraints for the whole-body controller task with respect to the optimization variables are constructed as follows: ;in, and This represents the slack variable that needs to be minimized. This indicates the task of the full-body controller. This represents the optimization variable.

7. The robot motion control method based on center-of-mass dynamics and hierarchical optimization as described in claim 1, characterized in that, The optimal solution of the optimization variables includes the optimal torque of each robot leg joint, and the method for obtaining the wheel angular velocity is as follows; Integrating the generalized acceleration in the optimal solution of the optimization variables yields the generalized velocity. The foot velocity is then obtained from the generalized contact Jacobian matrix. Projecting this velocity onto the rolling direction gives the wheel speed after full-body control, and finally the wheel angular velocity.

8. A robot motion control system based on center-of-mass dynamics and hierarchical optimization, characterized in that, include: The center of mass dynamics model building module is configured to: establish the center of mass dynamics model of the four-wheeled robot and set the state variables and input variables; The control optimization problem construction and solution module is configured to: construct a nonlinear model including constraints and cost functions to predict the control optimization problem based on the centroid dynamics model, combined with centroid-joint coupling dynamic constraints, wheel-ground rolling constraints and auxiliary constraints; and solve the problem using a real-time solution strategy based on discretization and iterative optimization to obtain the optimal state variables and optimal input variables. The hierarchical optimization module is configured to solve for the robot's optimal torque and wheel angular velocity based on the optimal state variables and optimal input variables, using a hierarchical optimization whole-body control strategy.

9. A computer-readable storage medium having a program stored thereon, characterized in that, When executed by the processor, the program implements the steps in the robot motion control method based on centroid dynamics and hierarchical optimization as described in any one of claims 1-7.

10. An electronic device comprising a memory, a processor, and a program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the steps in the robot motion control method based on centroid dynamics and hierarchical optimization as described in any one of claims 1-7.

Images