Quadruped Robot Dog Dynamic Grasping Method and System Combined with 6D Pose Estimation

Abstract

The present invention relates to the field of robotic dogs, and more specifically, to a dynamic grasping method and system for quadruped robotic dogs combined with 6D pose estimation. The method includes: modeling and optimizing a quadruped robotic dog with a robotic arm to obtain a dynamic model of the quadruped robotic dog, and designing a controller for the quadruped robotic dog according to the dynamic model of the quadruped robotic dog; using a 6D pose estimation method for CAD models to obtain real-time position and pose information of the item to be grasped; according to the position and pose information of the item to be grasped, controlling the quadruped robotic dog with a robotic arm to move through the quadruped robotic dog controller, and realizing dynamic grasping of the item to be grasped. This method uses a 6D pose estimation method for CAD models to obtain real-time position and pose information of the item to be grasped; controls the quadruped robotic dog to move and perform real-time planning through the quadruped robotic dog controller, and realizes dynamic grasping of the item to be grasped. And the grasping range can be customized to further improve the success rate of grasping items.

Description

Technical Field

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

[0002] With the progress of society and the development of technology, the application fields of mobile robots have gradually expanded. Currently, robots are being increasingly used in industrial manufacturing, agricultural production, home services, and other aspects. Quadruped robots can move on unstructured ground and have a certain load capacity. Adding a robotic arm to the robot can achieve grasping and handling tasks. As the most practical basic function of robots, grasping technology is crucial. Traditional robot grasping tasks mostly involve fixed-point grasping on an assembly line or using template matching to find the position of an object for grasping. This only applies to grasping static objects and cannot grasp dynamic items.

[0003] The prior art discloses a quadruped robot with a body pitching joint and a grasping mechanism and its grasping method, including a driving mechanism, a body, four robotic legs, and a grasping mechanism. The grasping method is as follows: First, drive the quadruped robot to move towards the target grasping object until the gripper is directly above the target grasping object; then coordinate the leg movement to make the robot move towards the target grasping object until the target grasping object is within the grasping range of the grasping mechanism; drive the spinal joint to make the robot's body bulge upward, and the first grasping component and the second grasping component form a grasping action on the target grasping object. During the grasping process, coordinate the leg movement to 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 its initial motion state, and then drive the robot to continue moving. The grasping mechanism of the present invention is simple, the control method is clear in hierarchy, precise in control, and stable and reliable in operation. However, this method still cannot grasp dynamic items. Summary of the Invention

[0004] The purpose of the present invention is to disclose a dynamic grasping method and system for quadruped robotic dogs combined with 6D pose estimation that can grasp dynamic items.

[0005] To achieve the above purpose, the present invention provides a dynamic grasping method for quadruped robotic dogs combined with 6D pose estimation, including:

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

[0007] S2: Design a controller for the quadruped robotic dog according to the dynamic model of the quadruped robotic dog;

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

[0009] S4: Based on the position and attitude information of the item to be grasped, control the quadruped robot with a manipulator to move through the quadruped robot controller, and achieve dynamic grasping of the item to be grasped.

[0010] Further, in step S1, the quadruped robot with a manipulator includes: the leg joints of the quadruped mobile platform, the base of the quadruped mobile platform, the manipulator joints, and the end effector of the manipulator.

[0011] Further, in step S1, modeling the quadruped robot with a manipulator includes:

[0012] Establish a floating-base dynamics model:

[0013]

[0014] In the formula, all subscripts b, l, a, and e respectively represent the base of the quadruped mobile platform, the leg joints of the quadruped mobile platform, the manipulator joints, and the end effector of the manipulator. M is the joint-space inertia matrix, Describing the nonlinear effects generated by centripetal force and Coriolis force, G represents the joint-space gravity, Represents the ground reaction force on the sole when the quadruped mobile platform contacts the ground, Represents the spatial force generated when the end effector of the manipulator interacts with the environment. J st , J e Are respectively the Jacobian matrices of the leg end effector and the end effector of the manipulator, and τ represents the joint torques.

[0015] Further, the optimization includes:

