CN114812546A - Shielded space individual soldier navigation pose correction method and device - Google Patents
Shielded space individual soldier navigation pose correction method and device Download PDFInfo
- Publication number
- CN114812546A CN114812546A CN202210415905.XA CN202210415905A CN114812546A CN 114812546 A CN114812546 A CN 114812546A CN 202210415905 A CN202210415905 A CN 202210415905A CN 114812546 A CN114812546 A CN 114812546A
- Authority
- CN
- China
- Prior art keywords
- error
- inertial navigation
- human body
- shaking
- correcting
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
Abstract
The invention discloses a method and a device for correcting individual navigation pose in a shielded space. Wherein, the method comprises the following steps: before strapdown inertial navigation resolving, initial alignment is carried out on an inertial navigation system by adopting a volume Kalman filtering shaking base accurate alignment method under a micro shaking base so as to avoid a micro shaking angle caused by the unfixed connection of a carrier coordinate system and a sensor coordinate system. The invention solves the technical problems of low positioning precision of underground and space-covering personnel caused by reference shaking caused by the environmental structure of a test field and difficult reliable fixed connection of a sensor.
Description
Technical Field
The invention relates to the field of navigation, in particular to a method and a device for correcting individual navigation pose in a shielded space.
Background
The method is suitable for the practical application requirements of individual soldiers and communication equipment in key development areas, major projects and key disaster-prone areas of the country, and needs personnel to investigate the underground and shield typical characteristics of space disaster environments on the spot, so that the acquisition of position and posture information of the personnel is particularly important.
The strapdown inertial navigation technology is one of the main technologies for autonomous positioning and orientation. However, because of system modeling errors and random drift errors of the inertial sensor, navigation information is generated through multiple integration, course information is resolved according to a gyroscope, and navigation accuracy is reduced along with footstep shaking and time accumulation.
Many researchers have studied the orientation of the positioning of the person. A real-time positioning system is designed on a visual platform based on a smart phone, real-time pedestrian motion information acquired by a micro inertial sensor is transmitted to the smart phone by means of a wireless network, the data of the sensor is processed by the smart phone, ZUPT, Zero Angular Rate Update (ZARU), a magnetic compass, a barometer and KF are integrated and resolved through gait detection, relevant errors are compensated and corrected, and information such as positions is resolved. An indoor positioning method based on a cascade evaluation framework is proposed, an inertial sensor is fused with KF to estimate the stepping pedestrian step state and direction, and the pedestrian position error is corrected and compensated through particle filtering and a nonlinear map matching technology. A person researches an indoor and outdoor fusion navigation positioning algorithm based on an IMU/GPS/magnetometer by using low-cost multi-sensor in an iPhone mobile phone, designs corresponding initial alignment, fuses specific force information acquired by an accelerometer and angular velocity information acquired by a gyroscope by using Kalman filtering, and feeds back and corrects KF state quantity by using the three-axis direction of carrier velocity as observed quantity in a fusion period of GPS and INS so as to obtain more accurate position, velocity and attitude information. An algorithm for correcting the course angle error by combining the foot-bound INS and the magnetometer is also researched, and a magnetometer error correction model and an algorithm for correcting the course angle on line are provided; when the pedestrian walks, the EKF is used for carrying out online estimation and real-time correction on the magnetometer, and then the ZUPT and the magnetometer are used for carrying out correction compensation on the foot-bound INS error.
However, the pedestrian inertial navigation algorithm at present has many disadvantages: the problem of divergence of course errors is not effectively solved; when the sensor and the human body can not be completely fixedly connected, the applicability of the traditional strapdown inertial navigation is poor. In the indoor movement of pedestrians, due to the stability of the movement of pedestrians, the defects can be optimized through an algorithm, but in a disaster environment, the human body has more complex movements, so that the positioning and orientation tasks of the human body are difficult to complete through a common algorithm. Therefore, under the conditions of reference shaking caused by complex motion of a human body in a disaster environment and difficult reliable fixed connection of a sensor, the error propagation rule of the traditional strapdown inertial navigation cannot represent the error propagation characteristic of real navigation parameters, and therefore a more accurate navigation algorithm cannot be provided.
In view of the above problems, no effective solution has been proposed.
Disclosure of Invention
The embodiment of the invention provides a method and a device for correcting individual navigation pose of a shielded space, which at least solve the technical problems of low positioning accuracy of underground and shielded space personnel caused by reference shaking and difficult reliable fixed connection of a sensor due to the environmental structure of a test field.
According to one aspect of the embodiment of the invention, a shielded space individual soldier navigation pose correction method is provided, and comprises the following steps: before strapdown inertial navigation resolving, initial alignment is carried out on an inertial navigation system by adopting a volume Kalman shaking base accurate alignment method under a micro shaking base so as to avoid a micro shaking angle caused by the unfixed connection of a carrier coordinate system and a sensor coordinate system.
According to another aspect of the embodiments of the present invention, there is also provided an individual soldier navigation pose correction apparatus for a shielded space, including: an initial alignment module configured to: before strapdown inertial navigation resolving, initial alignment is carried out on an inertial navigation system by adopting a volume Kalman shaking base accurate alignment method under a micro shaking base so as to avoid a micro shaking angle caused by the unfixed connection of a carrier coordinate system and a sensor coordinate system.
In the embodiment of the invention, before strapdown inertial navigation resolving, the inertial navigation system is initially aligned by adopting a volume Kalman shaking base accurate alignment method under a micro shaking base so as to avoid a micro shaking angle caused by the non-fixed connection of a carrier coordinate system and a sensor coordinate system, and further solve the technical problem of low positioning accuracy of underground and space shielding personnel caused by reference shaking and difficult reliable fixed connection of a sensor due to the environmental structure of a test field.
Drawings
The accompanying drawings, which are included to provide a further understanding of the invention and are incorporated in and constitute a part of this application, illustrate embodiment(s) of the invention and together with the description serve to explain the invention without limiting the invention. In the drawings:
FIG. 1 is a flow chart of a shielded space individual soldier navigation pose correction method according to a first embodiment of the invention;
FIG. 2A is a flow chart of an individual soldier navigation pose correction method in a shielded space according to a second embodiment of the invention;
FIG. 2B is a diagram of an initial alignment frame according to an embodiment of the present invention;
FIG. 2C is a flowchart of a strapdown inertial navigation error compensation method according to an embodiment of the present invention;
FIG. 2D is a flow chart of a method of estimating a state error according to an embodiment of the present invention;
FIG. 3 is a diagram of a result of a linear retrace walking navigation solution attitude according to an embodiment of the invention;
FIG. 4 is a diagram of pre-calibration simulated positioning results, in accordance with an embodiment of the present invention;
FIG. 5 is a diagram of EKF correction simulation positioning results according to an embodiment of the present invention;
FIG. 6 is a diagram of a CKF calibration simulation positioning result according to an embodiment of the invention;
FIG. 7 is a diagram of a result of a turn-back walk navigation solution attitude according to an embodiment of the invention;
FIG. 8 is a diagram of pre-calibration simulated positioning results, in accordance with an embodiment of the present invention;
FIG. 9 is a diagram of EKF correction simulation positioning results according to an embodiment of the present invention;
FIG. 10 is a diagram of simulated positioning results after CKF correction according to an embodiment of the present invention;
FIG. 11 is a flow chart of a further method for correcting individual navigation poses in a shielded space according to an embodiment of the invention;
fig. 12 is a schematic structural diagram of a shielded space individual navigation pose correction system according to an embodiment of the invention.
Detailed Description
In order to make the technical solutions of the present invention better understood, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
It should be noted that the terms "first," "second," and the like in the description and claims of the present invention and in the drawings described above are used for distinguishing between similar elements and not necessarily for describing a particular sequential or chronological order. It is to be understood that the data so used is interchangeable under appropriate circumstances such that the embodiments of the invention described herein are capable of operation in sequences other than those illustrated or described herein. Furthermore, the terms "comprises," "comprising," and "having," and any variations thereof, are intended to cover a non-exclusive inclusion, such that a process, method, system, article, or apparatus that comprises a list of steps or elements is not necessarily limited to those steps or elements expressly listed, but may include other steps or elements not expressly listed or inherent to such process, method, article, or apparatus.
SUMMARY
The existing pedestrian single-row calculation method has the defects that:
1. the problem of divergence of course errors is not effectively solved;
2. when the sensor and the human body can not be completely fixedly connected, the applicability of the traditional strapdown inertial navigation is poor;
3. in indoor movement of pedestrians, due to the stability of the movement of pedestrians, the defects can be optimized through an algorithm, but in a disaster environment, a human body has more complex movements, so that the positioning and orientation tasks of the human body are difficult to complete by a common algorithm.
4. The accuracy of the conventional positioning algorithm needs to be improved.
In consideration of the harshness of the underground environment and the complexity of the individual movement mode, the embodiment of the application provides an error correction method under the condition of strapdown navigation in order to solve the problems of reference shaking caused by complex movement of a human body and navigation error correction caused by the fact that a sensor is difficult to reliably and fixedly connect.
According to the correction method, according to a position, speed and attitude error equation derived from a strapdown error propagation mechanism, a human body pose constraint condition (a zero-speed state and a course angle state) in a human body motion sensor is fused to carry out volume Kalman filtering, and the optimal position, speed and attitude error under the condition of strapdown inertial navigation is estimated, so that the change of the position, speed and attitude error under the condition of complex motion can be better represented, and the accuracy of navigation error estimation is further improved.
Example 1
According to the embodiment of the invention, a shielded space individual navigation pose correction method is provided, as shown in fig. 1, the method comprises the following steps:
and S102, before strapdown inertial navigation resolving, performing initial alignment on the inertial navigation system by adopting a volume Kalman shaking base accurate alignment method under a micro shaking base.
The initial alignment needs to be carried out on the system before the system enters the strapdown inertial navigation solution, but a small shaking angle can occur due to the fact that a carrier coordinate system and a sensor coordinate system are not fixedly connected under the condition of pedestrian navigation, so that the CKF shaking base precision alignment method under the micro shaking base is mainly considered.
The inertial navigation system is initially aligned by adopting a volume Kalman shaking base accurate alignment method under the micro shaking base, so that a micro shaking angle caused by the unfixed connection of a carrier coordinate system and a sensor coordinate system can be avoided.
And step S104, correcting an error source of the inertial navigation system.
In one embodiment, human motion perceptron may be employed to obtain human pose constraints, wherein the human pose constraints include a zero velocity state and a pose of human motion. And then, the zero-speed state and the posture of the human motion are differenced with the speed and the posture of strapdown inertial navigation solution to obtain a measurement equation and a state equation of the volume Kalman.
Then, estimating the optimal error of the inertial navigation system based on the state equation and the measurement equation of the volume Kalman; and correcting the error source based on the estimated optimal error to obtain the optimal position and the optimal course of the inertial navigation system.
In an exemplary embodiment, the inertial navigation system may be corrected for error sources of at least one of: device errors, algorithm errors, small misalignment angle errors, and linear displacement errors.
In another exemplary embodiment, the small misalignment angle error can be compensated and corrected based on the optimal error, wherein the small misalignment angle error is a misalignment angle caused by the inertial navigation component being not fixedly connected due to human body shaking; and compensating and correcting the linear displacement error based on the optimal error, wherein the linear displacement error is the deviation between the acceleration value of the inertial navigation component and the real acceleration value of the human body caused by the lever arm effect existing between the inertial navigation component and the human body.
The embodiment combines the environmental structure and the equipment of the test field to test the inertia measurement unit, thereby improving the positioning of personnel in the underground and shielded space and having high directional precision.
Example 2
According to the embodiment of the invention, a shielded space individual navigation pose correction method is provided, as shown in fig. 2, the method comprises the following steps:
step S202, initial alignment.
Before the strapdown navigation calculation is carried out, the inertial navigation system needs to carry out initial alignment so as to determine information such as initial attitude, and the navigation performance of the inertial navigation system is directly influenced by the result of the initial alignment. The micro-amplitude shaking condition is small-amplitude shaking based on the static base, the shaking amplitude of the attitude angle is an angle or a grade, and at the moment, although the interference on the measurement information of the gyroscope and the accelerometer is small, a large error is introduced if the system is still regarded as a pure static base for alignment.
Therefore, on the basis of rough alignment, a cubature Kalman filtering CKF method based on inertial coordinate system measurement information is adopted to reduce the influence of micro-amplitude shaking interference, so that initial alignment under the micro-amplitude shaking base is realized. And according to the initial alignment state space model of the micro-shaking base, performing fine alignment on the micro-shaking base by using CKF.
In the initial alignment phase, the error state vector for CKF can be set to:
δv E 、δv N 、δv U respectively representing the velocity errors in the northeast direction under i series,
δφ E 、δφ N 、δφ U respectively representing attitude angle errors under the i series;
ε bx 、ε by 、ε bz respectively representing three-axis gyroscope biases;
Since the CKF uses a nonlinear model in discrete time, the system model needs to be discretized, and the system discretization state equation is:
X k+1 ＝f(X k )+W k (2)
wherein, X k+1 Representing the error state vector at time k + 1, X k Representing the error state vector at time k.
The system discretization measurement equation is as follows:
Z k ＝H k X k +V k (3)
wherein Z is k Represents the observed quantity at time k, V k And W k Respectively representing a measurement noise matrix and a system noise matrix, wherein the measurement noise matrix and the system noise matrix are white noise with zero mean, and if the speed error is taken as an observed quantity, the corresponding measurement matrix is as follows:
H k ＝[I 3×3 0 3×9 ] (4)
wherein, I 3×3 Representing an identity matrix.
The initial state of the system is estimated by using the model and the basic equation of CKF, and the error compensation is performed on the system by using a closed-loop form, so that the accurate and fast initial alignment is realized, and the basic framework is shown in FIG. 2B.
And S204, compensating the strapdown inertial navigation error.
In an exemplary embodiment, the strapdown inertial navigation error compensation, as shown in fig. 2C, may include the following steps S2042 to S2048.
Step S2042, establishing a state equation.
The state equation of the system is as follows:
wherein, F (t) represents the state transition matrix of the system, X (t) represents the state variable of the system, U (t) represents, G (t) represents the noise driving matrix of the system, W (t) b Indicating gyro angular velocity measurement white noise and accelerometer specific force measurement white noise.
Selecting system state variables as follows:
in the formula (I), the compound is shown in the specification,
to representTotal attitude angle error;
represents the total velocity error;
representing the total position error;
representing the attitude error of the traditional strapdown inertial navigation;
representing the traditional strapdown inertial navigation speed error;
representing the traditional strapdown inertial navigation speed error;
indicating attitude errors due to small misalignment angles;
indicating a small misalignment angle induced speed error;
indicating a small misalignment angle induced position error;
representing attitude errors caused by linear displacement;
representing velocity error caused by linear displacement;
indicating a position error caused by linear displacement; delta K G Representing a gyro scale factor error; delta K A Representing an accelerometer scale factor error; epsilon represents the gyro bias error;
indicating an accelerometer bias error.
The state transition matrix of the system is a 48 × 48 dimensional square matrix, and the specific form is as follows:
wherein M is aa 、M av 、M ap 、M 3 A direction cosine matrix representing a transformation between coordinate systems of attitude errors;
indicating an angular velocity error of the sensor; m vv 、M vp 、M va A direction cosine matrix representing the transformation between coordinate systems of the velocity error; m pv 、M pp A direction cosine matrix representing the transformation between the coordinate systems of the position error.
Wherein the content of the first and second substances,
a direction cosine matrix representing the transformation from the carrier coordinate system to the navigation coordinate system;
representing the specific force of the accelerometer output in a carrier coordinate system;
the theoretical angular velocity under b is shown.
Wherein the content of the first and second substances,
indicating an angular velocity error of the sensor;
representing the specific force of the accelerometer output in the sensor coordinate system;
a direction cosine matrix representing the transformation from the sensor coordinate system to the navigation coordinate system,
representing the angular acceleration of the sensor; r is c A lever arm representing a navigation system;
representing the angular velocity of the sensor.
Wherein the system noise driving array is
Wherein W represents gyro angular velocity measurement white noise and accelerometer specific force measurement white noise.
Wherein the content of the first and second substances,
representing white measurement noise of gyro angular velocity,
white noise, w, representing accelerometer specific force measurement gx 、w gy 、w gz 、w ax 、w ay 、w az Representing the gyro angular velocity and the accelerometer specific force measurement white noise on each axis, respectively.
Step S2044, establishing a measurement equation.
In the system, a human body motion sensor is used for acquiring a zero-speed state and a course angle state of a human body in real time, and a difference value between the speed and the posture of the strapdown inertial navigation system is used as an observed quantity.
The measurement equation of the system is as follows:
Z(t)＝H(t)X(t)+V(t) (15)
wherein Z represents an observed quantity.
Wherein phi is per Representing misalignment angle error, phi, of human motion sensor SINS Representing the SINS misalignment angle error, v per Indicating the velocity error, v, of the human motion sensor SINS Representing the SINS velocity error.
The measurement matrix is as follows.
V denotes measurement noise, specifically as follows.
Wherein, V φ Representing the attitude angle measurement noise,V v representing line speed measurement noise.
Step S2046, discretization of the continuous system.
The maximum characteristic of the discrete kalman filter equation is recursion, however, in practical applications, most navigation systems are continuous systems, so that discretization of the systems is required, that is, the discretization is to discretize the state transition matrix f (t) of the continuous systems into Φ k，k-1 Discretizing the system noise variance matrix Q (t) of the continuous system into Q k The specific process is as follows:
where T is the calculation period of the filter, A (T) k ) Representing a state transition matrix, F n Representing the state transition matrix to the power n, t k Representing a discrete time series, F representing a state transition matrix, and Q representing a system noise variance matrix of a continuous system.
Then, the discretized state equation is as follows:
wherein, X k Represents the state vector, phi k，k-1 Representing a state transition matrix, X k-1 Representing the state vector, Γ, of the preceding time instant k-1 Representing a noise matrix, Z k Represents an observation vector, H k Denotes an observation matrix, W k-1 、V k Representing uncorrelated white gaussian noise.
In step S2048, calibration is performed based on CKF.
And the complete Kalman filtering module is used as an estimator of navigation errors, estimates attitude errors, speed errors and position errors accumulated in the previous gait cycle in each support phase, and is used for correcting the navigation state solved by the strapdown algorithm so as to finish closed-loop Kalman filtering correction.
The system correction algorithm based on Kalman filtering estimates the system error state vector in real time, and compensates the pose information of the system by using the obtained error estimator, so that the error accumulation of the system is inhibited, and the positioning precision of the system is improved.
In this embodiment, CKF uses the third-order spherical radial volume rule to find 2n volume points. The estimation process of the system state error is shown in fig. 2D, and mainly includes the following steps:
step S502, parameter initialization.
The system error state vector X of the initial moment needs to be determined in the system initialization stage 0|0 State estimation covariance matrix P 0|0 And process noise covariance matrix Q k 。
Wherein, X 0 Indicating an initial state.
Step S504, time update.
Because the error compensation system adopts an indirect filtering method, the error state is estimated through the error amount, and finally the accurate attitude, speed and position are obtained.
The embodiment adopts a closed loop filtering algorithm with feedback correction to estimate the system error, namely the posterior error estimation of each moment
All are completely fed back to the nominal subsystem to modify the system state so as to complete closed-loop indirect filtering, and the error is corrected immediately
And setting zero. Therefore, in the closed-loop Kalman filtering process, the error state does not need to pass through the timeUpdating equations
Are passed on and the posterior error is estimated
Only with respect to the observed value at the current time.
(1) Calculating a volume point:
wherein the content of the first and second substances,
(Cholesky decomposition), ξ i Is the ith volume point, n is the state dimension, I is the identity matrix, s k Represents the Cholesky decomposition vector and,
indicating the predicted value of the state quantity.
(2) Propagation volume point:
wherein the content of the first and second substances,
represents the last moment state vector, u k Indicating a control quantity input.
(3) Calculating a state quantity predicted value and an error covariance predicted value:
step S506, measuring value constraint.
Is it determined whether the measurement value is constrained? If so, go to step S508, otherwise, go to step S504.
Step S508, system state estimation.
And (3) carrying out measurement updating:
(1) calculating a volume point:
wherein S is k+1|k Represents the Cholesky decomposition vector and,
indicating the predicted value of the state quantity.
(2) Propagation volume point:
wherein u is k Indicating a control quantity input.
(3) Calculating a measurement predicted value:
(3) system state estimation
For the system measurement error selection, the measurement error form is also varied due to different placement positions and different types of selected sensors. It is common that when the pedestrian is detected to be in a "zero transient state", the speed of the pedestrian is zero at the moment in theory, but the measurement value of the inertial sensor is not zero at the moment due to errors such as measurement. However, since the zero-speed state of the sensor before being placed on the chest is not obvious, the embodiment adopts the human motion sensor to acquire the human pose constraint condition to perform zero-speed correction and course angle correction. To obtain Z k Then, the measurement is updated.
Wherein phi is per Representing misalignment angle error, phi, of human motion sensor SINS Representing the SINS misalignment angle error, v per Indicating the velocity error, v, of the human motion sensor SINS Representing the SINS speed error.
(4) Measurement error covariance and cross-covariance:
wherein the content of the first and second substances,
the point of the propagation volume is represented,
indicating the measured predicted value, R k The covariance is represented as a function of time,
the point of the propagation volume is represented,
indicating the state prediction value.
(5) Gain update, state quantity, error covariance:
wherein the content of the first and second substances,
the cross-covariance is expressed as a cross-covariance,
which represents the covariance of the measurement error,
indicates the predicted value of the state quantity, Z k+1 Representing an observed value, P k+1|k Representing the error covariance prediction.
In the embodiment, the initial alignment is carried out by adopting the CKF shaking base accurate alignment method under the micro shaking base, so that a micro shaking angle caused by the unfixed connection of a carrier coordinate system and a sensor coordinate system under the condition of pedestrian navigation is avoided. And after correction, the course angle accumulated error, the spatial position error and the initial point position error are well inhibited, the CKF inhibiting effect is better than that of the EKF, and the relative closure degree of the curve is better.
Analysis of experiments
According to the embodiment of the application, the initial alignment is firstly carried out on the strapdown inertial navigation system, and then the error correction of the strapdown inertial navigation system is carried out. Two experiments of turning back walking and closed multi-circle walking are respectively carried out. The main verification of the retracing walking is that the inhibition effect of the strapdown error correction method on the displacement of the misalignment angular line is realized in the walking process. The closed multi-turn walking experiment mainly verifies that the position and posture effect of curve closing effect and strapdown error correction and correction is achieved due to the fact that large shaking exists between a sensor coordinate system and a carrier coordinate system. CKF inhibition was highlighted by comparing CKF to EKF.
(1) Strapdown position attitude error correction experiment for straight-line turn-back walking
And performing a linear retracing walking experiment, giving position and attitude settlement results before and after correction, wherein the attitude calculation result of the linear retracing walking navigation is shown in figure 3, the simulation positioning result before correction is shown in figure 4, and the EKF correction simulation positioning result is shown in figure 5. The simulated positioning result after CKF correction is shown in FIG. 6.
The result of the turn-back walking experiment shows that the accumulated errors of the course angle are restrained along with the error correction equation in the experiment, so that the positioning effect is improved. It can be seen from the experimental result diagram that the longer the position is due to the drift of the attitude, the longer the walking trajectory has drifted, and after error correction, the attitude error is made up, so that the walking trajectory is stabilized.
(2) Closed multi-turn walking strapdown position attitude error correction experiment
And (3) performing a closed multi-circle walking experiment, giving position and attitude settlement results before and after correction, wherein the attitude calculation result of the linear turn-back walking navigation is shown in fig. 7, the simulation positioning result before correction is shown in fig. 8, and the correction simulation positioning result of the EKF is shown in fig. 9. The simulated positioning result after CKF correction is shown in fig. 10.
The comparison of the results of the closed multi-circle walking experiment shows that the corrected attitude can be effectively changed, and due to the fact that the sensor and the carrier are not fixedly connected, the misalignment angle and the linear displacement at the turning position are suddenly increased, so that the attitude calculation is unstable, and the curve is not closed. The corrected attitude calculation method can inhibit errors in attitude calculation and the curve can be relatively closed.
And performing turn-back and closed multi-circle walking experiment simulation analysis, wherein the specific data result of position and attitude calculation error analysis is shown in the table. The experimental result shows that after correction, the course angle accumulated error, the spatial position error and the initial point position error are well inhibited, and the CKF inhibiting effect is better than that of the EKF.
Example 3
According to an embodiment of the invention, a shielded space individual navigation pose correction method is provided, as shown in fig. 11, the method includes the following steps:
step S302, resolving inertial navigation information based on data acquired by an inertial navigation component, and deducing speed and attitude information of a target to be navigated;
step S304, obtaining the zero-speed state and the posture of the target motion based on the data collected by the human motion sensor;
s306, fusing the speed and attitude information with the zero speed state and attitude, performing volume Kalman filtering, and solving out an optimal error value;
and step S308, compensating and correcting the first position, speed and attitude information based on the optimal error value.
In one exemplary embodiment, before performing the inertial navigation information solution, the method further comprises: carrying out initial alignment on the inertial navigation system by adopting a volume Kalman shaking base accurate alignment method under a micro shaking base, wherein the initial alignment comprises the following steps: discretizing the state equation and the measurement equation of the inertial navigation system to obtain a discretized state equation and a discretized measurement equation; estimating an initial state of the inertial navigation system based on the discretized state equation and discretized measurement equation and the volume kalman equation; and carrying out error compensation on the inertial navigation system by using a closed-loop form to realize the initial alignment.
In an exemplary embodiment, prior to performing the volumetric kalman filtering, the method further comprises: establishing a state equation and a measurement equation of the inertial navigation system; and discretizing the state transition matrix and the noise variance matrix of the state equation to obtain a discretized state equation.
In an exemplary embodiment, fusing the speed, the attitude information, and the zero-speed state and the attitude, performing a volume kalman filter, and solving an optimal error value, includes: determining an error state vector X of the inertial navigation system at an initial time 0|0 State estimation covariance matrix P 0|0 And process noise covariance matrix Q k (ii) a Error state vector X based on the inertial navigation system 0|0 State estimation covariance matrix P 0|0 And process noise covariance matrix Q k Updating time and calculating the predicted value of state quantity
Predicting a value based on the state quantity
Updating the measurement and calculating the predicted value
Predicting a value based on the state quantity
And the measured predicted value
And estimating the system state to solve the optimal error value.
In an exemplary embodiment, the error state vector X based on the inertial navigation system 0|0 State estimation covariance matrix P 0|0 And process noise covariance matrix Q k Updating time and calculating the predicted value of state quantity
The method comprises the following steps: estimating based on the posterior error
Sum process noise covariance matrix P k Calculating volume points of the state quantities; propagating volume points of the calculated state quantities; and calculating a predicted value of the state quantity and an error covariance predicted value based on the volume point of the state quantity.
In one exemplary embodiment, the state quantity prediction value is based on
Updating the measurement and calculating the predicted value
The method comprises the following steps: predicting a value based on the state quantity
And the difference covariance predicted value P k+1|k Calculating the measured volume point; propagating the calculated volumetric points of the quantity measurement; based on the measured volume points, a predicted value of the measured quantity is calculated.
In an exemplary embodiment, before performing the measurement update, the method further comprises: and acquiring human pose constraint conditions by adopting a human motion sensor, and performing zero-speed correction and course angle correction.
In one exemplary embodiment, the state quantity prediction value is based on
And the measured predicted value
Estimating the system state, and solving an optimal error value, wherein the method comprises the following steps: predicting a value based on the state quantity
And the measured predicted value
Determining a measurement error covariance and a cross covariance; solving the optimal error value based on the measurement error covariance and cross-covariance, wherein the optimal error value comprises at least one of: gain update, state quantity and error covariance.
In an exemplary embodiment, the compensating correction of the first position, velocity, and attitude information based on the optimal error value comprises at least one of: compensating and correcting the deviation of a small misalignment angle in the first position, speed and posture information based on the optimal error value, wherein the small misalignment angle is a misalignment angle caused by the fact that the inertial navigation component is not fixedly connected due to human body shaking; and compensating and correcting deviations of linear displacement in the first position, speed and posture information based on the optimal error value, wherein the linear displacement is the deviation between the acceleration value of the inertial navigation component and the real acceleration value and the direction of the human body caused by the lever arm effect existing between the inertial navigation component and the human body.
In the embodiment, the system needs to be initially aligned before entering strapdown inertial navigation solution, but a slight shaking angle can occur due to the unfixed connection of the carrier coordinate system and the sensor coordinate system under the condition of pedestrian navigation, so that the CKF shaking base accurate alignment method under the slight shaking base is adopted.
Furthermore, in pedestrian navigation systems, the main error sources are device errors, algorithm errors, small misalignment angle errors, and linear displacement errors. And by adopting the volume Kalman filtering, the chest does not have a zero-speed interval like feet, so the human body position and attitude constraint conditions are acquired by adopting the human body motion sensor. And obtaining a measurement equation according to the obtained zero-speed state and the obtained attitude of the human motion and the speed and the attitude calculated by the SINS. Therefore, the state and the measurement equation of the volume Kalman are established, the optimal error of the system can be estimated, and the optimal position and the optimal course of the system can be obtained.
In addition, the pedestrian error correction based on the CKF algorithm can effectively restrain small misalignment angle and linear displacement caused by human body shaking between the sensor and the carrier, so that the positioning accuracy is improved.
It should be noted that, for simplicity of description, the above-mentioned method embodiments are described as a series of acts or combination of acts, but those skilled in the art will recognize that the present invention is not limited by the order of acts, as some steps may occur in other orders or concurrently in accordance with the invention. Further, those skilled in the art should also appreciate that the embodiments described in the specification are preferred embodiments and that the acts and modules referred to are not necessarily required by the invention.
Through the above description of the embodiments, those skilled in the art can clearly understand that the method according to the above embodiments can be implemented by software plus a necessary general hardware platform, and certainly can also be implemented by hardware, but the former is a better implementation mode in many cases. Based on such understanding, the technical solutions of the present invention may be embodied in the form of a software product, which is stored in a storage medium (e.g., ROM/RAM, magnetic disk, optical disk) and includes instructions for enabling a terminal device (e.g., a mobile phone, a computer, a server, or a network device) to execute the method according to the embodiments of the present invention.
Example 4
The sensor coordinate system and the human body coordinate system are interfered by factors such as shaking, and the coordinate systems of the sensor coordinate system and the human body coordinate system are deviated. Where the deviation is mainly caused by small misalignment angles and linear displacements. The small misalignment angle is the misalignment angle caused by the inertial measurement unit not being fixedly connected due to human body shaking. The linear displacement is the lever arm effect existing between the sensor and the human body, which causes that the acceleration value of the sensor has certain deviation from the real acceleration value and the direction of the carrier.
According to the embodiment of the invention, the system firstly calibrates errors caused by small misalignment angles and linear displacement, namely errors between a carrier coordinate system and a human body coordinate system, adopts a derived position, speed and attitude error differential equation, performs Kalman filtering by fusing speed and attitude information of a human body motion sensor and a strapdown inertial navigation system as observed quantities to solve an optimal error value, and compensates system attitude, speed and position information.
As shown in fig. 12, the system comprises an inertial navigation component 112, a human motion sensor 114 and a shielded space individual navigation pose correction device 116. Inertial navigation assembly 112 includes an accelerometer and a gyroscope.
In one exemplary embodiment, the sheltered space individual navigation pose correction apparatus 116 includes an initial alignment module. The initial alignment module is configured to: before strapdown inertial navigation resolving, initial alignment is carried out on an inertial navigation system by adopting a volume Kalman shaking base accurate alignment method under a micro shaking base so as to avoid a micro shaking angle caused by the unfixed connection of a carrier coordinate system and a sensor coordinate system.
In an exemplary embodiment, the shelter space individual navigation pose correction apparatus 116 may further include a correction module. The correction module is configured to: correcting error sources of at least one of the following of the inertial navigation system: device error, algorithm error, small misalignment angle error, and linear displacement error
In an exemplary embodiment, the shelter space individual navigation pose correction apparatus 116 may further include a constraint module. The constraint module is configured to: acquiring human body pose constraint conditions acquired by a human body motion sensor, wherein the human body pose constraint conditions comprise a zero-speed state and a posture of human body motion; and subtracting the zero-speed state and the posture of the human motion and the speed and the posture of strapdown inertial navigation solution to obtain a measurement equation and a state equation of the volume Kalman.
In an exemplary embodiment, the shelter space individual navigation pose correction apparatus 116 may further include a correction module. The correction module is configured to: estimating the optimal error of the inertial navigation system based on the state equation and the measurement equation of the volume Kalman; and correcting the error source based on the estimated optimal error to obtain the optimal position and the optimal course of the inertial navigation system.
In an exemplary embodiment, the revision module is further configured to: compensating and correcting the deviation of the small misalignment angle based on the optimal error, wherein the small misalignment angle is a misalignment angle caused by the fact that the inertial navigation component is not fixedly connected due to human body shaking; and compensating and correcting the linear displacement error based on the optimal error, wherein the linear displacement error is the deviation between the acceleration value of the inertial navigation component and the real acceleration value of the human body caused by the lever arm effect existing between the inertial navigation component and the human body.
Example 5
The embodiment of the invention also provides a storage medium. Alternatively, in this embodiment, the storage medium described above may implement the methods in embodiments 1 to 3.
Optionally, in this embodiment, the storage medium may include, but is not limited to: a U-disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a removable hard disk, a magnetic or optical disk, and other various media capable of storing program codes.
The above-mentioned serial numbers of the embodiments of the present invention are merely for description and do not represent the merits of the embodiments.
The integrated unit in the above embodiments, if implemented in the form of a software functional unit and sold or used as a separate product, may be stored in the above computer-readable storage medium. Based on such understanding, the technical solution of the present invention may be embodied in the form of a software product, which is stored in a storage medium and includes several instructions for causing one or more computer devices (which may be personal computers, servers, network devices, etc.) to execute all or part of the steps of the method according to the embodiments of the present invention.
In the above embodiments of the present invention, the description of each embodiment has its own emphasis, and reference may be made to the related description of other embodiments for parts that are not described in detail in a certain embodiment.
In the several embodiments provided in the present application, it should be understood that the disclosed client may be implemented in other manners. The above-described embodiments of the apparatus are merely illustrative, and for example, the division of the units is only one type of division of logical functions, and there may be other divisions when actually implemented, for example, a plurality of units or components may be combined or may be integrated into another system, or some features may be omitted, or not executed. In addition, the shown or discussed mutual coupling or direct coupling or communication connection may be an indirect coupling or communication connection through some interfaces, units or modules, and may be in an electrical or other form.
The units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of the embodiment.
In addition, functional units in the embodiments of the present invention may be integrated into one processing unit, or each unit may exist alone physically, or two or more units are integrated into one unit. The integrated unit can be realized in a form of hardware, and can also be realized in a form of a software functional unit.
The foregoing is only a preferred embodiment of the present invention, and it should be noted that, for those skilled in the art, various modifications and decorations can be made without departing from the principle of the present invention, and these modifications and decorations should also be regarded as the protection scope of the present invention.
Claims (10)
1. A shielded space individual soldier navigation pose correction method is characterized by comprising the following steps: before strapdown inertial navigation resolving, initial alignment is carried out on an inertial navigation system by adopting a volume Kalman shaking base accurate alignment method under a micro shaking base so as to avoid a micro shaking angle caused by the unfixed connection of a carrier coordinate system and a sensor coordinate system.
2. The method of claim 1, wherein after initial alignment, the method further comprises: correcting error sources of at least one of the following of the inertial navigation system: device errors, algorithm errors, small misalignment angle errors, and linear displacement errors.
3. The method of claim 2, wherein prior to making the correction, the method further comprises:
acquiring human body pose constraint conditions by adopting a human body motion sensor, wherein the human body pose constraint conditions comprise a zero-speed state and a posture of human body motion;
and subtracting the zero-speed state and the posture of the human motion and the speed and the posture of strapdown inertial navigation solution to obtain a measurement equation and a state equation of the volume Kalman.
4. The method of claim 3, wherein making a correction comprises:
estimating the optimal error of the inertial navigation system based on the state equation and the measurement equation of the volume Kalman;
and correcting the error source based on the estimated optimal error to obtain the optimal position and the optimal course of the inertial navigation system.
5. The method of claim 4, wherein correcting the error source comprises at least one of:
compensating and correcting the small misalignment angle error based on the optimal error, wherein the small misalignment angle error is a misalignment angle caused by the fact that an inertial navigation component is not fixedly connected due to human body shaking;
and compensating and correcting the linear displacement error based on the optimal error, wherein the linear displacement error is the deviation between the acceleration value of the inertial navigation component and the real acceleration value of the human body caused by the lever arm effect existing between the inertial navigation component and the human body.
6. The utility model provides a shelter space individual soldier navigation position appearance correcting unit which characterized in that includes: an initial alignment module configured to: before strapdown inertial navigation resolving, initial alignment is carried out on an inertial navigation system by adopting a moving base fine alignment method of volume Kalman filtering under a micro-shaking base so as to avoid a micro shaking angle caused by the unfixed connection of a carrier coordinate system and a sensor coordinate system.
7. The apparatus of claim 6, further comprising a modification module configured to: correcting error sources of at least one of the following of the inertial navigation system: device errors, algorithm errors, small misalignment angle errors, and linear displacement errors.
8. The apparatus of claim 7, further comprising a constraint module configured to:
acquiring human body pose constraint conditions acquired by a human body motion sensor, wherein the human body pose constraint conditions comprise a zero-speed state and a posture of human body motion;
and subtracting the zero-speed state and the posture of the human motion and the speed and the posture of strapdown inertial navigation solution to obtain a measurement equation and a state equation of the volume Kalman.
9. The apparatus of claim 7, wherein the revision module is further configured to:
estimating the optimal error of the inertial navigation system based on the state equation and the measurement equation of the volume Kalman;
and correcting the error source based on the estimated optimal error to obtain the optimal position and the optimal course of the inertial navigation system.
10. The apparatus of claim 9, wherein the revision module is further configured to:
compensating and correcting the deviation of the small misalignment angle based on the optimal error, wherein the small misalignment angle is a misalignment angle caused by the fact that an inertial navigation component is not fixedly connected due to human body shaking;
and compensating and correcting the linear displacement error based on the optimal error, wherein the linear displacement error is the deviation between the acceleration value of the inertial navigation component and the real acceleration value of the human body caused by the lever arm effect existing between the inertial navigation component and the human body.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210415905.XA CN114812546A (en) | 2022-04-20 | 2022-04-20 | Shielded space individual soldier navigation pose correction method and device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210415905.XA CN114812546A (en) | 2022-04-20 | 2022-04-20 | Shielded space individual soldier navigation pose correction method and device |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114812546A true CN114812546A (en) | 2022-07-29 |
Family
ID=82505362
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210415905.XA Pending CN114812546A (en) | 2022-04-20 | 2022-04-20 | Shielded space individual soldier navigation pose correction method and device |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114812546A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116625407A (en) * | 2023-06-05 | 2023-08-22 | 泉州职业技术大学 | Intelligent micro-attitude measurement method and system |
-
2022
- 2022-04-20 CN CN202210415905.XA patent/CN114812546A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116625407A (en) * | 2023-06-05 | 2023-08-22 | 泉州职业技术大学 | Intelligent micro-attitude measurement method and system |
| CN116625407B (en) * | 2023-06-05 | 2024-02-20 | 泉州职业技术大学 | Intelligent micro-attitude measurement method and system |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Nazarahari et al. | 40 years of sensor fusion for orientation tracking via magnetic and inertial measurement units: Methods, lessons learned, and future challenges | |
| Ludwig et al. | Comparison of Euler estimate using extended Kalman filter, Madgwick and Mahony on quadcopter flight data | |
| CN109269471B (en) | Novel GNSS receiver inclination measuring system and method | |
| Kong | INS algorithm using quaternion model for low cost IMU | |
| CN101726295B (en) | Unscented Kalman filter-based method for tracking inertial pose according to acceleration compensation | |
| CN109163721A (en) | Attitude measurement method and terminal device | |
| Eling et al. | Development of an instantaneous GNSS/MEMS attitude determination system | |
| Stančić et al. | The integration of strap-down INS and GPS based on adaptive error damping | |
| CN113029199A (en) | System-level temperature error compensation method of laser gyro inertial navigation system | |
| Mansoor et al. | Improved attitude determination by compensation of gyroscopic drift by use of accelerometers and magnetometers | |
| Crocoll et al. | Unified model technique for inertial navigation aided by vehicle dynamics model | |
| Wang et al. | Direction cosine matrix estimation with an inertial measurement unit | |
| Li et al. | Research on multi-sensor pedestrian dead reckoning method with UKF algorithm | |
| CN111895988A (en) | Unmanned aerial vehicle navigation information updating method and device | |
| CN111189442B (en) | CEPF-based unmanned aerial vehicle multi-source navigation information state prediction method | |
| Luo et al. | A position loci-based in-motion initial alignment method for low-cost attitude and heading reference system | |
| Yang et al. | Quaternion-based Kalman filtering on INS/GPS | |
| Quan et al. | Interlaced optimal-REQUEST and unscented Kalman filtering for attitude determination | |
| Pei et al. | In-motion initial alignment using state-dependent extended Kalman filter for strapdown inertial navigation system | |
| Zhang et al. | Attitude determination using gyros and vector measurements aided with adaptive kinematics modeling | |
| Ludwig | Optimization of control parameter for filter algorithms for attitude and heading reference systems | |
| CN117289322A (en) | High-precision positioning algorithm based on IMU, GPS and UWB | |
| CN114812546A (en) | Shielded space individual soldier navigation pose correction method and device | |
| CN113465599B (en) | Positioning and orientation method, device and system | |
| Zhou et al. | Applying quaternion-based unscented particle filter on INS/GPS with field experiments |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |