Snakelike robot pose estimation method based on double IMUs (Inertial Measurement Units)

Abstract

The invention discloses a snakelike robot pose estimation method based on double IMUs, and belongs to the technical field of robots, and the scheme comprises the following steps: S1, obtaining two groups of three-axis acceleration information and three-axis angular velocity information of an IMU1 located at the head and an IMU2 located at the tail in real time; s2, calculating displacement and angle information of the IMU1 and the IMU2; s3, the position where the IMU1 is located serves as the origin of a robot coordinate system, and a coordinate mapping matrix is calculated; s4, estimating all joint angles according to the information of the two IMUs; and S5, the pose of the whole robot is obtained according to the joint angles. The two IMUs installed on the head connecting rod and the tail connecting rod of the robot are utilized, the structure is simplified, the calculated amount is reduced, stable and accurate pose estimation is achieved in combination with Kalman filtering, and the real-time requirement of SLAM is met.

Description

Technical Field

[0001] The present invention belongs to the technical field of robots, and in particular relates to a snake-like robot posture estimation method based on dual IMUs. Background Art

[0002] Snake-like robots are a classic application of bionic principles in robotics. Snakes, with their unique limbless locomotion and highly flexible body structure, demonstrate remarkable adaptability in complex environments. By mimicking the movement patterns and anatomy of snakes, snake-like robots have been designed with advantages such as a rational structure, flexible control, reliable performance, and strong scalability. They can perform movements that wheeled carts cannot, adapting to a wider range of exploration scenarios.

[0003] The snake-like robot can be simplified into a joint and connecting rod structure. The joints are composed of servos. The input control quantity of the robot is the angle of the servo of each joint. However, due to the accuracy of the servo, the error between the theoretical value and the actual value will accumulate with the joint. The error near the tail is already very large and cannot be used as a reference value for its posture estimation. It is best to accurately obtain the actual rotation angle of each joint servo in order to better estimate the posture of such a robot with high degrees of freedom. However, if an inertial measurement unit (IMU) is installed on each connecting rod, the communication wiring is more complicated, and the dimension of the fusion information to be processed is high, the amount of calculation is large, and it cannot meet the real-time requirements of SLAM (Simultaneous Localization and Mapping). Summary of the Invention

[0004] To solve the above problems, the present invention discloses a snake-like robot pose estimation method based on dual IMUs, which utilizes two IMUs installed on the head link and tail link of the robot and combines Kalman filtering to achieve stable and accurate pose estimation.

[0005] To achieve the above object, the technical solution of the present invention is as follows:

[0006] A snake-like robot based on dual IMUs includes n joints, n+1 links, and IMU1 and IMU2. The links are arranged in ascending order according to n, and adjacent links are connected by joints. Link 0 is the head, and link n is the tail. IMU1 and IMU2 are installed on the head link and the tail link, respectively, and both output three-axis acceleration information and three-axis angular velocity information, respectively.

[0007] A snake-like robot pose estimation method based on dual IMUs includes the following steps:

[0008] S1, real-time acquisition of two sets of three-axis acceleration information and three-axis angular velocity information of IMU1 located at the head and IMU2 located at the tail;

[0009] S2. Double-integrate and single-integrate the original three-axis acceleration data and three-axis angular velocity data of IMU1 and IMU2 respectively to obtain displacement and rotation angle;

[0010] S3, take the position of IMU1 as the origin, and establish the robot coordinate system from IMU1 along the direction of link 0 pointing to joint 1 as the positive direction of x-axis, and use q1 and As the displacement component and rotation component of the coordinate transformation matrix T between the robot coordinate system and the world coordinate system.

[0011] S4. Use the difference between the two IMU data as the rotation angle and angular velocity information of the robot's tail in the robot coordinate system as the observation value, perform kinematic modeling based on the robot structure, and solve the rotation angle of each joint through Kalman filtering.

