Wheel-legged quadruped robot state evaluation method and system, processing equipment and storage medium

Abstract

The invention provides a wheel-legged quadruped robot state estimation method and system, and the method comprises the steps: obtaining the stress information of four wheels of a wheel-legged robot, IMU information, and the joint angle and joint angular velocity of each robot leg joint through a six-dimensional force sensor, an IMU and a joint encoder; and then calculating the position and speed information of the foot end points of the four-foot wheel-leg robot in swinging or underground touching and the speed of the wheels. According to the IMU information, the acceleration of the four-foot wheel-leg robot is calculated, the current position and speed of a robot body are predicted according to the optimal estimated value, obtained through previous prediction, of the state of the robot body, and prediction data are obtained. And through extended Kalman filtering, fusing the calculated position and speed information of the foot end point when the foot end point swings or touches the ground and the speed of the wheel with the prediction data to obtain an optimal estimation value of the current body state of the robot.

Description

Technical Field

[0001] The present invention relates to the technical field of robots, and in particular to a method and system for evaluating the state of a wheel-legged quadruped robot, a processing device, and a storage medium. Background Art

[0002] In recent years, the development of quadruped robots has received unprecedented attention at home and abroad. Wheel-legged quadruped robots have been studied by many scholars. Currently, many countries have designed wheel-legged quadruped robots, such as the wheeled robot Stretch of Boston Dynamics in the United States, the ANYmal wheeled robot of ETH Zurich, the MITCheelah3 and Mini Cheetah series of the Massachusetts Institute of Technology, the "Jueying" series of Zhejiang University in China, the Unitree B-W wheeled robot of Unitree Technology, the Nezha BIT-NAZA wheeled robot of Beijing Institute of Technology, etc. The core control algorithm of the wheel-legged quadruped robot includes three parts, namely accurate state estimation, reasonable control model, and stable and reliable software framework. Among them, accurate state estimation is the prerequisite guarantee for the normal operation of the control algorithm. Without accurate state estimation, no matter how good the algorithm is, it cannot play its effect. Summary of the Invention

[0003] The technical problem to be solved by the present invention is to provide a high-precision state evaluation technology for the non-linear motion law of the wheel-legged quadruped robot.

[0004] The present invention solves the above technical problem by the following technical means:

[0005] A method for evaluating the state of a wheel-legged quadruped robot includes the following steps:

[0006] Obtain the force information of the four wheels of the wheel-legged robot, the IMU information, and the joint angles and joint angular velocities of each robot leg joint through a six-axis force sensor, an IMU, and a joint encoder, and then calculate the position and velocity information of the foot end point of the wheel-legged quadruped robot during swinging or touchdown and the velocity of the wheels.

[0007] Calculate the acceleration of the wheel-legged quadruped robot according to the IMU information, and predict the current position and velocity of the robot body based on the optimal estimated value of the robot body state obtained from the previous prediction to obtain prediction data.

[0008] Then, through the extended Kalman filter, fuse the calculated position and velocity information of the foot end point during swinging or touchdown and the velocity of the wheels with the prediction data to obtain the optimal estimated value of the current robot body state.

[0009] Furthermore, the specific calculation methods for the position and velocity information of the foot endpoints of the wheel-legged quadruped robot during swinging or touchdown and the velocity of the wheels are as follows:

[0010] Assume the forces in three directions on the left front wheel are F xLF , F yLF , F zLF , the forces in three directions on the right front wheel are F xRF , F yRF , F zRF , the forces in three directions on the left rear wheel are F xLH , F yLH , F zLH , the forces in three directions on the right rear wheel are F xRH , F yRH , F zRH ;

[0011] Judge the touchdown conditions of the four wheels according to the force information of each wheel. The judgment formula is shown in the following formula (1):

[0012]

[0013] Among them, sw i represents the touchdown condition of the i-th leg, F i represents the force received by the wheel where the i-th leg is located, and F_thres represents the force threshold of the wheel;

[0014] Next, calculate the attitude transformation matrix between the body frame and the reference frame according to the attitude angles output by the IMU information, and convert the body-frame acceleration output by the IMU to the reference-frame acceleration through coordinate transformation. The transformation from the ground inertial reference frame to the IMU body frame can be represented by a direction cosine matrix, which is mainly composed of the transformation matrix generated by rotation along the X axis, the transformation matrix RM θ generated by rotation along the Y axis, and the transformation matrix RM ψ generated by rotation along the Z axis. Then, the direction cosine matrix R represented by Euler angles can be expressed as shown in the following formula (2):

[0015]

[0016] Then, according to the joint information, that is, the angles and angular velocities of each joint obtained by the joint encoder, combined with kinematics, the positions of the ends of the four legs in the wheel-legged quadruped robot relative to the body frame are the end position x of the left front leg LF , the end position x of the right front leg RF , the end position x of the left rear leg LH , the end position x of the right rear leg RH , and the specific calculation formulas are shown in the following formulas (3)-(6):

[0017] x LF = f LF (l LF1 , l LF2 , l LF3 , q LF1 , q LF2 , q LF3 ) (3)

[0018] x RF = f RF (l RF1 , l RF2 , l RF3 , q RF1 , q RF2 , q RF3 ) (4)

[0019] x LH = f LH (l LH1 , l LH2 , l LH3 , q LH1 , q LH2 , q LH3 ) (5)

[0020] x RH = f RH (l RH1 , l RH2 , l RH3 , q RH1 , q RH2 , q RH3 ) (6)

[0021] where l... is the length between leg joints, and q... is the joint angle of leg joints.

[0022] According to the geometric structure relationship between the end of the leg and the wheel, the offsets of the contact points of the four wheels with the ground relative to the position of the end of the robot leg are offset LFw , offset RFw , offset LHw , offset RHw , so the positions x LFp , x RFp , x LHp , x RHp of the contact points of the wheels with the ground relative to the fixed coordinate system are calculated; the specific calculation formulas are shown as follows in equations (7)-(10):

[0023] x LFp = x LF - offset LFw (7)

[0024] xRFp = x RF - offset RFw (8)

[0025] x LHp = x LH - offset LHw (9)

[0026] x RHp = x RH - offset RHw (10)

