A quadruped mobile manipulator decoupled motion control method and system considering mechanical arm force

Abstract

The application discloses a kind of four-legged mobile operating robot decoupling motion control method and system considering mechanical arm force, belong to the field of foot type mobile operating robot motion control.It can track the desired motion trajectory of user input by linear model predictive control MPC planning;Based on the tracked motion trajectory, the mechanical arm joint control torque is calculated again through mechanical arm dynamics model and PD feedback;The whole body motion trajectory of four-legged robot considering the mechanical arm force calculated according to mechanical arm dynamics model is obtained by nonlinear model predictive control NMPC planning, to track the desired speed trajectory and desired force trajectory of user input;According to the desired speed trajectory and desired force trajectory of user input, the joint driving torque for tracking the desired trajectory is calculated based on hierarchical quadratic programming whole body controller WBC according to task priority.The application considers the force / torque of mechanical arm to body when four-legged robot motion control, to realize the decoupling control of mechanical arm and four-legged robot in four-legged mobile operating robot.

Description

Technical Field

[0001] This invention belongs to the field of motion control technology for legged mobile manipulators, specifically relating to a decoupled motion control method and system for a quadrupedal mobile manipulator that takes into account the force of the robotic arm. Background Technology

[0002] Quadruped robots are biomimetic robots that mimic the evolution of quadrupedal mammals. Thanks to their dynamic stability, discrete foot placement, and floating base, they possess excellent adaptability to complex environments and dynamic movement capabilities, making them widely used in environmental detection, material transportation, and industrial inspection. However, due to their body structure, quadruped robots can currently only achieve simple interactions with the environment through their body or feet and cannot actively perform complex tasks. Robotic arms, due to their flexible movement and high trajectory tracking accuracy, are widely used in automotive manufacturing, medical services, and aerospace. However, their inherent limitation—a fixed connection between their base and the environment preventing autonomous movement—limits them to performing tasks within a localized area, significantly restricting their operational flexibility and application scenarios.

[0003] Combining quadruped robots with robotic arms to create quadrupedal mobile manipulation robots not only retains the advantages of quadruped robots (flexible movement, good stability, strong environmental adaptability) and robotic arms (high precision, robustness, and high degrees of freedom), but also overcomes the inherent limitations of quadruped robots (inflexible operation) and robotic arms (inability to move autonomously). This increases the robotic arm's workspace and enables the quadruped robot to actively interact with its environment through the robotic arm. This will greatly broaden the applications of quadruped robots in disaster relief, self-repair, human-robot collaboration, and home services, and is of great significance in improving the practical application level of quadruped robots in production and daily life.

[0004] Current motion control methods for quadrupedal manipulators include reinforcement learning-based and optimization-based methods. Reinforcement learning-based methods learn control strategies through extensive training data, enabling the robot to acquire motion control capabilities. However, these methods often suffer from drawbacks such as difficulty in practical deployment, poor generalization ability, and inconsistent robot motion behavior. Optimization-based control methods typically treat the quadrupedal manipulator as a whole and then establish a unified optimization control strategy. These modeling control methods often suffer from high system dimensionality, difficulty in designing optimization strategies, susceptibility to local optima, and high computational complexity, limiting the robot's motion and operational performance. Existing technologies lack a decoupled control method that ensures decoupling optimization between the quadrupedal platform and the manipulator while explicitly incorporating the dynamic influence of the manipulator's forces / torques on the body during the whole-body planning stage. Furthermore, there is a lack of an integrated hierarchical optimization framework combining linear MPC (arm) – NMPC (body) – hierarchical QP-WBC (execution) to achieve a balance between real-time performance, robustness, and scalability. Summary of the Invention

[0005] This invention provides a decoupled motion control method and system for a quadrupedal mobile robot that considers the force exerted by the robotic arm. The method considers the force / torque exerted by the robotic arm on the robot body during the motion control of the quadrupedal robot, thereby achieving decoupled control between the robotic arm and the quadrupedal robot in the quadrupedal mobile robot.