[0012] S5. Calculate the coordinates of each joint point in the robot coordinate system based on the inferred rotation angle and the structural characteristics of the robot, and obtain its coordinates in the world coordinate system through the coordinate transformation matrix T to obtain the robot posture.

[0013] The details are as follows:

[0014] S1, real-time acquisition of two sets of three-axis acceleration information and three-axis angular velocity information from IMU1 located at the head and IMU2 located at the tail. They are recorded as:

[0015]

[0016] where a 1 and a 2 Represents the acceleration vectors measured by IMU1 and IMU2, including the components of the x, y, and z axes; ω 1 and ω 2 They represent the angular velocity vectors measured by IMU1 and IMU2 respectively, and also include the components of the x, y, and z axes.

[0017] S2. Double-integrate and single-integrate the original three-axis acceleration data and three-axis angular velocity data of IMU1 and IMU2 to obtain displacement and rotation angle

[0018] Double integration of the acceleration information yields the displacement:

[0019] q 1 =∫∫(a 1 -g)dt

[0020] q 2 =∫∫(a2 -g)dt

[0021] where q 1 and q 2 Respectively represent the displacement information obtained by IMU1 and IMU2, including the components of the x, y, and z axes, and g represents the acceleration of gravity;

[0022] Integrating the angular velocity gives the angle:

[0023]

[0024] in and They represent the angle information calculated by IMU1 and IMU2, including the components of the x, y, and z axes.

[0025] Since the IMU data is discretized after sampling, further discretization processing is required, so the integral should be written in summation form.

[0026] S3, take the position of IMU1 as the origin, and establish the robot coordinate system from IMU1 along the direction of link 0 pointing to joint 1 as the positive direction of x-axis, and use q1 and As the displacement component and rotation component of the coordinate transformation matrix T between the robot coordinate system and the world coordinate system.

[0027] Assume that the coordinates of the same point in the robot coordinate system are e and in the world coordinate system are h, then it should satisfy:

[0028] h=Te

[0029] The robot coordinate system coordinates of each joint point are calculated separately in the robot coordinate system, and then the coordinates in the world coordinate system can be obtained through the coordinate transformation matrix T. Finally, the calculated joint coordinates are converted to calculate the coordinates of each joint angle.

[0030] S4. Use the difference between the two IMU data as the rotation angle and angular velocity information of the robot's tail in the robot coordinate system as the observation value, perform kinematic modeling based on the robot structure, and solve the rotation angle of each joint through Kalman filtering.

[0031] Assume that the joint angle and angular velocity corresponding to joint i are θ i 、ω i (i ranges from 0 to n).

[0032] The state variable x is defined as:

[0033] x=[θ 1 ,θ 2 ...,θ n ,ω 1 ,ω 2...,ω n ] T

[0034] According to the recursive relationship, we get the equation system:

[0035]

[0036] Wherein, Δt represents the time interval between two consecutive samplings; represents the ideal angle control function f given to joint i n The sampling value of the second derivative of (t) at the k-1th step; and Represents θ respectively i With ω i The predicted value at step k; represents the process noise when the angle state of joint i is transferred from the k-1th step to the kth step; Represents the process noise when the angular velocity state of joint i is transferred from the k-1th step to the kth step.

[0037] Writing it in matrix form, we get the state transfer equation:

[0038] x k =Ax k-1 +Bu k +w k

[0039] The state transfer matrix A is:

[0040]

[0041] The control input matrix B is:

[0042]

[0043] Control input vector u k for:

[0044]

[0045] Process noise vector w k for:

[0046]

[0047] Let its covariance matrix be Q, according to the definition of covariance matrix:

[0048]

[0049] Where E(·) represents the expectation.

[0050] for For (i from 1 to n), it represents the process noise when the angle state of joint i is transferred from step k-1 to step k. All i from 1 to n correspond to are independent of each other, and their expectation is 0 and their variance is sqare(σ i ), in order to distinguish it from the superscript, sqare(·) is used to represent the square operation.

