End tracking method for foot-arm robot based on model predictive control and whole body force control

Abstract

The application provides an end tracking method for a foot-arm robot based on model predictive control and whole-body force control, and the method comprises the following steps: acquiring joint feedback data of a quadruped robot and a mechanical arm, and an end trajectory instruction of the mechanical arm at a current time; estimating a current pose and a current speed of a robot trunk part through a state estimator; obtaining a body position, a body speed, a body acceleration, a joint position, a joint speed, a joint acceleration and a foot end contact force trajectory of the quadruped robot in a time period as a first layer control architecture; establishing a whole-body dynamics model of the quadruped robot carrying the mechanical arm, adding balance related constraints, body and joint tracking constraints and foot end force tracking constraints as a second layer control architecture; and combining the first layer control architecture and the second layer control architecture to obtain joint torque of the quadruped robot carrying the mechanical arm. The application can realize tracking of an end trajectory while not violating the physical limit of the robot.

Description

Technical Field

[0001] This invention relates to the field of robot trajectory control technology, and in particular to an end-effector tracking method for a foot-arm robot based on model predictive control and whole-body force control. Background Technology

[0002] Existing control methods for quadruped robots equipped with robotic arms are mostly based on separate control of the legs and arms, and they do not adequately consider the constraints related to the trajectory tracking task at the end of the robotic arm, resulting in poor tracking performance for complex trajectories.

[0003] Compared to wheeled robots, quadruped robots have the advantage of good terrain mobility, making it easier to traverse stairs, ramps, ditches, and other terrains. Quadruped robots equipped with robotic arms have good mobility and operational capabilities, making them potential applications in factories, public services, disaster relief, and other scenarios.

[0004] However, most existing control methods for quadruped robots equipped with robotic arms adopt a split-type control approach and do not fully consider constraints related to end-effector trajectory tracking, such as end-effector position and velocity tracking, self-collision, and joint limit constraints. When executing complex end-effector trajectories, this may lead to poor end-effector tracking performance or even damage to the robot by violating its physical limits. Summary of the Invention

[0005] The purpose of this invention is to provide a method for end-effector tracking of a quadruped robot based on model predictive control and whole-body force control, which solves the problem that existing methods cannot achieve full-body coordination between the robotic arm and the quadruped robot to complete the end-effector trajectory tracking task without violating physical constraints.

[0006] To achieve the above objectives, the present invention provides the following technical solution:

[0007] A method for end-effector tracking of a legged robot based on model predictive control and whole-body force control includes:

[0008] Step 1: Obtain the joint feedback data of the quadruped robot and the robotic arm, as well as the end effector trajectory command of the robotic arm at the current moment;

[0009] Step 2: Estimate the current pose and current velocity of the robot's torso using a state estimator;

[0010] Step 3: Obtain the body position, body velocity, body acceleration, joint position, joint velocity, joint acceleration, and foot contact force trajectory of the quadruped robot within a time period, as the first layer of control architecture;

[0011] Step 4: Establish a full-body dynamic model of the quadruped robot equipped with a robotic arm, and add balance-related constraints, body and joint tracking constraints, and foot force tracking constraints as the second-layer control architecture; combine the first-layer control architecture and the second-layer control architecture to obtain the joint torque of the quadruped robot equipped with the robotic arm.

[0012] In step 1, the joint feedback data includes the current angle, current angular velocity, and current angular acceleration of the robot joint.

[0013] Furthermore, the current angular acceleration is obtained by performing a finite difference on the angular velocity.

[0014] In step 1, the end-effector trajectory command includes the three-dimensional coordinates and attitude of the robotic arm in the state estimation system at the current moment.

[0015] In step 2, the current pose includes the position and orientation of the robot torso in the state estimation coordinate system.

[0016] In step 2, the current velocity includes the translational velocity and rotational velocity of the robot torso in the state estimation coordinate system.