[0006] This invention is achieved through the following technical solution: A decoupled motion control system for a quadrupedal mobile manipulator that considers the forces exerted by a robotic arm, the method comprising the following steps: Step 1: The robotic arm control system plans and tracks the desired motion trajectory input by the user through linear model predictive control (MPC). Step 2: Based on the motion trajectory tracked in Step 1, calculate the joint control torque of the robotic arm through the robotic arm dynamics model and PD feedback; Step 3: Using nonlinear model predictive control (NMPC) planning, consider the quadruped robot's whole-body motion trajectory based on the robotic arm dynamics model calculated in Step 2, to obtain the desired velocity trajectory and desired force trajectory that track the user input. Step 4: Based on the desired velocity trajectory and desired force trajectory input by the user in Step 3, the whole-body controller WBC based on hierarchical quadratic programming calculates the joint driving torque for tracking the desired trajectory according to the task priority.

[0007] Furthermore, in step one, the desired motion trajectory of the robotic arm is planned by MPC as follows: the state variable of the control system is the joint position of the robotic arm, the input variable of the control system is the joint velocity of the robotic arm, the cost function is to minimize the joint velocity error, and the constraints include constraints based on linear interpolation reference trajectory and physical consistency.

[0008] Furthermore, the formula for calculating the joint torque of the robotic arm in step two is as follows:

[0009] in, For the mass matrix of the robotic arm, This includes nonlinear terms such as Coriolis force and centripetal force. For gravity, and These are the gains for joint position error and velocity error, respectively. , and These are the joint positions, velocities, and accelerations of the robotic arm. and These are the joint positions and speeds read back from the robotic arm.

[0010] Furthermore, step three, NMPC planning, specifically involves the following: the system state variables are the quadruped robot's center of mass momentum and generalized position in the world coordinate system; the system input variables are the foot contact force and joint velocity; the system cost function consists of the system state error and input error; and the constraints include state space equation constraints, foot friction constraints, joint position constraints, support leg stationary constraints, and initial state constraints. The optimal state and input trajectory of the quadruped robot system are obtained by solving the optimization problem based on quadratic programming. In step 3, the state-space equations consider the forces exerted by the robotic arm on the quadruped robot's body. The formulas for calculating the forces and torques exerted by the robotic arm base on the quadruped robot's body are as follows:

[0011] in, For the mass of the robotic arm, It is the gravitational acceleration vector. and For operating force and operating torque, and These are the position vectors of the robotic arm's center of mass relative to the quadruped robot's center of mass and the position vector of the robotic arm's end effector relative to the quadruped robot's center of mass, respectively. In this invention, it is assumed that the operating force is known. and operating torque .

[0012] Furthermore, step 4, modeling the WBC quadruped robot's whole-body controller, is a quadratic programming-based optimization problem. The objective equations are tracking the swing leg trajectory, body acceleration, and foot contact force. Constraints include floating basis dynamics equations, stationary support leg foot constraints, joint torque constraints, friction cone constraints, and joint position and velocity constraints. The optimization variables... Solve the problem.

[0013] A decoupled motion control system for a quadrupedal mobile robot considering the forces exerted by a robotic arm, characterized in that the system uses the aforementioned decoupled motion control method for a quadrupedal mobile robot considering the forces exerted by a robotic arm, and the system includes... Robotic arm control module: The robotic arm control system uses linear model predictive control (MPC) for planning and can track the user-inputted expected motion trajectory; Based on the tracked motion trajectory, the joint control torque of the robotic arm is calculated through the robotic arm dynamics model and PD feedback; Full-body control module: The NMPC planning of predictive control through nonlinear model takes into account the full-body motion trajectory of the quadruped robot based on the mechanical arm dynamics model calculated by the mechanical arm control module, and obtains the desired velocity trajectory and desired force trajectory that are tracked by the user input. Based on the desired velocity and force trajectories input by the user, the whole-body controller (WBC) using hierarchical quadratic programming calculates the joint driving torques for tracking the desired trajectories according to task priorities.

