A Method and System for Centroid Estimation and Compensation of a Variable Configuration Quadruped Robot with Arms

Abstract

The present disclosure provides a centroid estimation and compensation method and system for a variable configuration quadruped robot with arms, relating to the technical field of quadruped robot motion planning, including: obtaining state feedback information of the variable configuration quadruped robot during motion; based on the state feedback information, calculating the foot-end contact force of the supporting legs through the whole-body dynamics model of the variable configuration quadruped robot and a disturbance force observer based on generalized momentum; according to the foot-end contact force of the supporting legs, using a centroid estimation algorithm with an online recursive strategy to estimate the centroid offset value after being affected by unknown disturbances; according to the estimated centroid offset value, adjusting the motion trajectory planner and the centroid dynamics model, and based on the adjusted results, compensating for the centroid deviation through a whole-body controller in the null space to obtain the optimal joint torque for real-time servo control; the present invention realizes the stable control of the quadruped robot under internal or external force disturbances such as double-arm movement, leg configuration change, and unknown load.

Description

Technical Field

[0001] The present disclosure relates to the technical field of quadruped robot motion planning, and particularly to a method and system for centroid estimation and compensation of a variable configuration quadruped robot with arms. Background Art

[0002] Quadruped robots among legged robots have attracted the attention of many scientific researchers and research institutions due to their advantages such as simple structure, strong stability, and strong adaptability to rough terrain. In particular, variable configuration quadruped robots with two arms are favored because they have multiple configurations such as mammalian configuration, reptilian configuration, and two-arm mobile operation configuration, showing strong terrain adaptability, the ability to cross irregular obstacles, and two-arm mobile operation ability.

[0003] Model-based control methods rely on accurate dynamic models. However, the dynamic inertial parameters of the robot are affected by internal force factors such as the installation position of the manipulator, the moving position of the two arms, the configuration of the robot's legs, or the reading error of the dynamic parameters of the CAD software. At the same time, the robot is also affected by external factors such as suddenly applied unknown external loads or the two arms clamping heavy objects. These internal or external force factors will cause the centroid of the robot to deviate from the geometric center of the robot, thereby increasing the controller attitude angle tracking error and causing the robot's torso to tilt or even fall unstable. Therefore, it is necessary to accurately estimate the centroid deviation and correct the dynamic model parameters.

[0004] However, most of the existing centroid estimation and compensation methods require the robot to remain stationary during the centroid estimation process or can only identify the centroid during the static gait movement process. There are few methods for centroid estimation during the dynamic gait movement process. At the same time, for quadruped robots with manipulators, when grasping unknown targets, most current methods assume that the mass of the grasped target is known to establish a dynamic model. However, in the actual operation process, the mass of the grasped target cannot be obtained in real time, ignoring the centroid deviation caused by unknown loads. These methods will reduce the operation accuracy or motion efficiency of the robot. For quadruped robots with arms, the centroid deviation cannot be estimated in real time during the dynamic movement process, ultimately affecting the stability of the robot system. Summary of the Invention

[0005] In order to solve the above problems, the present disclosure proposes a method and system for centroid estimation and compensation of a variable configuration quadruped robot with arms, realizing the stable control of the dynamic movement of the quadruped robot under internal or external force disturbances such as two-arm movement, leg configuration change, and unknown load.

[0006] According to some embodiments, the present disclosure adopts the following technical solutions:

[0007] A method for centroid estimation and compensation of a variable configuration quadruped robot with arms, comprising:

[0008] Obtain the state feedback information of the variable configuration quadruped robot during movement;

[0009] Based on the state feedback information, calculate the foot-end contact force of the supporting leg through the whole-body dynamics model of the variable configuration quadruped robot and the disturbance force observer based on generalized momentum;

[0010] According to the foot-end contact force of the supporting leg, use the centroid estimation algorithm with an online recursive strategy to estimate the centroid offset value after being affected by unknown disturbances;

[0011] According to the estimated centroid offset value, adjust the motion trajectory planner and the centroid dynamics model. Based on the adjusted results, compensate for the centroid deviation through the whole-body controller in the null space to obtain the optimal joint torque for real-time servo control;

[0012] Among them, in the centroid estimation algorithm with an online recursive strategy, the difference in the foot-end contact forces of the front-back and left-right supporting legs is used to estimate the direction of centroid offset in the horizontal plane. A sliding window is introduced in the direction of centroid offset, and the offset is gradually identified by the way of stacking and sliding one window by one, and finally the offset estimation value of the centroid in the horizontal plane is obtained.

