A wearable multi-mems quasi-real-time collaborative navigation system and method

Abstract

The application relates to a wearable multi-MEMS quasi-real-time cooperative navigation system and method, and belongs to the field of intelligent measurement in the electronic industry. In the navigation system, each MEMS sensor chip constructs an inertial navigation subsystem, inertial navigation calculation and Kalman filtering are carried out in each sampling period in a stepping interval, when the stepping interval is completed, the Kalman filtering of each inertial navigation subsystem is stopped, backward smoothing is carried out on the state quantity obtained in the current stepping interval, then the state variables of all the inertial navigation subsystems are combined into joint state variables, constraint filtering updating and error feedback correction are carried out by using known prior information, the navigation result is timely sent through wired or wireless mode, and then the above steps are repeated in the next stepping interval. Compared with post-processing, the navigation result is sent in a quasi-real-time mode, and has strong engineering practicability.

Description

Technical Field

[0001] This invention relates to a wearable multi-MEMS near real-time collaborative navigation system and method, belonging to the field of intelligent measurement in the electronics industry. Background Technology

[0002] In recent years, with the rapid development of MEMS sensors in terms of size, weight, power consumption, and intelligence, navigation systems based on MEMS inertial sensors have received increasing attention. Wearable inertial navigation systems, in particular, generally possess advantages such as small size, light weight, low power consumption, good durability, low cost, and no radiation. They can provide fully autonomous pedestrian navigation services even in environments without external auxiliary information, and have very broad application prospects in pedestrian navigation and positioning fields such as fire rescue, individual soldier operations, and counter-terrorism operations.

[0003] However, MEMS inertial sensors also have significant drawbacks, generally suffering from low measurement accuracy and high noise. Due to drift errors in inertial devices and random noise in the signal, navigation errors accumulate over time, rendering the data almost unusable without external correction. Two solutions exist to address this problem: one is to fuse the inertial navigation system with other forms of positioning systems to improve navigation accuracy, such as integrating with GPS, vision systems, and wireless positioning systems. However, this approach significantly increases the overall system size, weight, power consumption, and complexity, severely impacting portability. The second solution is to incorporate human motion characteristics, utilizing information such as the "zero-speed interval" during the walking cycle to periodically correct navigation errors. Correction techniques based on human motion characteristics can suppress and correct navigation errors in horizontal attitude angles, velocity, and position using algorithms alone, without adding extra hardware, significantly improving the system's practicality at a low cost.

[0004] However, for pedestrian navigation and positioning applications, the biggest problem with correction techniques based on human motion characteristics is the unobservability of heading. This leads to heading error becoming the primary cause of positioning error over extended periods, and navigation accuracy deteriorates sharply with increasing heading angle error. This is a significant factor limiting the long-term positioning accuracy of inertial sensor-based pedestrian navigation systems. Currently, techniques such as zero-velocity correction, zero-angular-velocity correction, and zero-integral heading angular rate correction cannot effectively solve the problem of unobservable heading angle. Summary of the Invention

[0005] The technical problem solved by this invention is to overcome the shortcomings of the prior art and provide a wearable multi-MEMS quasi-real-time cooperative navigation system and method to suppress the heading angle error of the navigation system and ultimately improve heading and positioning accuracy.

[0006] The technical solution of the present invention is: a wearable multi-MEMS near real-time cooperative navigation system, the cooperative navigation system including a data processing chip and n MEMS sensor chips, where n≥2;

[0007] n MEMS sensor chips, whose positions in the navigation coordinate system are known, synchronously output the three-axis acceleration and three-axis angular velocity of the wearable part in the inertial coordinate system;

[0008] A data processing chip constructs n navigation subsystems. Within each step interval, the following processing is performed: Each navigation subsystem calculates the position, velocity, and attitude of the wearable device in the navigation coordinate system based on the data output from the MEMS sensor chip at each sampling cycle. The attitude angle error, velocity error, and position error of the wearable device in the navigation coordinate system, along with the gyroscope and accelerometer biases within the MEMS sensor chip, are used as state variables, and the velocity error as a measurement. A Kalman filter is used for measurement and time updates. Upon the end of the current step interval, the Kalman filter processing of each navigation subsystem is paused. The state variables obtained by the navigation subsystem in each sampling period within the current step interval are smoothed. The smoothed state variables of each navigation subsystem are then combined into a joint state variable. Combined with the position constraints of the MEMS sensor chip in the navigation coordinate system, constraint filtering is performed to update the joint state variable in each sampling period within the current step interval. The state variables corresponding to each navigation subsystem are then extracted from the joint state variable. The position, velocity, and attitude of the wearable part in the navigation coordinate system calculated by each navigation subsystem in each sampling period within the current step interval are fed back to correct the error. Finally, the Kalman filtering process of each navigation subsystem is restarted.

[0009] Preferably, the n MEMS sensor chips are integrated on the same circuit board.

[0010] Preferably, the data processing chip includes n inertial navigation calculation modules, a step interval condition judgment module, a constraint condition update module, n step interval smoothing modules, and n navigation error correction modules; the MEMS sensor chip, the inertial navigation calculation module, the step interval smoothing module, and the navigation error correction module correspond one-to-one.

[0011] The inertial navigation calculation module, together with the MEMS sensor chip, constitutes the navigation subsystem. After startup, this module selects the "East-North-Sky" geographic coordinate system as the navigation coordinate system. It acquires the three-axis acceleration and three-axis angular velocity output by the corresponding MEMS sensor chip in the inertial coordinate system at each sampling period, and performs inertial navigation calculation to obtain the attitude, velocity, and position information of the wearable part in the navigation coordinate system. The attitude angle error, velocity error, position error, MEMS gyroscope zero bias, and accelerometer zero bias in the navigation coordinate system are used as state variables, and the velocity error as a measurement. A Kalman filter is established, and zero-velocity detection is performed in each sampling period. If the wearable part is within the zero-velocity range in the current sampling period, a time update is performed first, followed by a measurement update; otherwise, only a time update is performed, and the one-step transition matrix Φ for each sampling period after the update is stored. k / k-1 Prior estimation of state quantities Posterior estimation of state quantities Prior estimation mean square error matrix P k / k-1 and the posterior estimated mean square error matrix P k value;

[0012] The step interval condition judgment module determines whether the wearing part has entered the zero speed interval and has lasted for a preset period of time in each sampling cycle. If so, it considers the current sampling cycle to be the last sampling cycle of the current step interval, starts the constraint condition update module, n step interval smoothing modules, n navigation error correction modules, and pauses n inertial navigation calculation modules.

[0013] The step interval smoothing module smooths the state variables obtained by the navigation subsystem for each sampling period.

[0014] The constraint update module combines the smoothed state variables of n navigation subsystems in each sampling period into a joint state variable, constructs a multi-MEMS joint processing system, and performs constraint filtering update processing on the joint state variable based on the constraint equations constructed by multiple MEMS sensor chips in the navigation coordinate system. After that, the work is paused.

[0015] The navigation error correction module updates the joint state variables from the constraint filtering. The state variables of the corresponding navigation subsystems are extracted and used to correct the attitude, velocity and position information in the navigation coordinate system calculated by the navigation subsystem in each sampling period within the step interval. Then, the corresponding step interval smoothing module is turned off, the corresponding inertial navigation calculation module is started, and the operation is paused.

[0016] Preferably, the time period is a preset M sampling periods, where M is greater than or equal to 10.

[0017] Preferably, the constraint filtering update process is as follows:

[0018] S6.1 Combine the smoothed state variables of the n navigation subsystems into a joint state variable X;

[0019] S6.2, Construct the joint state error covariance matrix P as follows:

[0020]

[0021] Among them, P ki , i = 1 to n is the state error covariance matrix of the i-th navigation subsystem in the k-th sampling period;

[0022] S6.3 Construct position constraint equations between each pair of MEMS sensor chips, totaling... Equations:

[0023]

[0024] Among them, P i P j Let d represent the positions of the i-th MEMS sensor chip MEMS-i and the j-th MEMS sensor chip MEMS-j in the navigation coordinate system. i,j Let be the distance between the i-th MEMS sensor chip MEMS-i and the j-th MEMS sensor chip MEMS-j;

[0025] S6.4. Using mathematical methods, construct the Lagrangian function, and obtain the joint state variables after constraint filtering update by solving for the stationary points of the Lagrangian function.

[0026]

[0027] in: Let be the objective function. These are the state variables after constraint filtering. Let W be the state variables before constraint filtering, and W be a positive definite projective symmetric matrix, where W = P. -1 , These are equality constraints. For positional constraints, λ is a Lagrange multiplier.

[0028] Preferably, the smoothing algorithm in the step interval smoothing module is as follows:

[0029]

[0030] Among them, K s,k Let P be the smoothing filter gain matrix for the k-th sampling period. f,k Let Φ be the state error covariance matrix of the Kalman filter in the k-th sampling period. k+1 / k Let P be the state transition matrix for the (k+1)th sampling period.f,k+1 / k Let the state error covariance matrix be the Kalman filter state error matrix for the (k+1)th sampling period. Let be the smoothed state variable after the kth sampling period. Let be the smoothed state variable after the (k+1)th sampling period. Let P be the state variable of the Kalman filter in the k-th sampling period. s,k Let P be the state error covariance matrix after smoothing and filtering in the k-th sampling period. s,k+1 The state error covariance matrix is ​​the result of smoothing and filtering during the (k+1)th sampling period. This is the prior estimate of the state variables for the (k+1)th sampling period.

[0031] Another technical solution of the present invention is: a wearable multi-MEMS quasi-real-time cooperative navigation method, which performs the following steps within a step interval:

[0032] S1. Construct n inertial navigation subsystems. The n inertial navigation subsystems select the "East-North-Sky" geographic coordinate system as the navigation coordinate system. Simultaneously acquire the three-axis acceleration and three-axis angular velocity measured by n MEMS sensor chips (104) in each sampling period. Perform inertial navigation calculations respectively to obtain the attitude, velocity and position information of the wearable parts corresponding to the n inertial navigation subsystems in the navigation coordinate system.

[0033] S2. The attitude angle error, velocity error, and position error of the wearable part in the navigation coordinate system, the zero bias of the gyroscope and the zero bias of the accelerometer in the MEMS sensor chip (104) are taken as state variables, and the velocity error is taken as a measurement. The Kalman filter method is used to update the measurement and time, and the one-step transition matrix Φ obtained in each sampling period is stored. k / k-1 Prior estimation of state quantities Posterior estimation of state quantities Prior estimation mean square error matrix P k / k-1 and the posterior estimated mean square error matrix P k value;

[0034] S3. Determine whether the wearing part has entered the zero speed range and has lasted for a preset period of time. If so, consider the current sampling period to be the last sampling period of the current step range and proceed to step S4; otherwise, return to step S1 for the next sampling period.

[0035] S4. Each inertial navigation subsystem stops Kalman filtering and begins backward smoothing to obtain the smoothed state variable estimate for each cycle within the current step interval.

[0036] S5. Combine the smoothed state variables corresponding to the n navigation subsystems in each sampling period within the current step interval into a joint state variable, construct a joint processing system, and perform constraint filtering update processing on the joint state variable based on the constraint equations constructed from the position information of multiple MEMS sensor chips in the navigation coordinate system; use the constraint-filtered updated joint state variable... The attitude, velocity, and position information in the navigation coordinate system calculated by each navigation subsystem in each sampling period within the step interval are corrected, and when the next sampling period arrives, the process returns to step S1.

[0037] Preferably, the time period is a preset M sampling periods, where M is greater than or equal to 10.

[0038] Preferably, the smoothing algorithm in the step interval smoothing module is as follows:

[0039]

[0040] Among them, K s,k Let P be the smoothing filter gain matrix for the k-th sampling period. f,k Let Φ be the state error covariance matrix of the Kalman filter in the k-th sampling period. k+1 / k Let P be the state transition matrix for the (k+1)th sampling period. f,k+1 / k Let the state error covariance matrix be the Kalman filter state error matrix for the (k+1)th sampling period. Let be the smoothed state variable after the kth sampling period. Let be the smoothed state variable after the (k+1)th sampling period. Let P be the state variable of the Kalman filter in the k-th sampling period. s,k Let P be the state error covariance matrix after smoothing and filtering in the k-th sampling period. s,k+1 The state error covariance matrix is ​​the result of smoothing and filtering during the (k+1)th sampling period. This is the prior estimate of the state variables for the (k+1)th sampling period.

[0041] Preferably, the constraint filtering update process is as follows:

[0042] S6.1 Combine the smoothed state variables of the n navigation subsystems into a joint state variable.

[0043] S6.2. Combine multiple inertial navigation subsystems into a multi-MEMS joint processing system. The state error covariance matrix of the multi-MEMS joint processing system is as follows:

[0044]

[0045] Among them, P ki, i = 1 to n is the state error covariance matrix of the i-th navigation subsystem in the k-th sampling period;

[0046] S6.3, Construct positional constraint formulas between any two MEMS sensor chips, totaling... Equations:

[0047]

[0048] Among them, P i P j Let d represent the positions of the i-th MEMS sensor chip MEMS-i and the j-th MEMS sensor chip MEMS-j in the navigation coordinate system. i,j Let be the distance between the i-th MEMS sensor chip MEMS-i and the j-th MEMS sensor chip MEMS-j;

[0049] S6.4. Using mathematical methods, construct the Lagrangian function, and obtain the joint state variables after constraint filtering update by solving for the stationary points of the Lagrangian function.

[0050]