[0014] Furthermore, the working method of the robotic arm control module is specifically as follows: the desired motion trajectory of the robotic arm is planned through MPC. Specifically, the state variable of the control system is the joint position of the robotic arm, the input variable of the control system is the joint velocity of the robotic arm, the cost function is to minimize the joint velocity error, and the constraints include constraints based on linear interpolation reference trajectory and physical consistency.

[0015] Furthermore, the operating method of the robotic arm control module also includes calculating the joint torque formula of the robotic arm as follows:

[0016] in, For the mass matrix of the robotic arm, This includes nonlinear terms such as Coriolis force and centripetal force. For gravity, and These are the gains for joint position error and velocity error, respectively. , and These are the joint positions, velocities, and accelerations of the robotic arm. and These are the joint positions and speeds read back from the robotic arm.

[0017] Furthermore, the working method of the whole-body control module is as follows: Step three, NMPC planning, specifically involves the following: the system state variables are the quadruped robot's center of mass momentum and generalized position in the world coordinate system; the system input variables are the foot contact force and joint velocity; the system cost function consists of the system state error and input error; and the constraints include state space equation constraints, foot friction constraints, joint position constraints, support leg stationary constraints, initial state constraints, etc. The optimal state and input trajectory of the quadruped robot system are obtained by solving the optimization problem based on quadratic programming. The state-space equation of the working method of the whole-body control module considers the force exerted by the robotic arm on the quadruped robot body. The formula for calculating the force and torque exerted by the robotic arm base on the quadruped robot body is as follows:

[0018] in, For the mass of the robotic arm, It is the gravitational acceleration vector. and For operating force and operating torque, and These are the position vectors of the robotic arm's center of mass relative to the quadruped robot's center of mass and the position vector of the robotic arm's end effector relative to the quadruped robot's center of mass, respectively; in this invention, it is assumed that the operating force is known. and operating torque .

[0019] Furthermore, the working method of the whole-body control module also includes modeling the WBC quadruped robot's whole-body controller as an optimization problem based on quadratic programming, with swing leg trajectory tracking, body acceleration tracking, and foot contact force tracking as the objective equations. Constraints include floating basis dynamics equation constraints, stationary support leg foot constraints, joint torque constraints, friction cone constraints, and joint position and velocity constraints. The optimization variables are... Solve the problem.

[0020] The beneficial effects of this invention are: Compared to high-dimensional optimization that unifies movement and manipulation in modeling, this invention employs a decoupling control strategy to decompose the original high-dimensional coupled optimization problem into two low-dimensional independent optimization problems, significantly reducing the computational complexity of the quadrupedal mobile manipulation robot and improving online solution speed and system scalability.

[0021] This invention uses linear interpolation to plan a reference trajectory from the starting position to the target position, and uses a linear model to predict and control the robotic arm's motion trajectory to satisfy the constraints and optimize the cost function. Under the premise of satisfying constraints such as joint position / velocity boundaries, it can generate smooth and executable joint velocity and position references in real time, ensuring the end effector's rapid response to the user's target and high-precision tracking, while reducing the impact and vibration that may be caused by sudden trajectory changes.

[0022] In this invention, the motion control of the quadruped robot takes into account the force exerted by the robotic arm on the quadruped robot body, thereby improving the robustness of the quadruped robot's motion control.

[0023] The decoupled control proposed in this invention allows the system to independently adjust the gait of the quadruped platform and the operating strategy of the robotic arm, enabling it to better adapt to complex unstructured environments and improving the system's environmental adaptability. Attached Figure Description

[0024] Figure 1 This is a block diagram of the present invention. Detailed Implementation

[0025] In the following description, specific details such as particular system architectures and techniques are set forth for illustrative purposes and not for limitation, in order to provide a thorough understanding of the embodiments of this application. However, those skilled in the art will understand that this application may also be implemented in other embodiments without these specific details. In other instances, detailed descriptions of well-known systems, apparatuses, circuits, and methods are omitted so as not to obscure the description of this application with unnecessary detail.

