A distance and angle based pedestrian collaborative navigation method

Abstract

The present application relates to a kind of pedestrian collaborative navigation method based on distance and angle, the collaborative navigation method includes: S1, by MIMU output each pedestrian respective three-axis accelerometer and three-axis gyroscope data, and carry out strapdown inertial navigation solution, again by the state of zero speed detection to the data of inertial navigation solution is carried out zero speed correction by extended Kalman filtering;S2, by UWB ranging angle measurement obtains the relative distance and angle between pedestrians as observation, then with the position information and heading angle information of N pedestrians as state quantity, carries out Kalman filtering, obtains the correction data of the position information and heading angle information of each pedestrian;S3, according to the position information and heading angle information of each pedestrian after correction collaborative navigation is carried out.The present application uses distance and angle as observation to correct pedestrian navigation after ZUPT auxiliary, solves the current pedestrian navigation heading angle drift and the problem of large amount of calculation of centralized filtering.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of navigation positioning, in particular to a pedestrian cooperative navigation method based on distance and angle. BACKGROUND

[0002] Pedestrian navigation algorithm based on navigation solution is a hot spot in recent years; in this method, IMU needs to be fixed to the human body, generally installed on the foot, and the principle is based on the integration principle of inertial navigation, which solves the data of accelerometer and gyroscope to obtain the motion information of the pedestrian, such as position, attitude and speed; however, due to the poor precision of inertial elements, the error is continuously accumulated with the growth of time, and a reliable navigation result cannot be obtained. Foxlin proposed a zero velocity update (ZUPT) algorithm in 2006. In ZUPT, the periodicity of the foot movement of the pedestrian is considered, and the motion state of the foot in a period is divided into step phase and static phase, the static phase is that the sole is supported on the ground, at this time the speed of the foot can be regarded as zero speed, then the zero speed is taken as the observation quantity of extended Kalman and is fused into the system. This method updates the pose of the pedestrian at the end of each period, and suppresses the inertial navigation drift. The emergence of ZUPT algorithm makes the pedestrian navigation system based on INS solution widely used. The pedestrian navigation system based on inertial navigation solution generally works on the basis of ZUPT assistance. However, due to the drift error of the gyroscope, the heading angle in the ZUPT algorithm is unobservable, which will cause the heading angle error to be uncorrected, and gradually accumulated, resulting in the drift of the heading angle.

[0003] It should be noted that the information disclosed in the above background section is only used to strengthen the understanding of the background of the present disclosure, and therefore can include information that does not constitute prior art known to those of ordinary skill in the art. SUMMARY

[0004] The purpose of the present application is to overcome the shortcomings of the prior art, and provide a pedestrian cooperative navigation method based on distance and angle, which solves the problem of accumulation and divergence of heading angle error in the existing pedestrian navigation based on ZUPT.

[0005] The purpose of the present application is achieved by the following technical scheme: a pedestrian cooperative navigation method based on distance and angle, the cooperative navigation method comprises:

[0006] S1, output the three-axis accelerometer and three-axis gyroscope data of each pedestrian respectively by MIMU, and perform strapdown inertial navigation solution, and then perform zero velocity correction on the data of inertial navigation solution by the state obtained by zero velocity detection through extended Kalman filtering;

[0007] S2, the relative distance and angle between pedestrians are obtained by UWB ranging and angle measurement as observation, and the position information and heading angle information of N pedestrians are taken as state quantity, Kalman filtering is carried out to obtain the corrected data of the position information and heading angle information of each pedestrian;

[0008] S3, cooperative navigation is carried out according to the corrected position information and heading angle information of each pedestrian.

[0009] The state obtained by the zero speed detection is zero speed corrected to the inertial navigation solution data by extended Kalman filtering, which specifically includes the following contents:

[0010] S101, the data of three-axis accelerometer and three-axis gyroscope are output by MIMU, respectively as f b ,ω b ;

[0011] S102, the zero speed state of the pedestrian is detected by accelerometer covariance detection method;

[0012] S103, the strapdown inertial navigation solution is carried out on the MIMU measurement data, when the zero speed state is detected, the zero speed pseudo observation is used to correct the inertial navigation solution by extended Kalman filtering.

[0013] The zero speed state of the pedestrian is detected by accelerometer covariance detection method, which includes: assuming that there are N sampling values in a certain period of time, that is, the size of the window is N, at this time, the zero speed state of MIMU is Wherein, f k is the output of the accelerometer at time k, T(f k ) is the test statistic of zero speed detection, and L is the threshold value.

[0014] The state model of the extended Kalman filter in S103 is:

[0015]

