Strong fault-tolerant small leg pedestrian inertial autonomous navigation method based on weighted observation function

Abstract

The disclosure provides a strong fault-tolerant calf type pedestrian inertial autonomous navigation method based on a weighted observation function. The method first identifies the standing phase according to the modulus value of ankle acceleration, and the time when the human body walks in the standing phase is the correction time; an ankle-calf kinematics model is constructed; under the Kalman filtering framework, an ankle-calf kinematics / inertial navigation combined navigation model is established based on the ankle-calf kinematics model and the inertial navigation error equation; a weighted observation function w k is constructed according to the observation error k As the weight, the calculation of the gain matrix and the covariance matrix in the Kalman filtering process is increased, and the degree of weakening of the observation information is controlled to suppress the negative effects brought by the false correction time. The robust information fusion algorithm with the weighted observation function is used to fuse the ankle-calf kinematics model information and the micro inertial navigation system information, thereby improving the convenience, robustness and adaptability of the overall system.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of navigation, guidance and control, and particularly relates to a strong fault-tolerant shank type pedestrian inertial autonomous navigation method based on a weighted observation function. BACKGROUND

[0002] The zero velocity update (ZUPT) method of the foot-worn micro inertial measure unit (MIMU) is a common pedestrian autonomous navigation and positioning algorithm, but it depends on foot wearing, which limits its wide use. The shank type MIMU pedestrian inertial autonomous navigation algorithm has attracted the attention of researchers due to its convenience and positioning accuracy, but the correction interval detection is inaccurate due to the absence of zero speed interval in shank movement and the large self-shaking, which further leads to an increased positioning error. The optimization of the current correction interval detector generally depends on prior human motion knowledge or a large amount of historical data, and the convenience, robustness and adaptability still need to be improved. SUMMARY

[0003] Therefore, the application provides a strong fault-tolerant shank type pedestrian inertial autonomous navigation algorithm based on a weighted observation function, which no longer focuses on the optimization of the correction interval detector, only uses a standing phase detection detector based on the acceleration module value of the ankle, and uses a robust information fusion algorithm based on a weighted observation function to fuse ankle-shank kinematic model information and micro inertial navigation system information to overcome the negative effects caused by the incorrect detection of the detector, and further improve the convenience, robustness and adaptability of the overall system.

[0004] To solve the above technical problems, the application is implemented as follows.

[0005] A strong fault-tolerant shank type pedestrian inertial autonomous navigation method based on a weighted observation function, the method comprising:

[0006] Step S1: preliminary identification of the standing phase is performed according to the acceleration module value of the ankle, and the time when the human body walks in the standing phase is the correction time;

[0007] Step S2: the ankle-shank kinematic model is constructed based on the fact that the speed of the human foot is close to 0 in the standing phase;

[0008] Step S3: the ankle-shank kinematic / inertial navigation combined navigation model is established based on the ankle-shank kinematic model and the inertial navigation error equation in the Kalman filter framework;

[0009] Step S4: the weighted observation function w is constructed according to the observation error k When the observation approaches the ideal, w kapproaching 1, when observing an anomaly, as the degree of anomaly increases, w k decreases; w k As the weight increases in the calculation of the gain matrix and the covariance matrix of the Kalman filtering process, the degree of weakening of the observation information is controlled, when w k is 0, the observation information is completely isolated, and the state estimation is completed only by relying on the prediction update; at the correction time, the Kalman filter observation update is carried out based on the ankle-calf kinematics / inertial navigation combined navigation model, and the inertial navigation error correction is carried out by using the filtering result.

[0010] Preferably, in the step S1, the modulus value of the ankle acceleration is calculated by using the measurement data of the micro inertial measurement unit MIMU arranged at any part of the calf

[0011]

[0012] wherein is the acceleration of the ankle, is the specific force of the ankle, is the specific force of the measurement point where the MIMU is located, ω b is the angular velocity of the measurement point, which is collected by the gyroscope in the MIMU; is the lever arm vector from the measurement point to the ankle, which is measured by a ranging device before navigation; α b is the angular acceleration of the measurement point, which is obtained by differentiating the angular velocity measured by the gyroscope in the MIMU; g is the gravitational acceleration, and the upper superscript b represents the carrier coordinate system.

[0013] Preferably, in the step S1, the modulus value of the ankle acceleration is calculated by using the measurement data of the micro inertial measurement unit MIMU arranged at any part of the calf The standing phase is identified as:

[0014] The rising edge trigger correction method is used to realize the rough detection of the standing phase, that is, the acceleration measurement sequence is windowed and slid, and if the modulus value of the ankle acceleration in the current time is less than the set threshold value, and the modulus value of the ankle acceleration in the last sliding window is greater than the set threshold value, the current time is taken as the correction time.

[0015] Preferably, the set threshold value γ foot is 0.1g.

[0016] Preferably, in the step S3, the ankle-calf kinematics model established is:

[0017]

[0018] wherein the upper superscript n is the navigation coordinate system, and the upper superscript b is the carrier coordinate system, the velocity of the measurement point, the attitude matrix of the measurement point, ω b the angular velocity of the measurement point, the lever arm vector of the measurement point to the ankle, r the measurement noise, R the measurement noise variance matrix, E[] represents expectation; I 3×3 I is a unit matrix, and α is a measurement noise adjustment coefficient.

[0019] Preferably, in the step S4, the constructed ankle-calf kinematics / INS combined navigation model is:

[0020] the position error of the INS the velocity error and the attitude error as system variables the Kalman filter system equation is established as:

[0021] X k =F k,k-1 X k-1 +Γ k-1 W k-1

[0022] wherein is the output value of the accelerometer in the navigation coordinate system, T s is the sampling period of the micro inertial measurement unit (MIMU), O 3×3 is a 3x3 zero matrix, I 3×3 is a 3x3 unit matrix; Γ k-1 is a process noise driving matrix, W k-1 is the process noise, H k is the observation matrix at time k;

[0023] the velocity error of the INS the Kalman filter observation equation is established as:

[0024] Z k =H k X k +r k

[0025] wherein the observation matrix r k is the measurement noise at time k, is the angular velocity of the measurement point at time k;

[0026] the velocity error is calculated according to the velocity pseudo-measurement provided by the ankle-calf kinematics model:

[0027]

[0028] wherein a velocity pseudo-measurement at time k provided for the ankle-shank kinematic model, a velocity in a navigation coordinate system of a micro-inertial navigation solution at time k.

[0029] Preferably, in the step S4, when Kalman filtering update is performed by using the ankle-shank kinematic / inertial navigation combined navigation model, the weight w is added in the calculation of the gain matrix and the covariance matrix k , and the negative influence of abnormal observation on state estimation is inhibited as follows:

[0030] The calculation of the gain matrix K k and the covariance matrix P k is adjusted by using the weight w k :

[0031]

[0032] wherein, is a one-step predicted state, is a state estimation at time k, P k,k-1 is a one-step predicted covariance matrix, P k is a covariance matrix at time k, Q k-1 is a process noise variance matrix, K k is a gain matrix at time k, H k is an observation matrix at time k, R k is a measurement noise variance matrix.

[0033] Preferably, in the step S4, the designed weight w k is as follows:

[0034]

[0035] wherein Z k is an observation variable in the ankle-shank kinematic / inertial navigation combined navigation model, c is an adjustment parameter.

[0036] Beneficial effects:

[0037] (1) The application proposes a strong fault-tolerant shank-type pedestrian inertial autonomous navigation algorithm based on a weighted observation function, simplifies the system design process, and effectively improves the robustness of shank-type pedestrian inertial autonomous navigation positioning.

[0038] (2) The application adopts a standing phase coarse detector based on the ankle acceleration module value, has the characteristics of simplicity, high efficiency and easy use, and can roughly detect the correction time.

[0039] (3) The application adopts a robust information fusion algorithm based on a weighted observation function, effectively overcomes the negative influence of wrong detection on the navigation positioning result, and improves the robustness of the overall algorithm. BRIEF DESCRIPTION OF DRAWINGS

[0040] Figure 1 A flowchart of the strong fault-tolerant shank type pedestrian inertial autonomous navigation method based on the weighted observation function.

[0041] Figure 2 A schematic diagram of the motion state of the foot landing stage. DETAILED DESCRIPTION

[0042] The application will be described in detail below with reference to the drawings and examples.

[0043] The application provides a strong fault-tolerant shank type pedestrian inertial autonomous navigation method based on a weighted observation function, as shown in the figure, which comprises the following steps: Figure 1

[0044] Step S1: According to the modulus value of the ankle acceleration Preliminary identification is performed in the standing phase, and the time when the human body walks in the standing phase is the correction time;

