Gait switching control method and system of foot type mechanical arm robot

Abstract

The invention belongs to the technical field of robot control, and particularly relates to a gait switching control method and system for a foot type mechanical arm robot, and the method comprises the following steps: calculating the current gait of the foot type mechanical arm robot and the position of a zero moment point of the gait to be switched; a zero moment point reference track of the foot type mechanical arm robot is planned; and controlling the foot type mechanical arm robot to move according to the zero moment point reference track by using a model predictive control algorithm, adjusting control parameters of the predictive control algorithm in real time according to a loss function, and outputting a state track and an input track in real time so as to realize gait switching of the foot type mechanical arm robot. According to the gait switching control method of the foot type mechanical arm robot, the influence of mechanical arm dynamics on the movement performance of the whole robot is fully considered, and gait switching control of the foot type robot can be achieved.

Description

Technical Field

[0001] The present invention belongs to the technical field related to robot control technology, and more specifically, relates to a gait switching control method and system for a foot-type manipulator robot. Background Art

[0002] Legged robots can achieve body movement by relying on discrete footholds, which has significant advantages over wheeled robots in terms of environmental adaptability. In addition to manipulating objects, the robotic arms installed on quadruped robots can also assist the robot in balancing to improve its dynamic motion performance. In certain specific situations, such as space-constrained environments or when leg joints are damaged, quadruped robots need to rely on two legs to maintain balance and mobility.

[0003] However, how to coordinate the movement of the quadruped robot and the robotic arm to enable the robot to achieve bipedal movement is a challenging problem, specifically how to control the legged robotic arm robot to enter the bipedal standing state and maintain the balance of the bipedal standing state. The existing technology lacks a gait switching control method for quadrupedal robotic arm robots. Summary of the invention

[0004] In view of the above defects or improvement needs of the prior art, the present invention provides a gait switching control method and system for a legged robotic arm robot, so as to realize the switching between a quadrupedal gait and a bipedal gait of a quadrupedal robotic arm robot.

[0005] To achieve the above object, according to one aspect of the present invention, a gait switching control method for a foot-type manipulator robot is provided, the method comprising the following steps:

[0006] S1 calculates the positions of the zero torque points of the current gait and the gait to be switched of the foot-type manipulator robot respectively;

[0007] S2 plans the motion trajectory of the zero torque point of the footed manipulator robot from the current gait to the gait to be switched based on the position and linear velocity of the zero torque point of the current gait and the gait to be switched and the time from the current gait to the gait to be switched, and the motion trajectory is used as the reference trajectory of the zero torque point;

[0008] S3 uses a predictive control algorithm to control the legged robotic arm robot to move along a zero-moment point reference trajectory, calculates the tracking error between the actual motion trajectory and the zero-moment point reference trajectory in real time, uses the tracking error to calculate the loss function of the predictive control algorithm, adjusts the control parameters of the predictive control algorithm in real time according to the loss function, and outputs the state trajectory and input trajectory in real time, thereby realizing the gait switching of the legged robotic arm robot; the predictive control algorithm takes the zero-moment point reference trajectory as input, and the state trajectory and input trajectory as output.

[0009] Further preferably, the loss function expression obtained in step S3 is:

[0010]

[0011] Where x and u represent the state trajectory and input trajectory respectively, P(x,t) is the position of the robot's zero torque point at the current time t, and P(t) ref is the zero moment point reference trajectory, the positive definite matrix Q and the semi-positive definite matrix R are the weight matrices for tracking the reference state trajectory and the input trajectory, and the matrix Q P It is the reference trajectory of the zero moment point P(t) ref The weight matrix of .

[0012] Further preferably, in step S3, the predictive control algorithm needs to satisfy the following constraints, floating base dynamics constraints:

[0013]

[0014] in, represents the linear momentum and angular momentum at the center of mass of the foot-type manipulator, Represents the generalized coordinates, including the six-dimensional coordinates q of the robot base coordinate system in the inertial system base ,q leg represents the leg joint angle of the quadruped robot, q arm Represents the joint angle of the robot arm, A b and A j Represents the center-of-mass momentum matrix A com The corresponding parts of the fuselage and the corresponding parts of the joints, f ci and They represent the vector pointing from the center of mass to the contact point and the three-dimensional contact force of the ground on the robot.