[0013] According to some embodiments, the present disclosure adopts the following technical solutions:

[0014] A centroid estimation and compensation system for a variable configuration quadruped robot with an arm, comprising:

[0015] A state acquisition module, configured to: obtain the state feedback information of the variable configuration quadruped robot during movement;

[0016] A contact force calculation module, configured to: based on the state feedback information, calculate the foot-end contact force of the supporting leg through the whole-body dynamics model of the variable configuration quadruped robot and the disturbance force observer based on generalized momentum;

[0017] An offset estimation module, configured to: according to the foot-end contact force of the supporting leg, use the centroid estimation algorithm with an online recursive strategy to estimate the centroid offset value after being affected by unknown disturbances;

[0018] An offset compensation module, configured to: according to the estimated centroid offset value, adjust the motion trajectory planner and the centroid dynamics model. Based on the adjusted results, compensate for the centroid deviation through the whole-body controller in the null space to obtain the optimal joint torque for real-time servo control;

[0019] Among them, in the centroid estimation algorithm of the online recursive strategy, the difference in the foot-end contact forces of the front and rear, left and right support legs is used to estimate the direction of centroid offset in the horizontal plane. A sliding window is introduced in the direction of centroid offset, and the offset is gradually identified by the way of stacking and sliding each window one by one, and finally the offset estimation value of the centroid in the horizontal plane is obtained.

[0020] According to some embodiments, the present disclosure adopts the following technical solutions:

[0021] A computer program product includes a computer program, and when the computer program is executed by a processor, it implements the centroid estimation and compensation method for a variable configuration quadruped robot with an arm.

[0022] According to some embodiments, the present disclosure adopts the following technical solutions:

[0023] A non-transitory computer-readable storage medium is used to store computer instructions, and when the computer instructions are executed by a processor, it implements the centroid estimation and compensation method for a variable configuration quadruped robot with an arm.

[0024] According to some embodiments, the present disclosure adopts the following technical solutions:

[0025] An electronic device includes: a processor, a memory, and a computer program; wherein, the processor is connected to the memory, the computer program is stored in the memory, and when the electronic device runs, the processor executes the computer program stored in the memory so that the electronic device executes and implements the centroid estimation and compensation method for a variable configuration quadruped robot with an arm.

[0026] Compared with the prior art, the beneficial effects of the present disclosure are as follows:

[0027] (1) By proposing a disturbance force observer based on generalized momentum, the present invention calculates the generalized momentum of the robot according to the state estimation information such as joint torque and joint speed during the movement of the robot, and calculates the accurate foot-end contact force of the robot by using the whole-body dynamics model and the disturbance force observer, realizing the accurate feedback of the foot-end interaction force and contact state of the robot.

[0028] (2) By proposing a centroid estimation algorithm based on an online recursive strategy, the present invention combines the estimated accurate foot-end contact force, uses the contact force difference between the front and rear, left and right support legs to estimate the direction of centroid offset in the horizontal plane, and adopts a recursive iteration strategy to estimate the centroid offset in the horizontal plane, realizing the accurate centroid deviation estimation of a variable configuration quadruped robot with a manipulator under various disturbance factors.

[0029] (3) By proposing a centroid compensator based on whole-body control, the present invention compensates for the centroid offset of the robot caused by internal force factors such as modeling errors, dual-arm movements, or leg configurations, and external force factors such as unknown loads, according to the estimated centroid offset of the robot system, in combination with a whole-body controller with task priorities and a motion trajectory planner, so as to achieve stable motion control and mobile operation of a quadruped robot with a manipulator. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] The accompanying drawings forming a part of this disclosure are used to provide a further understanding of the disclosure. The illustrative embodiments and descriptions thereof of the disclosure are used to explain the disclosure and do not constitute an improper limitation of the disclosure.

[0031] Figure 1 is a structural diagram of a variable-configuration quadruped dual-arm robot in Embodiment 1;

[0032] Figure 2 is a motion control block diagram of the variable-configuration quadruped dual-arm robot in Embodiment 1;

[0033] Figure 3 is an analysis schematic diagram of the tipping torque and foot-end sole force of the variable-configuration quadruped dual-arm robot in Embodiment 1. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0034] The present disclosure will be further described below in conjunction with the accompanying drawings and embodiments.

[0035] It should be noted that the following detailed description is exemplary and is intended to provide further explanation of the present disclosure. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by those of ordinary skill in the technical field to which the present disclosure belongs.

[0036] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present disclosure. As used herein, unless the context clearly indicates otherwise, the singular forms are also intended to include the plural forms. In addition, it should be understood that when the terms "include" and / or "comprise" are used in this specification, they indicate the presence of features, steps, operations, devices, components, and / or combinations thereof.

