State estimation method for multi-mode motion of wheel-foot robot

Abstract

The invention discloses a state estimation method for multi-mode motion of a wheel-foot robot. The method comprises the steps that joint motor data, wheel type motor data and IMU data are fused based on a Kalman filter to construct a state equation and an observation equation; wherein the observation equation calculates the accurate position and speed of a wheel-ground contact point by introducing a complete kinematics model and a first-order differential kinematics model of the biased / non-biased wheel-foot robot, and calculates the sliding displacement caused by the wheel-type additional degree of freedom by constructing an asymmetric wheel-type sliding motion model; therefore, the influence of the wheel type structure and the wheel type additional degree of freedom on speedometer updating is considered at the same time; and by introducing a reliable contact hypothesis of supporting phase wheels into an observation equation, fuselage height estimation drift is prevented. According to the invention, high-reliability state estimation of the wheel-foot robot can be realized in wheel-type, foot-type, wheel-foot hybrid and other modes, and the problem that the existing body state estimator cannot be suitable for the wheel-foot robot is solved.

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-legged robot, and is especially used for state estimation of a wheeled-legged robot under multi-mode motions such as foot-type, wheel-type, and wheel-leg hybrid types. Background Art

[0002] Wheeled-legged robots have become the preferred platform for many mission scenarios such as disaster relief, military operations, and material transportation, thanks to their ability to pass through complex terrains of legged robots and their high-speed and large-load capacity of wheeled robots. When facing a complex environment, robots need to know where they are at all times in order to reasonably adjust and switch control strategies. Therefore, many scholars have conducted extensive research on the state estimation of legged robots. The state estimation of legged robots is currently mainly divided into body state estimation and state estimation that integrates vision and radar perception. Body state estimation is only achieved by joint encoders and IMU data, while the latter is achieved by further integrating vision and radar on the basis of body state estimation. Although the latter has higher estimation accuracy, its cost is one order of magnitude higher. At the same time, for small legged robots, the large additional mass of radars and cameras often has a greater impact on the movement ability of legged robots. Therefore, low-cost body state estimation has stronger universality. The existing bipedal, quadrupedal, and multi-legged legged robots' body state estimators have been relatively mature. However, with the introduction of wheeled structures into wheeled robots, the core assumption that the supporting legs do not slip in the original leg state estimators 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 number CN117008623A discloses a state estimation method and system for multimodal perception of a legged robot. This method is based on the extended Kalman filter to fuse IMU and joint encoder data, and can realize the prediction of the state of the legged robot, such as the robot body position, body speed, and body posture. However, this method assumes that the supporting foot does not slip, which does not conform to the wheel-end sliding motion characteristics of a wheel-legged robot and cannot be applied to a wheel-legged robot. Chinese patent publication number CN108621161A discloses a state estimation method for a legged robot body based on multi-sensor information fusion. This method fuses IMU and legged robot kinematic information through Kalman filtering, and uses a foot-end force sensor to detect the ground contact state to prevent misjudgment of the supporting leg, thereby improving the accuracy of the state estimation of the legged robot. However, this method still assumes that the foot end of the supporting leg does not slip, so it cannot be applied to a wheel-legged robot. China Publication No. CN117008623A discloses a state estimation method and related equipment for a multi-legged robot. The method is based on a two-stage Kalman filter and further improves the accuracy of the state estimation of the legged robot by fusing IMU, joint encoders and visual sensors. However, since the wheeled motion characteristics are still not considered, it cannot be directly applied to the wheeled robot. In summary, the above-mentioned legged state estimator cannot be applied to the multi-mode motion of the wheeled robot, such as legged, wheeled and wheeled mixed, because it does not consider the wheeled motion characteristics. At present, there is 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, so as to give full play to the advantages of wheeled robots in combining the footed obstacle crossing ability and 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 wheeled-legged robots, which can provide accurate state estimation results for wheeled-legged robots in various motion modes such as foot-type, wheel-type, and wheel-foot mixed type, thereby solving the problem that the current existing state estimation methods cannot be applied to wheeled-legged robots.

[0005] The technical solution to achieve the purpose of the present invention is:

[0006] S1, collect the data of the wheel-foot robot joint motor, wheel motor and IMU, obtain the robot joint position and speed, wheel motor position and speed, three-degree-of-freedom linear acceleration of the fuselage, three-degree-of-freedom rotation angular velocity of the fuselage, and three-degree-of-freedom attitude Euler angle of the fuselage;

[0007] S2. According to 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, constructing a kinematic model and a first-order differential kinematic model of a biased / non-biased wheel-foot robot, and calculating the position of the wheel-ground contact point relative to the fuselage and the speed of the wheel-ground contact point relative to the fuselage based on the data in S1;

[0009] S4. Construct 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 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 wheels, v b-w The speed of the fuselage caused by the rotation of the wheels, They are the wheel end positions corresponding to the four legs respectively.