[0016] Wherein, δΦ,δλ,δh respectively represent three-dimensional position (latitude, longitude and height) error, δv e ,δv n ,δv u respectively represent the eastward, northward and upward velocity error, δθ,δγ, respectively represent the pitch angle, roll angle and yaw angle error of the inertial navigation system, δω x ,δω γ ,δω z respectively represent the drift of the gyroscope and δf x, δf y , δf z represents the zero bias of the accelerometer, a total of 15-dimensional error, F represents the dynamic transfer matrix of the system, G represents the noise distribution vector, and w(k) represents a Gaussian white noise;

[0017] The 15-dimensional error state equation is expressed as:

[0018]

[0019] wherein F rv , F rε and F vε sub-matrix represent the disturbance relationship between the position error, the speed error and the attitude error, is the conversion matrix of the b system to the n system, β f and βω are coefficients related to the zero bias of the accelerometer and the gyroscope;

[0020] The observation model of the extended Kalman filter is:

[0021]

[0022] wherein z k is the three-dimensional speed calculated by the inertial navigation, H k is a 3*15-dimensional matrix of [0 3×3 diag[1 1 1]0 3×6 ], v k represents a measurement noise sequence with zero mean, and R k represents a noise covariance matrix.

[0023] The Kalman filter specifically includes the following steps:

[0024] A1, predicting the state equation: predicting the system state at the next time point through the state transition matrix Φ k,k-1 of the system and the current state estimation value

[0025] A2, predicting the error covariance: predicting the state estimation error covariance matrix P k,k-1 at the next time point through the state transition matrix Φ k-1 of the system, the current state estimation error covariance matrix P k-1 (+), the noise driving matrix G k-1 and the system noise covariance matrix Q

[0026] A3, calculating the Kalman gain: determining the Kalman filter gain through the values of the state estimation error covariance matrix P k and the measurement noise covariance matrix R k ​​

[0027] A4, update state equation: by predicting state and measuring updated state estimation value, the optimal state estimation value at the current time is obtained

[0028] A5, update error covariance: by predicting state estimation error covariance matrix and measuring updated state estimation error covariance matrix, the optimal state estimation error covariance matrix P at the current time is calculated k (+)=(I-K k H k )P k (-)。

[0029] The relative distance and angle between pedestrians obtained by UWB ranging are taken as observation, and the position information and heading angle information of N pedestrians are taken as state quantity, Kalman filtering is carried out to obtain the correction data of the position information and heading angle information of each pedestrian, which specifically includes the following contents:

[0030] S201, the relative distance between pedestrians is obtained based on UWB ranging by formula , wherein represents the distance measurement between pedestrian i and pedestrian j at t time, represents distance measurement noise, represents the position vector of the i th pedestrian at the s th step, represents the position vector of the j th pedestrian at the s th step, and the relative angle between pedestrians is obtained based on UWB ranging by formula , wherein represents the angle measurement between pedestrian i and pedestrian j at t time, and respectively represent the position component of the i th pedestrian in the x axis direction at the s th step and the position component in the y axis direction;

[0031] S202, the position information and heading angle information of multiple pedestrians are taken as state quantity, the displacement increment and heading increment of each step of pedestrian are taken as system input, the motion model is established, and the state model of single pedestrian is obtained according to the state model of extended Kalman filtering

[0032]

[0033] , wherein x i (k) and y i (k) are the components of the position information of pedestrian i at k time in the horizontal plane, ψ i (k) is the heading angle state quantity, and Δψ i ​(k) is a heading angle increment, Delta x i (k) and Delta y i (k) is a position change amount of the pedestrian i at the k moment in the horizontal plane, and further, a state model of the plurality of pedestrians is

[0034] The obtained state quantity is converted from the navigation coordinate system to the carrier coordinate system, and the following formula is used And The measurement model between the pedestrians is obtained as follows:

[0035]

[0036] Wherein, R is a rotation matrix of the navigation coordinate system to the carrier coordinate system, L ij is an observation matrix H between the pedestrian i and the pedestrian j;

[0037] S203, Kalman filtering is performed: after each step of the pedestrian, Kalman filtering is performed to obtain the correction data of the position information and the heading angle information of each pedestrian.

[0038] The present application has the following advantages: a pedestrian cooperative navigation method based on distance and angle, which uses distance and angle as observation quantities to correct the pedestrian navigation after ZUPT assistance, solves the problems of current pedestrian navigation heading angle drift and large centralized filtering calculation amount. BRIEF DESCRIPTION OF DRAWINGS

[0039] Figure 1 is a flowchart of the present application;

[0040] Figure 2 is a schematic diagram of ZUPT assisted inertial navigation solution;

[0041] Figure 3 is a schematic diagram of pedestrian cooperative navigation. DETAILED DESCRIPTION

[0042] To make the purpose, technical scheme and advantages of the embodiments of the present application clearer, the technical scheme of the embodiments of the present application will be described clearly and completely below in combination with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all the embodiments. The components of the embodiments of the present application described and shown in the drawings here can be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present application provided in combination with the drawings of the present application is not intended to limit the protection scope of the claimed present application, but only represents selected embodiments of the present application. Based on the embodiments of the present application, all other embodiments obtained by those skilled in the art without creative labor are within the scope of protection of the present application. The present application will be further described below in combination with the drawings.

[0043] As Figure 1 shown, the present application relates to a distance and angle based pedestrian cooperative navigation method, mainly using foot micro inertial navigation system (MIMU) and UWB ranging and angle measuring equipment installed on pedestrians, exchanging relative information data through wireless communication, using ranging and angle measuring information to correct ZUPT assisted inertial navigation to solve position and heading angle error, specifically including the following contents:

[0044] S1, as Figure 2 shown, the data of three-axis accelerometer and three-axis gyroscope of each pedestrian is output by MIMU, and strapdown inertial navigation is solved, and the state obtained by zero speed detection is used to correct the data of inertial navigation solution by extended Kalman filtering;

[0045] S101, output the data of three-axis accelerometer and three-axis gyroscope by MIMU, respectively f b ,ω b ;

[0046] S102, the zero speed state of pedestrian is detected by accelerometer covariance detection method: the purpose of zero speed detection is to detect the time when MIMU is in zero speed state in a period of time. It is assumed that the sampling value in this period of time is W, that is, the size of window is N. That is, when the following formula is established, it is considered that MIMU is in zero speed state:

[0047]

[0048] Among them, f k is the output of accelerometer at time k, T(f k ) is the test statistic of zero speed detection, and L is the threshold value.

[0049] S103. The measured data of MIMU is solved by strapdown inertial navigation, when the zero speed state is detected, the zero speed pseudo observation is used to correct the inertial navigation solution by extended Kalman filtering, and the state model and observation model of extended Kalman filtering are described as follows:

[0050]

[0051] Among them, δΦ,δλ,δh respectively represent three-dimensional position (latitude, longitude and height) error, δv e ,δv n ,δv u respectively represent the speed error of east, north and sky, δθ,δγ, respectively represent the pitch angle, roll angle and yaw angle error of inertial navigation system, δω x ,δω γ ,δωz respectively represent the drift of the gyroscope and δf x y z respectively represent the zero offset of the accelerometer, a total of 15-dimensional error. F represents the dynamic transfer matrix of the system, G represents the noise distribution vector, and w(k) represents the Gaussian white noise. It can be found that the coupling relationship between the position, velocity and attitude is the dynamic transfer matrix F, and therefore the 15-dimensional error state equation can be expressed as:

[0052]

[0053] wherein, is the conversion matrix from b system to n system, β f and β ω are the coefficients related to the zero offset of the accelerometer and the gyroscope.

[0054] F rr is the relationship matrix of the position error rate of change and the velocity error, F rv is the relationship matrix of the position error and the velocity error, and is expressed as:

[0055]

[0056]

[0057] wherein, M is the radius of curvature of the meridian circle at the position of the carrier, N is the radius of curvature of the prime vertical circle at the position of the carrier, h is the height of the carrier, represents the first-order time derivative of the longitude λ, represents the first-order time derivative of the latitude Φ.

[0058] F vr , F vv and F vε respectively represent the relationship matrix of the velocity error rate of change and the position error, the velocity error and the attitude error, and are expressed as:

[0059]

[0060]

[0061]

[0062] wherein, v E , v N and v U respectively represent the eastward, northward and skyward velocity values of the carrier, f E , f N and f U ​​The specific force values of the carrier in the east, north and sky directions, respectively, γ represents the local gravity acceleration varying with the carrier dimension and height, w ie is the earth rotation angular velocity value;

[0063] F εr , F εv and F εε respectively represent the relationship matrix of the attitude error change rate with the position error, velocity error and attitude error, and can be expressed as:

[0064]

[0065]

[0066]

[0067] In order to apply KF, it needs to be linearized, and the first two terms of the Taylor expansion are retained, and the following is obtained:

[0068] Φ = (I + FΔt)

[0069] In the formula, I is a unit matrix; and Δt is a sampling time interval.

[0070] The observation model of the extended Kalman filter is:

[0071] z k = H k x k + η k

