Four-footed robot dog dynamic grabbing method and system combined with 6D attitude estimation

Abstract

The invention relates to the field of robot dogs, in particular to a four-footed robot dog dynamic grabbing method and system combined with 6D posture estimation. The method comprises the following steps: modeling and optimizing a quadruped robot dog with a mechanical arm to obtain a kinetic model of the quadruped robot dog, and designing a quadruped robot dog controller according to the kinetic model of the quadruped robot dog; using a 6D attitude estimation method of a CAD model to obtain real-time position and attitude information of the to-be-grabbed article; and according to the position and posture information of the to-be-grabbed object, the four-foot robot dog with the mechanical arm is controlled to move through the four-foot robot dog controller, and the to-be-grabbed object is dynamically grabbed. According to the method, a 6D attitude estimation method of a CAD model is used, and real-time position and attitude information of an object to be grabbed is obtained; the four-footed robot dog is controlled to move through the four-footed robot dog controller, real-time planning is carried out, and the to-be-grabbed object is dynamically grabbed. And the grabbing range can be customized to further improve the article grabbing success rate.

Description

Technical Field

[0001] The present invention relates to the field of robot dogs, and more specifically, to a dynamic grasping method and system for a quadruped robot dog combined with 6D posture estimation. Background Art

[0002] With the progress of society and the development of science and technology, the application field of mobile robots has gradually expanded. At present, robots are being used more and more in industrial manufacturing, agricultural production and household services. Quadruped robots can move on unstructured ground and have a certain load capacity. Adding a mechanical arm to the robot can realize grasping and handling tasks. As the most practical basic function of robots, grasping technology is crucial. Traditional robot grasping tasks are mostly fixed-point grasping on the assembly line, or using template matching to find the position of the object for grasping. It is only suitable for grasping static objects, but cannot grasp dynamic objects.

[0003] The prior art discloses a quadruped robot with a body pitch joint and a grasping mechanism and a grasping method thereof, comprising a driving mechanism, a body, four mechanical legs and a grasping mechanism. The grasping method is: first drive the quadruped robot to move toward the target grasping object until the hand claw is located directly above the target grasping object; then coordinate the leg movement to make the robot move toward the target grasping object until the target grasping object is located within the grasping range of the grasping mechanism; drive the spine joint to make the robot body bulge upward, grasping component one and grasping component two form a grasping action for the target grasping object, coordinate the leg movement during the grasping process, and keep the target grasping object within the grasping range of the grasping mechanism; after grasping the object, coordinate the leg movement to make the robot move upward until the robot returns to the initial motion state, and drive the robot to continue to move. The grasping mechanism of the present invention is simple, the control method has a clear hierarchy, the control is precise, and the operation is stable and reliable. However, this method still cannot grasp dynamic objects. Summary of the invention

[0004] The purpose of the present invention is to disclose a dynamic grasping method and system of a quadruped robot dog combined with 6D posture estimation that can grasp dynamic objects.

[0005] In order to achieve the above object, the present invention provides a dynamic grasping method of a quadruped robot dog combined with 6D posture estimation, comprising:

[0006] S1: Model and optimize the quadruped robot dog with a robotic arm to obtain the dynamic model of the quadruped robot dog

[0007] S2: Design a quadruped robot dog controller based on the quadruped robot dog dynamics model;

[0008] S3: Use the 6D posture estimation method of the CAD model to obtain the real-time position and posture information of the object to be grasped;

[0009] S4: According to the position and posture information of the object to be grasped, the quadruped robot dog with a mechanical arm is controlled to move through the quadruped robot dog controller, and dynamic grasping of the object to be grasped is realized.

[0010] Furthermore, in step S1, the quadruped robot dog with a robotic arm includes: a quadruped mobile platform leg joint, a quadruped mobile platform base, a robotic arm joint and a robotic arm end effector.

[0011] Furthermore, in step S1, modeling a quadruped robot dog with a robotic arm includes:

[0012] Establish the floating base dynamics model:

[0013]