[0051] in: Let be the objective function. These are the state variables after constraint filtering. Let W be the state variables before constraint filtering, and W be a positive definite projective symmetric matrix, where W = P. -1 , These are equality constraints. For positional constraints, λ is a Lagrange multiplier.

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

[0053] (1) This invention solves the problem that the heading of a traditional single MEMS sensor is unobservable under long-term use. By integrating two or more MEMS sensors in a system, multiple inertial navigation subsystems are constructed. By constructing constraint information based on physical space position and mutual coordination algorithms between each inertial navigation subsystem, the relative heading of the system is observable, thereby effectively suppressing the heading error of the system and greatly improving the navigation and positioning accuracy of the system.

[0054] (2) This invention improves the accuracy of navigation distance calculation by an order of magnitude by performing backward smoothing on navigation data through a step-by-step method. When a person walks, the posture and velocity information will change abruptly when transitioning from the swing phase to the support phase, and the position information will also show sawtooth fluctuations. This invention adopts a step-by-step backward smoothing technology, which uses the zero-velocity information on both sides of the swing phase to estimate and correct the navigation error in the swing phase stage, so as to ensure a smooth transition between the swing phase and the support phase, further improving navigation and positioning accuracy;

[0055] (3) Compared with the previous navigation systems that used smoothing technology and could only perform post-processing after all the collected data was obtained, the navigation system of the present invention completes sensor data acquisition, inertial navigation calculation, Kalman filtering and step backward smoothing processing, as well as collaborative navigation and error correction between multiple MEMS in real time during operation, and sends the navigation results to the outside world in a near real-time manner. The navigation information delay time is only one gait cycle, which greatly improves the engineering practicality of the system. Attached Figure Description

[0056] Figure 1 This invention relates to a wearable multi-MEMS near real-time cooperative navigation system.

[0057] Figure 2 This is a flowchart of the wearable multi-MEMS near real-time cooperative navigation algorithm in this invention;

[0058] Figure 3 These are the test results from embodiments of the present invention;

[0059] Figure 4 This is a magnified view of a portion of the test results in an embodiment of the present invention. Detailed Implementation

[0060] The present invention will now be described in further detail with reference to the accompanying drawings and specific examples:

[0061] like Figure 1 The wearable multi-MEMS near real-time cooperative navigation system shown is applied to pedestrian navigation. It consists of an upper shell 100, a lower shell 101, a battery 102, and a circuit board 103. The circuit board 103 contains n MEMS sensor chips 104, each with a fixed position on the circuit board and a known position in the navigation coordinate system, where n ≥ 2. The circuit board 103 includes a data processing chip 105 and a communication module 106. The data processing chip 105 includes, but is not limited to, processors such as ARM, DSP, and CPU, and is used to receive, process, fuse, correct, and frame all MEMS sensor data. The communication module 106 includes, but is not limited to, chips with serial ports, WIFI, Bluetooth, and Zigbee, and is used to transmit navigation results to external devices in real time via wired or wireless means.

[0062] n MEMS sensor chips 104, synchronously outputting the three-axis acceleration and three-axis angular velocity of the wearable part in the inertial coordinate system;

[0063] Data processing chip 105 constructs n navigation subsystems, performing the following processing in each step interval: Each navigation subsystem performs navigation calculations based on the data output by MEMS sensor chip 104 in each sampling cycle, obtaining the position, velocity, and attitude of the wearable part in the navigation coordinate system. The attitude angle error, velocity error, and position error of the wearable part in the navigation coordinate system, along with the gyroscope zero bias and accelerometer zero bias within MEMS sensor chip 104, are used as state variables, and the velocity error as a measurement. Kalman filtering is used for measurement updates and time updates. When the current step interval ends, the Kalman filtering of each navigation subsystem is paused. The process involves smoothing the state variables obtained by the navigation subsystem for each sampling period within the current step interval, then combining the smoothed state variables of each navigation subsystem into a joint state variable. Combined with the position constraints of the MEMS sensor chip 104 in the navigation coordinate system, constraint filtering is performed to update the joint state variable for each sampling period within the current step interval. The state variables corresponding to each navigation subsystem are then extracted from the joint state variable, and the position, velocity, and attitude of the wearable part in the navigation coordinate system calculated by each navigation subsystem in each sampling period within the current step interval are fed back to correct the error. Finally, the Kalman filtering process of each navigation subsystem is restarted.

[0064] like Figure 2 As shown, the data processing chip 105 includes n inertial navigation calculation modules, a step interval condition judgment module, a constraint condition update module, n step interval smoothing modules, and n navigation error correction modules; the MEMS sensor chip 104, the inertial navigation calculation module, the step interval smoothing module, and the navigation error correction module correspond one-to-one.

[0065] The inertial navigation calculation module, together with the MEMS sensor chip 104, constitutes the navigation subsystem. After startup, this module selects the "East-North-Sky" geographic coordinate system as the navigation coordinate system, acquires the three-axis acceleration and three-axis angular velocity in the inertial coordinate system output by the corresponding MEMS sensor chip 104 in each sampling period, and performs inertial navigation calculation to obtain the attitude, velocity, and position information of the wearable part in the navigation coordinate system. The attitude angle error, velocity error, position error, MEMS gyroscope zero bias, and accelerometer zero bias in the navigation coordinate system are used as state variables, and the velocity error is used as a measurement. A Kalman filter is established, and zero velocity detection is performed in each sampling period. If the wearable part is in the zero velocity range in the current sampling period, time update is performed first, followed by measurement update; otherwise, only time update is performed, and the one-step transition matrix Φ of each sampling period after the update is stored. k / k-1 Prior estimation of state quantities Posterior estimation of state quantities Prior estimation mean square error matrix P k / k-1 and the posterior estimated mean square error matrix P k value;

[0066] The step interval condition judgment module determines whether the wearing part has entered the zero speed interval and has lasted for a preset period of time in each sampling cycle. If so, it considers the current sampling cycle to be the last sampling cycle of the current step interval, starts the constraint condition update module, n step interval smoothing modules, n navigation error correction modules, and pauses n inertial navigation calculation modules.

[0067] The step interval smoothing module smooths the state variables obtained by the navigation subsystem for each sampling period.

[0068] The constraint update module combines the smoothed state variables of n navigation subsystems in each sampling period within the current step interval into a joint state variable, constructs a multi-MEMS joint processing system, and performs constraint filtering update processing on the joint state variable based on the constraint equations constructed by multiple MEMS sensor chips in the navigation coordinate system. After that, the work is paused.

[0069] The navigation error correction module updates the joint state variables from the constraint filtering. The corresponding state variables of the navigation subsystem are extracted and used to correct the attitude, velocity, and position information in the navigation coordinate system calculated by the navigation subsystem for each sampling period within the step interval. Then, the constraint update module and the n step interval smoothing module are closed, the n inertial navigation calculation modules are started, and the operation is paused. The time period is a preset M sampling periods, where M is greater than or equal to 10.