[0015] When the legged robot is in contact, the foot end point and the ground must obey the no-slip constraint:

[0016]

[0017] In the non-contact term, the foot end swing trajectory needs to follow the normal vector of the ground And track the reference trajectory v * (t), the contact force is zero when in non-contact condition:

[0018]

[0019] f ci =0

[0020] When the foot touches the ground, the contact force must satisfy the friction cone constraint:

[0021]

[0022] Where μ is the ground friction coefficient, and They represent the components of the ground contact force in three directions.

[0023] Further preferably, in step S3, the state trajectory includes the linear momentum and angular momentum of the leg-type robotic arm at the center of mass of the robotic arm, the joint angles of the legs, and the joint angles of the robotic arm in the current state; the input trajectory includes the three-dimensional contact force of the ground on the leg-type robotic arm robot, the joint velocity of the legs, and the joint velocity of the robotic arm.

[0024] Further preferably, the contact forces and joint torques of the robot are redistributed through hierarchical quadratic programming to track the optimized trajectory to meet the dynamic constraints and target tracking tasks.

[0025] Dynamic constraint tasks include floating base dynamic constraints, joint torque limit constraints, contact foot no-slip constraints, and friction cone constraints:

[0026]

[0027] Object tracking tasks include swinging foot tracking, floating base, robotic arm joint commands, and contact foot force tracking tasks:

[0028]

[0029] Among them J ci Represents the contact point c i The floating base Jacobi at and Represent the stiffness and damping coefficient of the foot swing trajectory tracking, and Represents the optimized foot joint angles The calculated desired foot position and velocity, and Represent the actual position and speed of the foot, and Represents the stiffness and damping coefficient of the robot end trajectory tracking, and Represents the current joint angle and joint speed of the robot arm.

[0030] Further preferably, the specific calculation method of the zero moment point position in step S1 is as follows:

[0031]

[0032] where x cog ,y cog and zcog Represents the coordinates of the robot's center of gravity in all directions in the inertial system, p x , p y and p z is the symbolic expression of the three-dimensional coordinates of the zero moment point.

[0033] Further preferably, Bezier curve fitting is used to generate the zero moment point reference trajectory, and the formula used to generate the zero moment point reference trajectory is as follows:

[0034] P(τ)=(1-τ) 3 A+3(1-τ) 2 τC+3(1-τ)τ 2 D+τ 3 B

[0035]

[0036] t=τ·T traj

[0037] Among them, control points A and B represent the initial and final two-dimensional coordinates of the zero moment point, and control points C and D are respectively along the initial velocity direction of the zero moment point. and the direction of the final velocity and Represents the initial speed value and the final speed value. The parameter τ∈[0,1] represents the entire process parameter from the initial point P1 to the final point P2. Add a time scale to the Bezier curve: t=τ·T traj ; Among them, T traj Represents the total duration of the zero moment point trajectory from the initial point P1 to the end point P2.

[0038] According to a second aspect of the present invention, a gait switching control system of a foot-type manipulator robot is provided for implementing any one of the above gait switching control methods, the gait switching control system comprising:

[0039] A calculation module, the calculation module is used to calculate the zero moment point position of the foot-type manipulator robot;

[0040] A zero-moment point planning module, wherein the zero-moment point planning module is used to plan a zero-moment point reference trajectory of the foot-type manipulator robot from a current gait to a gait to be switched;

[0041] A predictive control module is used to construct a predictive control algorithm with a zero-moment point reference trajectory as input and a state trajectory and an input trajectory as output, and use the predictive control algorithm to control the movement of the footed manipulator robot, and output the state trajectory and the input trajectory in real time to control the gait switching of the footed manipulator robot.

[0042] In general, the above technical solutions conceived by the present invention have the following beneficial effects compared with the prior art:

[0043] The present invention takes into account the influence of the mechanical arm dynamics on the overall motion performance of the robot, and proposes a gait switching control method and system for a legged mechanical arm robot based on zero-moment point trajectory planning, which assists the legged robot in completing bipedal gait switching and can realize stable bipedal gait switching and walking of a quadrupedal robot.