[0016] Project the floating-base dynamics model under the center of mass of the robot dog system. The dimension of the original floating-base dynamics equation of the robot dog is reduced from 6 + n l + n a To 6 dimensions, and get rid of the dependence on the joint torque τ, obtaining the center-of-mass dynamics model:

[0017]

[0018] Where Is the center-of-mass momentum matrix, which projects the joint velocities under the center of mass. r G,sti And r G,e Are respectively the position vectors from the contact foot and the end effector of the manipulator 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: the center-of-mass dynamics model

[0020] Set the origin of the base coordinate system B at the centroid position of the robotic dog, and keep the attitude definition unchanged. The centroid dynamics model is modeled as a single-rigid-body dynamics model:

[0021]

[0022] where m is the total mass of the robotic dog including the base, legs and manipulator, is the moment of inertia of the robotic dog relative to the world coordinate system I when the four limbs are in the nominal configuration, is the angular velocity of the base.

[0023] Furthermore, in step S2,

[0024] The quadruped robotic dog controller is usually executed by a model predictive control and a whole-body controller together. The model predictive control includes: using nonlinear model predictive control for trajectory optimization to achieve the planning of the motion trajectory of the quadruped robot, the limb motion trajectory and the contact force motion trajectory, and designing 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, and ensures the execution of tasks with high priorities when there are conflicts between control tasks;

[0026] The nonlinear model predictive control is constructed 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 mThey are the state vector and the input vector respectively. The cost function consists of the terminal cost Φ(x(T)) at time T and the process cost L(x(t), u(t), t) from time 0 to T. The time period T defines the prediction horizon of the nonlinear model predictive control. The second equation is the system model used in the optimization process, which establishes the mapping relationship between the state x(t) and the input u(t) and is used to describe and predict the dynamic behavior of the system. The third and fourth equations represent the equality constraint and the inequality constraint respectively, which can limit the input and state variables to ensure the safety and effectiveness of the control strategy. The fifth equation is the initialization of the state variables, which is used to update the state of the robotic dog. The nonlinear predictive control will solve the optimal control problem within 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 manipulator are all defined in the operational space. The maximum row rank of the Jacobian matrix of each trajectory tracking task is 6, and the dimension of the generalized variable q of the quadruped manipulator is much larger than 6. Therefore, the quadruped manipulator is highly redundant. The optimal solution ξ of the optimization variable can be obtained by using hierarchical optimization according to the priority of the tasks. * , which ensures the strict priority order among various tasks; combined with the first equation, the optimal solution ξ of the hierarchical optimization problem * is transformed into the driving joint torque of the robotic dog:

[0033]

[0034] where is the spatial force trajectory of the end effector of the manipulator generated by the whole-body planner, and are the control torques of each joint of the legs and each joint of the manipulator respectively.

[0035] Furthermore, in step S2, the priorities of the tasks include:

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

[0037] Second priority: end effector trajectory tracking task of the manipulator, base height trajectory tracking task, and base attitude trajectory tracking task;

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

[0039] Furthermore, in step S3, it includes: obtaining the homogeneous transformation matrix between the object coordinate system O obj and the camera coordinate system O cam in real time:

[0040]

[0041] where R cam_obj ∈R 3×3 is the rotation matrix, representing the rotational 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. The above information is used as the position and pose 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 into 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] where represents the transformation from the end effector of the robotic 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 pose of the end effector of the robotic arm as the initial pose The target pose is the pose of the object to be grasped. Considering the relative position between the gripper and the object, the grasping pose of the gripper relative to the object needs to be set Calculate the target pose of the gripper:

[0048]

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

[0050] Parameterize the time, set the total time of the motion as T total , and discretize the time into N time instants;

[0051] The position trajectory uses 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 P(0) = P start ,

[0055] End position, velocity, acceleration:

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

[0057] a is calculated through boundary conditions 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 ee (t) at time t can be represented by spherical linear interpolation:

[0060]

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

[0062] Calculate the angular velocity ω ee (t) and angular acceleration for subsequent control. Differentiate the above equation:

[0063]

[0064] Since we can get:

[0065]

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

[0067]

[0068] where represents quaternion multiplication. Solve for the angular velocity ω ee :

[0069]

[0070] Calculate the angular acceleration It is necessary to perform a time derivative on the angular velocity ω ee (t) again:

[0071]

[0072] where is the second derivative of the quaternion, obtained by performing a time derivative on the first derivative

[0073] Interpolate in the joint space using the quintic polynomial interpolation method through the obtained position trajectory and attitude trajectory to obtain the joint space trajectory q arm (t);

[0074] The joint angle trajectory of the end effector obtained by planning velocity acceleration As a reference for model predictive control, in the planner cost function of model predictive control, add a joint space tracking error term:

[0075]

[0076] where x ref , u ref are the reference state quantity and the reference input quantity respectively, Q q , Q dq , Q x is a positive semi-definite matrix, R u is a positive definite matrix, and the four define the weights of the relevant cost terms. After obtaining the reference trajectory and the desired force of the model predictive control plan, use whole-body control 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 pose estimation, including:

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

[0079] Controller module: Design a controller for the quadruped robot dog according to the dynamic model of the quadruped robot dog;

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

[0081] Grasping module: According to the position and attitude information of the item to be grasped, the quadruped robot dog with a robotic arm is controlled by the quadruped robot dog controller to move, and dynamic grasping of the item to be grasped is achieved.

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

[0083] The present invention uses a 6D pose estimation method for CAD models to obtain the real-time position and attitude information of the item to be grasped; according to the position and attitude information of the item to be grasped, the quadruped robot dog with a robotic arm is controlled by the quadruped robot dog controller to move and perform real-time planning, so as to achieve dynamic grasping of the item to be grasped. And the grasping range can be customized to further improve the success rate of grasping items. Description of the Drawings

[0084] Figure 1 It is a flowchart of the quadruped robot dog dynamic grasping method combining 6D pose estimation described in Embodiment 1;

[0085] Figure 2 It is a block diagram of the quadruped robot dog dynamic grasping system combining 6D pose estimation described in Embodiment 2; Detailed Embodiments

[0086] The drawings are only for illustrative purposes and should not be construed as a limitation of this patent;

[0087] The technical solution of the present invention will be further described below with reference to the drawings and embodiments.

[0088] Embodiment 1:

[0089] This embodiment provides a Figure 1 quadruped robot dog dynamic grasping method combining 6D pose estimation as shown in