[0070] The constraint filtering update process is as follows:

[0071] S6.1 Combine the smoothed state variables of the n navigation subsystems into a joint state variable X;

[0072] S6.2, Construct the joint state error covariance matrix P as follows:

[0073]

[0074] Among them, P ki , i = 1 to n is the state error covariance matrix of the i-th navigation subsystem in the k-th sampling period;

[0075] S6.3 Construct position constraint equations between each pair of MEMS sensor chips, totaling... Equations:

[0076]

[0077] Among them, P i P j Let d represent the positions of the i-th MEMS sensor chip MEMS-i and the j-th MEMS sensor chip MEMS-j in the navigation coordinate system. i,j Let be the distance between the i-th MEMS sensor chip MEMS-i and the j-th MEMS sensor chip MEMS-j.

[0078] S6.4. Using mathematical methods, construct the Lagrangian function, and obtain the joint state variables after constraint filtering update by solving for the stationary points of the Lagrangian function.

[0079]

[0080] in: Let be the objective function. These are the state variables after constraint filtering. Let W be the state variables before constraint filtering, and W be a positive definite projective symmetric matrix, where W = P. -1 , These are equality constraints. For positional constraints, λ is a Lagrange multiplier.

[0081] The smoothing algorithm in the step interval smoothing module is as follows:

[0082]

[0083] Among them, K s,k Let P be the smoothing filter gain matrix for the k-th sampling period. f,k Let Φ be the state error covariance matrix of the Kalman filter in the k-th sampling period. k+1 / k Let P be the state transition matrix for the (k+1)th sampling period. f,k+1 / k Let the state error covariance matrix be the Kalman filter state error matrix for the (k+1)th sampling period. Let be the smoothed state variable after the kth sampling period. Let be the smoothed state variable after the (k+1)th sampling period. Let P be the state variable of the Kalman filter in the k-th sampling period. s,k Let P be the state error covariance matrix after smoothing and filtering in the k-th sampling period. s，k+1 The state error covariance matrix is ​​the result of smoothing and filtering during the (k+1)th sampling period. This is the prior estimate of the state variables for the (k+1)th sampling period.

[0084] Based on the above system, the present invention also provides a wearable multi-MEMS near real-time cooperative navigation method, which performs the following steps within the step interval:

[0085] S1. Construct n inertial navigation subsystems. The n inertial navigation subsystems select the "East-North-Sky" geographic coordinate system as the navigation coordinate system. Simultaneously acquire the three-axis acceleration and three-axis angular velocity measured by n MEMS sensor chips (104) in each sampling period. Perform inertial navigation calculations respectively to obtain the attitude, velocity and position information of the wearable parts corresponding to the n inertial navigation subsystems in the navigation coordinate system.

[0086] The attitude data of the worn parts in the navigation coordinate system are calculated through the following steps:

[0087] S1.1 Obtain the triaxial angular velocities of the worn part in the inertial coordinate system. ;

[0088] S1.2, Based on the triaxial angular velocity of the wearing part in the inertial coordinate system The triaxial angular velocities of the worn parts in the navigation coordinate system were calculated.

[0089] From the angular velocity equation, we get:

[0090]

[0091] in: Let be the projection of the angular velocity of the vehicle coordinate system relative to the navigation coordinate system onto the vehicle coordinate system. Let be the projection of the angular velocity of the carrier coordinate system relative to the inertial coordinate system onto the carrier coordinate system. Let be the projection of the angular velocity of the Earth coordinate system relative to the inertial coordinate system onto the carrier coordinate system. This is the projection of the angular velocity of the navigation coordinate system relative to the Earth coordinate system onto the vehicle coordinate system.

[0092] For pedestrian navigation applications and considering the accuracy of MEMS sensors, the above formula is approximately equivalent to:

[0093]

[0094] S1.3 Calculate the posture quaternion Q of the wearing part in the current sampling period. k =[q1 q2 q3 q4]

[0095]

[0096] Where Δt is the triaxial angular velocity in the inertial coordinate system. Sampling period, Q is the coordinate transformation matrix from the vehicle coordinate system to the navigation coordinate system. k-1 The quaternion represents the posture of the wearing part during the last sampling period;

[0097] and Q k The initial values ​​are calculated from the initial attitude angles θ0, γ0, ψ0 of the wearing part in the navigation coordinate system obtained from the initial alignment, and then calculated from the continuously updated quaternions.

[0098] S1.4, Based on the quaternion Q of the posture of the wearing part in the current sampling period. k Calculate the coordinate transformation matrix from the vehicle coordinate system to the navigation coordinate system. :

[0099]

[0100] S1.5, Based on the coordinate transformation matrix from the vehicle coordinate system to the navigation coordinate system The attitude of the worn part in the navigation coordinate system is calculated, including pitch angle θ, roll angle γ, and yaw angle ψ. The specific calculation method is as follows:

[0101] Depend on get

[0102] θ = arcsin(T) 32 )

[0103]

[0104]

[0105] The velocity of the worn part in the navigation coordinate system is calculated through the following steps:

[0106] S1.6, Coordinate transformation matrix from vehicle coordinate system to navigation coordinate system Substituting into the force equation, we obtain the projection of the acceleration of the navigation coordinate system relative to the Earth coordinate system onto the navigation coordinate system.

[0107] The specific force equation is as follows:

[0108]

[0109] Among them, f b For the three axes of acceleration of the carrier, This is the projection of the angular velocity of the Earth coordinate system relative to the inertial coordinate system onto the navigation coordinate system. This is the projection of the velocity of the navigation coordinate system relative to the Earth coordinate system onto the navigation coordinate system. Let g be the projection of the angular velocity of the navigation coordinate system relative to the Earth coordinate system onto the navigation coordinate system. n This is the projection of gravitational acceleration onto the navigation coordinate system.

[0110] For pedestrian navigation applications and considering the accuracy of MEMS sensors, the above formula is approximately equivalent to:

[0111]

[0112] S1.7, from the formula Update the projection of the velocity of the navigation coordinate system relative to the Earth coordinate system onto the navigation coordinate system. This refers to the velocity of the worn part in the navigation coordinate system. This is the projection of the velocity of the navigation coordinate system relative to the Earth coordinate system in the navigation coordinate system during the previous sampling period. The projection of the velocity of the navigation coordinate system relative to the Earth coordinate system in the navigation coordinate system during the current sampling period.

[0113] The position of the measured wearable part in the navigation coordinate system is updated using the following equation:

[0114]

[0115] Among them, P k-1 P represents the position of the previous sampling period. k This represents the position of the current sampling period. This is the projection of the velocity of the navigation coordinate system relative to the Earth coordinate system in the navigation coordinate system during the previous sampling period.

[0116] In summary, the posture, velocity, and position information of the worn parts can be obtained.