[0037] Embodiment 1

[0038] In one embodiment of the present disclosure, a centroid estimation and compensation method for a variable-configuration quadruped robot with an arm is provided to achieve stable dynamic motion control of the quadruped robot under internal or external force disturbances such as dual-arm movement, leg configuration change, and unknown load. The following takes a quadruped robot with a variable-configuration dual-arm as shown in Figure 1 as an example to detail the specific implementation solution.

[0039] Figure 1 In it, the regulations for the directions of the world coordinate system {W}, the torso coordinate system {B}, and the hip joint coordinate system {H} are the same as those in the figure, and r i represents the position of the end of the i-th supporting leg relative to the torso coordinate system, and r com represents the offset value of the center of mass of the robot relative to the torso coordinate system, and l b and w b and h b respectively represent the length, width, and height of the robot's torso.

[0040] To solve the center of mass offset CoM of the quadruped robot shown in Figure 1 during the movement process due to the influence of the installation position, the movement of the manipulator, or unknown loads, it is necessary to design an estimation and compensation scheme for the center of mass offset to achieve the stable posture and accurate end following of the robot during the movement process. For this purpose, this embodiment proposes a method for estimating and compensating the center of mass offset of a quadruped robot with a manipulator, and the specific process is as shown in Figure 2 Based on the disturbance force observer of the generalized momentum, accurately estimate the foot-end contact force during the movement process; use the center of mass estimation algorithm of the online recursive strategy to accurately estimate the center of mass offset of the variable configuration quadruped robot with two arms; use the center of mass compensator based on the whole-body control to achieve the compensation and stable control of the center of mass offset of the variable configuration quadruped robot with two arms.

[0041] Step S1: Obtain the state feedback information of the variable configuration quadruped robot during the movement process.

[0042] The state feedback information here includes the torso position and speed information of the variable configuration quadruped robot, and the acquisition method is as follows:

[0043] First, collect the joint torques and joint angles through the proprioceptors of the robot.

[0044] Then, based on the joint angles, use the forward kinematics to calculate the torso position p com and speed information v .

[0045] Step S2: Based on the state feedback information, calculate the foot-end contact force of the supporting leg through the whole-body dynamics model of the variable configuration quadruped robot and the disturbance force observer based on the generalized momentum, specifically as follows:

[0046] (1) According to the whole-body dynamics model and the state feedback information, calculate the disturbance torque received by the robot from the ground during the movement process.

[0047] Specifically, according to the robot state feedback information, a full-body dynamics model of a variable-configuration quadruped robot with a robotic arm is established:

[0048]

[0049] Among them, is the inertia matrix, is the Coriolis and centrifugal forces, is the gravity term, represents the robot joint torque, is the corresponding leg joint motor torque, is the corresponding robotic arm joint motor torque, is the torque corresponding to the torso. q represents the robot torso pose, leg joint angles, and robotic arm joint angles. and are the corresponding angular velocity and angular acceleration respectively. represents the Jacobian matrices of the corresponding torso, legs, and robotic arm. f represents the contact force between the foot end and the ground.

[0050] According to the robot full-body dynamics model and state feedback information, when the robot is in motion and subjected to a disturbance torque from the ground, it is calculated as:

[0051]

[0052] Among them, is the selection matrix of the floating base, and are the angular velocity and angular acceleration of the corresponding torso pose and leg-arm joints respectively.

[0053] (2) Use a disturbance force observer to discretize the disturbance torque.

[0054] Discretize the disturbance torque using the formula:

[0055]

[0056] Among them, z is the z-domain variable, γ is the filter related to the cut-off frequency of the filter, and .

[0057] The sampling time of the data is 500 Hz. Assuming that the decomposition of the Coriolis and centrifugal forces satisfies , the above decomposition can be obtained as:

[0058]

[0059] Among them, represents the generalized momentum, and substituting it into the disturbance force observer gives the discretized disturbance torque:

[0060]

[0061] Among them, represents the gain related to the cut-off frequency, is the sampling time of the data.

[0062] (3) Based on the discretized disturbance torque, through the relationship between the Jacobian matrix of the robot leg and the joint torque, the foot-end contact force of the supporting leg is calculated.

[0063] According to the calculated disturbance torque , through the Jacobian matrix of the robot leg and the joint torque relationship, the accurate foot-end contact force is:

[0064]

[0065] Step S3: Based on the foot-end contact force of the supporting leg, use the center-of-mass estimation algorithm with an online recursive strategy to estimate the center-of-mass offset value after being subjected to an unknown disturbance.

[0066] Among them, the center-of-mass estimation algorithm with the online recursive strategy uses the difference in the foot-end contact forces of the front and rear, left and right supporting legs to estimate the direction of the center-of-mass offset in the horizontal plane. A sliding window is introduced in the direction of the center-of-mass offset, and the offset is gradually identified by the method of stacking and sliding one window by one. Finally, the offset estimation value of the center-of-mass in the horizontal plane is obtained. The specific steps are as follows:

[0067] (1) Assume that during the diagonal gait movement of the robot, the center of mass (Center of Mass, CoM) of the robot torso is located at the geometric center of the robot, and the robot system maintains balance. As Figure 3 shown, establish the relationship equation between the foot-end contact force of the supporting leg and the torso y-direction torque :

[0068]

[0069] Among them, in the formula and represent the foot positions of the front and rear supporting legs relative to the torso coordinate system {B} during the trotting motion. and represent the contact forces applied to the front and rear supporting legs during the trotting motion.

[0070] (2) Assume that due to the offset of the center of mass, the gravity mg of the robot generates an overturning torque along the y-axis as , where is the distance by which the center of mass of the robot in the x-direction in the torso coordinate system {B} deviates from the center of the torso.

[0071] This additional torque will break the equilibrium state of the robot. Therefore, additional contact forces and need to be applied to the robot so that the robot returns to the equilibrium state, which can be expressed by the formula:

[0072]

[0073] Therefore, combining the above and , the relationship between the contact force of the front support leg and the contact force of the rear support leg can be obtained as:

[0074]

[0075] where , respectively represent the foot positions of the front and rear support legs relative to the hip joint coordinate system {H}, and is the length of the torso.

[0076] Based on the stability of the diagonal legs and the stability of the torso standing height, it can be obtained that:

[0077]

[0078] When the change in the x-direction velocity is small, the longitudinal contact force remains stable, and and The changes of can be ignored, and it can be obtained that:

[0079]

[0080] Therefore, by combining the above formula and analyzing the difference between the contact force of the front support leg and the contact force of the rear support leg, the direction of the CoM offset can be estimated.

[0081] (3) Similarly, according to the overturning torque of the robot in the x-direction, the relationship between the contact force of the left support leg and the contact force of the right support leg is obtained as:

[0082]

[0083] Similar to the above process, based on the synchrony of the diagonal legs and ignoring and the variations, the following relationship can be obtained:

[0084]

[0085] According to the above formula, by analyzing the difference between the contact force of the left support leg and the contact force of the right support leg, the direction of the CoM offset can be estimated.

[0086] (4) The deviation of the CoM in the x and y directions is represented by a piecewise function as follows:

[0087]

[0088]

[0089] In the formula, , are the threshold values of the contact force difference between the front and rear support legs, , , similarly, , represent the threshold values of the contact forces of the left and right support legs. Among them, , ; if , then the CoM is in front of the trunk center. If , then the CoM is on the left side of the trunk center; the contact forces of the front support leg , the rear support leg , the left support leg , and the right support leg must all exceed the minimum contact force threshold to confirm that the foot end touches the ground.

[0090] To improve the accuracy of the CoM offset estimation, a recursive iteration strategy is used to estimate the offset, and a sliding window is introduced to reduce the hysteresis. Along the horizontal direction of the window, is superimposed as the variable N k , and the calculation formula is:

[0091]

[0092] where k x , k y represent the sampling time.

[0093] Using the recursive strategy for deviation identification and performing multiple corrections to determine the CoM offset, therefore, the position estimation of the CoM on the horizontal plane is:

[0094]

[0095] Among them, and are the proportionality coefficients in the x and y directions, and are the differential coefficients in the x and y directions respectively; the offset of the center of mass in the z direction is relatively small and basically does not generate an overturning torque. Therefore, it is assumed that the offset of the center of mass of the robot in the z direction is 0, and the offset value of the center of mass caused by internal force disturbance and external force disturbance in the horizontal plane is calculated to be .

[0096] Step S4: According to the estimated center-of-mass offset value, adjust the motion trajectory planner and the center-of-mass dynamics model. Based on the adjusted results, compensate for the center-of-mass deviation through the whole-body controller in the null space to obtain the optimal joint torque, and perform real-time servo control to achieve the stability and smooth operation of the robot system. The specific process is as follows:

[0097] (1) Compensate the center-of-mass offset value into the motion trajectory planner to compensate for the center-of-mass deviation of the foot-end trajectory in the support phase and the swing phase.