[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 according to the dynamic model of the quadruped robot dog;

[0092] S3: Use a 6D pose estimation method for CAD models to obtain the real-time position and attitude information of the item to be grasped;

[0093] S4: According to the position and attitude information of the item to be grasped, the quadruped robot dog with a robotic arm is controlled by the quadruped robot dog controller to move, and dynamic grasping of the item to be grasped is achieved.

[0094] The 6D pose estimation method of the present invention using a CAD model is used to obtain the real-time position and pose information of the item to be grasped; according to the position and pose information of the item to be grasped, a quadruped robot dog with a manipulator is controlled by a quadruped robot dog controller to move and perform real-time planning, so as to realize the dynamic grasping of the item to be grasped. And the grasping range can be customized to further improve the success rate of grasping the item.

[0095] Embodiment 2:

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

[0097] Further, in step S1, the quadruped robot dog with a manipulator includes: the leg joints of the quadruped mobile platform, the base of the quadruped mobile platform, the manipulator joints, and the end effector of the manipulator.

[0098] Further, in step S1, the modeling of the quadruped robot dog with a manipulator includes:

[0099] Establish a floating-base dynamics model:

[0100]

[0101] In the formula, all subscripts b, l, a, and e respectively represent the base of the quadruped mobile platform, the leg joints of the quadruped mobile platform, the manipulator joints, and the end effector of the manipulator. M is the joint space inertia matrix, describes the non-linear effects generated by the centripetal force and the Coriolis force, G represents the joint space gravity, represents the ground reaction force on the sole when the quadruped mobile platform contacts the ground, represents the spatial force generated when the end effector of the manipulator interacts with the environment, J st , J e are the Jacobian matrices of the leg end effector and the end effector of the manipulator respectively, and τ represents the joint torques.

[0102] Further, the optimization includes:

[0103] Project the floating-base dynamics model under the center of mass of the robot dog system. The dimension of the original robot dog floating-base dynamics equation is reduced from 6 + n l + n a to 6 dimensions, and the dependence on the joint torque τ is eliminated, obtaining the center-of-mass dynamics model:

[0104]

[0105] Among them is the center-of-mass momentum matrix, which projects the joint velocities to under the center of mass, r G,sti and r G,eare the position vectors from the contact feet and the end effector of the robotic arm to the centroid of the quadruped robot system, and \(mg\) is the gravitational force acting on the centroid.

[0106] Further, the optimization includes: the centroid dynamics model

[0107] Set the origin of the base coordinate system \(B\) at the centroid position of the quadruped robot, and keep the attitude definition unchanged. The centroid dynamics model is transformed into a single rigid body dynamics model:

[0108]

[0109] where \(m\) is the total mass of the quadruped robot including the base, legs and robotic arm, is the moment of inertia of the quadruped robot with respect to the world coordinate system \(I\) when the four limbs are in the nominal configuration, is the angular velocity of the base.

[0110] Further, in step S2,

[0111] The quadruped robot controller is usually executed by the model predictive control and the whole body controller together. The model predictive control includes: using the nonlinear model predictive control for trajectory optimization to achieve the planning of the motion trajectory of the quadruped robot, the motion trajectories of the four limbs and the contact force motion trajectory, and designing 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, and ensures the execution of tasks with high priorities when there are conflicts between control tasks;

[0113] The nonlinear model predictive control is constructed as an optimal control problem:

[0114]

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

[0116] \(H(x(t),u(t),t)\geq0\)

[0117] \(x(0)=x_0\)

[0118] where \(x(t)\in R\) n and \(u(t)\in R\) mThey are the state vector and the input vector respectively. The cost function consists 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 horizon of the nonlinear model predictive control. The second equation is the system model used in the optimization process, which establishes the mapping relationship between the state x(t) and the input u(t) and is used to describe and predict the dynamic behavior of the system. The third and fourth equations represent the equality constraint and the inequality constraint respectively, which can limit the input and state variables to ensure the safety and effectiveness of the control strategy. The fifth equation is the initialization of the state variable, which is used to update the state of the robotic dog. The nonlinear predictive control will solve the optimal control problem within 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 manipulator are all 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 manipulator is much larger than 6. Therefore, the quadruped manipulator is highly redundant. The optimal solution ξ of the optimization variable can be obtained by using hierarchical optimization according to the priority of the tasks. * , which ensures the strict priority order among various tasks. Combining with the first equation, the optimal solution ξ of the hierarchical optimization problem * is transformed into the driving joint torque of the robotic dog:

[0120]

[0121] where is the spatial force trajectory of the end effector of the manipulator generated by the whole body planner, and are the control torques of each joint of the leg and each joint of the manipulator respectively.

[0122] Furthermore, in step S2, the priorities of the tasks include:

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

[0124] Second priority: end effector trajectory tracking task of the manipulator, base height trajectory tracking task, and base attitude trajectory tracking task;

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

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

[0127]

[0128] where R cam_obj ∈R 3×3 is the rotation matrix, representing the rotational transformation from the object to be grasped to the camera, and p cam_obj =(p x , p y , p z ) represents the translation of the object in the x, y, and z directions. The above information is used as the position and pose 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 into 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] where represents the transformation from the end effector of the robotic arm to the base, T base_cam is the transformation matrix from the quadruped base to the camera coordinate system, and T base_obj is the position of the object to be grasped in the robot base coordinate system;

[0134] Set the current pose of the end effector of the robotic arm as the initial pose The target pose is the pose of the object to be grasped. Considering the relative position between the gripper and the object, the grasping pose of the gripper relative to the object needs to be set Calculate the target pose of the gripper:

[0135]

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

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

[0138] The position trajectory uses quintic polynomial interpolation:

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

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

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

[0142] End position, velocity, and acceleration:

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

[0144] a is calculated through the boundary conditions 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 ee (t) at time t can be represented by spherical linear interpolation:

[0147]

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

[0149] Calculate the angular velocity ω ee (t) and angular acceleration for subsequent control, and take the derivative of the above formula:

[0150]

[0151] Since we can get:

[0152]

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

[0154]

[0155] where represents quaternion multiplication, and solve for the angular velocity ω ee :

[0156]