[0051]

[0052] Where i ranges from 1 to n, and Var(·) represents the variance.

[0053] Likewise, for (i from 1 to n), all i from 1 to n corresponding are independent of each other, and their expectation is 0. Let the variance of the error caused by the servo accuracy be sqare(σ n+i );Right now

[0054]

[0055] Where i ranges from 1 to n.

[0056] because represents the process noise when the angular velocity state of joint i is transferred from the k-1th step to the kth step, represents the process noise when the angle state of joint i is transferred from the k-1th step to the kth step, and They are also independent of each other.

[0057] So we get vector w k Each component in is independent of each other, and its variance is known, so the covariance matrix Q is a diagonal matrix, and according to the formula:

[0058] Var(x)=E(x 2 )-[E(x)] 2

[0059] The elements on the diagonal of the covariance matrix are:

[0060]

[0061] Where i ranges from 1 to n.

[0062] Therefore, the covariance matrix Q satisfies:

[0063]

[0064] Where diag(a,b...c) represents a diagonal matrix with a,b...,c as diagonal elements.

[0065] The observation model consists of the angular velocity data output by IMU2 and IMU1, the difference between the processed angle data, and all control input functions, specifically:

[0066] z=[θ,ω,f 1 ,f 2 ...f n ] T

[0067] in:

[0068]

[0069] ω=ω 2 -ω 1

[0070] It means taking the measurement data of IMU2 minus the measurement data of IMU1, which reflects the movement angle of the robot's tail in the robot coordinate system.

[0071] Use the control function f i This ideal angle is used as a pseudo-observation to limit the range of each joint angle so that it does not deviate too much.

[0072] According to the connecting rod structure, the relationship between the observation value z and each joint angle and joint angular velocity is known as:

[0073]

[0074] where θ k With ω k Represent the sampling values ​​of θ and ω at the kth step, Indicates the cumulative error when the angular velocity is converted to an angle at step k and the error when the sum of all the previous servo angles is used as the tail angle. It represents the angular velocity measurement error of the IMU itself and the error of using the sum of all the previous servo angles as the tail angle, that is:

[0075]

[0076] in and They represent the cumulative error when the k-th step angular velocity is converted into angle and the angular velocity measurement error of the IMU itself. However, in essence, the IMU attitude angle is considered to be θ k =ω k-1 ·Δt+θ k-1 ,, if the measurement variance of IMU is The expectation is 0, and the following relationship is obtained:

[0077]

[0078] Indicates the error caused by using the input angle function of joint i as the actual angle of joint i, which should be consistent with Regarding:

[0079]

[0080] So the observation equation is:

[0081] z k =Hx k +v k

[0082] The observation matrix H is:

[0083]

[0084] Observation noise vector v k for:

[0085]

[0086] Let its covariance matrix be R:

[0087]

[0088] After expansion, it can also be divided into cross terms and square terms. and independent, and independent, are independent of each other, but and are not independent, so there are only two cross terms: The rest are all 0. The square term is the variance of each error variable.

[0089] again:

[0090]

[0091] but:

[0092]

[0093] According to the five equations of the Kalman filter process [1]

[0094] That is, the state prediction equation:

[0095]

[0096] Covariance prediction equation:

[0097]

[0098] Kalman gain calculation formula:

[0099]

[0100] Status Update:

[0101]

[0102] Covariance update:

[0103] P k|k =(IK k H)P k|k-1

[0104] in represents the posterior state estimator at the k-1th step; represents the prior state estimator of the kth step; P k-1|k-1 is the posterior state estimation error covariance matrix of the k-1th step; P k|k-1 K is the prior state estimation error covariance matrix of the k-th step; k is the Kalman gain at step k; represents the posterior state estimator of the kth step; P k|k represents the covariance matrix of the posterior state estimation error at the k-th step, and I is the identity matrix.

