A state estimation method for multi-modal motion of wheeled-legged robots

Abstract

The present invention discloses a state estimation method for multi-mode motion of wheeled-legged robots. The method comprises: constructing state equations and observation equations based on Kalman filter fusion of joint motor data, wheel motor data, and IMU data; wherein the observation equation calculates the accurate position and velocity of the wheel-ground contact point by introducing the complete kinematic model of the biased / unbiased wheeled-legged robot and the first-order differential kinematic model, and calculates the sliding displacement caused by the wheeled additional degree of freedom by constructing an asymmetric wheeled sliding motion model, thereby simultaneously considering the influence of the wheeled structure and the wheeled additional degree of freedom on the odometer update; and preventing the drift of the fuselage height estimation by introducing the assumption of reliable contact between the support phase and the wheel and the ground in the observation equation. The present invention can realize highly reliable state estimation of wheeled-legged robots in multiple modes such as wheeled, footed, and wheel-foot hybrid, solving the problem that the existing body state estimator cannot be applied to wheeled-legged robots.

Description

Technical Field

[0001] The present invention belongs to the field of robotics, and specifically is a state estimation method for multi-mode motion of a wheeled robot, and is especially used for state estimation of a wheeled robot under multi-mode motion such as foot-type, wheel-type, and wheel-foot hybrid motion. Background Art

[0002] Wheeled-legged robots, by combining the complex terrain navigability of legged robots with the high speed and high payload capacity of wheeled robots, have become the preferred platform for numerous mission scenarios, including disaster relief, military operations, and material transportation. When faced with complex environments, robots must constantly understand their location to rationally adjust and switch control strategies. Consequently, numerous researchers have conducted extensive research on state estimation for legged robots. Legged robot state estimation currently consists of two main approaches: body state estimation and state estimation that integrates vision and radar perception. Body state estimation relies solely on joint encoders and IMU data, while the latter further integrates vision and radar. While the latter offers higher estimation accuracy, it is an order of magnitude more expensive. Furthermore, for small legged robots, the significant added mass of radar and camera components often significantly impacts their mobility. Therefore, low-cost body state estimation offers greater universal applicability. The existing body state estimators of bipedal, quadrupedal, and multi-legged legged robots have been developed to a relatively mature level. However, with the introduction of wheeled structures into wheeled robots, the core assumption of no slippage of the supporting legs in the original body state estimators of legged robots is no longer valid. Therefore, the body state estimator of legged robots cannot be directly applied to wheeled robots. At the same time, a mature state estimation method for wheeled robots has not yet been formed. Therefore, it is urgent to propose a state estimator suitable for the multi-mode motion of wheeled robots, which provides an important prerequisite for fully utilizing the motion performance of wheeled robots.

[0003] Chinese Patent Publication No. CN117008623A discloses a state estimation method and system for multimodal perception of a legged robot. This method uses an extended Kalman filter to fuse IMU and joint encoder data, enabling prediction of legged robot states such as robot body position, body velocity, and body posture. However, this method assumes that the supporting leg does not slip, which does not conform to the wheel-end sliding motion characteristics of wheeled-legged robots and is therefore not applicable to wheeled-legged robots. Chinese Patent Publication No. CN108621161A discloses a state estimation method for a legged robot based on multi-sensor information fusion. This method fuses IMU and legged robot kinematic information through a Kalman filter and uses a foot-end force sensor to detect the ground contact state to prevent misjudgment of the supporting leg, improving the accuracy of legged robot state estimation. However, this method still assumes that the foot end of the supporting leg does not slip, making it unsuitable for wheeled-legged robots. Chinese publication number CN117008623A discloses a state estimation method and related equipment for a multi-legged robot. This method, based on a two-stage Kalman filter, further improves the accuracy of legged robot state estimation by fusing an IMU, joint encoders, and visual sensors. However, because it still does not consider the characteristics of wheeled motion, it cannot be directly applied to wheeled robots. In summary, the aforementioned legged state estimator, because it does not consider the characteristics of wheeled motion, cannot be applied to the multi-mode motion of wheeled robots, such as legged, wheeled, or mixed wheeled motion. Furthermore, there is currently no mature state estimation method for wheeled robots. Therefore, there is an urgent need for a state estimation method that can be applied to multiple motion modes of wheeled robots, fully leveraging the advantages of wheeled robots in combining legged obstacle surmounting capabilities with wheeled maneuverability. Summary of the Invention