[0117] S2. Using the attitude angle error, velocity error, and position error of the wearable part in the navigation coordinate system, the gyroscope zero bias and accelerometer zero bias of the MEMS sensor chip 104 as state variables, and the velocity error as a measurement variable, the Kalman filter method is used to update the measurement and time, and the one-step transition matrix Φ obtained in each sampling period is stored. k / k-1 Prior estimation of state quantities Posterior estimation of state quantities Prior estimation mean square error matrix P k / k-1 and the posterior estimated mean square error matrix P k Value; the specific method is as follows:

[0118] S2.1. Based on the attitude error equation, velocity error equation, and position error equation, the state equation expression can be obtained as follows:

[0119] X k =Φ k / k-1 X k-1 +Γ k-1 W k-1

[0120] X is the state variable, Φ is the one-step transition matrix, Γ is the process noise allocation matrix, W is the process noise matrix, k-1 and k represent the (k-1)th sampling period and the kth sampling period, respectively, and k / k-1 represents the one-step prediction from the (k-1)th sampling period to the kth sampling period.

[0121] in:

[0122]

[0123] δv represents the attitude angle error of the wearable part in the navigation coordinate system. x δv y δv z δx, δy, and δz represent the velocity error of the worn part in the navigation coordinate system, and ε represents the position error of the worn part in the navigation coordinate system. bx ε by ε bz To achieve zero bias in the gyroscope, To achieve zero bias in the accelerometer;

[0124] The one-step transition matrix is

[0125]

[0126] The process noise matrix is

[0127] W = [w gx w gy w gz w ax w ay w az ] T

[0128] Where W is the process noise, w gx w gy w gz The noise of the three-axis gyroscope is represented by w. ax w ay w az For the noise of the triaxial accelerometer, f b The acceleration of the carrier along its three axes in the inertial coordinate system.

[0129] The process noise allocation matrix is:

[0130]

[0131] S2.2 Regarding the zero-velocity error correction, the measurement equation expression can be obtained as follows:

[0132] Z k =H k X k +U k

[0133] Among them, the measurement is

[0134]

[0135] V x V y V z These are the three-axis components of the velocity of the worn part in the navigation coordinate system;

[0136] The measurement matrix is

[0137] H = [O 3×3 I 3×3 O 3×3 O 3×3 O 3×3 ]

[0138] The measurement noise matrix U is

[0139]

[0140] in, These are the three-axis speed error noises.

[0141] S2.3. Based on the Kalman filtering algorithm, the continuous equation is discretized and substituted into the following formula:

[0142] State prediction in one step

[0143]

[0144] in, This is the optimal estimate of the state from the previous sampling period. For state estimation from the previous sampling period to the current sampling period, Φ k / k-1 It is the one-step transition matrix from the previous sampling period to the current sampling period.

[0145] State one-step prediction mean square error matrix

[0146]

[0147] Among them, P k / k-1 Let P be the mean square error matrix from the previous sampling period to the current time. k-1 Let Γ be the mean square error matrix of the previous sampling period. k-1 The noise assignment matrix for the previous sampling period, Q k-1 This is the process noise covariance matrix of the previous sampling period.

[0148] Filter gain

[0149]

[0150] Among them, K k P is the filter gain for the current sampling period. k / k-1 H is the mean square error matrix for the current sampling period. k R is the measurement matrix for the current sampling period. k This is the measurement noise covariance matrix for the current sampling period.

[0151] State estimation

[0152]

[0153] in, This is the optimal estimate of the state for the current sampling period. For state estimation from the previous sampling period to the current sampling period, K k Z is the filter gain for the current sampling period. k For the current sampling period measurement, H k This is the measurement matrix for the current sampling period.

[0154] State estimation mean square error matrix

[0155] P k =(IK k H k )P k / k-1

[0156] Among them, P k Let P be the mean square error matrix for the current sampling period. k / k-1 Let I be the mean square error matrix from the previous sampling period to the current sampling period, and K be the identity matrix. k H is the filter gain for the current sampling period. k This is the measurement matrix for the current sampling period.

[0157] Since zero-velocity measurements only occur in the zero-velocity interval, the Kalman filter only updates the time and not the measurements within that interval. Once the zero-velocity interval is detected, the filter performs both time and measurement updates.

[0158] Each inertial navigation subsystem stores its own one-step transfer matrix Φ for each cycle. k / k-1 Prior estimation Posterior estimation Prior estimation mean square error matrix P k / k-1 and the posterior estimated mean square error matrix P k value;

[0159] S3. Determine whether the wearing part has entered the zero speed range and has lasted for a preset period of time. If so, consider the current sampling period to be the last sampling period of the current step range and proceed to step S4; otherwise, return to step S1 for the next sampling period to perform inertial navigation calculation and Kalman filtering for the next sampling period; the time period is a preset M sampling periods, where M is greater than or equal to 10.

[0160] After all inertial navigation subsystems complete the inertial navigation calculation, zero-velocity detection, Kalman filter time update, and measurement update for the current cycle, they determine whether the wearable multi-MEMS quasi-real-time cooperative navigation system has entered the zero-velocity interval for the current cycle. In a specific embodiment of the present invention, when the human motion state changes from the swing phase to the support phase, and the support phase state lasts for several cycles, it is considered that the wearable system has entered the zero-velocity interval.

[0161] S4. Each inertial navigation subsystem stops Kalman filtering and begins backward smoothing to obtain the smoothed state variable estimate for each cycle within the current step interval; the specific calculation steps are as follows:

[0162] S4.1 The initial values ​​of the backward smoothing state variables and mean square error matrix of each inertial navigation subsystem are set to the final values ​​of the Kalman filter state variables and mean square error matrix of that inertial navigation subsystem.

[0163] S4.2 Starting from the last frame of the current step interval, execute the following smoothing algorithm from back to front until the first frame of the current step interval ends:

[0164]

[0165] Among them, K s,k Let P be the smoothing filter gain matrix for the k-th sampling period. f,k Let Φ be the state error covariance matrix of the Kalman filter in the k-th sampling period. k+1 / k Let P be the state transition matrix for the (k+1)th sampling period. f,k+1 / k Let the state error covariance matrix be the Kalman filter state error matrix for the (k+1)th sampling period. Let be the smoothed state variable after the kth sampling period. Let be the smoothed state variable after the (k+1)th sampling period. Let P be the state variable of the Kalman filter in the k-th sampling period. s,k Let P be the state error covariance matrix after smoothing and filtering in the k-th sampling period. s,k+1 The state error covariance matrix is ​​the result of smoothing and filtering during the (k+1)th sampling period. This is the prior estimate of the state variables for the (k+1)th sampling period.