[0098] Specifically, according to the desired speed of the robot input by the user and the estimated center-of-mass offset value plan the foot-end trajectory of the support phase and the swing phase of the robot to achieve the compensation of the center-of-mass deviation of the foot-end trajectory. Adjust the planned foot-end trajectory, introduce the center-of-mass offset into the motion planner for compensation, which is expressed by the formula:

[0099]

[0100] Among them,

[0101]

[0102]

[0103] Among them, represents the desired foot-end trajectory of the jth leg foot. is the position of the trunk center at the kth time step in the world coordinate system {W}, represents the rotation matrix of the robot trunk relative to the world coordinate system {W}, is the position of the jth swing leg hip joint relative to the trunk coordinate system {B}. Therefore, is the position of the jth leg hip joint relative to the world coordinate system {W}. and represent the feedback speed and the desired speed of the robot trunk, Represents the desired angular velocity of the torso attitude, Represents the duration of the stance phase, Represents the standing height of the robot torso.

[0104] (2) According to the state feedback of the motion trajectory planner, compensate the dynamic model of the robot. According to the state space equation of the robot, calculate the optimal contact force at the foot end during the motion.

[0105] Specifically, compensate the center of mass offset value into the dynamic model of the torso center of mass to achieve offset compensation of the dynamic model and ensure the stability of the robot during the motion. It is expressed by the formula as:

[0106]

[0107] where, m is the body mass of the robot, and c is the number of limb contacts with the ground. 、 、 respectively represent the torso position, limb contact force and gravitational acceleration of the robot. is the position of the i-th contact point, is the offset of the CoM of the robot, both relative to the origin of the torso coordinate system. Represents the inertia tensor of the robot in the world coordinate system, and ω represents the angular velocity of the torso in the world coordinate system.

[0108] Construct the state space equation of the robot system, solve the above optimization problem as a quadratic programming, and calculate the optimal contact force at the foot end during the motion:

[0109]

[0110] where, is the calculated feedforward optimal foot end contact force, is the compensation contact force calculated according to the torso attitude deviation. Q and W are positive definite weight matrices, assigns weights to secondary objectives (such as regularization to make the solution bounded), C is the friction cone inequality constraint matrix approximated as a pyramid, 、 are the upper and lower bounds of the friction cone constraint respectively.

[0111] (3) Using the null-space based full-body controller, the desired accelerations and angular velocities that satisfy the robot's torso centroid task, torso attitude task, swinging leg trajectory task, and multi-limb operation task are solved, combined with the optimal foot-end contact force, a convex optimization function is constructed, the optimal interaction force of the limbs is calculated, and finally converted into joint torques to ensure the accurate execution of multiple tasks and achieve the centroid offset compensation and stable full-body control of the robot system after being subjected to various disturbances.

[0112] Specifically, through null-space projection, the task iteration is used to solve the desired accelerations that satisfy multiple tasks and the desired angular velocities , combined with the optimal feedforward contact force calculated according to the state equation constructed by the centroid dynamics and , a convex optimization function that satisfies various constraints such as the full-body dynamics model and friction cone constraint is constructed, and the minimum slack variables of the floating base acceleration and contact force under various constraints are solved

[0113]

[0114] where and are the slack variables of the floating base acceleration and the centroid dynamics optimal feedforward force respectively. is the feedforward force calculated by constructing an optimization problem according to the robot state space equation. and are weight matrices respectively. is the selection matrix of the floating base, is the linearized friction cone constraint matrix. represents the Jacobian matrices corresponding to the torso, legs, and arms.

[0115] This convex optimization function uses the QP optimizer QuadProg++ to calculate the optimal interaction force of the limbs and converts it into joint torques for servo control.

[0116] Embodiment 2

[0117] In an embodiment of the present disclosure, a centroid estimation and compensation system for a variable configuration quadruped robot with arms is provided, including:

[0118] A state acquisition module, configured to: acquire the state feedback information of the variable configuration quadruped robot during movement;

[0119] A contact force calculation module, configured to: based on the state feedback information, calculate the foot-end contact force of the supporting legs through the full-body dynamics model of the variable configuration quadruped robot and a disturbance force observer based on generalized momentum;

[0120] An offset estimation module, configured to: based on the foot-end contact force of the support legs, adopt a centroid estimation algorithm with an online recursive strategy to estimate the centroid offset value after being subjected to unknown disturbances;

[0121] An offset compensation module, configured to: according to the estimated centroid offset value, adjust the motion trajectory planner and the centroid dynamics model, and based on the adjusted results, compensate for the centroid deviation through a whole-body controller in the null space to obtain the optimal joint torque for real-time servo control;