[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

[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 wheels, 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 wheeled robot is calculated and updated as described in step S3, The first-order differential kinematics model of the biased / non-biased wheeled-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 sliding in step S2 is as follows:

[0019]

[0020] Furthermore, the kinematic model of the offset / non-offset wheel-foot robot in step S3 is as shown below:

[0021]

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

[0023]

[0024] Furthermore, step S4 expands the observation vector y31 × 1 dimension, which can introduce the odometer 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 the foot-type robot and the sliding motion characteristics of the wheel-type robot, and constructs a wheel-legged robot motion state estimator, which can provide accurate state estimation output for various motion modes of the wheel-legged robot, such as foot-type, wheel-type, and wheel-foot hybrid, thus solving the problem that the existing state estimation of foot-type robots cannot be directly transferred to wheel-legged robots;

[0028] (2) Compared with the traditional four-wheel vehicle motion model, the asymmetric wheeled sliding kinematic model proposed in the present invention takes into account the time-varying characteristics of the wheel end position of the wheeled robot, and can calculate the body displacement contributed by the wheeled motion when the wheeled robot's legs are in different positions and under different ground contact conditions, thus solving the problem that the traditional four-wheel vehicle kinematic model cannot be directly applied to the wheeled robot, and significantly improving 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 model and first-order differential kinematic model of the legged robot. They can be applied to wheel-legged robots with offset wheel and non-offset wheel structures at the same time, and have stronger versatility. Compared with directly using the legged 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-mode 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 an offset / non-offset wheel-legged robot. (a) is a schematic diagram of the kinematic model of an offset wheel-legged robot, and (b) is a schematic diagram of the kinematic model of a 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 connecting rod 7, thigh connecting rod 8, calf connecting rod 9, hip joint motor 10, thigh motor 11, calf motor 12, wheel axial section fitting circle 13. DETAILED DESCRIPTION

[0034] The present invention is further described below in conjunction with 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, and the method comprises the following steps:

[0036] S1, collect the data of the wheel-foot robot joint motor, wheel motor and IMU, obtain the robot joint position and speed, wheel motor position and speed, three-degree-of-freedom linear acceleration of the fuselage, three-degree-of-freedom rotation angular velocity of the fuselage, and three-degree-of-freedom attitude Euler angle of the fuselage;

[0037] S2. According to 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, constructing a kinematic model and a first-order differential kinematic model of a biased / non-biased wheel-foot robot, and calculating the position of the wheel-ground contact point relative to the fuselage and the speed of the wheel-ground contact point relative to the fuselage based on the data in S1;

[0039] S4. Construct 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 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 wheels, v b-w The speed of the fuselage caused by the rotation of the wheels, They are the wheel end positions corresponding to the four legs respectively.

[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

[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 wheels, 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 wheeled robot is calculated and updated as described in step S3, The first-order differential kinematics model of the biased / non-biased wheeled-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 sliding in step S2 is 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, and 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), the left front wheel (2), the right rear wheel (3), and the left rear wheel (4) in the fuselage coordinate system {B}.

[0051] Furthermore, the kinematic model of the offset / non-offset wheel-foot 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), θ1, θ2, θ3 are the angles of the hip joint motor (10), thigh motor (11), and calf motor joint (12), b is the distance between the wheel center and the end of the calf, and r f is the radius of the wheel axial cross-section fitting circle (13), and d is the distance between the center of the wheel axial cross-section fitting circle (13) and the center of the wheel.

[0054] Furthermore, the first-order differential kinematic model of the biased / non-biased wheeled 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 in the following formula:

[0058]

[0059] in is the a priori 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, and 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 signal, 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, step S4 expands the observation vector y 31×1dimension, external odometer outputs such as cameras, lidar, RTK, and motion capture systems can be introduced, such as the odometer output of the T265 camera. The expanded observation vector is

[0065] where p bex 、v bex They 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 the data of the wheel-foot robot joint motor, wheel motor and IMU, obtain the robot joint position and speed, wheel motor position and speed, three-degree-of-freedom linear acceleration of the fuselage, three-degree-of-freedom rotation angular velocity of the fuselage, and three-degree-of-freedom attitude Euler angle of the fuselage; S2. According to 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 used for the wheeled robot. S3, constructing a kinematic model and a first-order differential kinematic model of a biased / non-biased wheel-legged robot, and calculating the position of the wheel-ground contact point relative to the fuselage and the speed of the wheel-ground contact point relative to the fuselage based on the data in S1, so as to improve the calculation accuracy of the position and speed of the leg end; S4. Construct discrete Kalman filter state equation and observation equation: S4.1, the discrete Kalman filter state equation is shown in formula (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 p b is the fuselage position, υ b is the body speed, p b-w The displacement of the fuselage caused by the rotation of the wheels, υ b-w The speed of the fuselage caused by the rotation of the wheels, They are the wheel end positions corresponding to the four legs respectively. 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 wheels, 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 wheeled robot is calculated and updated as described in step S3, The first-order differential kinematics model of the biased / non-biased wheeled-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 sliding 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), the left front wheel (2), the right rear wheel (3), and the left rear wheel (4) in the fuselage coordinate system.

3. The state estimation method for multi-mode motion of a wheeled robot according to claim 1, characterized in that: The kinematic model of the biased / non-biased wheeled 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 cross-section fitting circle (13), and d is the distance between the center of the wheel axial cross-section fitting circle (13) and the center of the wheel.

4. The state estimation method for multi-mode motion of a wheeled robot according to claim 1, characterized in that: The first-order differential kinematic model of the biased / non-biased wheeled 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 a priori 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, such as 100.

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 signal, R corresponding to the vertical height position of the wheel-ground contact point 31×31 The initial value of part is set to a larger value, such as 1.

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

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

Images