[0166] S5. Combine the smoothed state variables corresponding to the n navigation subsystems in each sampling period within the current step interval into a joint state variable, construct a multi-MEMS joint processing system, and perform constraint filtering update processing on the joint state variable based on the constraint equations constructed from the position information of multiple MEMS sensor chips in the navigation coordinate system; use the updated joint state variable after constraint filtering. The attitude, velocity, and position information in the navigation coordinate system calculated by each navigation subsystem in each sampling period within the step interval are corrected, and when the next sampling period arrives, the process returns to step S1.

[0167] The constraint filtering update process is as follows:

[0168] S5.1 Combine the smoothed state variables of the n navigation subsystems into a joint state variable X;

[0169] S5.2. Combine multiple inertial navigation subsystems into a multi-MEMS joint processing system. The state error covariance matrix of the multi-MEMS joint processing system is as follows:

[0170]

[0171] Among them, P ki , i = 1 to n is the state error covariance matrix of the i-th navigation subsystem in the k-th sampling period; S6.3, construct the position constraint formula between each pair of MEMS sensor chips, a total of Equations:

[0172]

[0173] Among them, P i P j Let d represent the positions of the i-th MEMS sensor chip MEMS-i and the j-th MEMS sensor chip MEMS-j in the navigation coordinate system. i,j Let be the distance between the i-th MEMS sensor chip MEMS-i and the j-th MEMS sensor chip MEMS-j;

[0174] S5.3. Using mathematical methods, construct the Lagrangian function, and obtain the joint state variables after constraint filtering update by solving for the stationary points of the Lagrangian function.

[0175]

[0176] in: Let be the objective function. These are the state variables after constraint filtering. Let W be the state variables before constraint filtering, and W be a positive definite projective symmetric matrix, where W = P.-1 , These are equality constraints. For positional constraints, λ is a Lagrange multiplier.

[0177] This step performs constraint filtering updates based on known collaborative information, ensuring that the optimal solution of the joint system strictly satisfies the constraint relationships between the state variables.

[0178] In summary, this invention integrates multiple MEMS sensors onto the same circuit board, constructs constraint equations based on position information in physical space, and uses a cooperative navigation algorithm to perform near real-time backward smoothing and data fusion processing on information from multiple sensors within the system, thereby significantly improving filtering performance. Then, through mutual cooperative correction among the MEMS sensors, the system's heading angle error is suppressed, ultimately improving heading and positioning accuracy.

[0179] This invention solves the problem of unobservable absolute heading in a single MEMS system. By embedding two or more MEMS sensors and their cooperative navigation algorithm on a single circuit board, the relative heading of the system becomes observable, effectively suppressing heading errors. Furthermore, the system performs backward smoothing in a stepwise manner, improving the accuracy of navigation distance calculation by an order of magnitude.

[0180] Example:

[0181] The current embodiment takes dual MEMS as an example, including a wearable dual MEMS quasi-real-time cooperative navigation system hardware and a dual MEMS quasi-real-time backward smoothing navigation algorithm based on cooperative information. In the current embodiment, the wearable multi-MEMS quasi-real-time cooperative navigation system consists of an upper shell (100), a lower shell (101), a battery (102), and a circuit board (103). The circuit board (103) contains two MEMS sensor chips (104), and the position of each chip on the circuit board is fixed and known. Since the distance between the two is negligible compared to the pedestrian navigation distance and position divergence error, it can also be considered that the distance between the two MEMS sensor chips in the three directions is approximately 0.

[0182] The wearable multi-MEMS near real-time cooperative navigation system circuit board 103 includes a data processing chip 105 and a communication module 106. In this embodiment, the data processing chip 105 is an ARM chip, used to receive, process, fuse, correct, and frame the output data of all MEMS sensor chips; the communication module 106 in this embodiment is a WIFI chip, used to send the navigation results to the outside world in real time via WIFI wireless transmission.

[0183] A near real-time backward smoothing navigation algorithm based on cooperative information using dual MEMS is as follows: Figure 1As shown, the specific implementation involves performing the following steps for each progress interval:

[0184] S1. Both inertial navigation subsystems select the "East-North-Sky" geographic coordinate system as the navigation coordinate system, obtain the three-axis acceleration and three-axis angular velocity of the sensor in the inertial coordinate system, perform inertial navigation calculation, and obtain the attitude, velocity and position information of the sensor in the navigation coordinate system.

[0185] S2. The attitude angle error, velocity error, position error, MEMS gyroscope bias, and accelerometer bias of each inertial navigation subsystem in the navigation coordinate system are used as state variables, and the velocity error of the wearable part in the zero-velocity range is used as a measurement. A Kalman filter is established, and time updates are performed in each sampling period. In this embodiment, the Kalman filter adopts an open-loop Kalman filter algorithm based on ZUPT.

[0186] If the worn part is in the zero-velocity range during the sampling period, a measurement update is required. Since zero-velocity measurements only occur in the zero-velocity range, the Kalman filter only performs time updates and not measurement updates within the zero-velocity range; once the zero-velocity range is detected, the Kalman filter performs both time and measurement updates.

[0187] S3. Each inertial navigation subsystem stores its own one-step transfer matrix Φ for each cycle. k / k-1 Prior estimation Posterior estimation Prior estimation mean square error matrix P k / k-1 and the posterior estimated mean square error matrix P k value;

[0188] S4. After the two inertial navigation subsystems have completed the inertial navigation calculation, zero velocity detection, Kalman filter time update, and measurement update for the current sampling period, determine whether the wearable part of the wearable dual MEMS quasi-real-time cooperative navigation system has entered the zero velocity range in the current period. When the human motion state changes from the swing phase to the support phase and the support phase lasts for several periods (in this embodiment, it is set to 8 sampling periods), this is the end point of the current step interval. Proceed to step S5; otherwise, return to step S1 and proceed to the inertial navigation calculation and forward Kalman filter for the next period of the current step interval.

[0189] S5. The two inertial navigation subsystems stop forward filtering and begin backward smoothing to obtain the smoothed state variable estimates for each cycle within the current step interval. The calculation steps are as follows:

[0190] S5.1 The initial values ​​of the backward smoothing state variables and mean square error matrix of each inertial navigation subsystem are set to the final values ​​of the forward Kalman filter state variables and mean square error matrix of that inertial navigation subsystem.

[0191] S5.2 Starting from the last frame of the current step interval, execute the following smoothing algorithm from back to front until the first frame of the current step interval ends:

[0192]

[0193] Among them, K s,k Let P be the smoothing filter gain matrix at time k. f,k Let φ be the state error covariance matrix of the forward Kalman filter at time k. k+1 / k Let P be the state transition matrix at time k+1. f,k+1 / k Let k+1 be the state error covariance matrix of the forward Kalman filter. Let k be the state variable after smoothing and filtering. Let P be the state variable of the forward Kalman filter at time k. s,k Let be the state error covariance matrix after smoothing and filtering at time k.