[0122] Wherein, the centroid estimation algorithm with the online recursive strategy utilizes the difference in the foot-end contact forces of the front and rear, and left and right support legs to estimate the direction of the centroid offset in the horizontal plane. A sliding window is introduced in the direction of the centroid offset, and the offset amount is gradually identified by the way of stacking and sliding one window by one, and finally the offset estimation value of the centroid in the horizontal plane is obtained.

[0123] Embodiment 3

[0124] In an embodiment of the present disclosure, a computer program product is provided, including a computer program, and when the computer program is executed by a processor, it implements the centroid estimation and compensation method of a variable configuration quadruped robot with arms.

[0125] Embodiment 4

[0126] In an embodiment of the present disclosure, a non-transitory computer-readable storage medium is provided, and the non-transitory computer-readable storage medium is used to store computer instructions, and when the computer instructions are executed by a processor, it implements the centroid estimation and compensation method of a variable configuration quadruped robot with arms.

[0127] Embodiment 5

[0128] In an embodiment of the present disclosure, an electronic device is provided, including: a processor, a memory, and a computer program; wherein, the processor is connected to the memory, the computer program is stored in the memory, and when the electronic device runs, the processor executes the computer program stored in the memory so that the electronic device executes and implements the centroid estimation and compensation method of a variable configuration quadruped robot with arms.

[0129] This disclosure is described with reference to the flowcharts and / or block diagrams of methods, apparatuses (systems), and computer program products according to embodiments of the disclosure. It should be understood that each flow and / or block in the flowcharts and / or block diagrams, as well as the combination of flows and / or blocks in the flowcharts and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, such that the instructions executed by the processor of the computer or other programmable data processing device generate means for implementing the functions specified in one or more flows Figure 1 one or more flows and / or blocks Figure 1 or means for implementing the functions specified in one or more blocks.

[0130] These computer program instructions can also be loaded onto a computer or other programmable data processing device, such that a series of operation steps are executed on the computer or other programmable device to generate a computer-implemented process, so that the instructions executed on the computer or other programmable device provide steps for implementing the functions specified in one or more flows Figure 1 one or more flows and / or blocks Figure 1 or steps for implementing the functions specified in one or more blocks.

[0131] Although the specific embodiments of the disclosure have been described above in conjunction with the accompanying drawings, this is not a limitation on the protection scope of the disclosure. Those skilled in the art should understand that, based on the technical solutions of the disclosure, various modifications or variations that can be made by those skilled in the art without creative efforts are still within the protection scope of the disclosure.

Claims