[0004] The purpose of the present invention is to provide a state estimation method for multi-mode motion of wheel-legged robots, which can provide accurate state estimation results for wheel-legged robots in various motion modes such as foot-type, wheel-type, and wheel-foot mixed type, and solve the problem that the current state estimation methods cannot be applied to wheel-legged robots.

[0005] The technical solutions for achieving the purpose of the present invention are:

[0006] S1. Collect data from the joint motors, wheel motors, and IMU of the wheeled robot to obtain the robot joint positions and velocities, wheel motor positions and velocities, three-degree-of-freedom linear accelerations, three-degree-of-freedom rotational angular velocities, and three-degree-of-freedom attitude Euler angles of the robot.

[0007] S2. Based on the time-varying characteristics of the four-wheel position of the wheeled machine, an asymmetric wheeled sliding kinematic model is constructed, and the sliding displacement caused by the additional degree of freedom of the wheel is calculated based on the data described in S1;

[0008] S3. Construct a kinematic model and a first-order differential kinematic model of the biased / non-biased wheel-legged robot, and calculate the position of the wheel-ground contact point relative to the body and the speed of the wheel-ground contact point relative to the body based on the data in S1;

[0009] S4. Construct the discrete Kalman filter state equation and observation equation:

[0010] S4.1, the discrete Kalman filter state equation is as follows:

[0011] x k24×1 =A 24×24 x k-124×1 +B 24×3 u 3×1

[0012] x k24×1 is the current state vector, x k-124×1 is the state vector at the previous moment, A 24×24 is the system matrix, B 24×3 is the input matrix, u 3×1 is the input vector. The state vector is specifically p b is the fuselage position, v b is the body speed, p b-w The displacement of the fuselage caused by the rotation of the wheel, v b-w The speed of the fuselage caused by the rotation of the wheels, These are the wheel end positions corresponding to the four legs.

[0013] S4.2, the discrete Kalman filter observation equation is as follows:

[0014] y 31×1 =C 31×24 x 24×1

[0015] where y 31×1 is the observation vector, C 31×24 is the output matrix. The observation vector is specifically

[0016] are the positions of the wheel ends of the four legs relative to the fuselage, are the speeds of the wheel ends of the four legs relative to the fuselage (excluding the speed generated by the rotation of the wheels), v b-w is the measured value of the fuselage displacement caused by the rotation of the wheel, are the vertical positions of the wheel-ground contact points corresponding to the four legs. b-w The asymmetric wheel slip kinematic model is updated as described in step S2, The kinematic model of the biased / non-biased wheel-legged robot is calculated and updated as described in step S3. The first-order differential kinematics model of the biased / non-biased wheel-legged robot described in step S3 is calculated and updated.

[0017] S5. Initialize and iterate the discrete Kalman filter to output the estimated values ​​of the wheeled robot's body position and body velocity state.

[0018] Furthermore, the kinematic model of the asymmetric wheel slip in step S2 is shown as follows:

[0019]

[0020] Furthermore, the kinematic model of the biased / non-biased wheel-legged robot in step S3 is as follows:

[0021]

[0022] Furthermore, the first-order differential kinematic model of the biased / non-biased wheel-legged robot in step S3 is as follows:

[0023]

[0024] Furthermore, step S4 expands the observation vector y31 × 1 dimension, which can introduce the odometry output of external sensors such as cameras, lidar, RTK, and motion capture systems. The expanded observation vector is

[0025] where p bex 、v bex They are the external sensor odometer position output and speed output respectively.

[0026] Compared with the prior art, the present invention has the following significant advantages:

[0027] (1) Based on the Kalman filter, the present invention integrates the leg motion characteristics of a legged robot and the sliding motion characteristics of a wheeled robot to construct a wheeled-legged robot motion state estimator. This can provide accurate state estimation output for various motion modes of the wheeled-legged robot, such as foot-type, wheel-type, and wheel-leg hybrid motion modes. This solves the problem that the existing state estimation of legged robots cannot be directly transferred to wheeled-legged robots.

[0028] (2) Compared with the traditional four-wheeled vehicle kinematic model, the asymmetric wheeled sliding kinematic model proposed in this invention takes into account the time-varying characteristics of the wheel end position of the wheeled robot. It can calculate the body displacement contributed by wheeled motion when the wheeled robot's legs are in different positions and under different ground contact conditions. This solves the problem that the traditional four-wheeled vehicle kinematic model cannot be directly applied to the wheeled robot, and significantly improves the estimation accuracy of the displacement caused by wheeled sliding of the wheeled robot.