[0014] Wherein, all subscripts b, l, a, and e represent the quadruped mobile platform base, the quadruped mobile platform leg joints, the manipulator joints, and the manipulator end effector, respectively. M is the joint space inertia matrix. Describes the nonlinear effects caused by the centripetal force and the Coriolis force, G represents the joint space gravity, It represents the reaction force of the foot when the quadruped mobile platform contacts the ground. represents the spatial force generated when the end effector of the robot interacts with the environment, J st ,J e are the Jacobian matrices of the leg end effector and the robotic arm end effector, respectively, and τ represents the torque of each joint.

[0015] Further, the optimization includes:

[0016] The floating basis dynamics model is projected onto the center of mass of the robot dog system. The dimension of the original robot dog floating basis dynamics equation is increased from 6+n l +n a Reduced to 6 dimensions and freed from the dependence on joint torque τ, the center of mass dynamics model is obtained:

[0017]

[0018] in is the center of mass momentum matrix, which projects the velocity of each joint to the center of mass, r G,sti and r G,e are the position vectors from the contact foot and the end effector of the robotic arm to the center of mass of the robot dog system, and mg is the gravity acting on the center of mass.

[0019] Further, the optimization includes: Center of mass dynamics model

[0020] The origin of the base coordinate system B is set at the center of mass of the robot dog, the posture definition remains unchanged, and the center of mass dynamics model is transformed into a single rigid body dynamics model:

[0021]

[0022] Where m is the total mass of the robot dog including the base, legs and robotic arms. is the moment of inertia of the robot dog relative to the world coordinate system I when the limbs are in the nominal position, is the substrate angular velocity.

[0023] Further, in step S2,

[0024] The controller of the quadruped robot dog is usually executed by a model predictive control and a whole body controller, wherein the model predictive control includes: using nonlinear model predictive control to optimize the trajectory, realize the planning of the motion trajectory of the quadruped robot dog, the motion trajectory of the limbs and the motion trajectory of the contact force, and design a whole body controller based on hierarchical optimization to track the trajectory generated by the model predictive controller;

[0025] The whole body controller sets different priorities for different control tasks through hierarchical optimization. When conflicts arise between control tasks, it ensures the execution of tasks with high priority.

[0026] Nonlinear model predictive control is formulated as an optimal control problem:

[0027]

[0028] G(x(t),u(t),t)=0

[0029] H(x(t),u(t),t)≥0

[0030] x(0)=x0

[0031] where x(t)∈R n and u(t)∈R mare the state vector and input vector respectively. The cost function is composed of the terminal cost Φ(x(T)) at time T and the process cost L(x(t),u(t),t) from time 0 to time T. The time period T defines the prediction range of the nonlinear model predictive control. The second formula is the system model used in the optimization process. The mapping relationship between the state x(t) and the input u(t) is established to describe and predict the dynamic behavior of the system. The third and fourth formulas represent the equality constraints and inequality constraints respectively, which can restrict the input and state quantities to ensure the safety and effectiveness of the control strategy. The fifth formula is the state quantity initialization, which is used to update the state of the robot dog. The nonlinear predictive control will solve the optimal control problem in each control time interval to find the optimal input vector u(t) that minimizes the cost function and satisfies the system model, equality, inequality constraints and initialization conditions.

[0032] The trajectory tracking tasks of the quadruped robot are defined in the operation space. The maximum row rank of the Jacobian matrix of each trajectory tracking task is 6. The dimension of the generalized variable q of the quadruped robot is much larger than 6, so the quadruped robot is highly redundant. The optimal solution of the optimization variable ξ can be obtained by using hierarchical optimization according to the priority of the task. * , ensuring the strict priority order among the tasks; combined with the first formula, the optimal solution of the hierarchical optimization problem is ξ * Converted into the robot dog driving joint torque:

[0033]

[0034] in The space force trajectory of the robot end effector generated for the whole body planner, and are the control torques of each joint of the leg and each joint of the robotic arm respectively.

[0035] Furthermore, in step S2, the priority of the task includes:

[0036] First priority: physical consistency constraints, contact zero velocity constraints, friction cone constraints, and torque limit constraints;

[0037] Second priority: robot end effector trajectory tracking task, substrate height trajectory tracking task, and substrate posture trajectory tracking task;

[0038] Third priority: swing foot trajectory tracking tasks, plantar contact force tracking tasks, and base horizontal and longitudinal trajectory tracking tasks.

[0039] Further, in step S3, it includes: obtaining the coordinate system O of the object to be grasped in real time obj and the camera coordinate system O cam The homogeneous transformation matrix between:

[0040]

[0041] Where R cam_obj ∈R 3×3 is the rotation matrix, which represents the rotation transformation from the object to be grasped to the camera, p cam_obj =(p x ,p y ,p z ) represents the translation of the object in the x, y, and z directions, and the above information is used as the position and posture information of the object to be grasped.

[0042] Furthermore, in step S4, it includes:

[0043] Convert the transformation matrix between the object and the camera to the transformation between the robot base and the object:

[0044] T base_obj =T base_cam ×T cam_obj

[0045] T ee_obj =T ee_base ×T base_cam ×T cam_obj

[0046] in represents the transformation from the end gripper of the robot arm to the base, T base_cam is the transformation matrix from the quadruped base to the camera coordinate system, T base_obj is the position of the object to be grasped in the robot base coordinate system;

[0047] Set the current position of the gripper at the end of the robot arm as the initial position Target pose The position of the object to be grasped. Considering the relative position of the gripper and the object, it is necessary to set the gripping posture of the gripper relative to the object. Calculate the target position of the gripper:

[0048]

[0049] Generate a pose trajectory, including position trajectory and attitude trajectory:

[0050] Parameterize time and set the total time of the movement to T total , discretize time into N moments;

[0051] The position trajectory uses a quintic polynomial interpolation:

[0052] P cc (t) = a0 + a1t + a2t 2 +a3t3 +a4t 4 +a5t 5

[0053] Boundary conditions, initial position, velocity, acceleration:

[0054] P ee (0) = P start ,

[0055] End position, velocity, acceleration:

[0056] P ee (T total )=P goal ,

[0057] By calculating the boundary conditions, we get a i ;

[0058] The attitude trajectory is calculated using quaternion interpolation:

[0059] Given the initial quaternion q start and the target quaternion q goal , the interpolated quaternion q at time t ee (t) can be expressed by spherical linear interpolation:

[0060]

[0061] in θ=arccos( start ,q goal >), <*,*> represents the inner product of quaternion;

[0062] Calculate the angular velocity ω of the attitude interpolation ee (t) and angular acceleration For subsequent control, the derivative of the above formula is:

[0063]

[0064] because You can get:

[0065]

[0066] The relationship between the quaternion time inverse and the angular velocity is:

[0067]

[0068] in Represents quaternion multiplication, solving for the angular velocity ω ee : ​

[0069]

[0070] Calculating angular acceleration Required angular velocity ω ee (t) Take the time derivative again:

[0071]

[0072] in is the second-order derivative of the quaternion, obtained by differentiating the first-order derivative with time

[0073] The obtained position trajectory and posture trajectory are interpolated in the joint space using the quintic polynomial interpolation method to obtain the joint space trajectory q arm (t);

[0074] The planned end effector joint angle trajectory speed Acceleration As a reference for model predictive control, add the joint space tracking error term to the planner cost function of model predictive control:

[0075]

[0076] where x ref ,u ref are the reference state quantity and reference input quantity, Q q ,Q dq ,Q x is a semi-positive definite matrix, R u It is a positive definite matrix. The four define the weights of the relevant cost terms. After obtaining the reference trajectory and expected force planned by the model predictive control, the whole-body control is used to calculate the actual control torque to achieve grasping.

[0077] In addition, the present invention also provides a quadruped robot dog dynamic grasping system combined with 6D posture estimation, comprising:

[0078] Modeling module: Model and optimize the quadruped robot dog with a mechanical arm to obtain the dynamic model of the quadruped robot dog;

[0079] Controller module: Design a quadruped robot dog controller based on the quadruped robot dog dynamics model;

[0080] Position and posture information module: Use the 6D posture estimation method of the CAD model to obtain the real-time position and posture information of the object to be grasped;

[0081] Grasping module: According to the position and posture information of the object to be grasped, the quadruped robot dog with a mechanical arm is controlled by the quadruped robot dog controller to move and realize dynamic grasping of the object to be grasped.

[0082] Compared with the prior art, the technical solution of the present invention has the following beneficial effects:

[0083] The present invention uses the 6D posture estimation method of the CAD model to obtain the real-time position and posture information of the object to be grasped; according to the position and posture information of the object to be grasped, the quadruped robot dog with a mechanical arm is controlled to move and perform real-time planning through the quadruped robot dog controller to realize dynamic grasping of the object to be grasped. In addition, the grasping range can be customized to further improve the success rate of grasping the object. BRIEF DESCRIPTION OF THE DRAWINGS

[0084] Figure 1 This is a flow chart of the dynamic grasping method of a quadruped robot dog combined with 6D posture estimation described in Example 1;

[0085] Figure 2 This is a block diagram of a quadruped robot dog dynamic grasping system combined with 6D posture estimation as described in Example 2; DETAILED DESCRIPTION

[0086] The drawings are for illustrative purposes only and should not be construed as limiting the present patent;

[0087] The technical solution of the present invention is further described below in conjunction with the accompanying drawings and embodiments.

[0088] Embodiment 1:

[0089] This embodiment provides Figure 1 The quadruped robot dog dynamic grasping method combined with 6D posture estimation shown includes:

[0090] S1: Model and optimize the quadruped robot dog with a robotic arm to obtain the dynamic model of the quadruped robot dog;

[0091] S2: Design a quadruped robot dog controller based on the quadruped robot dog dynamics model;

[0092] S3: Use the 6D posture estimation method of the CAD model to obtain the real-time position and posture information of the object to be grasped;

[0093] S4: According to the position and posture information of the object to be grasped, the quadruped robot dog with a mechanical arm is controlled to move through the quadruped robot dog controller, and dynamic grasping of the object to be grasped is realized.

[0094] The present invention uses the 6D posture estimation method of the CAD model to obtain the real-time position and posture information of the object to be grasped; according to the position and posture information of the object to be grasped, the quadruped robot dog with a mechanical arm is controlled to move and perform real-time planning through the quadruped robot dog controller to realize dynamic grasping of the object to be grasped. In addition, the grasping range can be customized to further improve the success rate of grasping the object.

[0095] Embodiment 2:

[0096] This embodiment further discloses on the basis of the first embodiment:

[0097] Furthermore, in step S1, the quadruped robot dog with a robotic arm includes: a quadruped mobile platform leg joint, a quadruped mobile platform base, a robotic arm joint and a robotic arm end effector.

[0098] Furthermore, in step S1, modeling a quadruped robot dog with a robotic arm includes:

[0099] Establish the floating base dynamics model:

[0100]

[0101] Wherein, all subscripts b, l, a, and e represent the quadruped mobile platform base, the quadruped mobile platform leg joints, the manipulator joints, and the manipulator end effector, respectively. M is the joint space inertia matrix. Describes the nonlinear effects caused by the centripetal force and the Coriolis force, G represents the joint space gravity, It represents the reaction force of the foot when the quadruped mobile platform contacts the ground. represents the spatial force generated when the end effector of the robot interacts with the environment, J st ,J e are the Jacobian matrices of the leg end effector and the robotic arm end effector, respectively, and τ represents the torque of each joint.

[0102] Further, the optimization includes:

[0103] The floating basis dynamics model is projected onto the center of mass of the robot dog system. The dimension of the original robot dog floating basis dynamics equation is increased from 6+n l +n a Reduced to 6 dimensions and freed from the dependence on joint torque τ, the center of mass dynamics model is obtained:

[0104]

[0105] in is the center of mass momentum matrix, which projects the velocity of each joint to the center of mass, r G,sti and r G,eare the position vectors from the contact foot and the end effector of the robotic arm to the center of mass of the robot dog system, and mg is the gravity acting on the center of mass.

[0106] Further, the optimization includes: Center of mass dynamics model

[0107] The origin of the base coordinate system B is set at the center of mass of the robot dog, the posture definition remains unchanged, and the center of mass dynamics model is transformed into a single rigid body dynamics model:

[0108]

[0109] Where m is the total mass of the robot dog including the base, legs and robotic arms. is the moment of inertia of the robot dog relative to the world coordinate system I when the limbs are in the nominal position, is the substrate angular velocity.

[0110] Further, in step S2,

[0111] The controller of the quadruped robot dog is usually executed by a model predictive control and a whole body controller, wherein the model predictive control includes: using nonlinear model predictive control to optimize the trajectory, realize the planning of the motion trajectory of the quadruped robot dog, the motion trajectory of the limbs and the motion trajectory of the contact force, and design a whole body controller based on hierarchical optimization to track the trajectory generated by the model predictive controller;