[0017] In step 3, a center-of-mass dynamic model of the quadruped robot with a robotic arm is established. The state variables of the quadruped robot are x(t) = (h... com q b q J The input variables for the quadruped robot are:

[0018] In the formula, Let the linear momentum and angular momentum be the robot's center of mass. Let x, y, z coordinates of the center of mass of the quadruped robot in the global coordinate system, and roll, pitch, and yaw angles. The joint angles and angular velocities of each motor in the quadruped robot; For contact force;

[0019] The momentum of the center of mass is calculated using the mass-momentum matrix.

[0020] Calculate the joint angle limit constraints, joint velocity limit constraints, and end-effector trajectory tracking cost function of the robotic arm.

[0021] Furthermore, the equality constraint is the state-input equality constraint g1(x(t), u(t), t) of the quadruped robot:

[0022]

[0023] In the formula, c represents the absolute velocity at the point of contact of the leg. iThis indicates the contact state of the i-th leg. Indicates contact. This indicates no contact.

[0024] In step 4, the whole-body dynamics equations of the quadruped robot equipped with a robotic arm are as follows:

[0025] In the formula, For the generalized coordinates of the quadruped robot, For the generalized velocity of a quadruped robot, Let S be the mass matrix of the robot, C(q, u) and G(q) be the Coriolis force and gravity term acting on the robot, respectively, and S be the selection matrix. Let be the contact force of the contacting leg, and nc be the number of contacting legs. The stacking of Jacobian matrices for contact legs;

[0026] The generalized acceleration is optimized using a quadratic programming (QP) algorithm. The contact force λ is used as a decision variable for unified solution, and the decision variable is defined as follows: The QP problem then takes the following form:

[0027]

[0028] stCEξ+ce=0;

[0029] CIξ+ci≥0;

[0030] Constraints and performance indicators for the robot's whole-body control are constructed through three tracking tasks: body trajectory tracking, joint angle trajectory tracking, and contact leg force tracking. Weight parameters are then set to solve this QP problem, yielding the optimal solution after balancing the decision variables according to the set weight parameters. Then, the solution is obtained through the dynamic equations, as follows:

[0031]

[0032] A robot-wide dynamics model is used to establish PD (Power-Displacement) compensation control for the body and foot trajectories. Performance evaluation indicators for trajectory tracking and contact leg force tracking are incorporated into the quadratic programming (QP) paradigm along with the robot's constraint equations, ultimately achieving feedforward calculation of joint torques. Combined with joint PD feedback control, the final joint space driving torque command is generated as follows:

[0033]

[0034] In the formula, The joint reference positions and velocities obtained for the first-level trajectory planning module.

[0035] Furthermore, the constraints on quadruped robots include:

[0036] (1) Fuselage balance constraints:

[0037]

[0038] In the formula, The first six lines of the corresponding terms in the dynamic equation;

[0039] (2) Fixed contact point constraint:

[0040]

[0041] In the formula, The Jacobian matrix of the contact point;

[0042] (3) Friction cone constraint:

[0043] For the i-th contacting leg, the constraint is CI. fcon λ i In the form ≥0, CI in the formula fcon It has the following form:

[0044]

[0045] In the formula, n is the normal unit vector of the contact surface, h and l are unit vectors perpendicular to the normal vector, and μ s The static friction coefficient of the mating surfaces;

[0046] (4) Torque limit constraint:

[0047] For the joint motors of a robot, τ min ≤τ≤τ max Transforming this to decision variables, we have:

[0048]

[0049] This can then be expressed in the form of inequality constraints in the QP problem:

[0050]

[0051] In the formula, These are the joint-related terms in the dynamic equations;

[0052] Trajectory tracking can be represented in a least-squares form:

[0053]

[0054] In the formula,

[0055] In the formula, A1, b iDescriptions representing four tracking tasks, w i As weight.

[0056] Compared with existing technologies, the end-effector tracking method for legged robots based on model predictive control and whole-body force control provided by this invention has the following advantages:

[0057] In this invention, the trajectory tracking problem is transformed into a quadratic programming problem in the whole-body control problem. A model error compensation mechanism for the foot and fuselage trajectories is established using PD feedback compensation to ensure the robustness of torque control for motion trajectory tracking under model errors. Then, the multi-objective tracking indexes of the foot, fuselage trajectory, and foot contact force are comprehensively considered, along with the corresponding joint space constraint equations, to finally achieve joint torque control based on whole-body dynamics.

[0058] This invention considers the robot's whole-body center of mass dynamics to achieve coordinated movement of the legs and arms. At the same time, it adds end-effector trajectory tracking-related constraints, as well as joint limit and self-collision constraints, which can achieve end-effector trajectory tracking without violating the robot's physical limits. Attached Figure Description

[0059] To more clearly illustrate the technical solution of the present invention, the accompanying drawings used in the technical description will be briefly introduced below.

[0060] Figure 1 The control flow diagram provided by the present invention;

[0061] Figure 2 A force diagram of a quadruped robot equipped with a robotic arm provided by the present invention;

[0062] Figure 3 A simplified collision model diagram of a quadruped robot equipped with a robotic arm provided by the present invention. Detailed Implementation

[0063] The following detailed description provides further details on specific implementation methods.

[0064] The quadruped robot with a robotic arm in this invention includes a quadruped robot and a robotic arm, with a controller for both the quadruped robot and the robotic arm internally configured. The robotic arm includes a position controller, joint controller 1, joint controller 2, ..., joint controller n, and an end effector. In this embodiment, the robotic arm has six joints.

[0065] This invention proposes an end-effector tracking method for a legged robot based on model predictive control and whole-body force control, achieving end-effector tracking without violating physical constraints. These physical constraints mainly include self-collision constraints and joint position limit constraints. This invention considers the robot's whole-body center-of-mass dynamics to achieve coordinated legged-arm movement; simultaneously, it adds end-effector trajectory tracking-related constraints, as well as joint limit and self-collision constraints, enabling end-effector trajectory tracking without violating the robot's physical limits. This invention includes:

[0066] Step 1: Obtain joint feedback data of the quadruped robot and the robotic arm, as well as the pose command data of the robotic arm's end effector at the current moment.

[0067] The joint feedback data includes the robot joint's current position, current velocity, and current acceleration. The current angular acceleration is obtained by performing finite difference on the angular velocity. The end-effector trajectory command includes the robot arm's three-dimensional coordinates and orientation in the state estimation system at the current moment.

[0068] Step 2: Estimate the current pose and current velocity of the robot's torso using a state estimator. The current pose includes the position and orientation of the robot's torso in the state estimation coordinate system, and the current velocity includes the translational and rotational velocities of the robot's torso in the state estimation coordinate system.

[0069] Step 3: Using a nonlinear model predictive control method, add trajectory tracking task-related constraints and foot motion-related constraints to obtain the robot's body and joint positions, velocities, accelerations, and foot contact force trajectories over a time period, forming the first layer of control architecture.

[0070] Step 4: Establish a full-body dynamics model of the quadruped robot equipped with a robotic arm. Using quadratic programming, balance-related constraints, body and joint tracking constraints, and foot force tracking constraints are added to form the second-layer control architecture. The first and second-layer control architectures are combined to obtain the joint torques of the quadruped robot equipped with the robotic arm. The control block diagram is shown below. Figure 1 As shown.

[0071] The following section details the solution process in step 3.

[0072] Model predictive control (MPC) is typically written as solving the following optimal control problem:

[0073]

[0074] In the formula, These correspond to the state variables and input variables, respectively; φ(x(T)) and L(x(t), u(t), t) correspond to the final state performance and dynamic performance in the trajectory in the performance function, namely the Mayer term and the Language term; among the constraints, the first formula represents the robot's dynamic model, i.e., the model constraint; next are the state-input equality constraint g1(x(t), u(t), t)=0; and the state constraint g2(x(t), t)=0; as well as the state-input inequality constraint h(x(t)u(t), t)≥0; and the initial condition x(0)=x0.

[0075] The following describes the process of modeling the center of mass dynamics of the quadruped robot with a robotic arm established in this invention, setting the robot's state variables as x(t) = (h com q b q J The robot's input variables are:

[0076] In the formula, Let the linear momentum and angular momentum be the robot's center of mass. Let x, y, z coordinates of the robot's center of mass and roll, pitch, and yaw angles be defined in the global coordinate system. The joint angles and angular velocities of each motor in the robot; For contact force, such as Figure 2 As shown.

[0077] The quadruped robot model with arms of the present invention adopts the center of mass dynamics model, in which the momentum of each joint is projected onto the center of mass, thereby generating the momentum of the center of mass, and introducing the center of mass momentum matrix (CMM). The momentum of the center of mass is a function of the overall kinematics and inertia of the entire system, and can be calculated in a computer using an open-source dynamics library based on the full system model of the robot and the joint motor angles. After obtaining the CMM, the momentum of the center of mass can be expressed as:

[0078]

[0079] According to the principles of mechanics, the change in the momentum of the center of mass... It is related to the external forces and torques acting on it, that is:

[0080]

[0081]

[0082] In the formula, n c This represents the number of contact points. This yields the complete state-space expression for the center-of-mass dynamics model, i.e., the constraints...

[0083] The quadratic tracking performance function used in this invention is as follows:

[0084]

[0085] In the formula, r IE v IE ω IE These represent the position, velocity, and rotational angular velocity of the robotic arm's end effector, respectively, with the superscript "ref" corresponding to its reference value. ζ IE Q represents the difference between the current pose and the reference pose; Q and R are both weight matrices.

[0086] In this invention, the robotic arm is subject to unequal constraints including joint position limit constraints, joint velocity limit constraints, self-collision constraints, and friction cone constraints. The first two constraints are manifested as follows:

[0087] x min ≤x arm ≤x max

[0088] u min ≤u arm ≤u max

[0089] In the formula, x min x max u min u max These represent the lower and upper limits of the robotic arm joint positions; and the lower and upper limits of the joint velocities, respectively; x arm u arm These represent joint angle and velocity, respectively.

[0090] Regarding self-collision constraints, in this invention, the collision model between the quadruped robot and the robotic arm is simplified as follows: Figure 3 The distance between the centers of the spheres shown is less than the sum of their radii, i.e., d. collision >d min .

[0091] For a quadruped robot's legs to not slip while supporting itself, the contact force must be within the "friction cone," determined by the coefficient of friction between the ground and the leg.

[0092]

[0093] In the formula, μ is the coefficient of friction. These are the components of the contact force in the x, y, and z directions, respectively, and ε is a decimal value that adjusts the degree of constraint relaxation.

[0094] The robot is subject to equal constraints. Within a given walking trajectory, this invention specifies a gait table defining the rhythmic movements of its four legs. The gait table is a periodic table that periodically defines the contact states of the four legs. During the contact time, the velocity of a leg or arm is zero; for a leg during the swinging motion, the contact force is zero (ignoring air resistance), and for the robot to lift its leg, it must be perpendicular to the ground. There is a given velocity v. * (t). From this, we can obtain the state-input equality constraint g1(x(t), u(t), t) for the quadruped robot:

[0095]

[0096] In the formula, c represents the absolute velocity at the point of contact of the leg. i This indicates the contact state of the i-th leg. Indicates contact. This indicates no contact. At this point, the model predictive control part of the trajectory planning for the quadruped robot equipped with a robotic arm is complete. It includes the dynamic model, performance functions, state-input equality constraint g1(x(t), u(t), t)=0 and state-input inequality constraint h(x(t), u(t), t)≥0.

[0097] The following section details the solution process in step 4.

[0098] To achieve full-body torque control of the robot and track the trajectories of the center of mass, body, and feet based on the high-dimensional system dynamics of a quadruped robot equipped with a robotic arm, it is necessary to obtain joint torques as the final drive commands for the joint actuators according to the target motion trajectory. After obtaining the trajectory solved by the first-layer trajectory planner, the quadruped robot needs to track this trajectory. x* (t), u * (t). This requires the construction of a second-layer controller based on the robot's whole-body dynamics model. For a multi-rigid-body robot with a robotic arm, its whole-body dynamics equations can be expressed as follows:

[0099]

[0100] In the formula, For the generalized coordinates of the quadruped robot, For the generalized velocity of a quadruped robot. Let S be the mass matrix of the robot, where C(q, u) and G(q) correspond to the Coriolis force and gravity terms acting on the robot, respectively. The S matrix is ​​the selection matrix, choosing which degrees of freedom can be driven. For underactuated systems like quadruped robots, only the 12 joints of the robot's legs can be controlled, and the robot's body cannot be directly driven, so S = [0, u, g]. 18× 6I 18×12 ]and This is the joint driving torque. For the contact force of the contact leg, n c The number of legs in contact. For the stacking of Jacobian matrices of the contact legs.