[0045] Step S2: Based on the speed of the human foot The speed in the standing phase is close to 0, and the ankle-shank kinematics model is constructed;

[0046] Step S3: In the Kalman filtering framework, the ankle-shank kinematics / inertial navigation combined navigation model is established based on the kinematics model and the inertial navigation error equation established in step S2;

[0047] Step S4: According to the observation error, a weighted observation function w is constructed k When the observation approaches the ideal, w approaches 1, when the observation is abnormal, with the increase of the abnormal degree, w decreases; at the correction time, when the Kalman filtering update is performed based on the ankle-shank kinematics / inertial navigation combined navigation model, the weight w is increased in the gain matrix and the covariance matrix calculation, so as to suppress the negative influence of abnormal observation on state estimation; the filtering result is used for inertial navigation error correction. k k k

[0048] It can be seen that the application no longer focuses on the optimization of the correction interval detector, only uses the standing phase rough detector based on the ankle acceleration modulus value, fuses the ankle-shank kinematics model information and the micro inertial navigation system information by using the robust information fusion algorithm based on the weighted observation function, so as to overcome the negative influence caused by the detector wrong detection, and further improve the convenience, robustness and adaptability of the overall system. ​​​​

[0049] The implementation steps of the present application are described in detail below.

[0050] Step S1: Mid-stance phase rough detector design based on ankle acceleration module value.

[0051] The present application adopts a mid-stance phase rough detector based on ankle acceleration module value, adopts rising edge trigger correction method, and has the characteristics of simplicity, high efficiency and easy use in design and use.

[0052] The mid-stance phase refers to the stage of single foot full foot landing to the closest double foot distance in the human walking process. The present application selects the ankle-calf kinematics model information in the mid-stance phase, so it is necessary to design a gait detector to identify the mid-stance phase in the human walking process by using the inertial information measured by the micro inertial measurement unit (MIMU). According to the angular velocity synthesis theorem, only the angular velocity measured by the calf MIMU cannot calculate the angular velocity of the ankle joint, but according to the Coriolis theorem, the acceleration of the ankle joint can be calculated according to the acceleration measured by the calf MIMU. In theory, when the acceleration of the ankle joint is zero, it is judged that the human body is in the mid-stance phase at this moment.

[0053] According to the Coriolis theorem, the acceleration of the ankle part is calculated by using the acceleration and angular velocity of the measurement point:

[0054]

[0055] Wherein α b is the angular acceleration of the measurement point of the MIMU, is the specific force of the measurement point, is the specific force of the ankle, is the acceleration of the ankle, g is the acceleration of gravity, and ω b is the angular velocity of the measurement point, is the lever arm vector from the measurement point to the ankle, g is the acceleration of gravity, and the upper superscript b represents the carrier coordinate system.

[0056] However, in the actual environment, due to the existence of impact vibration, it is often not zero, but a relatively small value, and in order to filter out unreasonable correction moments as much as possible, the present application adopts rising edge trigger correction,

[0057] that is, when the following formula is satisfied, it is regarded as a correction moment:

[0058]

[0059] Wherein W is the window length of the acceleration measurement sequence, generally W is taken as 1 for simplifying the operation, γ foot is a set threshold, and γ foot is 0.1g. is the ankle acceleration, j is the window number, which is a positive integer. This formula indicates that the acceleration measurement sequence is windowed and sliding. If the module value of the ankle acceleration in the sliding window at the current moment is Less than the set threshold, and the modulus of ankle acceleration in the previous sliding window When it is greater than the set threshold, the current time is used as the correction time.

[0060] Because human motion is highly complex, variable, and subject to strong disturbances, coarse detectors based on the ankle acceleration modulus during standing can experience false positives. The proposed method overcomes the negative impact of these false positives through the robust information fusion algorithm in S4, which is based on a weighted observation function.

[0061] Step S2: Constructing the ankle-calf kinematic model.

[0062] like Figure 2 As shown in the figure, during the foot landing phase, the movement of the foot-ankle-calf can be regarded as the second-order inverted pendulum movement of HA-AM and TA-AM. The MIMU is fixed at point M along the z-axis in the MA direction. According to the Coriolis theorem, the angular velocity ω sensitive to the gyroscope is used. b Calculate the velocity of the measuring point M

[0063]