[0029] (3) The offset / non-offset wheel-legged robot kinematic model and first-order differential kinematic model proposed in the present invention further consider the characteristics of different wheel structures on the basis of the kinematics and first-order differential kinematic models of the leg-type robot. They can be applied to wheel-legged robots with offset wheel and non-offset wheel structures at the same time, and are more versatile. Compared with directly using the leg-type robot model, the accuracy of the leg end position and speed is greatly improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] Figure 1 An overview of the state estimation method for multi-modal motion of wheeled-legged robots;

[0031] Figure 2 Schematic diagram of the asymmetric wheel sliding kinematic model.

[0032] Figure 3 Schematic diagram of the kinematic model of offset / non-offset wheel-legged robot, (a) is the schematic diagram of the kinematic model of the offset wheel-legged robot, and (b) is the schematic diagram of the kinematic model of the non-offset wheel-legged robot.

[0033] In the figure: right front wheel 1, left front wheel 2, right rear wheel 3, left rear wheel 4, left virtual wheel 5, right virtual wheel 6, hip joint link 7, thigh link 8, calf link 9, hip joint motor 10, thigh motor 11, calf motor 12, and wheel axial section fitting circle 13. DETAILED DESCRIPTION

[0034] The present invention will be further described below with reference to the accompanying drawings and specific embodiments.

[0035] like Figure 1 FIG. 1 is an overview diagram of a multi-mode motion state estimation method for a wheeled robot according to the present invention, wherein the method comprises the following steps:

[0036] S1. Collect data from the joint motors, wheel motors, and IMU of the wheeled robot to obtain the robot joint positions and velocities, wheel motor positions and velocities, three-degree-of-freedom linear accelerations, three-degree-of-freedom rotational angular velocities, and three-degree-of-freedom attitude Euler angles of the robot.

[0037] S2. Based on the time-varying characteristics of the four-wheel position of the wheeled machine, an asymmetric wheeled sliding kinematic model is constructed, and the sliding displacement caused by the additional degree of freedom of the wheel is calculated based on the data described in S1;

[0038] S3. Construct a kinematic model and a first-order differential kinematic model of the biased / non-biased wheel-legged robot, and calculate the position of the wheel-ground contact point relative to the body and the speed of the wheel-ground contact point relative to the body based on the data in S1;

[0039] S4. Construct the discrete Kalman filter state equation and observation equation:

[0040] S4.1, the discrete Kalman filter state equation is as follows:

[0041] x k24×1 =A 24×24 x k124×1 +B 24×3 uu 3×1

[0042] x k24×1 is the current state vector, x k-124×1 is the state vector at the previous moment, A 24×24 is the system matrix, B 24×3 is the input matrix, u 3×1 is the input vector. The state vector is specifically p b is the fuselage position, v b is the body speed, p b-w The displacement of the fuselage caused by the rotation of the wheel, v b-w The speed of the fuselage caused by the rotation of the wheels, These are the wheel end positions corresponding to the four legs.

[0043] S4.2, the discrete Kalman filter observation equation is as follows:

[0044] y 31×1 =C 31×24 x 24×1

[0045] where y 31×1 is the observation vector, C 31×24 is the output matrix. The observation vector is specifically

[0046] are the positions of the wheel ends of the four legs relative to the fuselage, are the speeds of the wheel ends of the four legs relative to the fuselage (excluding the speed generated by the rotation of the wheels), v b-w is the measured value of the fuselage displacement caused by the rotation of the wheel, are the vertical positions of the wheel-ground contact points corresponding to the four legs. b-w The asymmetric wheel slip kinematic model is updated as described in step S2, The kinematic model of the biased / non-biased wheel-legged robot is calculated and updated as described in step S3. The first-order differential kinematics model of the biased / non-biased wheel-legged robot described in step S3 is calculated and updated.

[0047] S5. Initialize and iterate the discrete Kalman filter to output the estimated values ​​of the wheeled robot's body position and body velocity state.

[0048] Furthermore, the kinematic model of the asymmetric wheel slip in step S2 is shown as follows:

[0049]

[0050] like Figure 2 As shown, where v vl 、v vr are the linear velocities in the x direction of the left virtual wheel (5) and the y direction of the right virtual wheel (6), respectively. vl 、y vr The coordinates of the left virtual wheel (5) and the right virtual wheel (6) in the fuselage coordinate system {B}, v0, v1, v2, v3 are the x-direction linear velocities of the right front wheel (1), the left front wheel (2), the right rear wheel (3), and the left rear wheel (4), respectively. The linear velocities can be calculated by v i =ω i r is calculated, ω i is the wheel speed, r is the wheel radius, y0, y1, y2, y3 are the y-coordinates of the right front wheel (1), left front wheel (2), right rear wheel (3), and left rear wheel (4) in the fuselage coordinate system {B}.