[0101] In order to generalize acceleration The contact force λ is solved uniformly as a decision variable using a quadratic programming QP optimization algorithm, which can ensure the solution of joint torques while also tracking the motion trajectory and contact force. The decision variable is defined as... The QP problem can then be written in the following form:

[0102]

[0103] stCEξ+ce=0

[0104] CIξ+ci≥0

[0105] Robots are currently subject to the following four constraints:

[0106] 1. Fuselage balance constraints

[0107]

[0108] In the formula, These are the first six lines of the corresponding terms in the dynamic equation.

[0109] 2. Fixed contact point constraint

[0110] For the point where the quadruped robot contacts the ground, it is desirable that there is no relative slippage between it and the ground, i.e., p i = constant, which can be constrained by setting the second derivative of the contact point to 0.

[0111]

[0112] In the formula, Let be the Jacobian matrix of the contact points. This can be expressed in the form of inequality constraints in a QP problem as follows:

[0113]

[0114] 3. Friction cone constraint

[0115] For the i-th contacting leg, the constraint can be written as CI fcon λ i In the form ≥0, CI in the formula fcon It has the following form:

[0116]

[0117] In the formula, n is the normal unit vector of the contact surface, h and l are unit vectors perpendicular to the normal vector, and μ s denoted as the static friction coefficient of the mating surfaces.