[0157] Calculate the angular acceleration It is necessary to perform a time derivative on the angular velocity ω ee (t) again:

[0158]

[0159] where is the second derivative of the quaternion, obtained by performing a time derivative on the first derivative

[0160] Interpolate in the joint space using the obtained position trajectory and attitude trajectory with the fifth-order polynomial interpolation method to obtain the joint space trajectory q arm (t);

[0161] The joint angle trajectory of the end effector obtained by planning velocity acceleration As the reference of the model predictive control, in the planner cost function of the model predictive control, add the joint space tracking error term:

[0162]

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

[0164] The present invention uses a 6D pose estimation method for the CAD model to obtain the real-time position and pose information of the item to be grasped; according to the position and pose information of the item to be grasped, control the quadruped robot with a manipulator to move and perform real-time planning through the quadruped robot controller to achieve dynamic grasping of the item to be grasped. And the grasping range can be customized to further improve the success rate of grasping the item.

[0165] Embodiment 3:

[0166] This embodiment provides a quadruped robot dynamic grasping system combined with 6D pose estimation as shown in Figure 2 and includes:

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

[0168] Controller module: Design a controller for the quadruped robot dog according to the quadruped robot dog dynamics model.

[0169] Position and attitude information module: Use the 6D pose estimation method of the CAD model to obtain the position and attitude information of the item to be grasped.

[0170] Grasping module: According to the position and attitude information of the item to be grasped, control the quadruped robot dog with a robotic arm to move through the quadruped robot dog controller, and realize the dynamic grasping of the item to be grasped.

[0171] The present invention uses the 6D pose estimation method of the CAD model to obtain the real-time position and attitude information of the item to be grasped; according to the position and attitude information of the item to be grasped, control the quadruped robot dog with a robotic arm to move and perform real-time planning through the quadruped robot dog controller, so as to realize the dynamic grasping of the item to be grasped. And the grasping range can be customized to further improve the success rate of grasping items.

[0172] Obviously, the above embodiments of the present invention are merely examples for clearly illustrating the present invention, rather than limiting the implementation manners of the present invention. For those of ordinary skill 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 enumerate all the implementation manners here. Any modifications, equivalent replacements, and improvements made within the spirit and principle of the present invention shall be included in the protection scope of the claims of the present invention.

Claims

1. A dynamic grasping method for a quadruped robot dog combined with 6D pose estimation, characterized in that, Including: S1: Model and optimize a quadruped robot dog with a robotic arm to obtain the dynamic model of the quadruped robot dog; S2: Design a controller for the quadruped robot dog according to the dynamic model of the quadruped robot dog; Specifically: The controller of the quadruped robot dog is usually executed by the model predictive control and the whole-body controller together. Among them, the model predictive control includes: Using nonlinear model predictive control for trajectory optimization to realize the planning of the motion trajectory of the quadruped robot dog, the limb motion trajectory and the contact force motion trajectory, and designing 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, and ensures the execution of tasks with high priorities when conflicts occur between control tasks; The nonlinear model predictive control is constructed as an optimal control problem: s.t. G(x(t), u(t), t) = 0 H(x(t), u(t), t) ≥ 0 x(0) = x0 where \(x(t)\in\mathbb{R}\) n and \(u(t)\in\mathbb{R}\) m are the state vector and the input vector respectively. The cost function consists of the terminal cost \(\varPhi(x(T))\) at time \(T\) and the process cost \(L(x(t),u(t),t)\) from time \(0\) to \(T\). The time period \(T\) defines the prediction horizon of the nonlinear model predictive control. The second equation is the system model used in the optimization process, which establishes the mapping relationship between the state \(x(t)\) and the input \(u(t)\) and is used to describe and predict the dynamic behavior of the system. The third and fourth equations represent the equality constraint and the inequality constraint respectively, which can limit the input and state variables to ensure the safety and effectiveness of the control strategy. The fifth equation is the initialization of the state variables, which is used to update the state of the robotic dog. The nonlinear predictive control will solve the optimal control problem within 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 manipulator are all defined in the operational space. The maximum row rank of the Jacobian matrix for each trajectory tracking task is 6, and the dimension of the generalized variable q of the quadruped manipulator is much larger than 6. Therefore, the quadruped manipulator is highly redundant. By using hierarchical optimization according to the task priorities, the optimal solution ξ of the optimization variable can be obtained. * , which ensures the strict priority order among various tasks. Combining with the first equation, the optimal solution ξ of the hierarchical optimization problem * is transformed into the driving joint torques of the quadruped robot: Among them is the spatial force trajectory of the end effector of the robotic arm generated by the whole-body planner, and are the control torques of each joint of the legs and each joint of the robotic arm respectively; S3: Use the 6D pose estimation method of the CAD model to obtain the real-time position and pose information of the item to be grasped; S4: According to the position and pose information of the item to be grasped, control the quadruped robot dog with a robotic arm to move through the quadruped robot dog controller, and realize the dynamic grasping of the item to be grasped.