[0105] S5. Calculate the coordinates of each joint point in the robot coordinate system based on the inferred rotation angle and the structural characteristics of the robot, and obtain its coordinates in the world coordinate system through the coordinate transformation matrix T to obtain the robot posture.

[0106] For the two-dimensional model, the angular velocity and angle are only taken in the yaw angle direction. The more accurate joint yaw angle θ is obtained by using the above Kalman filter modeling solution. i (i goes from 1 to n), so do the transformation:

[0107]

[0108] Get the angle Θ of joint i relative to the positive direction of the x-axis i .

[0109] Assume the length of connecting rod i is l i (i ranges from 0 to n), the offset from IMU1 to joint 1 is b = [b x ,b y ] T . Let the robot coordinate system coordinate of joint i be p i =[x i ,y i ] T , then there is a recursive formula:

[0110]

[0111] Then, the coordinates of the joint in the world coordinate system are obtained according to the coordinate transformation matrix T:

[0112] p′ i =Tp i

[0113] If it is a three-dimensional model, each joint should contain six-dimensional information, namely the angle of the Euler angle in three directions and the angular acceleration. In the same way, the joint angle of each joint is estimated through the tail IMU combined with Kalman filtering. At this time, the joint coordinate estimation is solved using quaternion solution, etc.

[0114] The beneficial effects of the present invention are:

[0115] The present invention utilizes two IMUs installed on the head link and tail link of the robot, simplifies the structure, reduces the amount of calculation, and combines Kalman filtering to achieve stable and accurate pose estimation, meeting the real-time requirements of SLAM. BRIEF DESCRIPTION OF THE DRAWINGS

[0116] Figure 1 is a process flow chart of the present invention;

[0117] Figure 2 Schematic diagram of the joint-connecting rod structure of the snake-like robot of the present invention;

[0118] Figure 3 Schematic diagram of the coordinate system of the snake-like robot of the present invention;

[0119] Figure 4 Schematic diagram of the joint angles of the snake-like robot of the present invention; DETAILED DESCRIPTION

[0120] The present invention will be further described below with reference to the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are only used to illustrate the present invention and are not used to limit the scope of the present invention.

[0121] Before describing the specific solution, we will model the structure of the snake robot:

[0122] like Figure 2 As shown in the figure, the snake-like robot can be simplified into n joints, n+1 link structures and two IMUs, which are arranged in order from small to large according to n, and the link i-1 is connected to the link i through the movable joint i (i ranges from 1 to n), the link 0 is the head, and the link n is the tail. IMU1 and IMU2 are installed on the head link and the tail link respectively, and the two output three-axis acceleration information and three-axis angular velocity information respectively.

[0123] First, determine the number of joints n based on the robot structure used. All occurrences of i below represent descriptions of joint i, and 1≤i≤n. Set an appropriate sampling period Δt based on the selected servo and IMU model.

[0124] After the robot is powered on, the MCU (Microcontroller Unit) reads the initial data from the IMU1 mounted on the head and the IMU2 mounted on the tail, and performs self-calibration. At this point, the angles and displacements, as well as the angular and velocity values ​​of the IMU1 and IMU2 are assumed to be zero. After calibration, each joint is activated to move, and the three-axis acceleration and angular velocity values ​​of the IMU1 and IMU2 are collected in real time:

[0125]

[0126] where a 1 and a 2 Represents the acceleration vectors measured by IMU1 and IMU2, including the components of the x, y, and z axes; ω 1 and ω 2 They represent the angular velocity vectors measured by IMU1 and IMU2 respectively, and also include the components of the x, y, and z axes.

[0127] Solve it:

[0128] q 1 =∫∫(a 1 -g)dt

[0129] q 2 =∫∫(a 2 -g)dt

[0130]

[0131] where q 1 and q 2 Respectively represent the displacement information obtained by IMU1 and IMU2, including the components of the x, y, and z axes; and The x, y, and z axes are the angles calculated by IMU1 and IMU2, respectively. g represents the acceleration due to gravity. Since the velocity measured by the IMU is a specific acceleration, g needs to be added as a correction.