[0044] The present invention uses a nonlinear model predictive control algorithm based on center of mass dynamics to track the zero moment point reference trajectory and add the tracking error value to the loss function in the model predictive control. By combining the corresponding constraints, including floating base dynamics constraints, no sliding constraints, contact force constraints and friction cone constraints, the state trajectory and input trajectory are optimized to ensure the success of gait switching and maintain the balance and stability of the foot-type manipulator robot. The control framework proposed by the present invention can be extended to a variety of gait movements to cope with extreme scenarios such as robot joint damage. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 is a schematic structural diagram of a quadruped mechanical arm robot constructed according to a preferred embodiment of the present invention;

[0046] Figure 2 is a schematic diagram of a control framework constructed according to a preferred embodiment of the present invention;

[0047] Figure 3 It is a schematic diagram of a trajectory from a quadruped gait to a biped gait constructed according to a preferred embodiment of the present invention. DETAILED DESCRIPTION

[0048] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0049] The present application provides a gait switching control method for a quadruped robotic arm robot, the method comprising the following steps: S1 respectively calculates the positions of the zero torque points of the current gait and the gait to be switched of the legged robotic arm robot; S2 plans the motion trajectory of the zero torque point of the legged robotic arm robot from the current gait to the gait to be switched according to the positions and linear velocities of the zero torque points of the current gait and the gait to be switched and the time from the current gait to the gait to be switched, and the motion trajectory is used as the zero torque point reference trajectory; S3 constructs a predictive control algorithm with the zero torque point reference trajectory as input and the state trajectory and input trajectory as output, uses the predictive control algorithm to control the legged robotic arm robot to move according to the zero torque point reference trajectory, and calculates the tracking error between the actual motion trajectory and the zero torque point reference trajectory in real time, uses the tracking error to calculate the loss function of the predictive control algorithm, adjusts the control parameters of the predictive control algorithm in real time according to the loss function, and outputs the state trajectory and input trajectory in real time, thereby realizing the gait switching of the legged robotic arm robot.

[0050] In one possible embodiment, see Figure 1 and Figure 2 As shown in the figure, the quadruped robot consists of a 12-degree-of-freedom quadruped robot and a 6-degree-of-freedom robotic arm. The quadruped robot includes four legs, each leg has three joints, and is driven by a motor. The joints of each leg are hip joints, knee joints, and ankle joints. The robotic arm is mounted on the main body of the robot and contains six degrees of freedom joints for performing various manipulation tasks, such as grasping, carrying, and operating switches.

[0051] In a possible embodiment, the trajectory of the zero moment point (ZMP) is planned according to the different motion ranges of quadruped and biped gaits, see Figure 3 As shown, in quadrupedal gait, the ZMP moves within the support polygon formed by the four legs, and in bipedal gait, the ZMP moves within the support line segment formed by the two legs.

[0052] In order to achieve a stable switch from a quadruped gait to a bipedal gait, it is necessary to ensure that the ZMP trajectory transitions smoothly during the switching process. The specific steps are as follows: Calculate the ZMP position of the current quadruped gait:

[0053]

[0054] where x cog ,y cog and z cog Represents the coordinates of the robot's center of gravity in all directions in the inertial system, p x , p y and p z is the symbolic representation of the three-dimensional coordinates of ZMP. In particular, when the ground is flat, p z=0

[0055] Plan the trajectory of the ZMP moving from the center of the supporting polygon of the quadruped gait to the center of the supporting line segment of the bipedal gait. Use the Bezier curve to fit the ZMP trajectory to ensure a smooth transition of the trajectory. For the specific ZMP transition process, see Figure 3 The ZMP reference trajectory is generated using the following formula:

[0056] P(τ)=(1-τ) 3 A+3(1-τ) 2 τC+3(1-τ)τ 2 D+τ 3 B

[0057]

[0058] Among them, control points A and B represent the initial and final two-dimensional coordinates of ZMP, and control points C and D are respectively along the initial velocity direction of ZMP. and the direction of the final velocity and represents the initial velocity value and the final velocity value, P(t) represents the ZMP reference trajectory, and the parameter τ∈[0,1] represents the entire process parameter from the initial point P1 to the final point P2.

[0059] Add a time scale to the above Bezier curve, t = τ·T traj

[0060] Among them, T traj Represents the total duration of the ZMP reference trajectory from the initial point P1 to the end point P2.