2. The dynamic grasping method of the quadruped robot dog combined with 6D pose estimation according to claim 1, wherein In step S1, the quadruped robot dog with a robotic arm includes: the leg joints of the quadruped mobile platform, the base of the quadruped mobile platform, the robotic arm joints and the end effector of the robotic arm.

3. The dynamic grasping method of the quadruped robot dog combined with 6D pose estimation according to claim 2, characterized in that, In step S1, modeling the quadruped robot dog with a robotic arm includes: Establishing a floating-base dynamic model: All subscripts b, l, a, and e in the formula represent the base of the quadruped mobile platform, the leg joints of the quadruped mobile platform, the robotic arm joints, and the end effector of the robotic arm, respectively. M is the joint space inertia matrix. Describes the nonlinear effects generated by the centripetal force and the Coriolis force. G represents the joint space gravity. Represents the ground reaction force when the quadruped mobile platform contacts the ground. Represents the spatial force generated when the end effector of the robotic arm interacts with the environment. J st , J e Are the Jacobian matrices of the end effector of the leg and the end effector of the robotic arm, respectively. τ represents the joint torques.

4. The dynamic grasping method of the quadruped robot dog combined with 6D pose estimation according to claim 3, characterized in that The optimization includes: Project the floating-base dynamics model under the center of mass of the quadruped robot system. The dimension of the original floating-base dynamics equation of the quadruped robot is reduced from 6 + n l + n a to 6 dimensions, and the dependence on the joint torque τ is eliminated, obtaining the center-of-mass dynamics model: Among them is the center-of-mass momentum matrix, which projects each joint velocity onto 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 quadruped robot system, respectively, and mg is the gravity acting on the center of mass.

5. The dynamic grasping method of the quadruped robot dog combined with 6D pose estimation according to claim 1, characterized in that The optimization includes: Setting the origin of the base coordinate system B at the centroid position of the robot dog, keeping the attitude definition unchanged, and modeling the centroid dynamics as a single-rigid-body dynamics model: where m is the total mass of the quadruped robot including the base body, legs and robotic arm, is the moment of inertia of the quadruped robot with respect to the world coordinate system I when the four limbs are in the nominal configuration, is the angular velocity of the base body.

6. The dynamic grasping method of the quadruped robot dog combined with 6D pose estimation according to claim 1, characterized in that, In step S2, the priorities of the tasks include: The first priority: physical consistency constraints, contact zero-velocity constraints, friction cone constraints and torque limit constraints; The second priority: the trajectory tracking task of the end effector of the robotic arm, the trajectory tracking task of the base height, and the trajectory tracking task of the base attitude; The third priority: the trajectory tracking task of the swinging foot, the trajectory tracking task of the sole contact force, and the trajectory tracking task of the base horizontal and longitudinal directions.

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