[0064] Where n is the navigation coordinate system and b is the carrier coordinate system. is the arm vector from the measurement point to the ankle, l shank The distance from the measuring point to the ankle is usually obtained by a ruler or laser distance measurement. b is the angular velocity of the measuring point, is the posture matrix of the measurement point, is the ankle velocity in the n system, and r is the measurement noise.

[0065] In actual situations, using only the calf MIMU often fails to obtain accurate ankle velocity. However, when the human body is in the standing phase during walking, it can be approximately considered that Therefore, in actual use, the kinematic model is simplified to:

[0066]

[0067] Among them I 3×3 It is a 3×3 unit matrix, and α is generally set to 0.01.

[0068] S3: Construct an ankle-calf kinematics / inertial navigation integrated navigation model.

[0069] The present application establishes an ankle-calf kinematics / INS integrated navigation model based on the kinematics model and the INS error equation established in step S2 under the Kalman filtering framework, and specifically:

[0070] Firstly, the position error, velocity error and attitude error of the inertial navigation system are taken as system variables The Kalman filtering system equation is established:

[0071] X k =F k,k-1 X k-1 +Γ k-1 W k-1 (6)

[0072] Wherein is the output value of the accelerometer in the navigation coordinate system, T s is the sampling period of the MIMU, O 3×3 is a 3x3 zero matrix, I 3×3 is a 3x3 unit matrix; Γ k-1 is a process noise driving matrix, W k-1 is the process noise, H k is the observation matrix at time k.

[0073] Then, the Kalman filtering observation equation is established by taking the velocity error of the inertial navigation system as the observation variable:

[0074] Z k =H k X k +r k (7)

[0075] Wherein r k is the measurement noise at time k.

[0076] The velocity error is calculated according to the velocity pseudo-measurement provided by the kinematics model

[0077]

[0078] Wherein is the velocity in the navigation coordinate system calculated by the micro inertial navigation system at time k.

[0079] Step S4: Robust information fusion algorithm based on weighted observation function.

[0080] In order to overcome the negative impact caused by the false detection of the coarse detector in S2, the present application proposes a robust information fusion algorithm based on the weighted observation function to complete the robust estimation of the inertial navigation error state in the integrated navigation model in S3 and feedback correction, and specifically: ​

[0081] One-step prediction process:

[0082]

[0083] where, is one-step prediction state, is state estimation at k, P k,k-1 is one-step prediction of covariance matrix, P k is covariance matrix at k, Q k-1 is process noise variance matrix;

[0084] Gain matrix K k is calculated as:

[0085]

[0086] State estimation:

[0087]

[0088] Covariance matrix calculation:

[0089]

[0090] In the above process, w k is added in formula (11) (13), w k is calculated by the following weighted observation function:

[0091]

[0092] where c is a parameter that can be adjusted according to the human motion state, detector threshold and kinematic model. When the filter converges completely and the observation is ideal, at this time, the weight w k →1, and the update is performed according to the normal Kalman filter; when the observation is abnormal, becomes larger, at this time, the weight w k becomes smaller, the corresponding gain matrix is reduced, and the update degree of the covariance matrix is smaller, thereby inhibiting the negative impact of abnormal observation on state estimation, when , w k →0, at this time, the observation information is completely isolated, and the state estimation is completed only by the prediction update.

[0093] It can be seen that the present application adds w kAs the weight increases in the gain matrix and covariance matrix calculation of Kalman filtering process, the degree of control observation information is weakened. Then the information fusion algorithm with weighted observation function is used to fuse the ankle- lower leg kinematics model information and the micro inertial navigation system information, which can overcome the negative impact caused by the detector false detection, and further improve the convenience, robustness and adaptability of the overall system.

[0094] The above specific embodiments only describe the design principles of the present application, and the shapes and names of the components in the description can be different and are not limited. Therefore, those skilled in the art of the present application can modify or equivalently replace the technical solutions described in the foregoing embodiments; and these modifications and replacements do not deviate from the purpose and technical solutions of the present application, and should all belong to the protection scope of the present application.

Claims