1. A method for estimating and compensating the center of mass of a variable configuration quadruped robot with arms, characterized in that: include: Obtain state feedback information of the variable configuration quadruped robot during movement; Based on the state feedback information, the foot contact force of the supporting leg is calculated through the whole-body dynamics model of the variable configuration quadruped robot and the disturbance force observer based on generalized momentum. According to the contact force of the foot end of the supporting leg, the center of mass estimation algorithm with online recursive strategy is used to estimate the center of mass offset value after unknown disturbance. The calculation of the foot end contact force of the supporting leg is specifically as follows: According to the whole-body dynamics model and state feedback information, the disturbance torque of the ground during the robot's movement is calculated; According to the robot state feedback information, the whole body dynamics model of the variable configuration quadruped robot with a robotic arm is established: in, is the inertia matrix, is the Coriolis force and the centrifugal force, is the gravity term, represents the robot joint torque, is the corresponding leg joint motor torque, is the joint motor torque corresponding to the robotic arm, is the torque corresponding to the trunk; q Represents the robot's torso pose, leg joint angles, and arm joint angles; and are the corresponding angular velocity and angular acceleration respectively; represents the Jacobian matrix of the corresponding trunk, leg and robotic arm; f represents the contact force between the foot end and the ground; According to the robot's whole-body dynamics model and state feedback information, the disturbance torque of the ground during the robot's movement is calculated. for: in, is the selection matrix for the floating base, and are the angular velocity and angular acceleration corresponding to the trunk posture and leg-arm joints respectively; The disturbance torque is discretized using the disturbance force observer; Based on the discretized disturbance torque, the contact force at the foot end of the supporting leg is calculated through the relationship between the Jacobian matrix of the robot leg and the joint torque. According to the estimated center of mass offset value, the motion trajectory planner and the center of mass dynamics model are adjusted. Based on the adjusted results, the center of mass deviation is compensated by the whole body controller in the zero space to obtain the optimal joint torque and perform real-time servo control. The optimal joint torque is obtained as follows: The center of mass offset value is compensated into the motion trajectory planner, and the center of mass deviation is compensated for the foot end trajectory in the support phase and the swing phase; The desired speed of the robot according to the user input and the estimated centroid offset Plan the foot trajectory of the robot during the support phase and the swing phase to achieve compensation for the center of mass deviation of the foot trajectory; adjust the planned foot trajectory to offset the center of mass Introduced into the motion planner for compensation, expressed as: in, in, represents the expected foot end trajectory of the jth leg foot end; is the position of the torso center in the world coordinate system {W} at the kth time step, represents the rotation matrix of the robot torso relative to the world coordinate system {W}, is the position of the j-th swing leg hip joint relative to the trunk coordinate system {B}; therefore, is the hip joint position of the jth leg relative to the world coordinate system {W}; and represents the feedback velocity and the desired velocity of the robot trunk, represents the desired angular velocity of the trunk, represents the duration of the support phase, Indicates the standing height of the robot torso; According to the state feedback of the motion trajectory planner, the dynamic model of the robot is compensated, and the optimal contact force of the foot during the motion process is solved according to the state space equation of the robot; Offset the center of mass by Compensate to the trunk center of mass dynamics model to achieve offset compensation of the dynamics model and ensure the stability of the robot during movement. It can be expressed as: Among them, m is the body mass of the robot, and c is the number of limbs in contact with the ground; , , They represent the robot’s trunk position, limb contact force, and gravitational acceleration, respectively; is the position of the ith contact point, is the offset of the robot’s CoM, both relative to the origin of the torso coordinate system; represents the inertia tensor of the robot in the world coordinate system, and ω represents the angular velocity of the trunk in the world coordinate system; using the whole body controller based on null space, the expected acceleration and angular velocity that meet the robot's trunk center of mass task, trunk posture task, swing leg trajectory task, and multi-limb operation task are solved, and combined with the optimal contact force at the foot end, a convex optimization function is constructed to calculate the optimal interaction force of the limbs, which is finally converted into joint torque; Among them, the center of mass estimation algorithm of the online recursive strategy uses the difference in foot contact force between the front and rear and left and right supporting legs to estimate the direction of the center of mass offset in the horizontal plane, introduces a sliding window in the direction of the center of mass offset, and gradually identifies the offset by superimposing and sliding windows one by one, and finally obtains the estimated value of the center of mass offset in the horizontal plane.

2. The method for estimating and compensating the center of mass of a quadruped robot with arms according to claim 1, characterized in that: The state feedback information includes the trunk position and speed information of the variable configuration quadruped robot.

3. The method for estimating and compensating the center of mass of a quadruped robot with arms according to claim 1, characterized in that: The difference in the foot contact force between the front and rear and left and right supporting legs is used to estimate the direction of the center of mass deviation in the horizontal plane, specifically: Establish the relationship between the foot contact force of the supporting leg and the trunk torque, and calculate the overturning torque in the x-direction and y-direction; Based on the overturning torque in the x-direction and the y-direction, the difference between the contact force of the front supporting leg and the contact force of the rear supporting leg, and the difference between the contact force of the left supporting leg and the contact force of the right supporting leg are calculated respectively; Using the difference, estimate the direction of the centroid offset in the horizontal plane.

4. The method for estimating and compensating the center of mass of a quadruped robot with arms according to claim 1, characterized in that: The offset is gradually identified by sliding windows one by one, specifically: Use piecewise functions to represent the deviation of the center of mass offset in the x and y directions respectively; The directional deviation of each segment is used as a window. By superimposing and sliding each window one by one, the centroid offset direction is recalculated in turn to determine whether the centroid is offset. If the centroid is still offset, the next window is iteratively superimposed and recalculated. Otherwise, the current window superposition value is the final centroid offset value.