8. The dynamic grasping method of the quadruped robot dog combined with 6D pose estimation according to claim 1, characterized in that, In step S4, it includes: Converting the transformation matrix between the object and the camera into 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 Among them represents the transformation from the gripper at the end of the robotic 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 pose of the gripper at the end of the robotic arm as the initial pose Target pose It is the pose of the object to be grasped. Considering the relative position between the gripper and the object, it is necessary to set the grasping pose of the gripper relative to the object Calculate the target pose of the gripper: Generating a pose trajectory, including a position trajectory and a pose trajectory: Parameterize the time and set the total time of the motion to T total , and discretize the time into N moments; The position trajectory uses quintic polynomial interpolation: P cc P(t) = a0 + a1t + a2t 2 + a3t 3 + a4t 4 + a5t 5 Boundary conditions, initial position, velocity, acceleration: End position, velocity, acceleration: a is calculated through boundary conditions i ; The pose trajectory is calculated using quaternion interpolation: Given an initial quaternion q start and a target quaternion q goal , the interpolated quaternion q ee (t) at time t can be represented by spherical linear interpolation as follows: where θ = arccos(<q start , q goal >), <*, *> represents the inner product of quaternions; Calculate the angular velocity ω for attitude interpolation ee (t) and angular acceleration For subsequent control, take the derivative of the above equation: Since it can be obtained that: The relationship between the reciprocal of the quaternion time and the angular velocity is: where represents quaternion multiplication and solves for the angular velocity ω ee : Calculate the angular acceleration It is necessary to perform a time derivative on the angular velocity ω ee (t) again: where is the second derivative of the quaternion, obtained by taking the time derivative of the first derivative Interpolate in the joint space using the obtained position trajectory and attitude trajectory with the fifth-order polynomial interpolation method to obtain the joint space trajectory q arm (t); The joint angle trajectory of the end effector obtained by planning Velocity Acceleration As a reference for model predictive control, in the cost function of the model predictive control planner, add a joint space tracking error term: where x ref , u ref are the reference state quantity and the reference input quantity respectively, Q q , Q dq , Q x are positive semi-definite matrices, Q u is a positive definite matrix. The four define the weights of the relevant cost terms. After obtaining the reference trajectory and the desired force of the model predictive control plan, the actual control torque is calculated using whole-body control to achieve grasping.

9. A four-legged robot dog dynamic grasping system combined with 6D pose estimation, characterized in that, Including: Modeling module: Model and optimize a quadruped robot dog with a robotic arm to obtain the dynamic model of the quadruped robot dog; Controller module: Design a controller for the quadruped robot dog according to the dynamic model of the quadruped robot dog; The quadruped robot dog controller is usually executed by the model predictive control and the whole body controller together. The model predictive control includes: using the nonlinear model predictive control for trajectory optimization to achieve the planning of the movement trajectory of the quadruped robot dog, the limb movement trajectory, and the contact force movement trajectory, and designing 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, and ensures the execution of tasks with high priorities when there are contradictions between control tasks; The nonlinear model predictive control is constructed 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)\in\mathbb{R}\) n and \(u(t)\in\mathbb{R}\) m are the state vector and input vector respectively. The cost function consists of the terminal cost \(\varPhi(x(T))\) at time \(T\) and the process cost \(L(x(t),u(t),t)\) from time \(0\) to \(T\). The time interval \(T\) defines the prediction horizon of the nonlinear model predictive control. The second equation is the system model used in the optimization process, which establishes the mapping relationship between the state \(x(t)\) and the input \(u(t)\) and is used to describe and predict the dynamic behavior of the system. The third and fourth equations represent the equality constraint and inequality constraint respectively, which can limit the input and state variables to ensure the safety and effectiveness of the control strategy. The fifth equation is the initialization of the state variables, which is used to update the state of the robotic 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 manipulator are all defined in the operational space. The maximum row rank of the Jacobian matrix for each trajectory tracking task is 6, and the dimension of the generalized variable q of the quadruped manipulator is much larger than 6. Therefore, the quadruped manipulator is highly redundant. By using hierarchical optimization according to the priority of the tasks, the optimal solution ξ of the optimization variable can be obtained. * , which ensures the strict priority order among various tasks. Combining the first equation, the optimal solution ξ of the hierarchical optimization problem * is transformed into the driving joint torque of the quadruped robot: wherein is the spatial force trajectory of the end effector of the robotic arm generated by the full-body planner, and are the control torques of the joints of the legs and the joints of the robotic arm, respectively; Position and attitude information module: Using the 6D pose estimation method of the CAD model to obtain the real-time position and attitude information of the item to be grasped; Grasping module: According to the position and attitude information of the item to be grasped, control the quadruped robot dog with a robotic arm to move through the quadruped robot dog controller, and achieve the dynamic grasping of the item to be grasped.

Images