Inertial positioning method and system of robot
By fixing the IMU to the robot foot and combining the principle of strap-inner inertial navigation and Kalman filtering, the problems of low positioning accuracy and system complexity in the traditional method are solved, and high-precision and low-cost inertial positioning are achieved, which is suitable for multiple Planting ground environment.
Patent Information
- Application Number
- CN202411103568.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-13
- Publication Date
- 2024-11-15
AI Technical Summary
In traditional methods, the inertial positioning method of leg robots relies on external sensors or complex kinematic constraints, resulting in low positioning accuracy, high cost, high system complexity, failure in indoor or signal-impeded environments, and IMU sensors are easily affected by Zero bias and noise influence, resulting in the accumulation of positioning errors.
Fix the IMU to the robot foot, process data through the strap-inner inertial navigation principle and lever speed model, establish a foot phase detection model, use the Kalman filtering method to estimate the navigation error state, correct the speed, attitude and position, and reduce zero bias and noise Influence, simplify the data processing process.
It improves the positioning accuracy and practicality of the robot, is suitable for a variety of road surfaces, reduces system complexity and cost, effectively suppresses the accumulation of inertial navigation errors, and improves navigation reliability and real-timeness.
Smart Images

Figure CN118960728A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of robot positioning, and in particular to an inertial positioning method and system for a robot. Background Art
[0002] With the development of robotics technology, walking robots have attracted more and more attention and applications due to their superior terrain adaptability and flexibility. Among them, legged robots have higher requirements for positioning accuracy. However, the precise positioning of legged robots has always been a technical problem. Traditional positioning methods mainly rely on external sensors such as GPS and UWB ultra-wideband, but these methods often fail indoors or in signal-blocked environments. Inertial measurement units have become an important sensor in legged robot positioning systems due to their independence and lack of influence from the external environment.
[0003] In the prior art, the method of using IMU to locate legged robots usually requires placing the IMU inside the body, and through complex kinematic constraints and fusing sensor data to improve positioning accuracy. This method not only increases the complexity and cost of the system, but also increases the difficulty of data processing and the burden of robot modeling. The IMU sensor installed in the body is susceptible to zero bias and noise, and there is no fixed state observation, which is prone to accumulated errors. In addition, the robot's kinematic model is easily affected by the terrain, causing the foot to slip during the robot's gait cycle. Therefore, traditional leg odometers that rely on kinematic models are often affected by joint encoder noise and error accumulation, and have problems such as low positioning accuracy and poor practicality. Summary of the invention
[0004] Purpose of the invention: The purpose of the present invention is to provide a robot inertial positioning method and system with higher accuracy, stronger practicality and simpler process.
[0005] Technical solution: The inertial positioning method of the robot described in the present invention includes the following steps: fixing the IMU to the foot of the robot, and collecting data after initializing the IMU; the navigation system processes the data collected by the IMU based on the strapdown inertial navigation principle, and calculates the speed, posture, and position of the robot foot in real time; establishing a foot phase detection model to process the data collected by the IMU, and when it is detected that the robot foot is in a stationary phase, using a lever velocity model to calculate the estimated speed of the robot foot; estimating the navigation error state based on the speed and the estimated speed, and correcting the speed, posture, and position of the robot foot according to the navigation error state.
[0006] Further, initializing the IMU includes the following process:
[0007] Establish the IMU system;
[0008] Performing zero bias correction on the machine system;
[0009] The initial posture is solved in the calibrated machine system.
[0010] Furthermore, the zero bias is calculated by the following formula:
[0011]
[0012] In the formula, t1 represents the start time of the static interval, t2 represents the end time of the static interval, and w represents the three-axis output of the gyroscope; Indicates zero bias;
[0013] The initial attitude, including pitch angle and roll angle, is solved by the following formula:
[0014]
[0015] In the formula, a x 、a y 、a z The three-axis force output of the IMU system x ,a y ,a z}; γ represents the roll angle; θ represents the pitch angle.
[0016] Furthermore, the navigation system converts the IMU system into the navigation coordinate system based on the strapdown inertial navigation principle, and performs real-time calculation of the attitude, speed and position in the navigation coordinate system; the calculation process of the attitude is as follows:
[0017]
[0018] Where q represents a quaternion; represents the coordinate transformation matrix from the IMU system to the navigation coordinate system; ψ represents the heading angle; γ represents the pitch angle; θ represents the roll angle;
[0019] The formula for real-time speed calculation is as follows:
[0020]
[0021] In the formula, v n,k is the velocity of the IMU in the navigation coordinate system, the subscript n represents the navigation coordinate system, k+1 and k represent the time before and after the solution respectively; f b It represents the specific force output by the accelerometer; It is the projection of the earth's rotation angular velocity in the navigation coordinate system; is the projection of the earth's rotation angular velocity of the navigation coordinate system relative to the earth coordinate system; Δt is the update time interval;
[0022] The formula for real-time position calculation is as follows:
[0023] p n,k+1 =p n,k +v n,k Δt
[0024] Where Δt is the update time interval; v n,k is the velocity of the IMU in the navigation coordinate system; p n,k Indicates the estimated position in the navigation coordinate system at the current moment; p n,k+1 Represents the position estimate in the navigation coordinate system at the next moment.
[0025] Furthermore, establishing the foot phase detection model includes the following process:
[0026] The data collected by the IMU is processed using a generalized likelihood ratio test model to obtain detection statistics at multiple moments, and an adaptive threshold of the detection statistics is calculated; the following foot phase detection model is established based on the detection statistics and the adaptive threshold:
[0027]
[0028] Where T(n) represents the detection statistic at time n; γ n Represents the adaptive threshold; when G=1, it is the static phase, and when G=0, it is the swing phase.
[0029] Furthermore, the lever velocity model is as follows:
[0030]
[0031] In the formula, is the velocity estimate of the IMU generated by the lever velocity model; It is the three-axis angular velocity output of the IMU in the machine system; is the rotation radius vector of the machine system.
[0032] Furthermore, the navigation error state is estimated based on the Kalman filter method, which specifically includes the following process:
[0033] Establish the navigation error state equation of the navigation system;
[0034] Establishing a speed error observation equation according to the speed and the estimated speed;
[0035] A Kalman filter framework is established by the navigation error state equation and the speed error observation equation, and the navigation error state is solved by discretization.
[0036] Furthermore, the state equation is:
[0037] X=AX+GW
[0038] Where X represents the 18-dimensional navigation error state, and in, represents the platform error angle, δv represents the velocity error, δp represents the position error; ε w represents the gyro bias error, ε a represents the first-order Markov drift error of the accelerometer, h os represents the rotation radius error; A represents the state transfer matrix; G represents the navigation system noise transfer matrix; W represents the navigation system noise;
[0039] The velocity error observation equation is:
[0040]
[0041] Where Z represents the observation vector; represents the robot foot velocity calculated by the inertial navigation system; v lvm represents the estimated velocity of the robot foot obtained by the lever model; H is the observation matrix; X is the error state of the navigation system; n v To measure noise;
[0042] The posterior estimate of the navigation state error is obtained through discretization of Kalman filtering:
[0043]
[0044] Where H represents the observation matrix; Z represents the observation vector; represents the prior estimate of the state at time k obtained from the navigation error state equation; X k is the a posteriori estimate of the navigation error state obtained by solving the Kalman filter.
[0045] Furthermore, the navigation error state solved includes accelerometer deviation, gyroscope deviation, attitude deviation, velocity deviation, and position deviation;
[0046] The speed, posture and position of the robot foot are corrected by posture deviation, speed deviation and position deviation; the outputs of the accelerometer and gyroscope in the IMU are corrected by accelerometer deviation and gyroscope deviation.
[0047] The inertial positioning system of the robot described in the present invention comprises:
[0048] A deployment module is used to fix the IMU to the foot of the robot and perform data collection after initializing the IMU;
[0049] A state solving module is used to enable the navigation system to process the data collected by the IMU based on the strapdown inertial navigation principle and solve the speed, posture and position of the robot foot in real time;
[0050] A speed estimation module is used to establish a foot phase detection model to process the data collected by the IMU, and when it is detected that the robot foot is in a stationary phase, the lever speed model is used to calculate the estimated speed of the robot foot;
[0051] The state correction module is used to estimate the navigation error state according to the speed and the estimated speed, and correct the speed, posture and position of the robot foot according to the navigation error state.
[0052] Beneficial effects: The present invention has the following significant effects: 1. Higher precision: Based on the data acquisition by IMU, the present invention establishes a foot phase detection model and a lever speed model to further optimize the data, and solves the IMU data through the inertial navigation principle to realize reliable navigation and positioning of the robot; specifically, the IMU data is processed in real time to solve the strapdown inertial navigation state, and the static phase detection method is used to judge the static phase of the foot of the legged robot, and the lever speed model is applied within the static phase to obtain the real-time speed observation vector, and finally the data is fused in the Kalman filter to obtain the posterior estimated navigation error state, and then the navigation and positioning error is corrected by feedback, which can effectively extract the static and swinging phases of the foot, and combine the filtering method to effectively observe the speed of the IMU, which makes up for the problem of reliable inertial navigation of the legged robot. 1. It effectively suppresses the error divergence of the inertial navigation system, solves the problem of serious accumulation of navigation and positioning errors of legged robots caused by using only inertial measurement units, improves the navigation and positioning accuracy of legged robots, has the advantages of good real-time performance and strong reliability, and is suitable for engineering applications; 2. It is highly practical: the method proposed in the present invention is applicable to a variety of road surfaces, including but not limited to soft surfaces such as grass and hard surfaces such as cement. It has a wider range of applications, more complex scenarios, and strong practicality; 3. The process is simple: the present invention sets the IMU outside the robot, which reduces the zero bias and noise that are easy to occur when installed inside the robot. There is no fixed state observation, which is easy to produce accumulated errors, and there is no need for complex kinematic constraints to process the data collected by the IMU. The process is simpler, reducing the difficulty of data processing and the modeling process, and the cost is lower and more efficient. BRIEF DESCRIPTION OF THE DRAWINGS
[0053] Figure 1 This is a flow chart of the inertial positioning method proposed by the present invention;
[0054] Figure 2 This is a schematic diagram of the configuration of the robot and IMU;
[0055] Figure 3 This is a schematic diagram of the robot's stationary and swing phase partitions;
[0056] Figure 4 It is a schematic diagram of the lever velocity model when the robot is in the stationary phase;
[0057] Figure 5 Comparison chart of the effect verified for cement pavement;
[0058] Figure 6 This is a comparison chart of the effects verified on turf pavement. DETAILED DESCRIPTION
[0059] The present invention is further explained below in conjunction with the accompanying drawings and specific implementation methods.
[0060] See also Figure 1 As shown, the inertial positioning method of the robot described in the present invention includes the following steps: fixing the IMU to the foot of the robot, and collecting data after initializing the IMU; the navigation system processes the data collected by the IMU based on the strapdown inertial navigation principle, and calculates the speed, posture, and position of the robot foot in real time; establishing a foot phase detection model to process the data collected by the IMU, and when it is detected that the robot foot is in a stationary phase, using a lever velocity model to calculate the estimated speed of the robot foot; estimating the navigation error state according to the speed and the estimated speed, and correcting the speed, posture, and position of the robot foot according to the navigation error state.
[0061] The inertial positioning system of the robot described in the present invention comprises: a deployment module, used for fixing the IMU to the foot of the robot, and performing data collection after initializing the IMU; a state solution module, used for enabling the navigation system to process the data collected by the IMU based on the strapdown inertial navigation principle, and to solve the speed, posture and position of the robot foot in real time; a speed estimation module, used for establishing a foot phase detection model to process the data collected by the IMU, and when it is detected that the robot foot is in a stationary phase, using a lever speed model to calculate the estimated speed of the robot foot; and a state correction module, used for estimating the navigation error state according to the speed and the estimated speed, and correcting the speed, posture and position of the robot foot according to the navigation error state.
[0062] In this embodiment, the navigation system mainly includes an inertial measurement unit, i.e., an IMU. The IMU includes a gyroscope, an accelerometer, and a magnetometer. The calculation process of the Kalman filter is implemented by a Kalman filter. The robot adopts a legged robot, and in order to better fit the robot's feet and obtain better data measurement results, the IMU used is a leg-bound type.
[0063] See also Figure 2 , initializing the IMU includes the following process:
[0064] First, the IMU machine system is established. In this embodiment, the center of the leg-bound IMU is taken as the coordinate origin O, the right direction is the X-axis, the Y-axis is perpendicular to the X-axis and points forward, the Z-axis points upward, and the XYZ three axes satisfy the right-hand screw rule.
[0065] After that, the machine system is subjected to zero bias correction. When the leg-bound IMU is initially static, the gyro zero bias correction is performed according to the gyro output in the IMU, and the gyro zero bias is calculated and deducted from the IMU static data. The zero bias is calculated by the following formula:
[0066]
[0067] In the formula, t1 represents the start time of the static interval, t2 represents the end time of the static interval, and w represents the three-axis output of the gyroscope; Indicates zero bias.
[0068] Finally, the initial attitude is solved in the calibrated machine system. When the leg-bound IMU is initially static, the initial navigation attitude is solved according to the output of the accelerometer. The initial attitude is solved by the following formula, mainly including the pitch angle and roll angle:
[0069]
[0070] In the formula, a x 、a y 、a z The three-axis force output of the IMU system x ,a y ,a z}; γ represents the roll angle; θ represents the pitch angle.
[0071] After the IMU is initialized, the navigation system converts the IMU's body system into the navigation coordinate system based on the strapdown inertial navigation principle, and performs real-time calculations of the attitude, velocity, and position in the navigation coordinate system.
[0072] In this embodiment, the attitude update adopts the quaternion algorithm, and the attitude calculation process is as follows:
[0073]
[0074] Among them, q k represents the quaternion at the current moment; ω represents the angular increment of the gyroscope during the sampling period; Ω is the antisymmetric matrix formed by ω; q represents the quaternion; It represents the coordinate transformation matrix from the IMU system to the navigation coordinate system; ψ represents the heading angle; γ represents the pitch angle; θ represents the roll angle.
[0075] The speed of the leg-bound IMU is calculated using the strapdown inertial navigation algorithm. The formula for real-time speed calculation is as follows:
[0076]
[0077] In the formula, v n,kis the velocity of the IMU in the navigation coordinate system, the subscript n represents the navigation coordinate system, k+1 and k represent the time before and after the solution respectively; f b It represents the specific force output by the accelerometer; It is the projection of the earth's rotation angular velocity in the navigation coordinate system; It is the projection of the earth's rotation angular velocity of the navigation coordinate system relative to the earth coordinate system; Δt is the update time interval, which is usually the inverse of the IMU data output frequency.
[0078] The strapdown inertial navigation algorithm calculates the position of the leg-bound IMU. The formula for real-time position calculation is as follows:
[0079] p n,k+1 =p n,k +v n,k Δt
[0080] Where Δt is the update time interval; v n,k is the velocity of the IMU in the navigation coordinate system; p n,k Indicates the estimated position in the navigation coordinate system at the current moment; p n,k+1 Represents the position estimate in the navigation coordinate system at the next moment.
[0081] See also Figure 3 , establish a foot phase detection model to determine in real time whether the robot's foot is in a stationary state, so as to improve the accuracy of data processing. The specific process includes the following:
[0082] First, the generalized likelihood ratio test model is used to process the data collected by the IMU to obtain the detection statistic T(n) at n moments, and the adaptive threshold of the detection statistic is calculated. The formula is as follows:
[0083]
[0084] Where r is the size of the sliding window; and They represent the acceleration and angular velocity output of the machine system at time k respectively; and Respectively represent their variances; g is the magnitude of the gravity vector; || || is the norm, indicating the magnitude of the vector.
[0085] Define the set S of detection statistics data sets n For S n ={T(nr / 2),…,T(n),…,T(n+r / 2)}, defining the adaptive threshold γ n for: Where K1, K2 and K3 are hyperparameters; μ n and For the data set S nThe mean and variance of .
[0086] Finally, the stationary phase detection model is regarded as a binary hypothesis testing problem, G=1 represents the stationary phase, and G=0 represents the swing phase. Therefore, the following foot phase detection model is established by the detection statistics and adaptive threshold:
[0087]
[0088] Where T(n) represents the detection statistic at time n; γ n Represents the adaptive threshold; when G=1, it is the static phase, and when G=0, it is the swing phase.
[0089] See also Figure 4 After detecting the stationary phase, it is necessary to establish a lever velocity model to obtain a more accurate estimated velocity of the robot foot, so as to improve the accuracy of subsequent corrections. The establishment process requires obtaining the segment vector However, due to the tilted placement of the IMU on the robot leg, it is not possible to estimate directly in the b coordinate system. Therefore, the present invention considers that the IMU is firmly attached to the robot leg and both can be considered as rigid bodies, so the segment vector It remains unchanged during the movement, and then the rotation radius of the navigation system is measured at the initial rest state. To find out Initial coordinate transfer matrix during solution It is obtained by solving the initial attitude angle. The specific formula is as follows:
[0090]
[0091] In the formula, is the velocity estimate of the IMU generated by the lever velocity model; It is the three-axis angular velocity output of the IMU in the machine system; is the rotation radius vector of the machine system, and the formula is as follows:
[0092]
[0093]
[0094] In the formula, represents the coordinate transfer matrix at the initial stationary moment; ψ0, φ0, θ0 are the initial attitude angles, which are initialized by IMU; s and c represent sine and cosine functions, respectively.
[0095] After the estimated speed is obtained through the above process, the navigation error state is estimated based on the Kalman filter method, which specifically includes the following process:
[0096] First, a navigation error state equation of the navigation system is established. In this embodiment, a navigation error state equation composed of multiple errors in the northeast sky direction is established, specifically:
[0097] X=AX+GW
[0098] Where X represents the 18-dimensional navigation error state, and in, represents the platform error angle, δv represents the velocity error, δp represents the position error; ε w represents the gyro bias error, ε a represents the first-order Markov drift error of the accelerometer, h os represents the rotation radius error; A represents the state transfer matrix; G represents the navigation system noise transfer matrix; W represents the navigation system noise.
[0099] Afterwards, a speed error observation equation is established according to the speed and the estimated speed; the speed error observation equation is:
[0100]
[0101] Where Z represents the observation vector; represents the robot foot velocity calculated by the inertial navigation system; v lvm represents the estimated velocity of the robot foot obtained by the lever model; H is the observation matrix; X is the error state of the navigation system; n v is the measurement noise and represents the velocity observation uncertainty.
[0102] Finally, the Kalman filter framework is established by the navigation error state equation and the velocity error observation equation, and the navigation error state is solved by discretization. The posterior estimation of the navigation state error is obtained by discretization Kalman filtering. Solving the posterior estimation error state quantity X k Finally, the position, velocity, attitude and IMU device errors are corrected and the final result is output. The process of discretizing the navigation error state is as follows:
[0103]
[0104] Where H represents the observation matrix; Z represents the observation vector; represents the prior estimate of the state at time k obtained from the navigation error state equation; X k is the a posteriori estimate of the navigation error state obtained by solving the Kalman filter; A represents the state transfer matrix; Q represents the state transfer covariance matrix; represents the prior estimated covariance at time k; P k-1 represents the posterior estimated covariance at time k-1; K krepresents the Kalman filter gain; R represents the measurement noise covariance; represents the posterior estimated covariance at time k; I represents the identity matrix.
[0105] In this embodiment, the navigation error state obtained includes accelerometer deviation, gyroscope deviation, attitude deviation, speed deviation, and position deviation. The speed, attitude, and position of the robot foot are corrected by attitude deviation, speed deviation, and position deviation; the output of the accelerometer and gyroscope in the IMU is corrected by accelerometer deviation and gyroscope deviation. The speed, attitude, and position of the robot foot are corrected according to the obtained navigation error state. The specific formula is as follows:
[0106]
[0107] In the formula, represents the attitude of the navigation system after correction in the navigation coordinate system; v represents the speed of the navigation system after correction in the navigation coordinate system; p represents the position of the navigation system after correction in the navigation coordinate system; Indicates the attitude calculated by the navigation system in the navigation coordinate system; Indicates the attitude calculated by the navigation system in the navigation coordinate system; Indicates the position solved by the navigation system in the navigation coordinate system; represents attitude error; δv represents velocity error; δp represents position error;
[0108] The output of the IMU is corrected according to the solved navigation error state. The specific formula is as follows:
[0109]
[0110] In the formula, a raw is the original output of the accelerometer; w raw is the original output of the gyroscope; a is the corrected acceleration signal; w is the corrected angular velocity signal; ε a is the accelerometer bias; ε w is the gyro bias.
[0111] In addition, in order to verify and illustrate the effect of the method and system proposed in the present invention, field tests were conducted. From the test results, it can be seen that the method proposed in this application has the advantages of high precision and strong adaptability. The experimental scene is an open outdoor area, with cement pavement and grass pavement, respectively, and the total length of the route is 387.7m and 261.8m respectively. The method proposed in the present invention is compared with the positioning results of the traditional leg odometer and the real trajectory results.
[0112] See also Figure 5, which is a comparison chart of the cement road test results. In the cement road test, the legged robot B1 includes two actions: forward and turning. The solution results of the traditional legged odometer are as follows: Figure 5 As shown by the yellow line, the positioning error diverges rapidly, the average positioning error is 28.71 meters, and the maximum positioning error is 64.68 meters; the solution result of the inertial positioning method proposed by the present invention is as follows Figure 5 As shown by the orange line, the average positioning error of the present invention is 8.31 meters, and the maximum positioning error is 26.43 meters.
[0113] See also Figure 6 , which is a comparison chart of the cement road test results. In the grass road test, the legged robot B1 includes forward and backward movements. In the last straight section, the legged robot changes from forward to backward and finally retreats to the end point. The solution results of the traditional leg odometer are as follows Figure 6 As shown by the yellow line, the positioning error diverges seriously, the average positioning error is 9.52 meters, and the maximum positioning error is 17.60 meters. The solution result of the inertial positioning method suitable for legged robots according to the present invention is as follows: Figure 6 As shown by the orange line, the average positioning error of the present invention is 3.89 meters, and the maximum positioning error is 5.75 meters.
Claims
1. A robot inertial positioning method, characterized in that: The steps include: Fix the IMU to the foot of the robot, and collect data after initializing the IMU; The navigation system processes the data collected by the IMU based on the strapdown inertial navigation principle and calculates the speed, posture and position of the robot's foot in real time; Establishing a foot phase detection model to process the data collected by the IMU, and when it is detected that the robot foot is in a stationary phase, using a lever velocity model to calculate the estimated velocity of the robot foot; A navigation error state is estimated according to the speed and the estimated speed, and the speed, posture and position of the robot foot are corrected according to the navigation error state.
2. The inertial positioning method of a robot according to claim 1, characterized in that: Initializing the IMU includes the following process: Establish the IMU system; Performing zero bias correction on the machine system; The initial posture is solved in the calibrated machine system.
3. The inertial positioning method of a robot according to claim 2, characterized in that: The zero bias is calculated by the following formula: In the formula, t1 represents the start time of the static interval, t2 represents the end time of the static interval, and w represents the three-axis output of the gyroscope; Indicates zero bias; The initial attitude, including pitch angle and roll angle, is solved by the following formula: In the formula, a x 、a y 、a z The three-axis force output of the IMU system x ,a y ,a z }; γ represents the roll angle; θ represents the pitch angle.
4. The inertial positioning method of a robot according to claim 1, characterized in that: The navigation system converts the IMU system into the navigation coordinate system based on the strapdown inertial navigation principle, and performs real-time calculation of the attitude, speed and position in the navigation coordinate system. The attitude calculation process is as follows: Where q represents a quaternion; represents the coordinate transformation matrix from the IMU system to the navigation coordinate system; ψ represents the heading angle; γ represents the pitch angle; θ represents the roll angle; The formula for real-time speed calculation is as follows: In the formula, v n,k is the velocity of the IMU in the navigation coordinate system, the subscript n represents the navigation coordinate system, k+1 and k represent the time before and after the solution respectively; f b It represents the specific force output by the accelerometer; It is the projection of the earth's rotation angular velocity in the navigation coordinate system; is the projection of the earth's rotation angular velocity of the navigation coordinate system relative to the earth coordinate system; Δt is the update time interval; The formula for real-time position calculation is as follows: p n,k+1 =p n,k +v n,k Δt Where Δt is the update time interval; v n,k is the velocity of the IMU in the navigation coordinate system; p n,k Indicates the estimated position in the navigation coordinate system at the current moment; p n,k+1 Represents the position estimate in the navigation coordinate system at the next moment.
5. The inertial positioning method of a robot according to claim 1, characterized in that: Establishing the foot phase detection model includes the following process: The data collected by IMU is processed using the generalized likelihood ratio test model to obtain the detection statistics at multiple moments and calculate the adaptive threshold of the detection statistics. The following foot phase detection model is established based on the detection statistics and adaptive threshold: Where T(n) represents the detection statistic at time n; γ n Represents the adaptive threshold; when G=1, it is the static phase, and when G=0, it is the swing phase.
6. The inertial positioning method of a robot according to claim 1, characterized in that: The lever velocity model is as follows: In the formula, is the velocity estimate of the IMU generated by the lever velocity model; It is the three-axis angular velocity output of the IMU in the machine system; is the rotation radius vector of the machine system.
7. The inertial positioning method of a robot according to claim 1, characterized in that: The navigation error state is estimated based on the Kalman filter method, which specifically includes the following processes: Establish the navigation error state equation of the navigation system; Establishing a speed error observation equation according to the speed and the estimated speed; A Kalman filter framework is established by the navigation error state equation and the speed error observation equation, and the navigation error state is solved by discretization.
8. The inertial positioning method of a robot according to claim 1, characterized in that: The state equation is: X=AX+GW Where X represents the 18-dimensional navigation error state, and in, represents the platform error angle, δv represents the velocity error, δp represents the position error; ε w represents the gyro bias error, ε a represents the first-order Markov drift error of the accelerometer, h os represents the rotation radius error; A represents the state transfer matrix; G represents the navigation system noise transfer matrix; W represents the navigation system noise; The velocity error observation equation is: Where Z represents the observation vector; represents the robot foot velocity calculated by the inertial navigation system; v lvm represents the estimated velocity of the robot foot obtained by the lever model; H is the observation matrix; X is the error state of the navigation system; n v To measure noise; The navigation state error posterior estimation is obtained through discretization Kalman filtering: Where H represents the observation matrix; Z represents the observation vector; represents the prior estimate of the state at time k obtained from the navigation error state equation; X k is the a posteriori estimate of the navigation error state obtained by solving the Kalman filter.
9. The inertial positioning method of a robot according to claim 1, characterized in that: The navigation error state obtained includes accelerometer deviation, gyroscope deviation, attitude deviation, velocity deviation, and position deviation; The speed, posture and position of the robot foot are corrected by posture deviation, speed deviation and position deviation; the outputs of the accelerometer and gyroscope in the IMU are corrected by accelerometer deviation and gyroscope deviation.
10. An inertial positioning system for a robot, characterized in that: include: A deployment module is used to fix the IMU to the foot of the robot and perform data collection after initializing the IMU; A state solving module, used to enable the navigation system to process the data collected by the IMU based on the strapdown inertial navigation principle, and solve the speed, posture and position of the robot foot in real time; A speed estimation module is used to establish a foot phase detection model to process the data collected by the IMU, and when it is detected that the robot foot is in a stationary phase, the lever speed model is used to calculate the estimated speed of the robot foot; The state correction module is used to estimate the navigation error state according to the speed and the estimated speed, and correct the speed, posture and position of the robot foot according to the navigation error state.