[0112] The whole body controller sets different priorities for different control tasks through hierarchical optimization. When conflicts arise between control tasks, it ensures the execution of tasks with high priority.

[0113] Nonlinear model predictive control is formulated as an optimal control problem:

[0114]

[0115] G(x(t),u(t),t)=0

[0116] H(x(t),u(t),t)≥0

[0117] x(0)=x0

[0118] where x(t)∈R n and u(t)∈R mare the state vector and input vector respectively. The cost function is composed of the terminal cost Φ(x(T)) at time T and the process cost L(x(t),u(t),t) from time 0 to time T. The time period T defines the prediction range of the nonlinear model predictive control. The second formula is the system model used in the optimization process. The mapping relationship between the state x(t) and the input u(t) is established to describe and predict the dynamic behavior of the system. The third and fourth formulas represent the equality constraints and inequality constraints respectively, which can restrict the input and state quantities to ensure the safety and effectiveness of the control strategy. The fifth formula is the state quantity initialization, which is used to update the state of the robot dog. The nonlinear predictive control will solve the optimal control problem in each control time interval to find the optimal input vector u(t) that minimizes the cost function and satisfies the system model, equality, inequality constraints and initialization conditions.

[0119] The trajectory tracking tasks of the quadruped robot are defined in the operation space. The maximum row rank of the Jacobian matrix of each trajectory tracking task is 6. The dimension of the generalized variable q of the quadruped robot is much larger than 6, so the quadruped robot is highly redundant. The optimal solution of the optimization variable ξ can be obtained by using hierarchical optimization according to the priority of the task. * , ensuring the strict priority order among the tasks; combined with the first formula, the optimal solution of the hierarchical optimization problem is ξ * Converted into the robot dog driving joint torque:

[0120]

[0121] in The space force trajectory of the robot end effector generated for the whole body planner, and are the control torques of each joint of the leg and each joint of the robotic arm respectively.

[0122] Furthermore, in step S2, the priority of the task includes:

[0123] First priority: physical consistency constraints, contact zero velocity constraints, friction cone constraints, and torque limit constraints;

[0124] Second priority: robot end effector trajectory tracking task, substrate height trajectory tracking task, and substrate posture trajectory tracking task;

[0125] Third priority: swing foot trajectory tracking tasks, plantar contact force tracking tasks, and base horizontal and longitudinal trajectory tracking tasks.

[0126] Further, in step S3, it includes: obtaining the coordinate system O of the object to be grasped in real time obj and the camera coordinate system O cam The homogeneous transformation matrix between:

[0127]

[0128] Where R cam_obj ∈R 3×3 is the rotation matrix, which represents the rotation transformation from the object to be grasped to the camera, p cam_obj =(p x ,p y ,p z ) represents the translation of the object in the x, y, and z directions, and the above information is used as the position and posture information of the object to be grasped.

[0129] Furthermore, in step S4, it includes:

[0130] Convert the transformation matrix between the object and the camera to the transformation between the robot base and the object:

[0131] T base_obj =T base_cam ×T cam_obj

[0132] T ee_obj =T ee_base ×T base_cam ×T cam_obj

[0133] in represents the transformation from the end gripper of the robot arm to the base, T base_cam is the transformation matrix from the quadruped base to the camera coordinate system, T base_obj is the position of the object to be grasped in the robot base coordinate system;

[0134] Set the current position of the gripper at the end of the robot arm as the initial position Target pose The position of the object to be grasped. Considering the relative position of the gripper and the object, it is necessary to set the gripping posture of the gripper relative to the object. Calculate the target position of the gripper:

[0135]

[0136] Generate a pose trajectory, including position trajectory and attitude trajectory:

[0137] Parameterize time and set the total time of the movement to T total , discretize time into N moments;

[0138] The position trajectory uses a quintic polynomial interpolation:

[0139] P cc (t) = a0 + a1t + a2t 2 +a3t3 +a4t 4 +a5t 5

[0140] Boundary conditions, initial position, velocity, acceleration:

[0141] P ee (0) = P start ,

[0142] End position, velocity, acceleration:

[0143] P ee (T total )=P goal ,