[0132] In particular, for engineering applications, due to the discretization of digital signals, the integral operation should be replaced by summation, that is:

[0133]

[0134] in, and Represents the k-th sampling value (k-th step) of the three-axis acceleration vector of IMU1 and IMU2 respectively; and Represents the k-th sampling value of the three-axis angular velocity vector of IMU1 and IMU2 respectively.

[0135] and Respectively indicate through and The obtained velocities of IMU1 and IMU2 at the kth sampling time; and Respectively indicate through and The displacements of IMU1 and IMU2 at the kth sampling time are obtained; and Respectively indicate through and The obtained offset angles of IMU1 and IMU2 at the kth sampling time.

[0136] Here we only demonstrate the simplest IMU solution. In fact, in order to ensure the stability of the IMU, the solved data should be filtered.

[0137] The robot coordinate system is established with the IMU1 located on the head as the origin. If the coordinate of the same point in the robot coordinate system is e and the coordinate in the world coordinate system is h, then it should satisfy:

[0138] h=Te

[0139] The rotation component of the coordinate transformation matrix T can be obtained by The displacement component can be obtained by get.

[0140] Next, find all the matrices used in Kalman filtering.

[0141] For the state prediction equation:

[0142]

[0143] in, represents the posterior state estimator at the k-1th step; Represents the prior state estimator of the kth step, and needs to define the quantity to be estimated

[0144]

[0145] A is the state transfer matrix, and it can be seen from the modeling that:

[0146]

[0147] B is the control input matrix B, and it can be seen from the modeling that:

[0148]

[0149] u k To control the input vector, we can know from the modeling that:

[0150]

[0151] in, Represents the angle control input function f for each joint angle i The second derivative of (t) is taken at the k-1th step. i (t) can be taken as the control input angle function of each joint angle of the serpentine curve [2]. This function is a continuous differentiable function. Taking its second-order derivative, we can get but:

[0152]

[0153] For the covariance prediction equation:

[0154]

[0155] Among them, P k-1|k-1 is the posterior state estimation error covariance matrix of the k-1th step; P k|k-1 is the covariance matrix of the prior state estimation error at step k. Both are quantities to be iterated.

[0156] Q is the process noise covariance matrix, and it can be seen from the modeling that:

[0157]

[0158] in is the variance of the servo, which can be given in the data sheet or obtained through pre-testing; The variance of the error generated by approximating the angular acceleration using the second derivative of the control function is similar to Regarding, you can set

[0159]

[0160] Its coefficient 5 can be changed freely, but it is better to be greater than 1 and not too small.

[0161] The calculation formula for Kalman gain is:

[0162]

[0163] where K k Represents the Kalman gain coefficient of the kth step, which is the quantity to be determined at each step.

[0164] H is the observation matrix, and it can be seen from the modeling that:

[0165]

[0166] R is the observation noise covariance matrix, which can be seen from the modeling:

[0167]

[0168] Where (σ imu ) 2 is the variance of the IMU measurement noise, which can be obtained from the data sheet or calculated through extensive testing before starting.

[0169] represents the error of using the angle input control function directly as the actual angle of joint i, and its accuracy with the servo, that is, They are related, but due to the accumulation of time, they are definitely not equal. We can assume that:

[0170]

[0171] The coefficient 10 can be changed freely, but it is better to be greater than 1 and not too small.

[0172] For status updates:

[0173]

[0174] z k Represents the observed value of the kth step. From the modeling, we can know that:

[0175]

[0176] where θ k ,ω k They represent the rotation angle and angular velocity of the tail in the robot coordinate system, respectively.

[0177]

[0178] It represents the sampling of the k-th step of the angle input control function, specifically:

[0179]

[0180] For covariance updates:

[0181] P k|k =(IK k H)P k|k-1