[0118] 4. Torque limit constraint

[0119] The torque that a robot's joint motors can actually provide is limited, i.e., τ. min ≤τ≤τ max Transforming this to decision variables, we have:

[0120]

[0121] This can then be expressed in the form of inequality constraints in the QP problem:

[0122]

[0123] In the formula, These are the joint-related terms in the dynamic equations.

[0124] Trajectory tracking can be written in least squares form, namely:

[0125]

[0126] In the formula,

[0127]

[0128] In the formula, A1, b i Descriptions representing four tracking tasks, w i Let the weights be denoted as H. Then, the Hessian matrix and gradient term in the standard QP problem are represented as H = A. T A, g = -A T b. The following describes the three tracking tasks.

[0129] 1. Aircraft trajectory tracking

[0130] A1 = [J] b 0]

[0131]

[0132] 2. Joint angle trajectory tracking

[0133] For the i-th swing leg and the robotic arm:

[0134]

[0135]

[0136] 3. Contact leg force tracking

[0137] For the i-th contact leg:

[0138] A 3，i =[0I]

[0139] b 3，i =λ ref

[0140] The above three points establish the constraints and performance indicators for the robot's whole-body control. By setting appropriate weight parameters, this QP problem can be solved, thus obtaining the optimal solution after balancing the decision variables according to the set weight parameters. Then, the solution is obtained using the dynamic equation (1), as shown below:

[0141]

[0142] This approach establishes PD (Positioning-Driven) compensation control for the body and foot trajectories using a whole-body dynamics model of the robot. Performance evaluation metrics for trajectory tracking and contact leg force tracking are then incorporated into a quadratic programming (QP) paradigm, along with the constraint equations of the legged robot, ultimately achieving feedforward calculation of joint torques. Combined with joint PD feedback control, the final joint space driving torque command is generated, expressed as:

[0143]

[0144] In the formula, The joint reference positions and velocities obtained for the first-level trajectory planning module.

[0145] Therefore, this invention adopts a nonlinear model predictive control approach, which can add constraints related to the robot arm end-effector tracking task from the planning layer, so that the robot can track the robot arm end-effector trajectory while complying with robot joint limits and collision constraints.

[0146] This invention establishes a center-of-mass dynamic model of a quadruped robot equipped with a robotic arm, projects the momentum of each link of the robot onto the center of mass of the robot, and establishes the center-of-mass dynamic equation.

[0147] This invention assumes that the robot is a multi-rigid body and that the contact between the foot and the ground is a point contact. Based on Newton's and Euler's equations, a dynamic model of the center of mass of the quadruped robot equipped with a robotic arm is obtained.

[0148] This invention adds quadratic functions for tracking the position, rotation, linear velocity, and angular velocity of the robotic arm's end effector trajectory to the cost function in nonlinear model predictive control; it also adds quadratic constraints on the state input. This allows for the simultaneous tracking of both attitude and velocity terms of the trajectory.