[0144] By calculating the boundary conditions, we get a i ;

[0145] The attitude trajectory is calculated using quaternion interpolation:

[0146] Given the initial quaternion q start and the target quaternion q goal , the interpolated quaternion q at time t ee (t) can be expressed by spherical linear interpolation:

[0147]

[0148] in θ=arccos( start ,q goal >), <*,*> represents the inner product of quaternion;

[0149] Calculate the angular velocity ω of the attitude interpolation ee (t) and angular acceleration For subsequent control, the derivative of the above formula is:

[0150]

[0151] because You can get:

[0152]

[0153] The relationship between the quaternion time inverse and the angular velocity is:

[0154]

[0155] in Represents quaternion multiplication, solving for the angular velocity ω ee : ​

[0156]

[0157] Calculating angular acceleration Required angular velocity ω ee (t) Take the time derivative again:

[0158]

[0159] in is the second-order derivative of the quaternion, obtained by differentiating the first-order derivative with time

[0160] The obtained position trajectory and posture trajectory are interpolated in the joint space using the quintic polynomial interpolation method to obtain the joint space trajectory q arm (t);

[0161] The planned end effector joint angle trajectory speed Acceleration As a reference for model predictive control, add the joint space tracking error term to the planner cost function of model predictive control:

[0162]

[0163] where x ref ,u ref are the reference state quantity and reference input quantity, Q q ,Q dq ,Q x is a semi-positive definite matrix, R u It is a positive definite matrix. The four define the weights of the relevant cost terms. After obtaining the reference trajectory and expected force planned by the model predictive control, the whole-body control is used to calculate the actual control torque to achieve grasping.

[0164] The present invention uses the 6D posture estimation method of the CAD model to obtain the real-time position and posture information of the object to be grasped; according to the position and posture information of the object to be grasped, the quadruped robot dog with a mechanical arm is controlled to move and perform real-time planning through the quadruped robot dog controller to realize dynamic grasping of the object to be grasped. In addition, the grasping range can be customized to further improve the success rate of grasping the object.

[0165] Embodiment three:

[0166] This embodiment provides Figure 2 A quadruped robot dog dynamic grasping system combined with 6D posture estimation is shown, comprising:

[0167] Modeling module: Model and optimize the quadruped robot dog with a mechanical arm to obtain the dynamic model of the quadruped robot dog;

[0168] Controller module: Design a quadruped robot dog controller based on the quadruped robot dog dynamics model;

[0169] Position and posture information module: Use the 6D posture estimation method of the CAD model to obtain the position and posture information of the object to be grasped;

[0170] Grasping module: According to the position and posture information of the object to be grasped, the quadruped robot dog with a mechanical arm is controlled by the quadruped robot dog controller to move and realize dynamic grasping of the object to be grasped.

[0171] The present invention uses the 6D posture estimation method of the CAD model to obtain the real-time position and posture information of the object to be grasped; according to the position and posture information of the object to be grasped, the quadruped robot dog with a mechanical arm is controlled to move and perform real-time planning through the quadruped robot dog controller to realize dynamic grasping of the object to be grasped. In addition, the grasping range can be customized to further improve the success rate of grasping the object.

[0172] Obviously, the above embodiments of the present invention are merely examples for clearly illustrating the present invention, and are not intended to limit the embodiments of the present invention. For those skilled in the art, other different forms of changes or modifications can be made based on the above description. It is not necessary and impossible to list all the embodiments here. 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 claims of the present invention.

Claims

1. A quadruped robot dog dynamic grasping method combined with 6D posture estimation, characterized in that: include: S1: Model and optimize the quadruped robot dog with a robotic arm to obtain the dynamic model of the quadruped robot dog; S2: Design a quadruped robot dog controller based on the quadruped robot dog dynamics model; S3: Use the 6D posture estimation method of the CAD model to obtain the real-time position and posture information of the object to be grasped; S4: According to the position and posture information of the object to be grasped, the quadruped robot dog with a mechanical arm is controlled to move through the quadruped robot dog controller, and dynamic grasping of the object to be grasped is realized.

2. The method for dynamic grasping of a quadruped robot dog combined with 6D posture estimation according to claim 1, characterized in that: In step S1, a quadruped robot dog with a robotic arm includes: a quadruped mobile platform leg joint, a quadruped mobile platform base, a robotic arm joint and a robotic arm end effector.

3. The method for dynamic grasping of a quadruped robot dog combined with 6D posture estimation according to claim 2, characterized in that: In step S1, modeling a quadruped robot dog with a robotic arm includes: Establish the floating base dynamics model: Wherein, all subscripts b, l, a, and e represent the quadruped mobile platform base, the quadruped mobile platform leg joints, the manipulator joints, and the manipulator end effector, respectively. M is the joint space inertia matrix. Describes the nonlinear effects caused by the centripetal force and the Coriolis force, G represents the joint space gravity, It represents the reaction force of the foot when the quadruped mobile platform contacts the ground. represents the spatial force generated when the end effector of the robot interacts with the environment, J st ,J e are the Jacobian matrices of the leg end effector and the robotic arm end effector, respectively, and τ represents the torque of each joint.

4. The method for dynamic grasping of a quadruped robot dog combined with 6D posture estimation according to claim 3, characterized in that: Optimizations include: The floating basis dynamics model is projected onto the center of mass of the robot dog system. The dimension of the original robot dog floating basis dynamics equation is increased from 6+n l +n a Reduced to 6 dimensions and freed from the dependence on joint torque τ, the center of mass dynamics model is obtained: in is the center of mass momentum matrix, which projects the velocity of each joint to the center of mass, r G,sti and r G,e are the position vectors from the contact foot and the end effector of the robotic arm to the center of mass of the robot dog system, and mg is the gravity acting on the center of mass.

5. The method for dynamic grasping of a quadruped robot dog combined with 6D posture estimation according to claim 1, characterized in that: Optimization includes: Center of mass dynamics model The origin of the base coordinate system B is set at the center of mass of the robot dog, the posture definition remains unchanged, and the center of mass dynamics model is transformed into a single rigid body dynamics model: Where m is the total mass of the robot dog including the base, legs and robotic arms. is the moment of inertia of the robot dog relative to the world coordinate system I when the limbs are in the nominal position, is the substrate angular velocity.

6. The quadruped robot dog dynamic grasping method combined with 6D posture estimation according to claim 1, characterized in that: In step S2, The controller of the quadruped robot dog is usually executed by a model predictive control and a whole body controller, wherein the model predictive control includes: using nonlinear model predictive control to optimize the trajectory, realize the planning of the motion trajectory of the quadruped robot dog, the motion trajectory of the limbs and the motion trajectory of the contact force, and design a whole body controller based on hierarchical optimization to track the trajectory generated by the model predictive controller; The whole body controller sets different priorities for different control tasks through hierarchical optimization. When conflicts arise between control tasks, it ensures the execution of tasks with high priority. Nonlinear model predictive control is formulated as an optimal control problem: G(x(t),u(t),t)=0 H(x(t),u(t),t)≥0 x(0)=x0 where x(t)∈R n and u(t)∈R m are the state vector and input vector respectively. The cost function is composed of the terminal cost Φ(x(T)) at time T and the process cost L(x(t),u(t),t) from time 0 to time T. The time period T defines the prediction range of the nonlinear model predictive control. The second formula is the system model used in the optimization process. The mapping relationship between the state x(t) and the input u(t) is established to describe and predict the dynamic behavior of the system. The third and fourth formulas represent the equality constraints and inequality constraints respectively, which can restrict the input and state quantities to ensure the safety and effectiveness of the control strategy. The fifth formula is the state quantity initialization, which is used to update the state of the robot dog. The nonlinear predictive control will solve the optimal control problem in each control time interval to find the optimal input vector u(t) that minimizes the cost function and satisfies the system model, equality, inequality constraints and initialization conditions. The trajectory tracking tasks of the quadruped robot are defined in the operation space. The maximum row rank of the Jacobian matrix of each trajectory tracking task is 6. The dimension of the generalized variable q of the quadruped robot is much larger than 6, so the quadruped robot is highly redundant. The optimal solution of the optimization variable ξ can be obtained by using hierarchical optimization according to the priority of the task. * , ensuring the strict priority order among the tasks; combined with the first formula, the optimal solution of the hierarchical optimization problem is ξ * Converted into the robot dog driving joint torque: in The space force trajectory of the robot end effector generated for the whole body planner, and are the control torques of each joint of the leg and each joint of the robotic arm respectively.