[0182] There are no other matrices that need to be defined.

[0183] The specific process of Kalman filtering is as follows:

[0184] ① Initialize state variables Its error covariance matrix P k|k

[0185] ②First calculate the prior state variables and prior error covariance matrix

[0186] According to the state prediction equation:

[0187]

[0188] Calculate the prior estimate vector of the state variables

[0189] Use the covariance prediction equation:

[0190]

[0191] Calculate the prior error covariance matrix P of the state variables k|k-1

[0192] ③ According to prior

[0193] Use the Kalman gain calculation formula:

[0194]

[0195] Calculate the Kalman gain K k

[0196] Use K k Status update:

[0197]

[0198] Get the posterior estimate vector of the state variable

[0199] Use K k Covariance update:

[0200] P k|k =(IK k H)P k|k-1

[0201] Get the posterior error covariance matrix P of the state variable k|k

[0202] ④ Make each step With P k|k As the output of this step and the initial amount of the next step, steps ② to ③ are repeated in a loop.

[0203] For each step, the more accurate state estimate Restore the robot joint coordinates.

[0204] For the two-dimensional model, the angular velocity and angle are only taken in the yaw angle direction. By using the above Kalman filter modeling, the more accurate joint yaw angle can be obtained. We do the transformation:

[0205]

[0206] The angle of joint i relative to the positive direction of the x-axis can be obtained

[0207] Assume the length of connecting rod i is l i (i ranges from 0 to n), the offset from IMU1 to joint 1 is b = [b x ,b y ] T . Let the robot coordinate system coordinate of joint i be Then there is a recursive formula:

[0208]

[0209] Then, the coordinates of the joint in the world coordinate system are obtained according to the coordinate transformation matrix T:

[0210]

[0211] If it is a three-dimensional model, each joint should contain six-dimensional information, namely the angle of the Euler angle in three directions and the angular acceleration. In this way, the joint angle of each joint can be estimated through the tail IMU combined with Kalman filtering. At this time, the joint coordinate estimation needs to be solved using quaternion solution, etc.

[0212] It should be noted that the above content merely illustrates the technical idea of ​​the present invention and cannot be used to limit the scope of protection of the present invention. For ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present invention. These improvements and modifications all fall within the scope of protection of the claims of the present invention.

Claims

1. A snake-like robot based on dual IMU, characterized by: It includes n joints, n+1 links, IMU1 and IMU2. The links are arranged in ascending order according to n. Adjacent links are connected by joints. Link 0 is the head and link n is the tail. IMU1 and IMU2 are installed on the head link and the tail link respectively. The two output three-axis acceleration information and three-axis angular velocity information respectively.

2. The method for snake-like robot pose estimation based on dual IMU according to claim 1, characterized in that: The following steps are involved: S1, real-time acquisition of two sets of three-axis acceleration information and three-axis angular velocity information of IMU1 located at the head and IMU2 located at the tail; S2. Double-integrate and single-integrate the original three-axis acceleration data and three-axis angular velocity data of IMU1 and IMU2 respectively to obtain displacement and rotation angle; S3. Establish the robot coordinate system with the position of IMU1 as the origin, and the direction from IMU1 along the link 0 to joint 1 as the positive direction of the x-axis, and use q1 and φ1 as the displacement component and rotation component of the coordinate transformation matrix T between the robot coordinate system and the world coordinate system respectively; S4. Using the difference between the two IMU data as the rotation angle and angular velocity information of the robot's tail in the robot coordinate system as the observation value, perform kinematic modeling based on the robot structure, and solve the rotation angle of each joint through Kalman filtering; S5. Calculate the coordinates of each joint point in the robot coordinate system based on the inferred rotation angle and the structural characteristics of the robot, and obtain its coordinates in the world coordinate system through the coordinate transformation matrix T to obtain the robot posture.