[0072] In the formula, z k is the zero-speed pseudo-observation, H k is a 3*15 matrix of [0 3×3 diag[1 1 1]0 3×6 ].

[0073] The system noise w k and the measurement noise η k represent mutually independent zero-mean white noise processes with known variances, and have:

[0074]

[0075]

[0076] In the formula, Q k and R k are known positive definite covariance matrices, which are the system noise covariance matrix and the measurement noise covariance matrix, respectively.

[0077] Further, the Kalman filter includes the following contents:

[0078] A1, Predict the state equation: by the state transition matrix of the system Φ k,k-1 and the current state estimate Predict the system state at the next time

[0079] A2, Predict the error covariance: by the state transition matrix of the system Φ k,k-1 , the current state estimation error covariance matrix P k-1 (+), and the noise driving matrix G k-1 and the system noise covariance matrix Q k-1 , the state estimation error covariance matrix at the next time

[0080] A3, Calculate the Kalman gain: by the value of the state estimation error covariance matrix P k and the measurement noise covariance matrix R k to determine the Kalman filter gain

[0081] A4, Update the state equation: by the predicted state and the measurement updated state estimate value, get the optimal state estimate value at the current time

[0082] A5, Update the error covariance: by the predicted state estimation error covariance matrix and the measurement updated state estimation error covariance matrix, calculate the optimal state estimation error covariance matrix P k (+)= (I-K k H k )P k (-).

[0083] Where the state estimation error covariance matrix P k is expressed as:

[0084]

[0085] Each item in the matrix represents the diagonal matrix of the error level of each state quantity, i.e. (position, velocity, attitude and gyroscope drift and accelerometer zero offset). Since the system observation is velocity, the measurement noise covariance matrix R k is expressed as:

[0086]

[0087] The diagonal line in the matrix represents the variance of the velocity measurement in the east-north-sky direction.

[0088] S2, as Figure 3The relative distance and angle between pedestrians are obtained by UWB ranging and angle measurement as observations, and the position information and heading angle information of N pedestrians are taken as state variables for Kalman filtering.

[0089] The relative distance between pedestrians obtained by UWB ranging is represented as:

[0090]

[0091] wherein, represents the distance measurement between pedestrian i and pedestrian j at time t, represents the distance measurement noise, represents the position vector of the i-th pedestrian at the s-th step, represents the position vector of the j-th pedestrian at the s-th step.

[0092] The relative angle between pedestrians obtained by UWB ranging is represented as:

[0093]

[0094] wherein, represents the angle measurement between pedestrian i and pedestrian j at time t, and respectively represent the position component of the i-th pedestrian in the x-axis direction and the position component in the y-axis direction at the s-th step.

[0095] S202, taking the position information and heading angle information of multiple pedestrians as state variables, taking the displacement increment and heading increment of each step of the pedestrian as system input, establishing a motion model, and obtaining the state model of a single pedestrian according to the state model of the extended Kalman filter The state model of a single pedestrian is:

[0096]

[0097] wherein, x i (k) and y i (k) are the components of the position information of pedestrian i at time k in the horizontal plane, ψ i (k) is the heading angle state variable, Δψ i (k) is the heading angle increment, Δx i (k) and Δy i (k) are the components of the position change of pedestrian i at time k in the horizontal plane, and the state model of multiple pedestrians is:

[0098] Since the data obtained by UWB ranging and angle measurement is in the carrier coordinate system, the obtained state variables are converted from the navigation coordinate system to the carrier coordinate system through the formula and The measurement model between pedestrians is obtained as follows:

[0099]

[0100] wherein R is a rotation matrix of a navigation coordinate system to a carrier coordinate system, L ij is an observation matrix H between pedestrian i and pedestrian j;

[0101] S203, Kalman filtering is performed to obtain the correction data of the position information and the heading angle information of each pedestrian after each step of the pedestrian.

[0102] S3, collaborative navigation is performed according to the corrected position information and the heading angle information of each pedestrian.

[0103] The above only describes the preferred embodiments of the present application, and it should be understood that the present application is not limited to the forms disclosed herein, and should not be considered as excluding other embodiments, but can be used in various other combinations, modifications and environments, and can be modified within the scope of the concepts described herein by the above teachings or related art or knowledge. Any modification and change made by those skilled in the art without departing from the spirit and scope of the present application shall be within the protection scope of the claims of the present application.

Claims