[0061] In order to track the planned ZMP reference trajectory P(t), this embodiment uses a nonlinear model predictive control algorithm to track the reference state trajectory x ref , input trajectory u ref and ZMP reference trajectory P(t) ref , the operating loss function of model predictive control is as follows:

[0062]

[0063] Among them, P(x, t) is the position of the robot's ZMP at the current time t, and the positive definite matrix Q and the semi-positive definite matrix R are the weight matrices for tracking the reference state trajectory and the input trajectory. P is to track the ZMP reference trajectory P(t) ref The weight matrix of .

[0064] In order to ensure that the control strategy of the legged manipulator does not violate the laws of physics, the model predictive control needs to meet certain constraints. First, the floating base dynamics constraints must be met.

[0065]

[0066] in, represents the linear momentum and angular momentum at the center of mass of the foot-type manipulator, Represents the generalized coordinates, including the six-dimensional coordinates q of the robot base coordinate system in the inertial system base ,q leg represents the leg joint angle of the quadruped robot, q arm Represents the joint angle of the robot arm. b and A j Represents the center-of-mass momentum matrix A com The corresponding parts of the fuselage and the corresponding parts of the joints. ci and They represent the vector pointing from the center of mass to the contact point and the three-dimensional contact force of the ground on the robot.

[0067] When the legged robot is in contact, the foot end point and the ground must obey the no-slip constraint:

[0068]

[0069] In the non-contact term, the foot end swing trajectory needs to follow the normal vector of the ground And track the reference trajectory v * (t), the contact force is zero when in non-contact condition:

[0070]

[0071] f ci =0

[0072] When the foot touches the ground, the contact force must satisfy the friction cone constraint:

[0073]

[0074] Where μ is the ground friction coefficient, and They represent the components of the ground contact force in three directions.

[0075] After optimization by the nonlinear model predictive controller (NMPC), an optimized state variable trajectory is obtained. And the input variable trajectory

[0076] In one possible embodiment, the optimized trajectory can be tracked using whole body control (WBC). The WBC controller redistributes contact forces, generalized accelerations, and joint torques through hierarchical quadratic programming to track the optimized trajectory and meet dynamic constraints and priority tasks. The control variables of the WBC are as follows:

[0077]

[0078] The priority task allocation is adopted to divide the tasks into dynamic constraint tasks and target tracking tasks, among which the dynamic constraint tasks are met first. The dynamic constraint tasks include floating base dynamic constraint, joint torque limit constraint, contact foot no-slip constraint and friction cone constraint.

[0079]

[0080] The target tracking tasks include swing foot tracking, floating base, robot arm joint commands, and contact foot force tracking tasks.

[0081]

[0082] Among them J ci Represents the contact point c i The floating base Jacobi at and Represent the stiffness and damping coefficient of the foot swing trajectory tracking, and Represents the optimized foot joint angles The calculated desired foot position and velocity, and Represent the actual position and speed of the foot, and Represents the stiffness and damping coefficient of the robot end trajectory tracking, and Represents the current joint angle and joint speed of the robot arm.

[0083] By planning the ZMP trajectory of the footed robotic arm robot and adding the ZMP trajectory tracking loss function to the robot's nonlinear model predictive controller, the optimized state trajectory and input trajectory are obtained. Finally, the optimized trajectory is tracked using a whole-body motion controller based on hierarchical quadratic optimization to achieve the task of bipedal gait walking of the footed robotic arm.

[0084] It will be easily understood by those skilled in the art that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A gait switching control method for a foot-type manipulator robot, characterized in that: The following steps are involved: S1 calculates the positions of the zero torque points of the current gait and the gait to be switched of the foot-type manipulator robot respectively; S2 plans the motion trajectory of the zero torque point of the footed manipulator robot from the current gait to the gait to be switched based on the position and linear velocity of the zero torque point of the current gait and the gait to be switched, as well as the time from the current gait to the gait to be switched, and the motion trajectory is used as the reference trajectory of the zero torque point; S3 uses a predictive control algorithm to control the legged robotic arm robot to move along a zero-moment point reference trajectory, calculates the tracking error between the actual motion trajectory and the zero-moment point reference trajectory in real time, uses the tracking error to calculate the loss function of the predictive control algorithm, adjusts the control parameters of the predictive control algorithm in real time according to the loss function, and outputs the state trajectory and input trajectory in real time, thereby realizing the gait switching of the legged robotic arm robot; the predictive control algorithm takes the zero-moment point reference trajectory as input, and the state trajectory and input trajectory as output.