3. The method for snake-like robot pose estimation based on dual IMU according to claim 2, characterized in that: Step S1 acquires two sets of three-axis acceleration information and three-axis angular velocity information of the IMU1 located at the head and the IMU2 located at the tail in real time; they are recorded as: where a 1 and a 2 Represents the acceleration vectors measured by IMU1 and IMU2, including the components of the x, y, and z axes; ω 1 and ω 2 They represent the angular velocity vectors measured by IMU1 and IMU2 respectively, and also include the components of the x, y, and z axes.

4. The method for snake-like robot pose estimation based on dual IMU according to claim 2, characterized in that: In step S2, the original three-axis acceleration data and three-axis angular velocity data of IMU1 and IMU2 are double-integrated and single-integrated to obtain the displacement and rotation angle. Double integration of the acceleration information yields the displacement: where q 1 and q 2 Respectively represent the displacement information obtained by IMU1 and IMU2, including the components of the x, y, and z axes, and g represents the acceleration of gravity; Integrating the angular velocity gives the angle: in and Respectively represent the angle information obtained by IMU1 and IMU2, including the components of the x, y, and z axes; Since the IMU data is discretized after sampling, further discretization processing is required, so the integral should be written in summation form.

5. The method for estimating the snake-like robot posture based on dual IMUs according to claim 2, characterized in that: In step S3, the position of IMU1 is used as the origin, and the robot coordinate system is established from IMU1 along the direction of link 0 pointing to joint 1 as the positive direction of the x-axis, and q1 and As the displacement and rotation components of the coordinate transformation matrix T between the robot coordinate system and the world coordinate system; Assume that the coordinates of the same point in the robot coordinate system are e and in the world coordinate system are h, then it should satisfy: h=Te The robot coordinate system coordinates of each joint point are calculated separately in the robot coordinate system, and then the coordinates in the world coordinate system are obtained through the coordinate transformation matrix T; finally, the calculated joint coordinates are converted to calculate the coordinates of each joint angle.