7. The method for dynamic grasping of a quadruped robot dog combined with 6D posture estimation according to claim 6, characterized in that: In step S2, the priorities of the tasks include: First priority: physical consistency constraints, contact zero velocity constraints, friction cone constraints, and torque limit constraints; Second priority: robot end effector trajectory tracking task, substrate height trajectory tracking task, and substrate posture trajectory tracking task; Third priority: swing foot trajectory tracking tasks, plantar contact force tracking tasks, and base horizontal and longitudinal trajectory tracking tasks.

8. The quadruped robot dog dynamic grasping method combined with 6D posture estimation according to claim 1, characterized in that: In step S3, it includes: obtaining the coordinate system O of the object to be grasped in real time obj and the camera coordinate system O cam The homogeneous transformation matrix between: Where R cam_obj ∈R 3×3 is the rotation matrix, which represents the rotation transformation from the object to be grasped to the camera, p cam_obj =(p x , p y , p z ) represents the translation of the object in the x, y, and z directions, and the above information is used as the position and posture information of the object to be grasped.

9. The quadruped robot dog dynamic grasping method combined with 6D posture estimation according to claim 1, characterized in that: In step S4, it includes: Convert the transformation matrix between the object and the camera to the transformation between the robot base and the object: T base_obj =T base_cam ×T cam_obj T ee_obj =T ee_base ×T base_cam ×T cam_obj in represents the transformation from the end gripper of the robot arm to the base, T base_cam is the transformation matrix from the quadruped base to the camera coordinate system, T base_obj is the position of the object to be grasped in the robot base coordinate system; Set the current position of the gripper at the end of the robot arm as the initial position Target pose The position of the object to be grasped. Considering the relative position of the gripper and the object, it is necessary to set the gripping posture of the gripper relative to the object. Calculate the target position of the gripper: Generate a pose trajectory, including position trajectory and attitude trajectory: Parameterize time and set the total time of the movement to T total , discretize time into N moments; The position trajectory uses a quintic polynomial interpolation: P cc (t)=a0+a1t+a2t 2 +a3t 3 +a4t 4 +a5t 5 Boundary conditions, initial position, velocity, acceleration: End position, velocity, acceleration: By calculating the boundary conditions, we get a l ; The attitude trajectory is calculated using quaternion interpolation: Given the initial quaternion q start and the target quaternion q goal , the interpolated quaternion q at time t ee (t) can be expressed by spherical linear interpolation: in θ=arccos( start ,q goal >), <, *, > represent the inner product of quaternions;​ Calculate the angular velocity ω of the attitude interpolation ee (t) and angular acceleration For subsequent control, the derivative of the above formula is: because You can get: The relationship between the quaternion time inverse and the angular velocity is: in Represents quaternion multiplication, solving for the angular velocity ω ee : Calculating angular acceleration Angular velocity ω is required ee (t) Take the time derivative again: in is the second-order derivative of the quaternion, obtained by differentiating the first-order derivative with time The obtained position trajectory and posture trajectory are interpolated in the joint space using the quintic polynomial interpolation method to obtain the joint space trajectory q arm (t); The planned end effector joint angle trajectory speed Acceleration As a reference for model predictive control, add the joint space tracking error term to the planner cost function of model predictive control: where x ref ,u ref are the reference state quantity and reference input quantity, Q q , Q dq , Q x is a semi-positive definite matrix, R u It is a positive definite matrix. The four define the weights of the relevant cost terms. After obtaining the reference trajectory and expected force planned by the model predictive control, the whole-body control is used to calculate the actual control torque to achieve grasping.

10. A quadruped robot dog dynamic grasping system combined with 6D posture estimation, characterized in that: include: Modeling module: Model and optimize the quadruped robot dog with a mechanical arm to obtain the dynamic model of the quadruped robot dog; Controller module: Design a quadruped robot dog controller based on the quadruped robot dog dynamics model; Position and posture information module: Use the 6D posture estimation method of the CAD model to obtain the real-time position and posture information of the object to be grasped; Grasping module: According to the position and posture information of the object to be grasped, the quadruped robot dog with a mechanical arm is controlled by the quadruped robot dog controller to move and realize dynamic grasping of the object to be grasped.

Images