[0051] Furthermore, the kinematic model of the biased / non-biased wheel-legged robot in step S3 is as follows:

[0052]

[0053] like Figure 3 As shown, l1, l2, l3 are the lengths of the hip joint link (7), thigh link (8), and calf link (9), respectively; θ1, θ2, θ3 are the angles of the hip joint motor (10), thigh motor (11), and calf motor joint (12), respectively; b is the distance between the wheel center and the end of the calf; r f is the radius of the wheel axial section fitting circle (13), and d is the distance between the center of the wheel axial section fitting circle (13) and the center of the wheel.

[0054] Furthermore, the first-order differential kinematic model of the biased / non-biased wheel-legged robot in step S3 is as follows:

[0055]

[0056] Furthermore, in step S4.2 When the corresponding leg is a supporting leg, it is set to 0, that is, the height of the supporting leg wheel end is 0 in the world system.

[0057] Furthermore, the iterative calculation process of the discrete Kalman filter in step S5 is shown as follows:

[0058]

[0059] in is the prior state vector at the current moment, is the posterior state vector at the previous moment, is the prior covariance matrix of the state vector at the current moment, The posterior covariance matrix of the state vector at the previous moment, is the posterior covariance matrix of the state vector at the current moment, K k24×24 is the Kalman filter gain matrix, Q 24×24 is the process noise covariance matrix, R 31×31 is the measurement noise covariance matrix.

[0060] Furthermore, in order to reduce the contribution of the swing leg kinematic information, during the discrete Kalman filter iterative calculation process in step S5, Q 24×24 、R 31×31 The covariance part of the matrix corresponding to the swing leg is set to the maximum value, for example, assuming Q 24×24 、R 31×31 The covariance block matrix corresponding to the swing leg in the matrix is ​​X1, so X1 is set as follows:

[0061]

[0062] Among them, init is Q 24×24 、R 31×31 The value set during initialization, ratio is the phase progress ratio of the supporting leg.

[0063] Furthermore, considering the large wheel-ground contact impact and the large noise signals, R corresponding to the vertical height position of the wheel-ground contact point 31×31 The initial value is set to a larger value, such as 1.

[0064] Furthermore, the step S4 is performed by expanding the observation vector y 31×1dimension, external odometer outputs such as cameras, lidar, RTK, and motion capture systems can be introduced, such as the T265 camera odometer output. The expanded observation vector is

[0065] where p bex 、v bex are the position output and speed output of the external sensor odometer, respectively. Accordingly, the vector and matrix dimensions involved in the iterative calculation process of the discrete Kalman filter in step S5 will also change, as shown in the following formula:

[0066]

Claims

1. A state estimation method for multi-mode motion of a wheeled robot, characterized in that: include: S1. Collect data from the joint motors, wheel motors, and IMU of the wheeled robot to obtain the robot joint positions and velocities, wheel motor positions and velocities, three-degree-of-freedom linear accelerations, three-degree-of-freedom rotational angular velocities, and three-degree-of-freedom attitude Euler angles of the robot. S2. Based on the time-varying characteristics of the four-wheel position of the wheeled robot, an asymmetric wheeled sliding kinematic model is constructed. Based on the data described in S1, the sliding displacement caused by the additional degree of freedom of the wheel is calculated to solve the problem that the kinematic model of the traditional four-wheeled vehicle cannot be directly applied to the wheeled robot; S3. Construct a kinematic model and a first-order differential kinematic model of the biased / non-biased wheel-legged robot. Based on the data in S1, calculate the position of the wheel-ground contact point relative to the body and the speed of the wheel-ground contact point relative to the body, thereby improving the accuracy of leg end position and speed calculation. S4. Construct the discrete Kalman filter state equation and observation equation: S4.1, the discrete Kalman filter state equation is shown in equation (a): x k24×1 =A 24×24 x k-124×1 +B 24×3 u 3×1 (a) x k24×1 is the current state vector, x k-124×1 is the state vector at the previous moment, A 24×24 is the system matrix, B 24×3 is the input matrix, u 3×1 is the input vector; the state vector is specifically p b is the fuselage position, υ b is the body speed, p b-w The fuselage displacement caused by the rotation of the wheels, υ b-w The speed of the fuselage caused by the rotation of the wheels, are the wheel end positions corresponding to the four legs; S4.2, the discrete Kalman filter observation equation is shown in equation (b): y 31×1 =C 31×24 x 24×1 (b) where y 31×1 is the observation vector, C 31×24 is the output matrix; the observation vector is specifically are the positions of the wheel ends of the four legs relative to the fuselage, are the speeds of the wheel ends of the four legs relative to the fuselage, excluding the speed generated by the rotation of the wheels, υ b-w is the measured value of the fuselage displacement caused by the rotation of the wheel, are the vertical height positions of the wheel-ground contact points corresponding to the four legs; b-w The asymmetric wheel slip kinematic model is updated as described in step S2, The kinematic model of the biased / non-biased wheel-legged robot is calculated and updated as described in step S3. The first-order differential kinematic model of the biased / non-biased wheel-legged robot described in step S3 is calculated and updated; S5. Initialize and iterate the discrete Kalman filter to output the estimated values ​​of the wheeled robot's body position and body velocity state.