[0194] S6. Combine the two inertial navigation subsystems into a dual-MEMS joint system, and perform constraint filtering updates based on known cooperative information to ensure that the optimal solution of the joint system strictly satisfies the constraint relationships between the state variables. The calculation method is as follows:

[0195] S6.1. Using the smoothed state variables X1 and X2 of the two subsystems, a joint state variable X is formed. The state variables of the joint system are:

[0196] X = [X1 X2] T

[0197] S6.2. Multiple inertial navigation subsystems are combined into a multi-MEMS joint processing system. The state error covariance matrix of the multi-MEMS joint processing system is:

[0198]

[0199] S6.2 Constructing Equality Constraint Formulas:

[0200] DX = d

[0201] Where: D is a 3×30 design matrix:

[0202] D = [O 3×6 I 3×3 O 3×6 O 3×6 -I 3×3 O 3×6 ]

[0203] d represents the distance between the two MEMS in three directions within the navigation coordinate system:

[0204] d = [x k1-x k2 y k1 -y k2 z k1 -z k2 ] T

[0205] Where: x k1 x k2 The eastward position y is calculated by the inertial navigation of MEMS1 and MEMS2 sensors at time k. k1 y k2 The northward position z is calculated by the inertial navigation of MEMS1 and MEMS2 sensors at time k. k1 z k2 It is the celestial position calculated by the MEMS1 and MEMS2 sensors at time k using inertial navigation.

[0206] S6.3. Construct the Lagrange equations using mathematical methods. By finding the Lagrange stationary points and solving the equality constraint equations, i.e., substituting them into the following formula, the joint state variables after constraint filtering update can be obtained.

[0207]

[0208] S7. Estimate the state variables of the joint system. The state variables are restored to the two inertial navigation subsystems X1 and X2. Each subsystem uses the state variable estimates updated by constraint filtering. Starting from the first frame of the current step interval, the navigation parameters are fed back and corrected frame by frame to correct the attitude, velocity and position information of each inertial navigation subsystem in the navigation coordinate system until the last frame of the current step interval.

[0209] S8. Pack the navigation results corrected by the two inertial navigation subsystems in the current step interval into frames and send them to the host computer. Then return to step S1 and start the inertial navigation calculation and forward Kalman filtering for the first cycle in the next step interval until the pedestrian navigation system stops working.

[0210] Experimental verification was conducted for this embodiment, and the specific experimental methods are as follows:

[0211] The wearable dual-MEMS quasi-real-time cooperative navigation system developed by Times Optoelectronics Co., Ltd. was worn on the right foot of the test participant via a strap. After standing still for 5 seconds at a starting point on the third floor of an office building, the participant walked counter-clockwise around the rectangular stairwell and returned to the starting point. During the experiment, the server received navigation data in real time from the navigation system, including the navigation trajectories of the first and second inertial navigation subsystems, as well as the navigation trajectory of the dual-MEMS quasi-real-time cooperative navigation system with backward smoothing. This data was plotted as curves on the same graph. Figure 3and Figure 4 As shown.

[0212] 6. The experimental results were analyzed, and the conclusions are as follows:

[0213] Depend on Figure 3 It can be seen that, compared with a single MEMS inertial navigation system, the dual-MEMS quasi-real-time cooperative navigation system can effectively suppress the divergence of heading errors, and the positioning accuracy of the cooperative navigation algorithm is much higher than that of a single inertial navigation subsystem; Figure 4 It can be seen that, compared with the cooperative navigation algorithm without backsmoothing, the motion trajectory is smoother, the jump caused by zero-speed correction is basically eliminated, and the distance accuracy is significantly improved.

[0214] The above description is only the best specific implementation method and test results of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the protection scope of the present invention.

[0215] The contents not described in detail in this specification are common knowledge to those skilled in the art.

Claims

1. A wearable multi-MEMS near real-time cooperative navigation system, characterized in that... It includes a data processing chip (105) and n MEMS sensor chips (104). ; n MEMS sensor chips (104) are known in the navigation coordinate system and synchronously output the three-axis acceleration and three-axis angular velocity of the wearable part in the inertial coordinate system. The data processing chip (105) constructs n navigation subsystems, and performs the following processing in each step interval: Each navigation subsystem performs navigation calculations based on the data output by the MEMS sensor chip (104) in each sampling cycle to obtain the position, velocity, and attitude of the wearable part in the navigation coordinate system. The attitude angle error, velocity error, and position error of the wearable part in the navigation coordinate system, the gyroscope zero bias and accelerometer zero bias in the MEMS sensor chip (104) are used as state variables, and the velocity error is used as a measurement. The Kalman filter method is used to update the measurement and time. When the current step interval ends, the Kalman filter of each navigation subsystem is paused. The filtering process smooths the state variables obtained by the navigation subsystem in each sampling period within the current step interval. Then, the smoothed state variables of each navigation subsystem are combined into a joint state variable. Combined with the position constraints of the MEMS sensor chip (104) in the navigation coordinate system, constraint filtering is performed to update the joint state variable in each sampling period within the current step interval. Then, the state variables corresponding to each navigation subsystem are extracted from the joint state variable and fed back to correct the position, velocity, and attitude of the wearable part in the navigation coordinate system calculated by each navigation subsystem in each sampling period within the current step interval. After that, the Kalman filtering process of each navigation subsystem is restarted. The constraint filtering update process is as follows: S6.1 Combine the smoothed state variables of the n navigation subsystems into a joint state variable. ; S6.2 Constructing the joint state error covariance matrix for: in, For the first The first navigation subsystem The state error covariance matrix of the sampling period; S6.3 Construct the equality constraint equations between each pair of MEMS sensor chips, totaling... Equations: in, , The first The MEMS sensor chip MEMS-i, the first The position of the MEMS sensor chip MEMS-j in the navigation coordinate system For the first The MEMS sensor chip MEMS-i and the first The distance between MEMS sensor chips MEMS-j; S6.

4. Using mathematical methods, construct the Lagrangian function, and by solving for the stationary points of the Lagrangian function, obtain the joint state variables after constraint filtering update. ; in: Let be the objective function. For the joint state variables updated by constraint filtering, For the joint state variables before constraint filtering update, It is a positive definite projective symmetric matrix. , For position constraints, It is a Lagrange multiplier.

2. The wearable multi-MEMS near real-time cooperative navigation system according to claim 1, characterized in that, The n MEMS sensor chips (104) are integrated on the same circuit board.