2. The gait switching control method of a foot-type manipulator robot according to claim 1, characterized in that: The loss function expression obtained in step S3 is: Where x and u represent the state trajectory and input trajectory respectively, P(x,t) is the position of the robot's zero torque point at the current time t, and P(t) ref is the zero moment point reference trajectory, the positive definite matrix Q and the semi-positive definite matrix R are the weight matrices for tracking the reference state trajectory and the input trajectory, and the matrix Q P It is the reference trajectory of the zero moment point P(t) ref The weight matrix of .

3. The gait switching control method of a foot-type manipulator robot according to claim 1, characterized in that: The constraints of the predictive control algorithm in step S3 include floating base dynamics constraints, no-slip constraints, contact force constraints, and friction cone constraints.

4. The gait switching control method of a foot-type manipulator robot according to claim 3, characterized in that: The floating base dynamic constraint expression is: The no-slip constraint expression is: The contact force constraint expression is: f ci ＝0 The friction cone constraint expression is: in, represents the linear momentum and angular momentum at the center of mass of the foot-type manipulator, Represents the generalized coordinates, including the six-dimensional coordinates q of the robot base coordinate system in the inertial system base ,q leg represents the leg joint angle of the quadruped robot, q arm Represents the joint angle of the robot arm, A b and A j Represents the center-of-mass momentum matrix A com The corresponding parts of the fuselage and the corresponding parts of the joints, f ci and They represent the vector from the center of mass to the contact point and the three-dimensional contact force of the ground on the robot; Represents the speed between the end point of the foot and the ground, Represents the normal vector of the ground, v * (t) is the reference speed, μ is the ground friction coefficient, and They represent the components of the ground contact force in three directions.

5. The gait switching control method of a foot-type manipulator robot according to claim 1, characterized in that: In step S3, the state trajectory includes the linear momentum and angular momentum of the leg-type manipulator robot at the center of mass of the manipulator arm in the current state, the joint angles of the legs, and the joint angles of the manipulator arm; the input trajectory includes the three-dimensional contact force of the ground on the leg-type manipulator robot, the joint velocity of the legs, and the joint velocity of the manipulator arm.

6. The gait switching control method of a foot-type manipulator robot according to claim 1, characterized in that: The calculation method of the position of the zero moment point in step S1 is as follows: Among them, x cog ,y cog and z cog Represents the coordinates of the robot's center of gravity in all directions in the inertial system, p x , p y and p z is the symbolic expression of the three-dimensional coordinates of the zero moment point.

7. The gait switching control method of a foot-type manipulator robot according to claim 1, characterized in that: The specific implementation method of step S2 is to use Bezier curve fitting to generate a zero moment point reference trajectory, and the zero moment point reference trajectory is as follows: P(τ)=(1-τ) 3 A+3(1-t) 2 τC+3(1-τ)τ 2 D+t 3 B t=τ·T traj Among them, control points A and B represent the initial and final two-dimensional coordinates of the zero moment point, and control points C and D are respectively along the initial velocity direction of the zero moment point. and the direction of the final velocity and represents the initial speed value and the final speed value, the parameter τ∈[0,1] represents the entire process parameter from the initial point P1 to the final point P2, t is the time scale, T traj Represents the total duration of the zero moment point trajectory from the initial point P1 to the end point P2.

8. A gait switching control system for a foot-type manipulator robot, characterized in that: Used to implement the gait switching control method according to any one of claims 1 to 6, the gait switching control system comprises: A calculation module, the calculation module is used to calculate the zero moment point position of the foot-type manipulator robot; A zero-moment point planning module, wherein the zero-moment point planning module is used to plan a zero-moment point reference trajectory of the foot-type manipulator robot from a current gait to a gait to be switched; A predictive control module is used to construct a predictive control algorithm with a zero-moment point reference trajectory as input and a state trajectory and an input trajectory as output, and use the predictive control algorithm to control the movement of the footed manipulator robot, and output the state trajectory and the input trajectory in real time to control the gait switching of the footed manipulator robot.

Images