5. A mass center estimation and compensation system for a quadruped robot with arms, characterized in that: include: The state acquisition module is configured to: obtain state feedback information of the variable configuration quadruped robot during movement; The contact force calculation module is configured to: calculate the foot end contact force of the supporting leg based on the state feedback information through the whole body dynamics model of the variable configuration quadruped robot and the disturbance force observer based on generalized momentum; The calculation of the foot end contact force of the supporting leg is specifically as follows: According to the whole-body dynamics model and state feedback information, the disturbance torque of the ground during the robot's movement is calculated; According to the robot state feedback information, the whole body dynamics model of the variable configuration quadruped robot with a robotic arm is established: in, is the inertia matrix, is the Coriolis force and the centrifugal force, is the gravity term, represents the robot joint torque, is the corresponding leg joint motor torque, is the joint motor torque corresponding to the robotic arm, is the torque corresponding to the trunk; q Represents the robot's torso pose, leg joint angles, and arm joint angles; and are the corresponding angular velocity and angular acceleration respectively; represents the Jacobian matrix of the corresponding trunk, leg and robotic arm; f represents the contact force between the foot end and the ground; According to the robot's whole-body dynamics model and state feedback information, the disturbance torque of the ground during the robot's movement is calculated. for: in, is the selection matrix for the floating base, and are the angular velocity and angular acceleration corresponding to the trunk posture and leg-arm joints respectively; The disturbance torque is discretized using the disturbance force observer; Based on the discretized disturbance torque, the contact force at the foot end of the supporting leg is calculated through the relationship between the Jacobian matrix of the robot leg and the joint torque. The offset estimation module is configured to: estimate the center of mass offset value after being subjected to unknown disturbances by using a center of mass estimation algorithm of an online recursive strategy according to the foot end contact force of the supporting leg; The offset compensation module is configured to: adjust the motion trajectory planner and the center of mass dynamics model according to the estimated center of mass offset value, and based on the adjusted result, compensate the center of mass deviation through the whole body controller of the null space to obtain the optimal joint torque and perform real-time servo control; The optimal joint torque is obtained as follows: The center of mass offset value is compensated into the motion trajectory planner, and the center of mass deviation is compensated for the foot end trajectory in the support phase and the swing phase; The desired speed of the robot according to the user input and the estimated centroid offset Plan the foot trajectory of the robot during the support phase and the swing phase to achieve compensation for the center of mass deviation of the foot trajectory; adjust the planned foot trajectory to offset the center of mass Introduced into the motion planner for compensation, expressed as: in, in, represents the expected foot end trajectory of the jth leg foot end; is the position of the torso center in the world coordinate system {W} at the kth time step, represents the rotation matrix of the robot torso relative to the world coordinate system {W}, is the position of the j-th swing leg hip joint relative to the trunk coordinate system {B}; therefore, is the hip joint position of the jth leg relative to the world coordinate system {W}; and represents the feedback velocity and the desired velocity of the robot trunk, represents the desired angular velocity of the trunk, represents the duration of the support phase, Indicates the standing height of the robot torso; According to the state feedback of the motion trajectory planner, the dynamic model of the robot is compensated, and the optimal contact force of the foot during the motion process is solved according to the state space equation of the robot; Offset the center of mass by Compensate to the trunk center of mass dynamics model to achieve offset compensation of the dynamics model and ensure the stability of the robot during movement. It can be expressed as: Among them, m is the body mass of the robot, and c is the number of limbs in contact with the ground; , , They represent the robot’s trunk position, limb contact force, and gravitational acceleration, respectively; is the position of the ith contact point, is the offset of the robot’s CoM, both relative to the origin of the torso coordinate system; represents the inertia tensor of the robot in the world coordinate system, and ω represents the angular velocity of the trunk in the world coordinate system; using the whole body controller based on null space, the expected acceleration and angular velocity that meet the robot's trunk center of mass task, trunk posture task, swing leg trajectory task, and multi-limb operation task are solved, and combined with the optimal contact force at the foot end, a convex optimization function is constructed to calculate the optimal interaction force of the limbs, which is finally converted into joint torque; Among them, the center of mass estimation algorithm of the online recursive strategy uses the difference in foot contact force between the front and rear and left and right supporting legs to estimate the direction of the center of mass offset in the horizontal plane, introduces a sliding window in the direction of the center of mass offset, and gradually identifies the offset by superimposing and sliding windows one by one, and finally obtains the estimated value of the center of mass offset in the horizontal plane.

6. A computer program product comprising a computer program, characterized in that When the computer program is executed by a processor, the method for estimating and compensating the center of mass of a deformable quadruped robot with arms as described in any one of claims 1 to 4 is implemented.

7. A non-transitory computer-readable storage medium, characterized in that: The non-transitory computer-readable storage medium is used to store computer instructions. When the computer instructions are executed by the processor, a method for estimating and compensating the center of mass of a deformable quadruped robot with arms as described in any one of claims 1 to 4 is implemented.

8. An electronic device, characterized in that: include: A processor, a memory and a computer program; wherein the processor is connected to the memory, the computer program is stored in the memory, and when the electronic device is running, the processor executes the computer program stored in the memory so that the electronic device executes a method for estimating and compensating the center of mass of a deformable quadruped robot with arms as described in any one of claims 1 to 4.

Images