[0149] This invention adds collision constraints to the inequality constraints in nonlinear model predictive control, and the robot collision model is approximated using multiple collision spheres. This prevents the robot from colliding with itself or the environment while tracking its end-effector trajectory.

[0150] This invention adds foot motion-related constraints to nonlinear model predictive control: when the foot is in contact with the ground, a zero-velocity constraint and a contact friction cone constraint are added; when the foot is swinging, a vertical velocity trajectory tracking constraint and a zero foot contact force constraint are added.

[0151] The nonlinear model predictive control part of this invention solves for the robot's body and joint positions, velocities, accelerations, and foot contact force trajectories over a time period.

[0152] This invention is based on Lagrangian dynamics, assuming the robot is a multi-rigid-body structure, and establishes a whole-body dynamics model of a quadruped robot equipped with a robotic arm. Since the robot is underactuated, the first six rows of the whole-body dynamics model are the underactuated parts related to the body, and the rest are the joint-actuated parts.

[0153] The whole-body force control model of this invention adopts the whole-body dynamics model of a quadruped robot equipped with a robotic arm. The cost function includes tracking terms for the position, velocity, and acceleration of the body and joints, as well as tracking terms for foot contact force. The constraints are equality constraints, foot friction cone constraints, joint torque limit constraints, and foot contact static constraints formed by the dynamic model.

[0154] The force control part of the quadruped robot equipped with a robotic arm in this invention is described and solved in real time using a quadratic programming problem.

[0155] This invention combines a first-layer control architecture and a second-layer control architecture. The nonlinear model prediction control module of the first layer solves for the position, velocity, acceleration trajectory of the fuselage and joints and the contact force trajectory of the foot end over a period of time. The whole-body force control part of the second layer tracks the optimal trajectory of the first layer and solves for the joint torque.

[0156] In this invention, both control layers are solved in real time to ensure the robot's balance and robustness.

[0157] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A foot-arm robot end tracking method based on model predictive control and whole-body force control, characterized in that, The method comprises the following steps: Step 1: acquiring joint feedback data of the quadruped robot and the mechanical arm, and an end trajectory instruction of the end of the mechanical arm at the current time, the joint feedback data comprising a current angle, a current angular velocity and a current angular acceleration of the robot joint, the current angular acceleration being obtained by performing finite difference on the angular velocity, and the end trajectory instruction comprising three coordinates and a pose of the mechanical arm in a state estimation system at the current time; Step 2: estimating a current pose and a current speed of the robot trunk by a state estimator, the current pose comprising a position and a pose of the robot trunk in the state estimation coordinate system, and the current speed comprising a translational speed and a rotational speed of the robot trunk in the state estimation coordinate system; Step 3: obtaining a body position, a body speed, a body acceleration, a joint position, a joint speed, a joint acceleration and a foot end contact force trajectory of the quadruped robot in a time period, as a first layer control architecture; Step 4: establishing a whole body dynamics model of the quadruped robot carrying the mechanical arm, and adding balance related constraints, body and joint tracking constraints and foot end force tracking constraints, as a second layer control architecture; In combination with the first layer control architecture and the second layer control architecture, a joint torque of the quadruped robot carrying the mechanical arm is obtained.

Images