1. A distance and angle based pedestrian collaborative navigation method, characterized in that: The cooperative navigation method comprises: S1, outputting three-axis accelerometer and three-axis gyroscope data of each pedestrian through MIMU, performing strapdown inertial navigation solution, and performing zero speed correction on the data of the inertial navigation solution through the state obtained by zero speed detection by using extended Kalman filtering; S2, obtaining the relative distance and angle between pedestrians as observation by UWB ranging and angle measurement, taking the position information and heading angle information of N pedestrians as state variables, performing Kalman filtering, and obtaining the corrected data of the position information and heading angle information of each pedestrian; S3, performing cooperative navigation according to the corrected position information and heading angle information of each pedestrian; The specific steps of the extended Kalman filtering are as follows: S101, output data of a three-axis accelerometer and a three-axis gyroscope by the MIMU, respectively ; S102, detecting the zero speed state of the pedestrian by using the accelerometer covariance detection method; S103, performing strapdown inertial navigation solution on the MIMU measurement data, and when the zero speed state is detected, performing zero speed correction on the inertial navigation solution by using the zero speed pseudo-observation through the extended Kalman filtering; The state model of the extended Kalman filtering in S103 is: ； where, , denote the latitude, longitude and altitude errors, respectively, denote the east, north and sky velocity errors, respectively, denote the pitch, roll and yaw errors of the inertial navigation system, respectively, denote the gyroscope drift and denote the accelerometer bias, in total 15 dimensional errors, denotes the dynamic transition matrix of the system, denotes the noise distribution vector, denotes the Gaussian white noise; The 15-dimensional error state equation is expressed as: ； wherein, and The sub-matrices represent the disturbance relationship between position error, velocity error and attitude error, is is related to the transformation matrix of the system, and is the coefficient related to the accelerometer and gyroscope zero offset; The observation model of the extended Kalman filtering is: ； wherein is a three-dimensional velocity of the inertial navigation solution, is a 3 x 15 matrix, diag[1 1 1] ] of the measurement noise sequence, represents a zero-mean measurement noise sequence, denotes a noise covariance matrix.

2. The method of claim 1, wherein: The method for detecting the zero speed state of the pedestrian by using the accelerometer covariance detection method comprises the following steps: assuming that there are N sampling values in a certain period of time, i.e., the size of the window is N, and the zero speed state of the MIMU is wherein, , is the output of the accelerometer at the k moment, is a test statistic for zero speed detection, is a threshold value. 3.The distance and angle based pedestrian collaborative navigation method of claim 1, wherein: The specific steps of the Kalman filtering are as follows: A1, prediction state equation: through the state transition matrix of the system and the current state estimate , predict the system state at the next time ; A2, prediction error covariance: by the state transition matrix of the system , the current state estimation error covariance matrix , and the noise driving matrix and the system noise covariance matrix , predict the next time state estimation error covariance matrix ; A3, compute Kalman gain: determine Kalman filter gain by the value of state estimation error covariance matrix and measurement noise covariance matrix ;​ A4, update the state equation: by predicting the state and measuring the updated state estimation value, the optimal state estimation value at the current time is obtained ; A5, update error covariance: calculate the optimal state estimation error covariance matrix at the current time by predicting the state estimation error covariance matrix and the measurement updated state estimation error covariance matrix .

4. The method of claim 1, wherein: The specific steps of the extended Kalman filtering are as follows: S201, the relative distance between pedestrians is obtained based on UWB ranging by formula , wherein, represents the distance measurement between pedestrian i and pedestrian j at time t, represents the distance measurement noise, represents the position vector of the i-th pedestrian at the s-th step, represents the position vector of the j-th pedestrian at the s-th step, the relative angle between pedestrians is obtained based on UWB ranging by formula , wherein, represents the angle measurement between pedestrian i and pedestrian j at time t, and respectively represent the position component of the i-th pedestrian in the x-axis direction and the position component in the y-axis direction at the s-th step; S202, taking the position information and the heading angle information of the plurality of pedestrians as state quantities, taking the displacement increment and the heading increment of each step of the pedestrian as system input, establishing a motion model, and obtaining a state model of a single pedestrian according to a state model of an extended Kalman filter The state model of a single pedestrian is obtained as = ； wherein, and is the component of the position information of the pedestrian i at the k moment on the horizontal plane, is the heading angle state variable, Δ is the heading angle increment, and is the component of the position change of the pedestrian i at the k moment on the horizontal plane, and the state model of the plurality of pedestrians is obtained as ; The state quantity obtained is converted from the navigation coordinate system to the carrier coordinate system through formula and The measurement model between pedestrians is obtained as: ； wherein R is a rotation matrix from the navigation coordinate system to the carrier coordinate system, Hijis an observation matrix between pedestrian i and pedestrian j; S203, performing Kalman filtering: after each step of the pedestrian, performing Kalman filtering to obtain the corrected data of the position information and heading angle information of each pedestrian.

Images