[0027] Combined with the information in the IMU and the contact situation between the contact point and the ground, the position of the foot end point of the quadruped wheeled- legged robot during swinging or touchdown can be calculated;

[0028] By kinematic calculation of the Jacobian matrix between the joints of the leg and combining the joint angular velocities read by the joint encoder, the contribution of the leg to the contact point velocity of the quadruped wheeled- legged robot can be calculated. The specific calculation formula is shown as follows in equations (11)-(14):

[0029]

[0030] where, is the joint angular velocity of the leg joint, and J is the Jacobian matrix;

[0031] According to the information obtained by the joint encoder, the angular velocity of the wheel can be obtained. Combining with the radius of the wheel, the rotational speeds of the four wheels can be calculated The specific calculation formula is shown as follows in equations (15)-(18):

[0032]

[0033] where, are the angular velocities of the four wheels respectively, and r w is the radius of the four wheels. Therefore, the velocity of the foot end point of the quadruped wheeled- legged robot can be calculated. The specific calculation formula is shown as follows in equations (19)-(22):

[0034]

[0035] Furthermore, based on the touchdown information of the foot end of the wheeled- legged quadruped robot with the ground, the velocity of the foot end point of the quadruped wheeled- legged robot during swinging or touchdown can be calculated.

[0036] Furthermore, the specific method for predicting the current position and velocity of the robot body is as follows:

[0037] First, according to the IMU information, calculate the acceleration of the quadruped wheeled- legged robot in the world coordinate system. The specific calculation formula is shown as follows in equation (23):

[0038] a world = R·a IMU (23)

[0039] where a IMU is the acceleration directly output by the IMU;

[0040] Then, based on the previous state x k-1 , the current control input u k and the current measurement z k , estimate the mean μ and covariance P of the Gaussian distribution of the state x k ; First, use the nonlinear discrete transition function f(·) to predict the state, and then correct it through the observation function h(·); The specific calculation formulas for the transition function and the observation function are shown in the following formula (24):

[0041]

[0042] where u k is the input, a definite quantity, that is, the acceleration a world of the wheeled - legged quadruped robot in the world coordinate system, x k-1 and x k represent the state quantities at times k - 1 and k, as shown in the following formula (25):