1. A strong fault-tolerant lower leg pedestrian inertial autonomous navigation method based on weighted observation function, characterized in that: The method includes: Step S1: Preliminary identification of the stance phase is performed based on the modulus of the ankle acceleration, and the time when the human body is in the stance phase is the correction time; Step S2: constructing an ankle-calf kinematic model based on the human foot velocity being close to 0 in the stance phase; Step S3: establishing an ankle-calf kinematics / inertial navigation integrated navigation model based on the ankle-calf kinematics model and the inertial navigation error equation within a Kalman filter framework; Step S4: Construct a weighted observation function w based on the observation error k , when the observation approaches the ideal, w k Approaching 1, when the observation is abnormal, as the degree of abnormality increases, w k Reduce; w k As a weight increase in the calculation of the gain matrix and covariance matrix of the Kalman filtering process, it controls the degree to which the observation information is weakened. When w k When it is 0, the observation information is completely isolated and the state estimation is completed only by predictive update. At the correction time, the Kalman filter observation update is performed based on the ankle-calf kinematics / INS integrated navigation model, and the filtering result is used to correct the INS error. Among them, the weight w k for: where Z k is the observed variable in the ankle-calf kinematics / inertial navigation integrated navigation model, c is the adjustment parameter; is the one-step prediction state of Kalman filtering, H k is the observation matrix at time k in Kalman filtering.

2. The method according to claim 1, wherein In step S1, the module value of the ankle acceleration is calculated using the measurement data of the micro inertial measurement unit MIMU installed at any part of the lower leg. in is the ankle acceleration, For the ankle strength, is the specific force at the measurement point where the MIMU is located, ω b is the angular velocity of the measurement point, acquired by the gyroscope in the MIMU; The arm vector from the measurement point to the ankle is measured using a distance measuring device before navigation; α b is the angular acceleration of the measurement point, which is obtained by differentiating the angular velocity measured by the gyroscope in the MIMU; g is the acceleration due to gravity, and the superscript b indicates the carrier coordinate system.

3. The method according to claim 1, wherein In step S1, the identification of the stance phase according to the module value of the ankle acceleration is: The rising edge trigger correction method is used to achieve a rough detection of the stance phase, that is, the acceleration measurement sequence is windowed and sliding, if the modulus of the ankle acceleration in the sliding window at the current moment is Less than the set threshold, and the modulus of ankle acceleration in the previous sliding window When it is greater than the set threshold, the current time is used as the correction time.

4. The method according to claim 3, wherein Set the threshold γ foot is 0.1g, where g is the acceleration due to gravity.

5. The method according to claim 1, wherein In step S3, the ankle-calf kinematic model established is: The superscript n is the navigation coordinate system, and the superscript b is the carrier coordinate system. is the velocity of the measuring point, is the posture matrix of the measurement point, ω b is the angular velocity collected at the measurement point, is the arm vector from the measurement point to the ankle, r is the measurement noise, R is the measurement noise variance matrix, and E[] represents the expectation; I 3×3 is the unit matrix, and α is the measurement noise adjustment coefficient.

6. The method according to claim 1, wherein In step S4, the ankle-calf kinematics / inertial navigation integrated navigation model constructed is: Position error based on inertial navigation Speed ​​error and attitude error As a system variable Establish the Kalman filter system equation: X k =F k,k-1 X k-1 +Γ k-1 W k-1 in is the output value of the accelerometer in the navigation coordinate system, T s is the sampling period of micro inertial navigation MIMU, O 3×3 is a 3×3 zero matrix, I 3×3 is a 3×3 identity matrix; Γ k-1 is the process noise driving matrix, W k-1 is the process noise, H k is the observation matrix at time k; Velocity error of inertial guidance As the observation variable, the Kalman filter observation equation is established: Z k =H k X k +r k The observation matrix r k is the measurement noise at time k, is the angular velocity of the measurement point at time k; The velocity error is calculated based on the velocity pseudo-measurements provided by the ankle-calf kinematic model: in is the velocity pseudo-measurement at time k provided by the ankle-calf kinematic model, is the velocity in the navigation coordinate system calculated by the micro-inertial navigation at time k.

7. The method according to claim 6, wherein In step S4, when performing Kalman filter update using the ankle-calf kinematics / inertial navigation integrated navigation model, a weight w is added to the calculation of the gain matrix and the covariance matrix. k , suppressing the negative impact of abnormal observations on state estimation is: Using weight w k Adjust the gain matrix K k and the covariance matrix P k Calculation: in, is the one-step prediction state, is the state estimate at time k, P k,k-1 is the one-step prediction of the covariance matrix, P k is the covariance matrix at time k, Q k-1 is the process noise variance matrix, K k is the gain matrix at time k, H k is the observation matrix at time k, R k is the measurement noise variance matrix.

Images