3. The wearable multi-MEMS near real-time cooperative navigation system according to claim 1, characterized in that, The data processing chip (105) includes n inertial navigation calculation modules, step interval condition judgment modules, constraint condition update modules, n step interval smoothing modules, and n navigation error correction modules; the MEMS sensor chip (104), inertial navigation calculation modules, step interval smoothing modules, and navigation error correction modules correspond one-to-one. The inertial navigation calculation module, together with the MEMS sensor chip (104), constitutes a navigation subsystem. After the module is started, it selects the "East-North-Sky" geographic coordinate system as the navigation coordinate system, obtains the three-axis acceleration and three-axis angular velocity output by the corresponding MEMS sensor chip (104) in the inertial coordinate system in each sampling period, performs inertial navigation calculation, and obtains the attitude, velocity and position information of the wearable part in the navigation coordinate system. The attitude angle error, velocity error, position error, MEMS gyroscope zero bias and accelerometer zero bias in the navigation coordinate system are used as state variables, and the velocity error is used as a measurement. A Kalman filter is established, and zero velocity detection is performed in each sampling period. If the wearable part is in the zero velocity range in the current sampling period, the time is updated first, and then the measurement is updated. Otherwise, only the time is updated, and the first measurement is stored. k One-step state transition matrix for each sampling period Prior estimation of state quantities Posterior estimation of state quantities Prior estimation mean square error matrix and the mean square error matrix of the posterior estimate value; The step interval condition judgment module determines whether the wearing part has entered the zero speed interval and has lasted for a preset period of time in each sampling cycle. If so, it considers the current sampling cycle to be the last sampling cycle of the current step interval, starts the constraint condition update module, n step interval smoothing modules, n navigation error correction modules, and pauses n inertial navigation calculation modules. The step interval smoothing module smooths the state variables obtained by the navigation subsystem for each sampling period. The constraint update module combines the smoothed state variables of n navigation subsystems in each sampling period into a joint state variable, constructs a multi-MEMS joint processing system, and performs constraint filtering update processing on the joint state variable based on the constraint equations constructed by multiple MEMS sensor chips in the navigation coordinate system. After that, the work is paused. The navigation error correction module updates the joint state variables from the constraint filtering. The state variables of the corresponding navigation subsystems are extracted and used to correct the attitude, velocity and position information in the navigation coordinate system calculated by the navigation subsystem in each sampling period within the step interval. Then, the corresponding step interval smoothing module is turned off, the corresponding inertial navigation calculation module is started, and the operation is paused.

4. A wearable multi-MEMS near real-time cooperative navigation system according to claim 3, characterized in that, The time period is a preset M sampling period, where M is greater than or equal to 10.

5. A wearable multi-MEMS near real-time cooperative navigation system according to claim 3, characterized in that, The smoothing algorithm in the step interval smoothing module is as follows: in, For the first k Smoothing filter gain matrix for each sampling period For the first k The state error covariance matrix of the Kalman filter for each sampling period For the first k+ The state transition matrix for one sampling period in one step. For the first k+ The state error covariance matrix of a Kalman filter in one sampling period For the first k The smoothed state variables after each sampling period For the first k+ The state variable after smoothing for one sampling period, For the first k The state variables of the Kalman filter in each sampling period, Let be the state error covariance matrix after smoothing and filtering in the k-th sampling period. For the first k+ The state error covariance matrix after smoothing and filtering over one sampling period. For the first k+ Prior estimation of state variables for one sampling period.

6. A wearable multi-MEMS near real-time cooperative navigation method, characterized in that, Perform the following steps within the step interval: S1. Construct n inertial navigation subsystems. The "East-North-Sky" geographic coordinate system is selected as the navigation coordinate system for the n inertial navigation subsystems. The three-axis acceleration and three-axis angular velocity measured by each sampling period of the n MEMS sensor chips (104) are acquired synchronously. Inertial navigation calculations are performed respectively to obtain the attitude, velocity and position information of the wearable parts corresponding to the n inertial navigation subsystems in the navigation coordinate system. S2. The attitude angle error, velocity error, and position error of the wearable part in the navigation coordinate system, the zero bias of the gyroscope and the zero bias of the accelerometer in the MEMS sensor chip (104) are taken as state variables, and the velocity error is taken as a measurement variable. The Kalman filter method is used to update the measurement and time, and the data is stored. k One-step state transition matrix for each sampling period Prior estimation of state quantities Posterior estimation of state quantities Prior estimation mean square error matrix and the mean square error matrix of the posterior estimate value; S3. Determine whether the wearing part has entered the zero-speed range and has remained there for a preset period of time. If so, consider the current sampling period to be the last sampling period of the current step range, and proceed to step S4; otherwise, return to step S1 for the next sampling period. S4. Each inertial navigation subsystem stops Kalman filtering and begins backward smoothing to obtain the smoothed state variable estimate for each cycle within the current step interval. S5. Combine the smoothed state variables corresponding to n navigation subsystems in each sampling period within the current step interval into a joint state variable, construct a joint processing system, and perform constraint filtering and update processing on the joint state variable based on the constraint equations constructed from the position information of multiple MEMS sensor chips in the navigation coordinate system. Joint state variables updated using constraint filtering The attitude, velocity and position information in the navigation coordinate system calculated by each navigation subsystem in each sampling period within the step interval are corrected, and when the next sampling period arrives, the process returns to step S1. The constraint filtering update process is as follows: S6.1 Combine the smoothed state variables of the n navigation subsystems into a joint state variable. ; S6.

2. Combine multiple inertial navigation subsystems into a multi-MEMS joint processing system. The state error covariance matrix of the multi-MEMS joint processing system is as follows: in, For the first The first navigation subsystem The state error covariance matrix of the sampling period; S6.3, construct the equality constraint equations between each pair of MEMS sensor chips, a total of Equations: in, , The first The MEMS sensor chip MEMS-i, the first The position of the MEMS sensor chip MEMS-j in the navigation coordinate system For the first The MEMS sensor chip MEMS-i and the first The distance between MEMS sensor chips MEMS-j; S6.

4. Using mathematical methods, construct the Lagrangian function, and by solving for the stationary points of the Lagrangian function, obtain the joint state variables after constraint filtering update. ; in: Let be the objective function. For the joint state variables updated by constraint filtering, For the joint state variables before constraint filtering update, It is a positive definite projective symmetric matrix. , For position constraints, It is a Lagrange multiplier.

7. A wearable multi-MEMS quasi-real-time cooperative navigation method according to claim 6, characterized in that, The time period is a preset M sampling period, where M is greater than or equal to 10.

8. A wearable multi-MEMS quasi-real-time cooperative navigation method according to claim 6, characterized in that, In step S4, the smoothing algorithm is as follows: in, For the first k Smoothing filter gain matrix for each sampling period For the first k The state error covariance matrix of the Kalman filter for each sampling period For the first k+ The state transition matrix for one sampling period in one step. For the first k+ The state error covariance matrix of a Kalman filter in one sampling period For the first k The smoothed state variables after each sampling period For the first k+ The state variable after smoothing for one sampling period, For the first k The state variables of the Kalman filter in each sampling period, Let be the state error covariance matrix after smoothing and filtering in the k-th sampling period. For the first k+ The state error covariance matrix after smoothing and filtering over one sampling period. For the first k+ Prior estimation of state variables for one sampling period.

Images