[0026] It should be understood that, when used in this specification and the appended claims, the term "comprising" indicates the presence of the described features, integrals, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, integrals, steps, operations, elements, components and / or collections thereof.

[0027] It should also be understood that the terminology used in this application specification is for the purpose of describing particular embodiments only and is not intended to limit the application. As used in this application specification and the appended claims, the singular forms “a,” “an,” and “the” are intended to include the plural forms unless the context clearly indicates otherwise.

[0028] The following is in conjunction with the appendix to this application specification. Figure 1 The technical solutions in the embodiments of this application are clearly and completely described. Obviously, the described embodiments are only a part of the embodiments of this application, and not all of the embodiments. Based on the embodiments in this application, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of this application.

[0029] Many specific details are set forth in the following description in order to provide a full understanding of this application. However, this application may also be implemented in other ways different from those described herein. Those skilled in the art can make similar extensions without departing from the spirit of this application. Therefore, this application is not limited to the specific embodiments disclosed below.

[0030] Implementation Method 1 This embodiment provides a decoupled motion control method for a quadrupedal mobile robot considering the force of a robotic arm. The method includes the following steps: Step 1: The robotic arm control system plans and tracks the desired motion trajectory input by the user through linear model predictive control (MPC). Step 2: Based on the motion trajectory tracked in Step 1, calculate the joint control torque of the robotic arm through the robotic arm dynamics model and PD feedback; Step 3: Using nonlinear model predictive control (NMPC) planning, consider the quadruped robot's whole-body motion trajectory based on the robotic arm dynamics model calculated in Step 2, to obtain the desired velocity trajectory and desired force trajectory that track the user input. Step 4: Based on the desired velocity trajectory and desired force trajectory input by the user in Step 3, the whole-body controller WBC based on hierarchical quadratic programming calculates the joint driving torque for tracking the desired trajectory according to the task priority.

[0031] This invention proposes for the first time a control architecture that completely decouples the quadruped robot platform from the robotic arm, taking into account the force of the robotic arm. It decomposes the complex multi-body coupling system into two independent control subsystems and significantly reduces the computational complexity of the system through independent motion planning and controller design. The NMPC plan innovatively considers the dynamic influence of the robotic arm's motion on the quadruped robot's center of mass. By introducing compensation terms for the robotic arm's center of mass position vector and end effector position vector, precise dynamic balance control is achieved. By using decoupling design, the quadruped robot and the robotic arm can compute their respective control commands in parallel, avoiding the high-dimensional optimization problem in traditional coupled control and significantly improving the real-time performance of the control system. By adopting a unified framework of "arm / body decoupling + arm-body action compensation + hierarchical optimization solution", we can explicitly model and compensate for the posture / center of mass disturbances introduced by the robotic arm without significantly increasing the overall optimization dimension, thus achieving comprehensive improvement in real-time performance, robustness and scalability.

[0032] Furthermore, in step one, the linear model predictive control (MPC) planning can track the user's input desired motion trajectory, referencing... Figure 1 The user inputs the desired pose of the robotic arm's end effector. First, the joint angles corresponding to the desired pose are obtained through inverse kinematics calculations of the robotic arm. The current joint angle of the robotic arm The linear interpolation period can be obtained from hardware, based on the joint velocity. T Then, the difference trajectory from the starting point to the target point is obtained through linear interpolation.

[0033] in Indicates the duration of the exercise.

[0034] The reference trajectory obtained by linear interpolation is optimized using linear model predictive control. The system state in the system state-space equations. The system input is Then the loading space equation for a continuous-time system is:

[0035] in, Input the unit matrix.

[0036] Therefore, the following optimization control problem can be obtained.

[0037] in, and Let the process cost function and terminal cost function be represented. In this embodiment, we do not distinguish between terminal cost and process cost, that is... , Input the weight matrix into the system. This represents a state equality constraint, and the desired joint trajectory obtained through linear interpolation will be used as the reference value for this constraint. The state input inequality constraints represent joint position constraints, joint velocity constraints, and self-collision constraints in this embodiment. This indicates the initial state of the robotic arm.

[0038] Furthermore, in step two, the joint control torque of the robotic arm is calculated using the robotic arm dynamics model and PD feedback, and the desired joint angle of the robotic arm is obtained through model predictive control. and desired joint angular velocity The desired acceleration of the joint is obtained through velocity difference. Then, the feedforward joint control torque based on the robotic arm dynamics is

[0039] The current position of the robotic arm joints can be obtained from the robotic arm end. and joint velocity Then the feedback joint torque obtained through PD calculation is

[0040] The final joint torque used for robotic arm control is

[0041] in, For the mass matrix of the robotic arm, This includes nonlinear terms such as Coriolis force and centripetal force. For gravity, and These are the joint position error and velocity error gains, respectively.

[0042] Furthermore, in step three, the NMPC planner uses a nonlinear model to predict and control the robot's full-body motion trajectory, taking into account the forces acting on the robotic arm. The NMPC planner solves the optimal control problem to plan the robot's full-body motion trajectory. The optimal control problem can be expressed as:

[0043] in, and Let the process cost function and terminal cost function be represented. In this embodiment, we do not distinguish between terminal cost and process cost, that is... , and These are the positive definite system state weight matrix and input weight matrix, respectively. For system input, For the foot-ground contact force of the quadruped robot, The velocity of the leg joints of the quadruped robot. For system status, For the center of mass momentum of the quadruped robot, For the generalized position of quadruped robots, and These represent the generalized position of the quadruped robot's body and the position of its leg joints, respectively, and the corresponding generalized velocities and accelerations of the quadruped robot are as follows: and The rate of change of the center of mass momentum considering the force exerted by the robotic arm on the machine body can be expressed as:

[0044] in, For the quality of the quadruped robot, The force exerted by the robotic arm on the machine body. To support the position of the leg tips relative to the quadruped robot's center of mass, For sufficient ground contact torque, This is the torque exerted by the robotic arm on the machine body.

[0045] For the forces and torques exerted by the robotic arm on the machine body, we neglect the effects of inertia and nonlinear terms caused by the robotic arm's motion. Therefore, the expressions for calculating the forces and torques are as follows:

[0046] Furthermore, step four specifically involves WBC calculating the joint torques that satisfy multiple task priorities by solving a hierarchical quadratic programming problem.

[0047] in, To optimize variables, This refers to the driving torque for the joints of a quadruped robot.

[0048] like Figure 1 As shown, the desired pose of the robotic arm's end effector is manually set. The positions of each joint corresponding to this desired pose are calculated based on the robotic arm's inverse kinematics, and these positions are used as target joint positions. The current joint positions are used as starting joint positions. Linear interpolation is used to obtain the desired linearly interpolated joint trajectory from the starting joint position to the target joint position. This trajectory is then optimized using MPC (Multi-Process Control) to generate a joint motion trajectory that satisfies the robot's constraints and optimizes the cost function. The theoretical joint torque for tracking the desired trajectory is then calculated using the robotic arm's dynamics equations. Since there is a deviation between the desired and actual trajectories, a PD (Positive Displacement) controller is used to obtain the torque to compensate for the trajectory deviation. The torque calculated by dynamics and the PD-compensated torque are added together to obtain the final control torque for the robotic arm.

[0049] like Figure 1 As shown, the leg joint positions are read from the quadruped robot. Leg joint speed fuselage angular velocity and fuselage linear acceleration Based on state estimation for legged robots—consistent fusion of leg kinematics and IMU—the position and velocity of the quadruped robot's body are estimated, and then stacked with the measured leg joint positions and velocities to form the actual generalized position of the quadruped robot. and generalized speed .

[0050] like Figure 1 As shown, the quadruped robot's joint layer control uses the driving torque of the leg joints calculated by WBC. The joint torques calculated by PD feedback are added together to obtain the robot's leg joint torques. The calculation equation is as follows:

[0051] In the formula, and These represent the gains for leg joint position error and velocity error, respectively.

[0052] A motion control method and system that decouples the planning and control of a quadruped robot and a robotic arm, and explicitly compensates for the force / torque exerted by the robotic arm on the robot body during the whole-body planning stage of the quadruped robot.

[0053] For complex quadruped robot motion tasks, they can be decomposed into a series of sub-tasks, including floating basis dynamics equations, joint torque constraints, support leg friction cone constraints, support leg non-contact motion constraints, body linear acceleration tracking, body angular acceleration tracking, swing leg motion tracking, and contact force tracking. Following a quadratic programming approach, swing leg motion tracking, body linear acceleration tracking, body angular acceleration tracking, and contact force tracking are set as the cost function of the quadratic programming problem. The floating basis dynamics equations and support leg non-contact motion constraints are set as equality constraints, and the joint torque constraints and support leg friction cone constraints are set as inequality constraints. By solving the above quadratic programming problem, the optimization variables that satisfy the equality and inequality constraints and minimize the cost function are obtained, and these are used as control variables to control the motion of the quadruped robot.

[0054] By applying this embodiment, the present invention enables decoupled control of a quadrupedal manipulator, decomposing the original high-dimensional coupled optimization problem into two low-dimensional independent optimization problems. This significantly reduces the computational complexity of the quadrupedal manipulator and considers the influence of the manipulator's forces in the quadrupedal robot's motion control, improving the robustness of the motion control. Simultaneously, by planning the manipulator's trajectory through interpolation followed by optimization, smooth and executable joint velocity and position references are generated in real time, ensuring rapid response and high-precision tracking of the end effector to the user's target, while reducing the impact and vibration that may be caused by sudden trajectory changes.

[0055] Implementation Method 2 This embodiment provides a decoupled motion control system for a quadrupedal mobile robot that considers the forces exerted by a robotic arm. The system uses the decoupled motion control method for a quadrupedal mobile robot that considers the forces exerted by a robotic arm as described in Embodiment 1. The system includes... Robotic arm control module: The robotic arm control system uses linear model predictive control (MPC) for planning and can track the user-inputted expected motion trajectory; Based on the tracked motion trajectory, the joint control torque of the robotic arm is calculated through the robotic arm dynamics model and PD feedback; Full-body control module: The NMPC planning of predictive control through nonlinear model takes into account the full-body motion trajectory of the quadruped robot based on the mechanical arm dynamics model calculated by the mechanical arm control module, and obtains the desired velocity trajectory and desired force trajectory that are tracked by the user input. Based on the desired velocity and force trajectories input by the user, the whole-body controller (WBC) using hierarchical quadratic programming calculates the joint driving torques for tracking the desired trajectories according to task priorities.

[0056] Furthermore, the working method of the robotic arm control module is specifically as follows: the desired motion trajectory of the robotic arm is planned through MPC. Specifically, the state variable of the control system is the joint position of the robotic arm, the input variable of the control system is the joint velocity of the robotic arm, the cost function is to minimize the joint velocity error, and the constraints include constraints based on linear interpolation reference trajectory and physical consistency.

[0057] Furthermore, the operating method of the robotic arm control module also includes calculating the joint torque formula of the robotic arm as follows:

[0058] in, For the mass matrix of the robotic arm, This includes nonlinear terms such as Coriolis force and centripetal force. For gravity, and These are the gains for joint position error and velocity error, respectively. , and These are the joint positions, velocities, and accelerations of the robotic arm. and These are the joint positions and speeds read back from the robotic arm.

[0059] Furthermore, the working method of the whole-body control module is as follows: Step three, NMPC planning, specifically involves the following: the system state variables are the quadruped robot's center of mass momentum and generalized position in the world coordinate system; the system input variables are the foot contact force and joint velocity; the system cost function consists of the system state error and input error; and the constraints include state space equation constraints, foot friction constraints, joint position constraints, support leg stationary constraints, initial state constraints, etc. The optimal state and input trajectory of the quadruped robot system are obtained by solving the optimization problem based on quadratic programming. The state-space equation of the working method of the whole-body control module considers the force exerted by the robotic arm on the quadruped robot body. The formula for calculating the force and torque exerted by the robotic arm base on the quadruped robot body is as follows:

[0060] in, For the mass of the robotic arm, It is the gravitational acceleration vector. and For operating force and operating torque, and These are the position vectors of the robotic arm's center of mass relative to the quadruped robot's center of mass and the position vector of the robotic arm's end effector relative to the quadruped robot's center of mass, respectively. In this invention, it is assumed that the operating force is known. and operating torque .

[0061] Furthermore, the working method of the whole-body control module also includes modeling the WBC quadruped robot's whole-body controller as an optimization problem based on quadratic programming, with swing leg trajectory tracking, body acceleration tracking, and foot contact force tracking as the objective equations. Constraints include floating basis dynamics equation constraints, stationary support leg foot constraints, joint torque constraints, friction cone constraints, and joint position and velocity constraints. The optimization variables are... Solve the problem.

[0062] Implementation Method 3 This invention provides an electronic device including a memory, a processor, and a computer program stored in the memory and executable on the processor. The memory stores software programs and modules, and the processor executes various functional applications and data processing by running the software programs and modules stored in the memory. The memory and processor are connected via a bus. Specifically, the processor implements any step in Embodiment 1 by running the computer program stored in the memory.

[0063] It should be understood that, in the embodiments of the present invention, the processor may be a Central Processing Unit (CPU), but it may also be other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or any conventional processor.

[0064] Memory may include read-only memory, flash memory, and random access memory, and provides instructions and data to the processor. Some or all of the memory may also include non-volatile random access memory.

[0065] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit them. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should all be included within the protection scope of the present invention.

Claims

1. A decoupled motion control method for a quadrupedal mobile manipulator considering the force of a robotic arm, characterized in that, The method includes the following steps: Step 1: The robotic arm control system plans and tracks the desired motion trajectory input by the user through linear model predictive control (MPC). Step 2: Based on the motion trajectory tracked in Step 1, calculate the joint control torque of the robotic arm through the robotic arm dynamics model and PD feedback; Step 3: Using nonlinear model predictive control (NMPC) planning, consider the quadruped robot's whole-body motion trajectory based on the robotic arm dynamics model calculated in Step 2, to obtain the desired velocity trajectory and desired force trajectory that track the user input. Step 4: Based on the desired velocity trajectory and desired force trajectory input by the user in Step 3, the whole-body controller WBC based on hierarchical quadratic programming calculates the joint driving torque for tracking the desired trajectory according to the task priority; The specific steps of step three, NMPC planning, are as follows: the system state variables are the quadruped robot's center of mass momentum and generalized position in the world coordinate system; the system input variables are the foot contact force and joint velocity; the system cost function consists of the system state error and input error; and the constraints include state space equation constraints, foot friction constraints, joint position constraints, support leg stationary constraints, and initial state constraints. The optimal state and input trajectory of the quadruped robot system are obtained by solving the optimization problem based on quadratic programming. In step 3, the state-space equations consider the forces exerted by the robotic arm on the quadruped robot's body. The formulas for calculating the forces and torques exerted by the robotic arm base on the quadruped robot's body are as follows: in, For the mass of the robotic arm, It is the gravitational acceleration vector. and For operating force and operating torque, and These are the position vectors of the robotic arm's center of mass relative to the quadruped robot's center of mass and the position vector of the robotic arm's end effector relative to the quadruped robot's center of mass, respectively. Step 4, modeling the WBC quadruped robot's whole-body controller, is an optimization problem based on quadratic programming. The objective equations are tracking the swing leg trajectory, body acceleration, and foot contact force. Constraints include floating basis dynamics equations, stationary foot end constraints for the supporting leg, joint torque constraints, friction cone constraints, and joint position and velocity constraints. The optimization variables... Solve the problem.