6. The method for snake-like robot pose estimation based on dual IMUs according to claim 2, characterized in that: In step S4, the difference between the two IMU data is used as the rotation angle and angular velocity information of the robot tail in the robot coordinate system as the observation value, kinematic modeling is performed according to the robot structure, and the rotation angle of each joint is solved by Kalman filtering; Assume that the joint angle and angular velocity corresponding to joint i are θ i 、ω i , where i ranges from 0 to n; The state variable x is defined as: x=[θ 1 ,i 2 ...,i n ,oh 1 ,oh 2 ...oh n ] T According to the recursive relationship, we get the equation system: Wherein, Δt represents the time interval between two consecutive samplings; represents the ideal angle control function f given to joint i n The sampling value of the second derivative of (t) at the k-1th step; and Represents θ respectively i With ω i The predicted value at step k; represents the process noise when the angle state of joint i is transferred from the k-1th step to the kth step; Represents the process noise when the angular velocity state of joint i is transferred from the k-1th step to the kth step; Writing it in matrix form, we get the state transfer equation: x k =Ax k-1 +Bu k +w k The state transfer matrix A is: The control input matrix B is: Control input vector u k for: Process noise vector w k for: Let its covariance matrix be Q, according to the definition of covariance matrix: Where E(·) represents the expectation; it is divided into square terms and cross terms; for For (i from 1 to n), it represents the process noise when the angle state of joint i is transferred from step k-1 to step k. All i from 1 to n correspond to are independent of each other, and their expectation is 0 and their variance is sqare(σ i ), in order to distinguish it from the superscript, sqare(·) is used to represent the square operation; that is, Where i ranges from 1 to n, and Var(·) represents the variance. Likewise, for For all i from 1 to n, are independent of each other, and their expectation is 0. Let the variance of the error caused by the servo accuracy be sqare(σ n+i );Right now Where i ranges from 1 to n; because represents the process noise when the angular velocity state of joint i is transferred from the k-1th step to the kth step, represents the process noise when the angle state of joint i is transferred from the k-1th step to the kth step, and They are also independent of each other; So we get vector w k Each component in is independent of each other, and its variance is known, so the covariance matrix Q is a diagonal matrix, and according to the formula: Var(x)=E(x 2 )-[Ex)] 2 The elements on the diagonal of the covariance matrix are: Where i ranges from 1 to n; Therefore, the covariance matrix Q satisfies: Where diag(a,b...c) represents a diagonal matrix with a,b...,c as diagonal elements; The observation model consists of the angular velocity data output by IMU2 and IMU1, the difference between the processed angle data, and all control input functions, specifically: z=[θ,ω,f 1 ,f 2 ...f n ] T in: oh = oh 2 -oh 1 The measurement data of IMU2 is subtracted from the measurement data of IMU1, which reflects the movement angle of the robot's tail in the robot coordinate system. According to the connecting rod structure, the relationship between the observation value z and each joint angle and joint angular velocity is known as: where θ k With ω k Represent the sampling values ​​of θ and ω at the kth step, Indicates the cumulative error when the angular velocity is converted to an angle at step k and the error when the sum of all the previous servo angles is used as the tail angle. It represents the angular velocity measurement error of the IMU itself and the error of using the sum of all the previous servo angles as the tail angle, that is: in and They represent the cumulative error when the k-th step angular velocity is converted into angle and the angular velocity measurement error of the IMU itself. However, in essence, the IMU attitude angle is considered to be θ k =ω k-1 ·Δt+θ k-1 , if the measurement variance of IMU is The expectation is 0, and the following relationship is obtained: represents the error caused by using the input angle function of joint i as the actual angle of joint i, and Regarding: So the observation equation is: z k =Hx k +v k The observation matrix H is: Observation noise vector v k for: Let its covariance matrix be R: After expansion, it is divided into cross terms and square terms; and independent, and independent, are independent of each other, but and are not independent, so there are only two cross terms: The rest are all 0; the square term is the variance of each error variable; again: but: According to the 5 equations of the Kalman filter process That is, the state prediction equation: Covariance prediction equation: Kalman gain calculation formula: Status Update: Covariance update: P k|k =(I-K k H)P k|k-1 in represents the posterior state estimator at the k-1th step; represents the prior state estimator of the kth step; P k-1|k-1 is the posterior state estimation error covariance matrix of the k-1th step; P k|k-1 K is the prior state estimation error covariance matrix of the k-th step; k is the Kalman gain at step k; represents the posterior state estimator of the kth step; P k|k represents the posterior state estimation error covariance matrix of the k-th step, and I is the identity matrix.

7. The method for snake-like robot pose estimation based on dual IMUs according to claim 2, characterized in that: In step S5, the coordinates of each joint point in the robot coordinate system are calculated based on the inferred rotation angle and the structural characteristics of the robot, and the coordinates in the world coordinate system are obtained through the coordinate transformation matrix T to obtain the robot posture; For the two-dimensional model, the angular velocity and angle are only taken in the yaw angle direction. The more accurate joint yaw angle θ is obtained by using the above Kalman filter modeling solution. i , where i ranges from 1 to n, and the transformation is: Get the angle Θ of joint i relative to the positive direction of the x-axis i ; set up The length of connecting rod i is l i , where i ranges from 0 to n, and the offset from IMU1 to joint 1 is b = [b x ,b y ] T ; Let the robot coordinate system coordinate of joint i be p i =[x i ,y i ] T , then there is a recursive formula: Then, the coordinates of the joint in the world coordinate system are obtained according to the coordinate transformation matrix T: p′ i =Tp i If it is a three-dimensional model, each joint should contain six-dimensional information, namely the angle of the Euler angle in three directions and the angular acceleration. In the same way, the joint angle of each joint is estimated through the tail IMU combined with Kalman filtering. At this time, the joint coordinate estimation is solved using quaternion solution, etc.

Images