2. A state estimation method for multi-mode motion of a wheeled robot according to claim 1, characterized in that: The kinematic model of the asymmetric wheel slip in step S2 is shown in formula (c): where υ vl 、υ vr are the linear velocities in the x direction of the left virtual wheel (5) and the y direction of the right virtual wheel (6), respectively. v1 、y vr The coordinates of the left virtual wheel (5) and the right virtual wheel (6) in the fuselage coordinate system, υ0, υ1, υ2, υ3 are the x-direction linear velocities of the right front wheel (1), the left front wheel (2), the right rear wheel (3), and the left rear wheel (4), respectively. The linear velocities can be calculated by υ i =ω i r is calculated, ω i is the wheel speed, r is the wheel radius, y0, y1, y2, y3 are the y-coordinates of the right front wheel (1), left front wheel (2), right rear wheel (3), and left rear wheel (4) in the fuselage coordinate system.

3. A state estimation method for multi-mode motion of a wheeled robot according to claim 2, characterized in that: The kinematic model of the biased / non-biased wheel-legged robot in step S3 is shown in formula (d): Where l1, l2, l3 are the lengths of the hip joint link (7), thigh link (8), and calf link (9), respectively; θ1, θ2, θ3 are the angles of the hip joint motor (10), thigh motor (11), and calf motor joint (12), respectively; b is the distance between the wheel center and the end of the calf; r f is the radius of the wheel axial section fitting circle (13), and d is the distance between the center of the wheel axial section fitting circle (13) and the center of the wheel.

4. A state estimation method for multi-mode motion of a wheeled robot according to claim 3, characterized in that: The first-order differential kinematic model of the biased / non-biased wheel-legged robot in step S3 is shown in formula (e):

5. The state estimation method for multi-mode motion of a wheeled robot according to claim 1, characterized in that: In step S4.2 When the corresponding leg is a supporting leg, it is set to 0, that is, the height of the supporting leg wheel end is 0 in the world system.

6. The state estimation method for multi-mode motion of a wheeled robot according to claim 1, characterized in that: The iterative calculation process of the discrete Kalman filter in step S5 is shown in formula (f): in is the prior state vector at the current moment, is the posterior state vector at the previous moment, is the prior covariance matrix of the state vector at the current moment, The posterior covariance matrix of the state vector at the previous moment, is the posterior covariance matrix of the state vector at the current moment, K k24×24 is the Kalman filter gain matrix, Q 24×24 is the process noise covariance matrix, R 31×31 is the measurement noise covariance matrix.

7. The discrete Kalman filter iterative calculation process according to claim 6, characterized in that: In order to reduce the contribution of the swing leg kinematic information, Q 24×24 、R 31×31 The covariance portion of the matrix corresponding to the swing leg is set to a maximum value.

8. The discrete Kalman filter iterative calculation process according to claim 6, characterized in that: Considering the large wheel-ground contact impact and the large noise signals, R corresponding to the vertical height position of the wheel-ground contact point 31×31 part, the initial value is set to a larger value.

9. covariance matrix setting method according to claim 7, is characterized in that, The swing leg and the supporting leg can be determined by a method based on a time phase process, a method based on a foot end force sensor, or a method based on joint torque estimation.

10. The state estimation method for multi-mode motion of a wheeled robot according to claim 1, characterized in that: Step S4 is to expand the observation vector y 31×1 dimension, which can introduce the odometer output of external sensors such as cameras, lidar, RTK, and motion capture systems. The extended observation vector is where p bex 、υ bex They are the external sensor odometer position output and speed output respectively.

Images