2. The method according to claim 1, characterized in that, In step one, the desired motion trajectory of the robotic arm is planned using MPC. Specifically, the state variable of the control system is the joint position of the robotic arm, and the input variable of the control system is the joint velocity of the robotic arm. The cost function is to minimize the joint velocity error, and the constraints include constraints based on the linear interpolation reference trajectory and physical consistency.

3. The method according to claim 1, characterized in that, The formula for calculating the joint torque of the robotic arm in step two is as follows: in, For the mass matrix of the robotic arm, This includes nonlinear terms involving Coriolis force and centripetal force. For gravity, and These are the gains for joint position error and velocity error, respectively. , and These are the joint positions, velocities, and accelerations of the robotic arm. and These are the joint positions and speeds read back from the robotic arm.

4. A decoupled motion control system for a quadrupedal mobile robot considering the force of a robotic arm, characterized in that, The system uses the decoupled motion control method for a quadrupedal mobile manipulator considering the forces of a robotic arm as described in any one of claims 1-3, and the system includes... Robotic arm control module: The robotic arm control system uses linear model predictive control (MPC) for planning and can track the user-inputted expected motion trajectory; Based on the tracked motion trajectory, the joint control torque of the robotic arm is calculated through the robotic arm dynamics model and PD feedback; Full-body control module: The NMPC planning of predictive control through nonlinear model takes into account the full-body motion trajectory of the quadruped robot based on the mechanical arm dynamics model calculated by the mechanical arm control module, and obtains the desired velocity trajectory and desired force trajectory that are tracked by the user input. Based on the desired velocity and force trajectories input by the user, the whole-body controller (WBC) using hierarchical quadratic programming calculates the joint driving torques for tracking the desired trajectories according to task priorities.

5. The system according to claim 4, characterized in that, The working principle of the robotic arm control module is as follows: the desired motion trajectory of the robotic arm is planned through MPC. Specifically, the state variable of the control system is the joint position of the robotic arm, and the input variable of the control system is the joint velocity of the robotic arm. The cost function is to minimize the joint velocity error. The constraints include constraints based on linear interpolation reference trajectory and physical consistency.

6. The system according to claim 4, characterized in that, The working principle of the robotic arm control module also includes the following formula for calculating the joint torque of the robotic arm: in, For the mass matrix of the robotic arm, This includes nonlinear terms involving Coriolis force and centripetal force. For gravity, and These are the gains for joint position error and velocity error, respectively. , and These are the joint positions, velocities, and accelerations of the robotic arm. and These are the joint positions and speeds read back from the robotic arm.

7. The system according to claim 4, characterized in that, The working method of the whole body control module is as follows: Step 3, NMPC planning, is as follows: The system state variables are the quadruped robot's center of mass momentum and generalized position in the world coordinate system; the system input variables are the foot contact force and joint velocity; the system cost function consists of the system state error and the input error; the constraints include state space equation constraints, foot friction constraints, joint position constraints, support leg stationary constraints, and initial state constraints; the optimal state and input trajectory of the quadruped robot system are obtained by solving the optimization problem based on quadratic programming. The working principle of the whole-body control module is as follows: its state-space equations consider the forces exerted by the robotic arm on the quadruped robot body, and the formulas for calculating the forces and torques exerted by the robotic arm base on the quadruped robot body are as follows: in, For the mass of the robotic arm, It is the gravitational acceleration vector. and For operating force and operating torque, and These are the position vectors of the robotic arm's center of mass relative to the quadruped robot's center of mass and the position vector of the robotic arm's end effector relative to the quadruped robot's center of mass, respectively.

8. The system according to claim 4, characterized in that, The working principle of the whole-body control module also includes that the WBC quadruped robot's whole-body controller is modeled as an optimization problem based on quadratic programming, with swing leg trajectory tracking, body acceleration tracking, and foot contact force tracking as the objective equations. Constraints include floating basis dynamics equation constraints, stationary support leg foot constraints, joint torque constraints, friction cone constraints, and joint position and velocity constraints. The optimization variables are... Solve the problem.

Images