[0043] x k-1 = [p v q p w v w p1 p2 p3 p4 b f b ω T (25)

[0044] where p and v are respectively the velocities of the body of the wheeled - legged quadruped robot, q is the attitude of the robot, p w and v w are respectively the contributions of the wheel driving force to the position and velocity of the body of the wheeled - legged quadruped robot, p1, p2, p3, p4 are respectively the positions of the contact points between the wheels of the wheeled - legged quadruped robot and the ground, b f and b ω are respectively the accelerometer bias and the gyroscope bias;

[0045] Next, the optimal estimated value of f(x k-1 , u k , w k-1 ) at the k - 1 moment and the predicted value of h(x k , v k ) at the k moment ​Performing Taylor expansion at a point and omitting the high-order terms above the second order, the following formula (26) can be obtained:

[0046]

[0047] Wherein, is the first-order partial derivative term obtained after expansion. During the expansion process, the terms with higher degrees are omitted, and the state estimation equation is transformed into an approximate linearized equation;

[0048] Finally, the prediction equation and prediction covariance matrix of the extended Kalman filter are as follows formula (27):

[0049]

[0050] Wherein, is the first-order partial derivative term after Taylor expansion, is the expected value of the state quantity, and P k is the covariance matrix of the system;

[0051] Therefore, the optimal estimated value of the robot body state obtained through the prediction equation and the previous prediction is used to obtain the current prediction data of the robot.

[0052] Furthermore, the calculation process of the optimal estimated value of the current robot body state is as follows:

[0053] The state data estimated last time is subjected to prior estimation through the prediction equation in formula (27) to obtain the current prediction data of the robot. Then, the error covariance matrix P k-1 estimated last time is calculated to obtain the prior error covariance matrix

[0054] The formula for calculating the Kalman gain is as follows formula (28):

[0055]

[0056] Wherein, R is the measurement covariance matrix, is the first-order partial derivative term after Taylor expansion;

[0057] Posterior estimation is performed, and the calculation formula is as follows formula (29):

[0058]

[0059] The calculation formula for updating the posterior covariance matrix is as follows formula (30):

[0060]

[0061] The present invention also provides a state evaluation system for a wheel-legged quadruped robot, including:

[0062] A measurement and calculation module, configured to obtain the force information of the four wheels of the wheeled-leg robot, the IMU information, and the joint angles and joint angular velocities of each robot leg joint through a six-axis force sensor, an IMU, and joint encoders, and calculate the position and velocity information of the foot end point of the quadruped wheeled-leg robot during swinging or touchdown and the velocity of the wheels according to the force information, the IMU information, and the joint information.

[0063] A prediction module, configured to calculate the acceleration of the quadruped wheeled-leg robot in the world coordinate system according to the IMU information, and predict the current position, velocity of the robot body, and the position and velocity of the contribution of the driving force of the wheels to the robot body based on the optimal estimated value of the robot body state obtained from the previous prediction, so as to obtain prediction data.

[0064] An optimal estimation module, configured to fuse the calculated position and velocity information of the foot end point during swinging or touchdown and the velocity of the wheels with the prediction data through an extended Kalman filter to obtain the optimal estimated value of the current robot body state.

[0065] Further, the specific calculation method of the position and velocity information of the foot end point of the quadruped wheeled-leg robot during swinging or touchdown and the velocity of the wheels in the measurement and calculation module is as follows:

[0066] Assume that the three-direction forces received by the left front wheel are F xLF , F yLF , F zLF , the three-direction forces received by the right front wheel are F xRF , F yRF , F zRF , the three-direction forces received by the left rear wheel are F xLH , F yLH , F zLH , the three-direction forces received by the right rear wheel are F xRH , F yRH , F zRH ;

[0067] Judge the touchdown conditions of the four wheels according to the force information of each wheel, and the judgment formula is shown in the following formula (1):

[0068]

[0069] where sw i represents the touchdown condition of the i-th leg, F i represents the force received by the wheel where the i-th leg is located, and F_thres represents the force threshold of the wheel.

[0070] Next, calculate the attitude transformation matrix between the body-fixed frame and the reference frame based on the attitude angles output by the IMU, and transform the body-fixed frame acceleration output by the IMU into the reference frame acceleration through coordinate system transformation. The transformation from the ground inertial reference frame to the IMU body-fixed frame can be represented by a direction cosine matrix, which is mainly composed of the transformation matrix generated by rotation along the X-axis the transformation matrix RM generated by rotation along the Y-axis θ and the transformation matrix RM generated by rotation along the Z-axis ψ Combined, the direction cosine matrix R expressed in Euler angles can be represented as shown in Equation (2) below:

[0071]

[0072] Then, based on the joint information, i.e., the angles and angular velocities of each joint obtained from the joint encoders, and combined with kinematic calculations, the position distributions of the ends of the four legs of the wheel-legged quadruped robot relative to the body-fixed frame are the position x of the end of the left front leg LF , the position x of the end of the right front leg RF , the position x of the end of the left hind leg LH , and the position x of the end of the right hind leg RH . The specific calculation formulas are shown in Equations (3)-(6) below:

[0073] x LF =f LF (l LF1 ,l LF2 ,l LF3 ,q LF1 ,q LF2 ,q LF3 ) (3)

[0074] x RF =f RF (l RF1 ,l RF2 ,l RF3 ,q RF1 ,q RF2 ,q RF3 ) (4)

[0075] x LH =f LH (l LH1 ,l LH2 ,l LH3 ,q LH1 ,q LH2 ,q LH3 ) (5)

[0076] x RH =f RH (l RH1 ,l RH2 ,l RH3,q RH1 ,q RH2 ,q RH3 ) (6)

[0077] Among them, l... is the length between the leg joints, and q… is the joint angle of the leg joints.

[0078] Furthermore, according to the geometric relationship between the end of the leg and the wheels, the offsets of the contact points of the four wheels with the ground relative to the position of the end of the robot leg are respectively offset LFw 、offset RFw 、offset LHw 、offset RHw , so the positions x LFp 、x RFp 、x LHp 、x RHp of the contact points of the wheels with the ground relative to the fixed coordinate system are calculated; the specific calculation formulas are shown in the following formulas (7)-(10):

[0079] x LFp = x LF - offset LFw (7)

[0080] x RFp = x RF - offset RFw (8)

[0081] x LHp = x LH - offset LHw (9)

[0082] x RHp = x RH - offset RHw (10)

[0083] Combined with the information in the IMU and the contact situation between the contact points and the ground, the position of the foot end point of the quadruped wheeled-leg robot during swinging or touchdown can be calculated;

[0084] By kinematic calculation of the Jacobian matrix between the leg joints and combining the joint angular velocities read by the joint encoders, the contribution of the leg to the contact point velocity of the quadruped wheeled-leg robot can be calculated. The specific calculation formulas are shown in the following formulas (11)-(14):

[0085]

[0086]

[0087] Among them, is the joint angular velocity of the leg joint, and J is the Jacobian matrix;

[0088] The angular velocity of the wheels can be obtained based on the information acquired by the joint encoders, and the rotational speeds of the four wheels can be calculated by combining with the radii of the wheels. The specific calculation formulas are shown as follows in formulas (15)-(18):

[0089]

[0090] Among them, are the angular velocities of the four wheels respectively, and r w are the radii of the four wheels. Therefore, the speeds of the foot endpoints of the quadruped wheel-legged robot can be calculated, and the specific calculation formulas are shown as follows in formulas (19)-(22):

[0091]

[0092] Furthermore, based on the touchdown information between the foot endpoints of the wheel-legged quadruped robot and the ground, the speeds of the foot endpoints of the quadruped wheel-legged robot during swinging or touchdown can be calculated.

[0093] Furthermore, the specific method for predicting the current position and speed of the robot body in the prediction module is as follows:

[0094] First, according to the IMU information, calculate the acceleration of the quadruped wheel-legged robot in the world coordinate system, and the specific calculation formula is shown as follows in formula (23):

[0095] a world = R · a IMU (23)

[0096] Among them, a IMU is the acceleration directly output by the IMU;

[0097] Then, according to the previous state x k-1 , the current control input u k and the current measurement z k , estimate the mean μ and covariance P of the Gaussian distribution of the state x k ; first use the nonlinear discrete transition function f(·) to predict the state, and then correct it through the observation function h(·); the specific calculation formulas for the transition function and the observation function are shown as follows in formula (24):

[0098]

[0099] Among them, u k is the input, which is a definite quantity, that is, the acceleration a world of the quadruped wheel-legged robot in the world coordinate system, x k-1 and x kDenote the state variables at times \(k - 1\) and \(k\) as shown in the following equation (25):

[0100] x k-1 =[p v q p w v w p1 p2 p3 p4 b f b ω T (25)

[0101] where \(p\) and \(v\) are the velocities of the quadruped wheeled - legged robot body respectively, \(q\) is the attitude of the robot, \(p w and \(v w are the contributions of the wheel driving force to the position and velocity of the quadruped wheeled - legged robot body respectively, \(p1\), \(p2\), \(p3\), \(p4\) are the positions of the contact points between the wheels of the quadruped wheeled - legged robot and the ground, \(b f and \(b ω are the accelerometer bias and gyroscope bias respectively;

[0102] Next, perform a Taylor expansion of \(f(x k-1 ,u k ,w k-1 ) at the optimal estimated value at time \(k - 1\) point and \(h(x k ,v k ) at the predicted value at time \(k\) point, and omitting the high - order terms above the second - order, we can obtain the following equation (26):

[0103]

[0104] where, is the first - order partial derivative term obtained after expansion. In the expansion process, the terms with higher degrees are omitted, and the state - estimation equation is transformed into an approximate linearized equation;

[0105] Finally, the prediction equation and prediction covariance matrix of the extended Kalman filter are as follows in equation (27):

[0106]

[0107] where, is the first - order partial derivative term after Taylor expansion, is the expected value of the state variable, and \(P k is the covariance matrix of the system;

[0108] Therefore, the optimal estimated value of the robot body state obtained through the prediction equation and the previous prediction is used to obtain the current predicted data of the robot.

[0109] ​Further, the calculation process of the optimal estimated value of the current robot body state in the optimal estimation module is as follows:

[0110] Perform prior estimation on the state data estimated last time through the prediction equation in formula (27) to obtain the predicted data of the robot at present. Then, calculate the last estimated error covariance matrix P k-1 to obtain the prior error covariance matrix

[0111] The formula for calculating the Kalman gain is as follows (formula 28):

[0112]

[0113] where R is the measurement covariance matrix, is the first-order partial derivative term after Taylor expansion;

[0114] Perform posterior estimation, and the calculation formula is as follows (formula 29):

[0115]

[0116] The calculation formula for updating the posterior covariance matrix is as follows (formula 30):

[0117]

[0118] The present invention also provides a processing device, including at least one processor and at least one memory communicatively connected to the processor, wherein: the memory stores program instructions executable by the processor, and the processor can execute the above method by calling the program instructions.

[0119] The present invention also provides a computer-readable storage medium, and the computer-readable storage medium stores computer instructions, and the computer instructions enable the computer to execute the above method.

[0120] The advantages of the present invention are as follows:

[0121] The present invention obtains the force information of the four wheels of the wheel-legged robot, the IMU information, and the joint angles and joint angular velocities of each robot leg joint through a six-axis force sensor, an IMU, and joint encoders, and calculates the position and velocity information of the foot endpoints of the quadruped wheel-legged robot during swinging or touchdown and the velocities of the wheels according to the force information, the IMU information, and the joint information. According to the IMU information, the acceleration of the quadruped wheel-legged robot in the world coordinate system is calculated, and the current position and velocity of the robot body are predicted based on the optimal estimated value of the robot body state obtained from the previous prediction to obtain prediction data. Through the extended Kalman filter, the calculated position and velocity information of the foot endpoints during swinging or touchdown and the velocities of the wheels are fused with the prediction data to obtain the optimal estimated value of the current robot body state.

[0122] Specifically, for the wheel-legged robot of the present invention, the movement of the wheel-legged robot can be divided into three movement modes. The first one: each joint of the leg remains fixed, and only the wheels move; the second one: the wheels are fixed and do not slide, and the system controls the legs to move with the set gait; the third one: the wheels of the robot slide and the system controls the legs to move with the set gait, that is, the hybrid movement of the wheel-legged robot is realized. Therefore, the information on the contact between the wheels and the ground (the position and velocity of the contact point between the wheels and the ground and the velocities of the wheels) is considered in kinematics. Further, the contribution of the wheels to the position and velocity of the robot is also considered in the state variables of the robot, so a more accurate robot state can be estimated. In addition, the wheel-legged robot makes non-linear movements when switching between different movement modes, and the extended Kalman filter can update the linearized model in each movement state to handle these complex dynamic behaviors. Therefore, this method enhances the robustness and stability of the system. Brief Description of the Drawings

[0123] Figure 1 It is a flowchart of the method for estimating the state of the wheel-legged quadruped robot in the embodiment of the present invention;

[0124] Figure 2 It is a structural block diagram of the system for multi-sensor information fusion in the embodiment of the present invention. Detailed Embodiment

[0125] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the embodiments of the present invention. Obviously, the described embodiments are some, but not all, of the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0126] This embodiment provides a method for estimating the state of a wheel-legged quadruped robot. Figure 1It is a flowchart of the state estimation method for a wheel-legged quadruped robot according to an embodiment of the present application. As Figure 1 shown, the flowchart includes the following steps:

[0127] Step S101, through a six-axis force sensor, an IMU, and joint encoders, obtain the force information of the four wheels of the wheel-legged robot, the IMU information, and the joint angles and joint angular velocities of each robot leg joint, and calculate the position and velocity information of the foot endpoints of the quadruped wheel-legged robot in the swing or touchdown state and the velocities of the wheels according to the force information, IMU information, and joint information;

[0128] Specifically, assume that the three-direction forces received by the left front wheel are F xLF , F yLF , F zLF , the three-direction forces received by the right front wheel are F xRF , F yRF , F zRF , the three-direction forces received by the left rear wheel are F xLH , F yLH , F zLH , the three-direction forces received by the right rear wheel are F xRH , F yRH , F zRH .

[0129] First, judge whether the foot end is in contact with the ground according to the force condition of the foot end of the wheel-foot robot. Therefore, the part where the wheel contacts the ground is used as the contact point, and the touchdown conditions of the four wheels are judged according to the force information. Among them, the touchdown conditions of the four wheels can be divided into: all four wheels are in contact with the ground, the diagonally distributed wheels are in contact with the ground, or in a swing state. The specific judgment formula is shown in the following formula (1):

[0130]

[0131] Among them, sw i represents the touchdown condition of the i-th leg, F i represents the force received by the wheel where the i-th leg is located, and F_thres represents the force threshold of the wheel. According to the above formula, the touchdown conditions of the four wheels can be represented by 1 or 0 as: [1,1,1,1], [1,0,0,1], [0,1,1,0]. The order distribution of the four wheels is the left front wheel, the right front wheel, the left rear wheel, and the right rear wheel. It should be noted that the forces received by the four wheels are directly obtained by the six-axis force sensor. By comparing with the set force threshold, that is, using the judgment formula (1), the touchdown information of the four wheels can be obtained.

[0132] Next, calculate the attitude transformation matrix between the body-fixed frame and the reference frame based on the attitude angles output by the IMU, and convert the body-fixed frame acceleration output by the IMU into the reference frame acceleration through coordinate system transformation. The transformation from the ground inertial reference frame to the body-fixed frame of the IMU can be represented by a direction cosine matrix, which is mainly composed of the transformation matrix generated by rotation along the X-axis the transformation matrix RM generated by rotation along the Y-axis θ and the transformation matrix RM generated by rotation along the Z-axis ψ Combined, the direction cosine matrix R expressed in Euler angles can be represented as shown in Equation (2) below:

[0133]

[0134] Then, according to the joint information, that is, the angles and angular velocities of each joint obtained by the joint encoder, combined with kinematics, calculate the position distribution of the ends of the four legs of the wheel-legged quadruped robot relative to the body-fixed frame as the position x of the end of the left front leg LF , the position x of the end of the right front leg RF , the position x of the end of the left hind leg LH , and the position x of the end of the right hind leg RH . The specific calculation formulas are shown in Equations (3)-(6) below:

[0135] x LF =f LF (l LF1 ,l LF2 ,l LF3 ,q LF1 ,q LF2 ,q LF3 ) (3)

[0136] x RF =f RF (l RF1 ,l RF2 ,l RF3 ,q RF1 ,q RF2 ,q RF3 ) (4)

[0137] x LH =f LH (l LH1 ,l LH2 ,l LH3 ,q LH1 ,q LH2 ,q LH3 ) (5)

[0138] x RH =f RH (l RH1 ,l RH2 ,l RH3,q RH1 ,q RH2 ,q RH3 ) (6)

[0139] Among them, l… is the length between leg joints, and q... is the joint angle of leg joints.

[0140] According to the geometric structure relationship between the end of the leg and the wheel, the offsets of the contact points of the four wheels with the ground relative to the position of the robot leg end are offset LFw , offset RFw , offset LHw , offset RHw , so the positions x LF p, x RF p, x LHp , x RH p of the wheel-ground contact points relative to the fixed coordinate system are calculated. The specific calculation formulas are shown in the following formulas (7)-(10):

[0141] x LFp = x LF - offset LFw (7)

[0142] x RFp = x RF - offset RFw (8)

[0143] x LHp = x LH - offset LHw (9)

[0144] x RHp = x RH - offset RHw (10)

[0145] Combined with the information in the IMU and the contact situation between the contact point and the ground, the position of the foot end point of the quadruped wheel-leg robot during swinging or touchdown can be calculated.

[0146] By kinematic calculation of the Jacobian matrix between the leg joints and combining the joint angular velocities read by the joint encoders, the contribution of the leg to the contact point velocity of the quadruped wheel-leg robot can be calculated. The specific calculation formulas are shown in the following formulas (11)-(14):

[0147]

[0148] Among them, is the joint angular velocity of the leg joint, and J is the Jacobian matrix.

[0149] The angular velocity of the wheels can be obtained based on the information from the joint encoders, and combined with the radius of the wheels, the rotational speeds of the four wheels can be calculated. The specific calculation formulas are shown as follows in equations (15)-(18):

[0150]

[0151] Among them, are the angular velocities of the four wheels respectively, and r w is the radius of the four wheels. Therefore, the velocity of the foot endpoints of the quadruped wheeled-leg robot can be calculated, and the specific calculation formulas are shown as follows in equations (19)-(22):

[0152]

[0153] Furthermore, based on the touchdown information between the foot endpoints of the wheeled-leg quadruped robot and the ground, the velocities of the foot endpoints of the quadruped wheeled-leg robot during swinging or touchdown can be calculated.

[0154] Step S102: Calculate the acceleration of the quadruped wheeled-leg robot in the world coordinate system according to the IMU information, and predict the current position, velocity of the robot body, and the position and velocity of the contribution of the wheel driving force to the robot body based on the optimal estimated value of the robot body state obtained from the previous prediction, to obtain prediction data;

[0155] First, calculate the acceleration of the quadruped wheeled-leg robot in the world coordinate system according to the IMU information. The specific calculation formula is shown as follows in equation (23):

[0156] a world = R·a IMU (23)

[0157] Among them, a IMU is the acceleration directly output by the IMU.

[0158] Then, based on the previous state x k-1 , the current control input u k and the current measurement z k , estimate the mean μ and covariance P of the Gaussian distribution of the state x k . First, use the nonlinear discrete transition function f(·) to predict the state, and then correct it through the observation function h(·). Both of these functions are affected by the zero-mean Gaussian process noise w k ~N(0,Q k ) and the measurement noise v k ~N(0,P k ). The specific calculation formulas for the transition function and the observation function are shown as follows in equation (24):

[0159]

[0160] Among them, u k is the input, which is a definite quantity, that is, the acceleration a of the quadruped wheel-legged robot in the world coordinate system world , x k-1 and x k represent the state quantities at times k-1 and k, as shown in the following equation (25):

[0161] x k-1 = [p v q p w v w p1 p2 p3 p4 b f b ω T (25)

[0162] Among them, p and v are respectively the speeds of the quadruped wheel-legged robot body, q is the posture of the robot, p w and v w are respectively the contributions of the wheel driving force to the position and speed of the quadruped wheel-legged robot body, p1, p2, p3, p4 are respectively the positions of the contact points between the wheels of the quadruped wheel-legged robot and the ground, b f and b ω are respectively the accelerometer bias and the gyroscope bias.

[0163] Next, the optimal estimated value of f(x k-1 , u k , w k-1 ) at time k-1 and the predicted value of h(x k , v k ) at time k are Taylor-expanded at these points and high-order terms above the second order are omitted, and the following equation (26) can be obtained:[[]]END]]

[0164]

[0165] Among them, is the first-order partial derivative term obtained after expansion. In the expansion process, terms with higher orders are omitted, and the state estimation equation is transformed into an approximate linearized equation.

[0166] Finally, the prediction equation and the prediction covariance matrix of the extended Kalman filter are as follows in equation (27):

[0167]

[0168] Among them, is the first-order partial derivative term after Taylor expansion, is the expected value of the state quantity, and P k is the covariance matrix of the system. ​

[0169] Therefore, the optimal estimated value of the robot body state obtained through the prediction equation and the previous prediction Obtain the current prediction data of the robot.

[0170] Step S103: Through the extended Kalman filter, fuse the calculated position and velocity information of the foot endpoints during swing or touchdown and the velocity of the wheels with the prediction data to obtain the optimal estimated value of the current robot body state.

[0171] In this embodiment, through the extended Kalman filter, the calculated position and velocity information of the foot endpoints during swing or touchdown and the velocity of the wheels are fused with the prediction data to obtain the optimal estimated value of the current robot body state. The specific calculation process is as follows:

[0172] First, perform a priori estimation on the state data estimated last time through the prediction equation in formula (27) to obtain the current prediction data of the robot. Then, calculate the previous estimated error covariance matrix P k-1 to obtain the a priori error covariance matrix

[0173] The formula for calculating the Kalman gain is as follows in formula (28):

[0174]

[0175] where R is the measurement covariance matrix, is the first-order partial derivative term after Taylor expansion.

[0176] Perform posteriori estimation, and the calculation formula is as follows in formula (29):

[0177]

[0178] The formula for updating the posteriori covariance matrix is as follows in formula (30):

[0179]

[0180] Through steps S101 to S103, in this embodiment, information is obtained through the torso IMU, six-dimensional force sensor, and each joint encoder to obtain the position and velocity information of different foot endpoints of the wheel-legged quadruped robot during swing or touchdown and the velocity of the wheels. Then, based on the optimal estimated value of the robot body state obtained from the previous prediction, the current position, velocity of the robot body, and the position and velocity of the contribution of the driving force of the wheels to the robot body are predicted to obtain prediction data; finally, the prediction data and the observation data are fused through the extended Kalman filter to obtain relatively accurate robot body state information for the control of the wheel-legged quadruped robot.

[0181] Figure 2 is a structural block diagram of a multi-sensor information fusion system according to an embodiment of the present application. As Figure 2 shown, the system includes a measurement and calculation module 21, a prediction module 22, and an optimal estimation module 23.

[0182] The measurement and calculation module 21 is configured to obtain the force information of the four wheels of the wheel-legged robot, the IMU information, and the joint angles and joint angular velocities of each robot leg joint through a six-dimensional force sensor, an IMU, and joint encoders, and calculate the position and velocity information of the foot endpoints of the quadruped wheel-legged robot during swinging or touchdown and the velocities of the wheels according to the force information, the IMU information, and the joint information; the prediction module 22 is configured to calculate the acceleration of the quadruped wheel-legged robot in the world coordinate system according to the IMU information, and predict the current position and velocity of the robot body and the position and velocity of the contribution of the driving force of the wheels to the robot body based on the optimal estimated value of the robot body state obtained from the previous prediction, to obtain prediction data; the optimal estimation module 23 is configured to fuse the calculated position and velocity information of the foot endpoints during swinging or touchdown and the velocities of the wheels with the prediction data through an extended Kalman filter to obtain the optimal estimated value of the current robot body state.

[0183] Through the above system, in this embodiment, information is obtained through the torso IMU, the six-dimensional force sensor, and each joint encoder to obtain the position and velocity information of different foot endpoints of the wheel-legged quadruped robot during swinging or touchdown and the velocities of the wheels. Then, based on the optimal estimated value of the robot body state obtained from the previous prediction, the current position and velocity of the robot body and the position and velocity of the contribution of the driving force of the wheels to the robot body are predicted to obtain prediction data; finally, the prediction data and the observation data are fused through an extended Kalman filter to obtain relatively accurate robot body state information for the control of the wheel-legged quadruped robot.

[0184] The above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions described in the foregoing embodiments, or perform equivalent replacements for some of the technical features; and these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for evaluating the state of a wheel-legged quadruped robot, characterized in that, including the following methods: Obtain the force information of the four wheels of the wheel-legged robot, the IMU information, and the joint angles and joint angular velocities of each robot leg joint through a six-axis force sensor, an IMU, and joint encoders, and then calculate the position and velocity information of the foot endpoints of the quadruped wheel-legged robot during swinging or touchdown and the velocities of the wheels; Calculate the acceleration of the quadruped wheel-legged robot according to the IMU information, and predict the current position and velocity of the robot body based on the optimal estimated value of the robot body state obtained from the previous prediction to obtain prediction data; Then, through the extended Kalman filter, fuse the calculated position and velocity information of the foot endpoints during swinging or touchdown and the velocities of the wheels with the prediction data to obtain the optimal estimated value of the current robot body state.

2. The state evaluation method of a wheel-leg quadruped robot according to claim 1, characterized in that The specific calculation methods for the position and velocity information of the foot endpoints of the quadruped wheel-legged robot during swinging or touchdown and the velocities of the wheels are as follows: Assume that the three-direction forces on the left front wheel are F xLF , F yLF , F zLF , and the three-direction forces on the right front wheel are F xRF , F yRF , F zRF , and the three-direction forces on the left rear wheel are F xLH , F yLH , F zLH , and the three-direction forces on the right rear wheel are F xRH , F yRH , F zRH ; Judge the touchdown conditions of the four wheels according to the force information of each wheel. The judgment formula is shown in the following formula (1): Among them, sw i represents the ground contact condition of the i-th leg, and F i represents the force received by the wheel where the i-th leg is located, and F_thres represents the force threshold of the wheel; Next, calculate the attitude transformation matrix between the body frame and the reference frame based on the attitude angles output by the IMU, and convert the body-frame acceleration output by the IMU into the reference-frame acceleration through coordinate transformation. The transformation from the ground inertial reference frame to the IMU body frame can be represented by a direction cosine matrix, which is mainly composed of the transformation matrix RM generated by rotation about the Y axis θ and RM generated by rotation about the Z axis ψ Combined, the direction cosine matrix R represented by Euler angles can be expressed as shown in Equation (2) below: Then, based on the joint information, i.e., the angles and angular velocities of each joint obtained by the joint encoder, and combined with kinematic calculations, the position distributions of the ends of the four legs of the wheel-legged quadruped robot relative to the fixed coordinate system are the position x of the end of the left front leg LF , the position x of the end of the right front leg RF , the position x of the end of the left hind leg LH , and the position x of the end of the right hind leg RH . The specific calculation formulas are shown in the following formulas (3)-(6): x LF = f LF (l LF1 , l LF2 , l LF3 , q LF1 , q LF2 , q LF3 ) (3) x RF = f RF (l RF1 , l RF2 , l RF3 , q RF1 , q RF2 , q RF3 ) (4) x LH = f LH (l LH1 , l LH2 , l LH3 , q LH1 , q LH2 , q LH3 ) (5) x RH = f RH (l RH1 , l RH2 , l RH3 , q RH1 , q RH2 , q RH3 ) (6) where, l... is the length between leg joints, and q… is the joint angle of the leg joint. According to the geometric structure relationship between the ends of the legs and the wheels, the offsets of the contact points of the four wheels with the ground relative to the positions of the ends of the robot legs are respectively offset LFw , offset RFw , offset LHw , offset RHw . Therefore, the positions x LFp , x RFp , x LHp , x RHp of the contact points of the wheels with the ground relative to the fixed coordinate system are calculated. The specific calculation formulas are shown in the following formulas (7)-(10): x LFp = x LF - offset LFw (7) x RFp = x RF - offset RFw (8) x LHp = x LH - offset LHw (9) x RHp = x RH - offset RHw (10) Combined with the information in the IMU and the contact situation between the contact point and the ground, the position of the foot endpoints of the quadruped wheel-legged robot during swinging or touchdown can be calculated; Calculate the contribution of the leg to the contact point velocity of the quadruped wheel-legged robot by calculating the Jacobian matrix between the leg joints through kinematics and combining the joint angular velocities read by the joint encoders. The specific calculation formulas are shown in the following formulas (11)-(14): Among them, is the joint angular velocity of the leg joint, and J is the Jacobian matrix; The angular velocity of the wheels can be obtained based on the information acquired by the joint encoders, and the rotational speeds of the four wheels can be calculated by combining with the radii of the wheels. The specific calculation formulas are shown in the following formulas (15)-(18): Among them, are the angular velocities of the four wheels respectively, and r w is the radius of the four wheels. Therefore, the velocity of the foot tip of the quadruped wheel-legged robot can be calculated, and the specific calculation formula is shown as follows in equations (19)-(22): Then, according to the touchdown information between the foot end of the wheel-legged quadruped robot and the ground, the velocity of the foot endpoints of the quadruped wheel-legged robot during swinging or touchdown can be calculated.

3. A method for evaluating the state of a wheel-legged quadruped robot according to claim 2, characterized in that, The specific method for predicting the current position and velocity of the robot body is as follows: First, calculate the acceleration of the quadruped wheel-legged robot in the world coordinate system according to the IMU information. The specific calculation formula is shown in the following formula (23): a world = R·a IMU (23) where a IMU is the acceleration directly output by the IMU; Then, according to the previous state xk-1, the current control input uk, and the current measurement zk, estimate the mean μ and covariance P of the Gaussian distribution of the state xk; first use the nonlinear discrete transition function f(·) to predict the state, and then correct it through the observation function h(·); the specific calculation formulas for the transition function and the observation function are shown in the following formula (24): Among them, u k is the input, which is a definite quantity, namely the acceleration a of the quadruped wheel-legged robot in the world coordinate system world , x k-1 and xk represent the state quantities at times k-1 and k, as shown in the following formula (25): x k-1 =]p v q p w v w p1 p2 p3 p4 b f b ω T (25)​ where p and v are the velocities of the quadruped wheel-legged robot body respectively, q is the pose of the robot, p w and v w are the contributions of the wheel driving force to the position and velocity of the quadruped wheel-legged robot body respectively, p1, p2, p3, and p4 are the positions of the contact points between the wheels of the quadruped wheel-legged robot and the ground, b f and b ω are the accelerometer bias and the gyroscope bias respectively; Next, the optimal estimate value of f(x k-1 , u k , w k-1 ) at the k-1 moment and the predicted value of h(x k , v k ) at the k moment are Taylor-expanded at these points and high-order terms above the second order are omitted, resulting in the following formula (26): Among them, is the first-order partial derivative term obtained after expansion. In the expansion process, terms with higher degrees are omitted, and the state estimation equation is transformed into an approximate linearized equation; Finally, the prediction equation and prediction covariance matrix of the extended Kalman filter are as follows in formula (27): Among them, is the first-order partial derivative term after Taylor expansion, and x k is the expected value of the state quantity, and P k is the covariance matrix of the system; therefore, the optimal estimated value of the robot body state obtained through the prediction equation and the previous prediction is used to obtain the current prediction data of the robot.

4. A method for evaluating the state of a wheel-legged quadruped robot according to claim 3, characterized in that, The calculation process of the optimal estimated value of the current robot body state is as follows: Perform a prior estimate on the state data estimated last time through the prediction equation in formula (27) to obtain the predicted data of the robot at present. Then, calculate the last estimated error covariance matrix P k-1 to obtain the prior error covariance matrix The formula for calculating the Kalman gain is as follows in formula (28): where R is the measurement covariance matrix, is the first-order partial derivative term after Taylor expansion; Perform posterior estimation, and the calculation formula is as follows in formula (29): The calculation formula for updating the posterior covariance matrix is as follows in formula (30):

5. A state evaluation system for a wheel-legged quadruped robot, characterized in that, including: A measurement and calculation module configured to obtain the force information of the four wheels of the wheel-legged robot, the IMU information, and the joint angles and joint angular velocities of each robot leg joint through a six-axis force sensor, an IMU, and joint encoders, and calculate the position and velocity information of the foot endpoints of the quadruped wheel-legged robot during swinging or touchdown and the velocities of the wheels according to the force information, IMU information, and joint information; A prediction module, configured to calculate the acceleration of the quadruped wheeled-leg robot in the world coordinate system according to the IMU information, and predict the current position, velocity of the robot body, and the position and velocity of the contribution of the driving force of the wheels to the robot body based on the optimal estimated value of the robot body state obtained from the previous prediction, so as to obtain prediction data; An optimal estimation module, configured to fuse the calculated position and velocity information of the foot end point during swing or touchdown and the velocity of the wheels with the prediction data through an extended Kalman filter to obtain the optimal estimated value of the current robot body state.

6. The state evaluation system of a wheel-legged quadruped robot according to claim 5, characterized in that The specific calculation methods for the position and velocity information of the foot end point of the quadruped wheeled-leg robot during swing or touchdown and the velocity of the wheels in the measurement and calculation module are as follows: Assume that the three-direction forces on the left front wheel are F xLF , F yLF , F zLF , and the three-direction forces on the right front wheel are F xRF , F yRF , F zRF , and the three-direction forces on the left rear wheel are F xLH , F yLH , F zLH , and the three-direction forces on the right rear wheel are F xRH , F yRH , F zRH ; Judge the touchdown conditions of the four wheels according to the force information of each wheel, and the judgment formula is shown in the following formula (1): Among them, sw i represents the touchdown condition of the i-th leg, and F i represents the force received by the wheel where the i-th leg is located, and F_thres represents the force threshold of the wheel; Next, calculate the attitude transformation matrix between the body frame and the reference frame based on the attitude angles output by the IMU, and convert the body-frame acceleration output by the IMU to the reference-frame acceleration through coordinate transformation. The transformation from the ground inertial reference frame to the IMU body frame can be represented by a direction cosine matrix, which is mainly composed of the transformation matrix generated by rotation along the X-axis the transformation matrix RM generated by rotation along the Y-axis θ and the transformation matrix RM generated by rotation along the Z-axis ψ Combined, the direction cosine matrix R represented by Euler angles can be expressed as shown in Equation (2) below: Then, according to the joint information, i.e., the angles and angular velocities of each joint obtained by the joint encoder, and combining with kinematic calculations, the position distributions of the ends of the four legs of the wheel-legged quadruped robot relative to the fixed coordinate system are the position x of the end of the left front leg LF , the position x of the end of the right front leg RF , the position x of the end of the left hind leg LH , and the position x of the end of the right hind leg RH . The specific calculation formulas are shown in the following formulas (3)-(6): x LF = f LF (l LF1 , l LF2 , l LF3 , q LF1 , q LF2 , q LF3 ) (3) x RF = f RF (l RF1 , l RF2 , l RF3 , q RF1 , q RF2 , q RF3 ) (4) x LH = f LH (l LH1 , l LH2 , l LH3 , q LH1 , q LH2 , q LH3 ) (5) x RH = f RH (l RH1 , l RH2 , l RH3 , q RH1 , q RH2 , q RH3 ) (6) where l... is the length between the leg joints, and q… is the joint angle of the leg joints. According to the geometric structure relationship between the ends of the legs and the wheels, the offsets of the contact points of the four wheels with the ground relative to the positions of the ends of the robot legs are offset LFw , offset RFw , offset LHw , and offset RHw . Therefore, the positions x LFp , x RFp , x LHp , and x RHp of the contact points of the wheels with the ground relative to the fixed coordinate system are calculated. The specific calculation formulas are shown as follows in formulas (7)-(10): x LFp = x LF - offset LFw (7) x RFp = x RF - offset RFw (8) x LHp = x LH - offset LHw (9) x RHp = x RH - offset RHw (10) Combined with the information in the IMU and the contact situation between the contact point and the ground, the position of the foot end point of the quadruped wheeled-leg robot during swing or touchdown can be calculated; By kinematically calculating the Jacobian matrix between the leg joints and combining the joint angular velocity read by the joint encoder, the contribution of the leg to the contact point velocity of the quadruped wheeled-leg robot can be calculated. The specific calculation formulas are shown in the following formulas (11)-(14): Among them, is the joint angular velocity of the leg joint, and J is the Jacobian matrix; The angular velocity of the wheels can be obtained based on the information acquired by the joint encoders, and the rotational speeds of the four wheels can be calculated by combining with the radius of the wheels. The specific calculation formulas are shown as the following formulas (15)-(18): Among them, are the angular velocities of the four wheels respectively, and r w is the radius of the four wheels. Therefore, the velocity of the foot tip of the quadruped wheel-legged robot can be calculated, and the specific calculation formula is shown in the following equations (19)-(22): Furthermore, according to the touchdown information between the foot end of the wheeled-leg quadruped robot and the ground, the velocity of the foot end point of the quadruped wheeled-leg robot during swing or touchdown can be calculated.

7. The state evaluation system of a wheel-legged quadruped robot according to claim 6, characterized in that, The specific methods for predicting the current position and velocity of the robot body in the prediction module are as follows: First, calculate the acceleration of the quadruped wheeled-leg robot in the world coordinate system according to the IMU information, and the specific calculation formula is shown in the following formula (23): a world = R·a IMU (23) where a IMU is the acceleration directly output by the IMU; Then, based on the previous state x k-1 , the current control input u k and the current measurement z k , estimate the mean μ and covariance P of the Gaussian distribution of the state x k ; first, use the nonlinear discrete transition function f(·) to predict the state, and then correct it through the observation function h(·); the specific calculation formulas for the transition function and the observation function are as shown in the following formula (24): where, u k is the input, which is a definite quantity, namely the acceleration a of the quadruped wheel-leg robot in the world coordinate system world , x k-1 and x k represent the state quantities at times k-1 and k, as shown in the following equation (25): x k-1 = [p v q p w v w p1 p2 p3 p4 b f b ω T (25)​ where p and v are the velocities of the quadruped wheel-legged robot body respectively, q is the pose of the robot, p w and v w are the contributions of the wheel driving force to the position and velocity of the quadruped wheel-legged robot body respectively, p1, p2, p3, and p4 are the positions of the contact points between the wheels of the quadruped wheel-legged robot and the ground, b f and b ω are the accelerometer bias and the gyroscope bias respectively; Next, the optimal estimated value of f(x k-1 , u k , w k-1 ) at the k-1 moment and the predicted value x of h(x k , v k ) at the k moment k are Taylor-expanded at these points and high-order terms above the second order are omitted, resulting in the following formula (26): Among them, is the first-order partial derivative term obtained after expansion. During the expansion process, terms with higher degrees are omitted, and the state estimation equation is transformed into an approximate linearized equation; Finally, the prediction equation and prediction covariance matrix of the extended Kalman filter are shown in the following formula (27): Among them, is the first-order partial derivative term after Taylor expansion, is the expected value of the state quantity, P k is the covariance matrix of the system; Therefore, the optimal estimated value of the robot's body state obtained through the prediction equation and the previous prediction Obtain the current prediction data of the robot.

8. The state evaluation system of a wheel-legged quadruped robot according to claim 7, characterized in that The calculation process of the optimal estimated value of the current robot body state in the optimal estimation module is as follows: Perform a prior estimate on the last estimated state data through the prediction equation in formula (27) to obtain the predicted data of the robot at present. Then, calculate the last estimated error covariance matrix P k-1 to obtain the prior error covariance matrix The formula for calculating the Kalman gain is shown in the following formula (28): where R is the measurement covariance matrix, is the first-order partial derivative term after Taylor expansion; Perform posterior estimation, and the calculation formula is shown in the following formula (29): The calculation formula for updating the posterior covariance matrix is shown in the following formula (30):

9. A processing device, characterized in that, Comprising at least one processor and at least one memory communicatively connected to the processor, wherein: the memory stores program instructions executable by the processor, and the processor can execute the method according to any one of claims 1 to 4 by calling the program instructions.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores computer instructions, and the computer instructions cause the computer to execute the method according to any one of claims 1 to 4.

Images