CN110207692A - A kind of inertia pre-integration pedestrian navigation method of map auxiliary - Google Patents
A kind of inertia pre-integration pedestrian navigation method of map auxiliary Download PDFInfo
- Publication number
- CN110207692A CN110207692A CN201910393891.4A CN201910393891A CN110207692A CN 110207692 A CN110207692 A CN 110207692A CN 201910393891 A CN201910393891 A CN 201910393891A CN 110207692 A CN110207692 A CN 110207692A
- Authority
- CN
- China
- Prior art keywords
- moment
- speed
- error
- zero
- integration
- 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.)
- Granted
Links
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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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
-
- 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 kind of inertia pre-integration pedestrian navigation methods of map auxiliary.Period acquires k moment accelerometer data and gyro data；Utilize the navigation information of the inertial sensor measurement data pre-integration prediction k moment carrier of acquisition；Judge whether navigation system initializes, if initialized, carries out map match detection, if successful match, joint map, inertial error progress pose global optimization are then carried out, and exports carrier navigation letter, if it fails to match, then carry out zero-speed detection, if non-zero-speed, then carries out inertia pre-integration and solve pose, if it exists zero-speed, then combine inertial error, zero-speed constrained optimization solves pose, output carrier navigation letter.The present invention carries out pre-integration Optimization Solution using inertial sensor measured value and exports pedestrian navigation information, and carries out zero-speed about beam alignment and optimize, and introduce cartographic information and optimize to global track, eliminates accumulated error.
Description
Technical field
The invention belongs to pedestrian navigation technical fields, in particular to a kind of inertia pre-integration pedestrian navigation method.
Background technique
Pedestrian navigation technology is that today's society is studied greatly with one in field of navigation technology in life and applies hot spot.Inertia
Sensor can sensitive own rotation angular speed and acceleration information, there is high independence and anti-interference, can apply
It is that there is broad based growth prospect and the larger technology using potential quality in navigation field in various complex environments.
Pedestrian navigation technology is all based on the extremely derivative technology of Kalman filtering mostly at present, and solution process is sought being current
The optimal solution at moment, but the case where be not global optimum.Simultaneously because inertia device is pure autonomic sensor, can not receive outer
Sector signal correct course, when navigating in environment indoors, often the priori map message of available current environment and navigate
To amendment, be only that simple error is average for the amendment in course based on the pedestrian navigation of Kalman Filter Technology, can not be anti-
Truth is answered, it is bad hence for the correction effect of error, cause accuracy decline.
Summary of the invention
In order to solve the technical issues of above-mentioned background technique is mentioned, the invention proposes a kind of pre- products of inertia of map auxiliary
Cumulative errors are accurately estimated using pre-integration technology combination figure Optimization Framework, obtain global optimum and lead by people from branch's air navigation aid
Boat information.
In order to achieve the above technical purposes, the technical solution of the present invention is as follows:
A kind of inertia pre-integration pedestrian navigation method of map auxiliary, is fixed on pedestrian's foot carrier for inertial sensor,
The inertial sensor includes gyroscope and accelerometer, which comprises the following steps:
(1) period acquisition k moment accelerometer data and gyro data；
(2) navigation information of the inertial sensor measurement data pre-integration prediction k moment carrier obtained using step (1),
Obtain pre-integration predicted value；
(3) judge whether navigation system initializes, if no initializtion, is initialized, obtain the inclined of inertial sensor
The estimated value of difference and weight component, and go to step (1)；If initialized, (4) are entered step；
(4) map match detection is carried out, if successful match, it is excellent to carry out joint map, the inertial error progress pose overall situation
Change, and gos to step (6)；(5) are entered step if it fails to match；
(5) zero-speed detection is carried out, if non-zero-speed, then inertia pre-integration is carried out and solves pose；Zero-speed if it exists is then combined
Inertial error, zero-speed constrained optimization solve pose；
(6) carrier navigation information is exported, and gos to step (1).
Further, the navigation information of the carrier includes attitude quaternion, three-dimensional position and three-dimensional velocity information；It is described
Attitude quaternion is used to indicate three attitude angles including roll angle, pitch angle and yaw angle, wherein roll angle is carrier
Around the rotational angle of the Y direction of navigational coordinate system, pitch angle is rotational angle of the carrier around the X-direction of navigational coordinate system,
Yaw angle is carrier around the rotational angle of the Z-direction of navigational coordinate system, and direction of rotation is all satisfied the right-hand rule；The three-dimensional
Position is the projection in each axis in navigation for carrier positions, and three-dimensional velocity is the projection in each axis in navigation for bearer rate.
Further, each coordinate system is defined:
Using the position of current time carrier as origin construct body coordinate system b, X-axis, Y-axis and Z axis respectively with it is current when
Carve dextrad, forward direction and the day Xiang Chonghe of carrier；Inertial coodinate system i, X-axis, Y are constructed by origin of the position of initial time carrier
Axis and Z axis the day Xiang Chonghe with the dextrad of initial time carrier, forward direction and local level respectively；With rule identical with inertial system
Then construct navigational coordinate system n.
Further, in step (2), the method for obtaining pre-integration predicted value is as follows:
It is constant it is assumed that its measurement model is as follows based on gravity in inertial sensor:
In above formula,And fn(k) be respectively accelerometer measured value and true value,WithRespectively top
The measured value and true value of spiral shell instrument, ηaIt (k) is the random noise of k moment accelerometer, ηg(k) making an uproar at random for k moment gyroscope
Sound, gnGravity under being for navigation,Spin matrix for k moment navigational coordinate system to body coordinate system, ba(k) be k when
The accelerometer Measurement Biases at quarter, bgIt (k) is the gyroscope Measurement Biases at k moment, then to ba(k) and bg(k) have:
Wherein,WithFor ba(k) and bg(k) differential, ηbaFor accelerometer white noise, ηbgIt is white for gyroscope
Noise；
To inertia measurement information carry out pre-integration process be the time is numbered using two groups of different intervals, wherein
It is k and k () with the number under discrete periodic Δ t, is i with the number under n discrete periodic n Δ t, i.e., when the i moment is with k (i)
Quarter is expressed as synchronization, there is k (i)-k (i-1)=n, by time t (i)-t (i-1)=k (i) Δ t- (k (i)-n) Δ t, t
It (i) is the total time passed through to the current i moment, then:
In above formula, Δ Ri-1,iFor from the i moment to the spin matrix at i-1 moment, Δ vi-1,iFor the i-1 moment to the speed at i moment
Spend variable quantity, Δ pi-1,iFor from the i-1 moment to the location variation at i moment, RiFor i moment slave system to the rotation of navigation system
Matrix,Spin matrix for the i-1 moment from navigation system to body system, viFor speed of the i moment in navigation system, vi-1For i-
Speed of 1 moment in navigation system, Δ tk(i-1),iFor the time cycle from k (i-1) moment to the i moment, Δ Rk(i-1),kFor from k
Spin matrix of the moment to k (i-1) moment, piFor position of the i moment in navigation system, pi-1It is the i-1 moment in navigation system
Position；
Exp:R3→ SO (3) represents index throwing
Shadow mapping, by an attitude vectorsThe spin matrix being projected as in Lie group；Wherein,For any three-dimensional vector, ‖ ‖ indicates quilt
It is limited within the scope of any one other continuous 2 π,For vectorAntisymmetric matrix, I be unit battle array, R3For algebra sky
Between, SO (3) is Lie group space.
Further, in step (3), the process of initialization is as follows:
(301) start time at static k (i) moment obtains all inertia measurement information at the i-th=1 moment and carries out inertia
Pre-integration, and be using the body system of start time as navigation；
(302) using the zero-speed of Still time it is assumed that being calculated by the prediction of optimization pre-integration and static hypothesis rotational difference
The measured value of gyroscope is carried out pre-integration in initial zero-speed section by the initial deviation of gyroscope:
Wherein, the quiet continuous change of error ignored in the only moment, bg(1)=bg(2)=...=bg(k (1) -1),The error for indicating gyroscope is bg(1) k (1) moment to 1 moment when
Spin matrixPartial derivative,For k moment spin matrixRelative to gyro error
bg(1) derivative；
Due to optimizing above-mentioned function so it is unit battle array I that attitudes vibration, which measures, for stationary state and obtaining initialization gyroscope
Deviation；
(303) estimate to determine component of the current roll pitching in navigation system using gravity:
Due to zero-speed it is assumed that obtaining gravity point when all gravity of measuring value of accelerometer is in the projection in being of navigating
The initial value of amountgbFor projection of the gravity under body system；
(304) gravity is expressed are as follows:
gb=RbiExp(δθ)gi
≈Rbigi-Rbi(gi)×δθ
Wherein, RbiFor the variation relation of inertial system and body system, ()×To negate symmetrical matrix, δ θ=[δ θx δθy 0]
For the disturbance of posture, δ θxFor the angular disturbance rotated around x axis, δ θyFor the angular disturbance rotated around y-axis, giFor under inertial system
Navigation system is aligned, so that weight component is vertical with local level in navigation system by gravity with inertial system；
Spin matrix is converted into attitude quaternion, conversion process is as follows:
Wherein, q0、q1、q2、q3For attitude quaternion, rijThe element arranged for the i-th row jth in spin matrix.
Further, in step (4), the method for map match detection is as follows:
Using the current scene two-dimensional map obtained in advance, spot level point is set, coordinate setting initial position with
It will be projected to inertial coodinate system behind course, elevation of the carrier under inertial system is changed and is detected with angle change, is detected
Equation is as follows:
In previous movement section, if its elevation variation peak is more than the threshold epsilon h being set in advance, i.e., if deposited
In the height z at k momentkVariation relative to the k-1 moment | zk-zk-1| > ε h then moves first point of zero velocity behind section herein,
Position isPoint is corrected as map match, and utilizes the speed v at current timekX, y-component compensate position deviation,
For position after compensation, vi- 1 (x, y, 0) is the speed at i-1 moment (x, y, 0), vi(x, y, 0) is at i moment (x, y, 0)
Speed, eliminate elevation speed influence, that is, be shown below:
Further, in step (4), the method that joint map, inertial error carry out pose global optimization is as follows:
Global error is added in the carrier navigation position information at current time and the error of the matched location information of point map
It optimizes, to carry out global pose optimization；Optimization aim be point map from last Jing Guo successful match till now
Inertia pre-integration predicted state value χ: [x in all q zero-speeds interval between the map match point of detection1,x2…xq],Wherein,For q moment slave system to navigation system spin matrix,
The position in navigation system is tied up to for q moment body,The speed in navigation system is tied up to for q moment body,Gyroscope when for q
Error,For q brief acceleration meter error, EIMU(q, q+1) is the pose Estimation Optimization function for detecting successfully moment and lower a moment:
Wherein:
eb=b (q+1)-b (q)
In above formula,For the position of current detection matching value, ΣIWith ΣRFor information matrix, for describing inertial error
Between relationship, ρ be robust kernel function, for resisting erroneous point bring error, eRThe error for being inertia in posture, evFor
Error of the inertia in speed, epThe error for being inertia in position, ebThe variation for being inertia on error transfer, Δ Rq,q+1(bg
It (q)) be gyro error is bg(q) from the q+1 moment to q moment spin matrix, Δ v underq,q+1(bg(q),baIt (q)) is gyroscope
Error is bg(q), accelerometer error ba(q) from the q moment to q+1 moment velocity variations under,It is the q+1 moment from inertia
It is the spin matrix to body system, δ bgIt (q) is q moment gyro error variable quantity, δ ba(q) become for q moment accelerometer error
Change amount；
Represent logarithm projection
Mapping, is transformed to a rotation angle for spin matrixIt is any spin matrix, subscript with rotary shaft unit vector an a, R
" ∨ " expression takes converse symmetrical matrix；
eROnly optimize roll and pitch angle of the flow pattern integral measured by gyroscope with gravity estimation in optimization process
Difference；
After carrying out joint map matching optimization, recursion again is carried out to position and yaw using speed,To global position and yaw angle into
Row optimization；
It carries out solving above-mentioned majorized function using newton-column Weinberg alternative manner, when obtaining optimal solution or the number of iterations
The solving result of current global state amount is exported when reaching maximum number of iterations.
Further, in step (5), the method for zero-speed detection is as follows:
Zero-speed detection is carried out using the Zero velocity Updating of generalized likelihood-ratio test method, phase of standing mutually and walk is in pedestrian
Probability is estimated, thus to zero-speed state T (zn) estimated:
Wherein, N is sliding window section, Ωn={ k ∈ N:n≤k≤N-1 },For acceleration in sliding window section
The average value of output is counted,For secondly norm mould is long,WithFor the noise variance of accelerometer and gyroscope, g is indicated
Local gravity component；
When detecting T (zn) be less than threshold gamma ', then it is assumed that into standing static phase be then zero-speed state, wherein γ '=-
2ln (γ)/N, γ are set according to actual conditions.
Further, in step (5), the method that joint inertial error, zero-speed constrained optimization solve pose is as follows:
After detecting zero-speed, then present carrier pre-integration attitude value is compared with zero-speed and obtains error, thus excellent
Change the estimation to inertial error；
To be divided into q between current zero-speed, increase zero-speed constraint between current zero-speed q and upper zero-speed q-1, optimization aim isMajorized function is as follows:
It carries out solving above-mentioned majorized function using newton-column Weinberg alternative manner, when obtaining optimal solution or the number of iterations
The solving result of current quantity of state is exported when reaching maximum number of iterations.
By adopting the above technical scheme bring the utility model has the advantages that
The present invention effectively can correct course and location error in pedestrian navigation using map in environment indoors, acquisition
High reliability and high-precision pedestrian navigation information, have a good application prospect.
Detailed description of the invention
Fig. 1 is flow chart of the method for the present invention.
Specific embodiment
Below with reference to attached drawing, technical solution of the present invention is described in detail.
The present invention devises a kind of inertia pre-integration pedestrian navigation method of map auxiliary, as shown in Figure 1, by inertia sensing
Device is fixed on pedestrian's foot carrier, and the inertial sensor includes gyroscope and accelerometer, which is characterized in that steps are as follows:
Step 1: the period acquires k moment accelerometer data and gyro data；
Step 2: the navigation information of k moment carrier is predicted using the inertial sensor measurement data pre-integration that step 1 obtains,
Obtain pre-integration predicted value；
Step 3: judging whether navigation system initializes, if no initializtion, is initialized, obtain inertial sensor
The estimated value of deviation and weight component, and go to step 1；If initialized, 4 are entered step；
Step 4: carrying out map match detection, if successful match, carry out joint map, inertial error carries out the pose overall situation
Optimization, and go to step 6；5 are entered step if it fails to match；
Step 5: carrying out zero-speed detection, if non-zero-speed, then carry out inertia pre-integration and solve pose；Zero-speed if it exists, then
Joint inertial error, zero-speed constrained optimization solve pose；
Step 6: output carrier navigation information, and go to step 1.
In the present embodiment, the navigation information of the carrier includes attitude quaternion, three-dimensional position and three-dimensional velocity information；
The attitude quaternion is used to indicate three attitude angles including roll angle, pitch angle and yaw angle, wherein roll angle is
For carrier around the rotational angle of the Y direction of navigational coordinate system, pitch angle is angle of rotation of the carrier around the X-direction of navigational coordinate system
Degree, yaw angle are carrier around the rotational angle of the Z-direction of navigational coordinate system, and direction of rotation is all satisfied the right-hand rule；It is described
Three-dimensional position is the projection in each axis in navigation for carrier positions, and three-dimensional velocity is the throwing in each axis in navigation for bearer rate
Shadow.
In the present embodiment, each coordinate system is defined:
Using the position of current time carrier as origin construct body coordinate system b, X-axis, Y-axis and Z axis respectively with it is current when
Carve dextrad, forward direction and the day Xiang Chonghe of carrier；Inertial coodinate system i, X-axis, Y are constructed by origin of the position of initial time carrier
Axis and Z axis the day Xiang Chonghe with the dextrad of initial time carrier, forward direction and local level respectively；With rule identical with inertial system
Then construct navigational coordinate system n.
In the present embodiment, step 2 is realized using following preferred embodiment:
The method for obtaining pre-integration predicted value is as follows:
It is constant it is assumed that its measurement model is as follows based on gravity in inertial sensor:
In above formula,And fn(k) be respectively accelerometer measured value and true value,WithRespectively top
The measured value and true value of spiral shell instrument, ηaIt (k) is the random noise of k moment accelerometer, ηg(k) making an uproar at random for k moment gyroscope
Sound, gnGravity under being for navigation,Spin matrix for k moment navigational coordinate system to body coordinate system, ba(k) be k when
The accelerometer Measurement Biases at quarter, bgIt (k) is the gyroscope Measurement Biases at k moment, then to ba(k) and bg(k) have:
Wherein,WithFor ba(k) and bg(k) differential, ηbaFor accelerometer white noise, ηbgIt is white for gyroscope
Noise；
The process for carrying out pre-integration to inertia measurement information is that the process that pre-integration is carried out to inertia measurement information is to adopt
The time is numbered with two groups of different intervals, wherein being k and k () with the number under discrete periodic Δ t, with n discrete weeks
Number under phase n Δ t is i, i.e. the i moment and k (i) moment is expressed as synchronization, has k (i)-k (i-1)=n, by time t
(i)-t (i-1)=k (i) Δ t- (k (i)-n) Δ t, t (i) is the total time passed through to the current i moment, then:
In above formula, Δ Ri-1,iFor from the i moment to the spin matrix at i-1 moment, Δ vi-1,iFor the i-1 moment to the speed at i moment
Spend variable quantity, Δ pi-1,iFor from the i-1 moment to the location variation at i moment, RiFor i moment slave system to the rotation of navigation system
Matrix,Spin matrix for the i-1 moment from navigation system to body system, viFor speed of the i moment in navigation system, vi-1For i-
Speed of 1 moment in navigation system, Δ tk(i-1),iFor the time cycle from k (i-1) moment to the i moment, Δ Rk(i-1),kFor from k
Spin matrix of the moment to k (i-1) moment, piFor position of the i moment in navigation system, pi-1It is the i-1 moment in navigation system
Position；
Exp:R3→ SO (3) represents index throwing
Shadow mapping, by an attitude vectorsThe spin matrix being projected as in Lie group；Wherein,For any three-dimensional vector, ‖ ‖ indicates quilt
It is limited within the scope of any one other continuous 2 π,For vectorAntisymmetric matrix, I be unit battle array, R3For algebra sky
Between, SO (3) is Lie group space.
In the present embodiment, step 3 is realized using following preferred embodiment:
The process of initialization is as follows:
301, start time at static k (i) moment obtains all inertia measurement information at the i-th=1 moment and to carry out inertia pre-
Integral, and be using the body system of start time as navigation；
302, using the zero-speed of Still time it is assumed that being calculated by the prediction of optimization pre-integration and static hypothesis rotational difference
The measured value of gyroscope is carried out pre-integration in initial zero-speed section by the initial deviation of gyroscope:
Wherein, the quiet continuous change of error ignored in the only moment, bg(1)=bg(2)=...=bg(k (1) -1),The error for indicating gyroscope is bg(1) k (1) moment to 1 moment when
Spin matrixPartial derivative,For k moment spin matrixRelative to gyro error
bg(1) derivative；
Due to optimizing above-mentioned function so it is unit battle array I that attitudes vibration, which measures, for stationary state and obtaining initialization gyroscope
Deviation；
303, estimate to determine component of the current roll pitching in navigation system using gravity:
Due to zero-speed it is assumed that obtaining gravity point when all gravity of measuring value of accelerometer is in the projection in being of navigating
The initial value of amountgbFor projection of the gravity under body system；
304, gravity is expressed are as follows:
gb=RbiExp(δθ)gi
≈Rbigi-Rbi(gi)×δθ
Wherein, RbiFor the variation relation of inertial system and body system, ()×To negate symmetrical matrix, δ θ=[δ θx δθy 0]
For the disturbance of posture, δ θxFor the angular disturbance rotated around x axis, δ θyFor the angular disturbance rotated around y-axis, giFor under inertial system
Navigation system is aligned, so that weight component is vertical with local level in navigation system by gravity with inertial system；
Spin matrix is converted into attitude quaternion, conversion process is as follows:
Wherein, q0、q1、q2、q3For attitude quaternion, rijThe element arranged for the i-th row jth in spin matrix.
In the present embodiment, step 4 is realized using following preferred embodiment:
The method of map match detection is as follows:
Using the current scene two-dimensional map obtained in advance, spot level point is set, coordinate setting initial position with
It will be projected to inertial coodinate system behind course, elevation of the carrier under inertial system is changed and is detected with angle change, is detected
Equation is as follows:
In previous movement section, if its elevation variation peak is more than the threshold epsilon h being set in advance, i.e., if deposited
In the height z at k momentkVariation relative to the k-1 moment | zk-zk-1| > ε h then moves first point of zero velocity behind section herein,
Position isPoint is corrected as map match, and utilizes the speed v at current timekX, y-component compensate position deviation,
For position after compensation, vi-1(x, y, 0) is the speed at i-1 moment (x, y, 0), vi(x, y, 0) is at i moment (x, y, 0)
Speed, eliminate elevation speed influence, that is, be shown below:
If there are the variations of height at stair | zk-zk-1| > ε h can be used as in last stage rank of stair
Map index point, height change is from having nothing at this time, i.e., | zk-zk-1| < ε h, using the position of stair at this time as map match position
It sets a little.
The method that joint map, inertial error carry out pose global optimization is as follows:
Global error is added in the carrier navigation position information at current time and the error of the matched location information of point map
It optimizes, to carry out global pose optimization；Optimization aim be point map from last Jing Guo successful match till now
Inertia pre-integration predicted state value χ: [x in all q zero-speeds interval between the map match point of detection1,x2...xq],Wherein,For q moment slave system to navigation system spin matrix,
The position in navigation system is tied up to for q moment body,The speed in navigation system is tied up to for q moment body,Gyroscope when for q
Error,For q brief acceleration meter error, EIMU(q, q+1) is the pose Estimation Optimization function for detecting successfully moment and lower a moment:
Wherein:
eb=b (q+1)-b (q)
In above formula,For the position of current detection matching value, ΣIWith ΣRFor information matrix, for describing inertial error
Between relationship, ρ be robust kernel function, for resisting erroneous point bring error, eRThe error for being inertia in posture, evFor
Error of the inertia in speed, epThe error for being inertia in position, ebThe variation for being inertia on error transfer, Δ Rq,q+1(bg
It (q)) be gyro error is bg(q) from the q+1 moment to q moment spin matrix, Δ v underq,q+1(bg(q),baIt (q)) is gyroscope
Error is bg(q), accelerometer error ba(q) from the q moment to q+1 moment velocity variations under,It is the q+1 moment from inertia
It is the spin matrix to body system, δ bgIt (q) is q moment gyro error variable quantity, δ ba(q) become for q moment accelerometer error
Change amount；
Represent logarithm projection
Mapping, is transformed to a rotation angle for spin matrixIt is any spin matrix, subscript with rotary shaft unit vector an a, R
" ∨ " expression takes converse symmetrical matrix；
eROnly optimize roll and pitch angle of the flow pattern integral measured by gyroscope with gravity estimation in optimization process
Difference；
After carrying out joint map matching optimization, recursion again is carried out to position and yaw using speed,To global position and yaw angle into
Row optimization；
Solve using newton-column Weinberg alternative manners such as figure optimization open source library G2O, GTSAM, Ceres above-mentioned excellent
Change function, the solving result of current global state amount is exported when obtaining optimal solution or the number of iterations reaches maximum number of iterations.
In the present embodiment, step 5 is realized using following preferred embodiment:
The method of zero-speed detection is as follows:
Zero-speed detection is carried out using the Zero velocity Updating of generalized likelihood-ratio test method, phase of standing mutually and walk is in pedestrian
Probability is estimated, thus to zero-speed state T (zn) estimated:
Wherein, N is sliding window section, Ωn={ k ∈ N:n≤k≤N-1 },For acceleration in sliding window section
The average value of output is counted,For secondly norm mould is long,WithFor the noise variance of accelerometer and gyroscope, g is indicated
Local gravity component；
When detecting T (zn) be less than threshold gamma ', then it is assumed that into standing static phase be then zero-speed state, wherein γ '=-
2ln (γ)/N, γ are set according to actual conditions.
The method that joint inertial error, zero-speed constrained optimization solve pose is as follows:
After detecting zero-speed, then present carrier pre-integration attitude value is compared with zero-speed and obtains error, thus excellent
Change the estimation to inertial error；
To be divided into q between current zero-speed, increase zero-speed constraint between current zero-speed q and upper zero-speed q-1, optimization aim isMajorized function is as follows:
Solve using newton-column Weinberg alternative manners such as figure optimization open source library G2O, GTSAM, Ceres above-mentioned excellent
Change function, the solving result of current quantity of state is exported when obtaining optimal solution or the number of iterations reaches maximum number of iterations.
Embodiment is merely illustrative of the invention's technical idea, and this does not limit the scope of protection of the present invention, it is all according to
Technical idea proposed by the present invention, any changes made on the basis of the technical scheme are fallen within the scope of the present invention.
Claims (9)
1. a kind of inertia pre-integration pedestrian navigation method of map auxiliary, is fixed on pedestrian's foot carrier, institute for inertial sensor
Stating inertial sensor includes gyroscope and accelerometer, which comprises the following steps:
(1) period acquisition k moment accelerometer data and gyro data；
(2) navigation information of the inertial sensor measurement data pre-integration prediction k moment carrier obtained using step (1), is obtained
Pre-integration predicted value；
(3) judge whether navigation system initializes, if no initializtion, is initialized, obtain inertial sensor deviation and
The estimated value of weight component, and go to step (1)；If initialized, (4) are entered step；
(4) map match detection is carried out, if successful match, carries out joint map, inertial error progress pose global optimization, and
Go to step (6)；(5) are entered step if it fails to match；
(5) zero-speed detection is carried out, if non-zero-speed, then inertia pre-integration is carried out and solves pose；Zero-speed if it exists then combines inertia
Error, zero-speed constrained optimization solve pose；
(6) carrier navigation information is exported, and gos to step (1).
2. the inertia pre-integration pedestrian navigation method of map auxiliary according to claim 1, which is characterized in that the carrier
Navigation information includes attitude quaternion, three-dimensional position and three-dimensional velocity information；The attitude quaternion is for indicating to include roll
Three attitude angles including angle, pitch angle and yaw angle, wherein roll angle is turn of the carrier around the Y direction of navigational coordinate system
Dynamic angle, pitch angle are rotational angle of the carrier around the X-direction of navigational coordinate system, and yaw angle is carrier around navigational coordinate system
The rotational angle of Z-direction, direction of rotation are all satisfied the right-hand rule；The three-dimensional position is each axis in navigation for carrier positions
In projection, it in navigation is projection in each axis that three-dimensional velocity, which is bearer rate,.
3. the inertia pre-integration pedestrian navigation method of map auxiliary according to claim 1, which is characterized in that define each coordinate
System:
Body coordinate system b is constructed by origin of the position of current time carrier, X-axis, Y-axis and Z axis are carried with current time respectively
Dextrad, forward direction and the day Xiang Chonghe of body；Using the position of initial time carrier as origin construct inertial coodinate system i, X-axis, Y-axis with
The Z axis day Xiang Chonghe with the dextrad of initial time carrier, forward direction and local level respectively；With regular structure identical with inertial system
Build navigational coordinate system n.
4. the inertia pre-integration pedestrian navigation method of map auxiliary according to claim 1, which is characterized in that in step (2)
In, the method for obtaining pre-integration predicted value is as follows:
It is constant it is assumed that its measurement model is as follows based on gravity in inertial sensor:
In above formula,And fn(k) be respectively accelerometer measured value and true value,WithRespectively gyroscope
Measured value and true value, ηaIt (k) is the random noise of k moment accelerometer, ηgIt (k) is the random noise of k moment gyroscope, gn
Gravity under being for navigation,Spin matrix for k moment navigational coordinate system to body coordinate system, baIt (k) is the k moment
Accelerometer Measurement Biases, bgIt (k) is the gyroscope Measurement Biases at k moment, then to ba(k) and bg(k) have:
Wherein,WithFor ba(k) and bg(k) differential, ηbaFor accelerometer white noise, ηbgFor gyroscope white noise；
To inertia measurement information carry out pre-integration process be the time is numbered using two groups of different intervals, wherein with from
The number dissipated under period Δ t is k and k (), is i with the number under n discrete periodic n Δ t, i.e. i moment and k (i) timetable
It is shown as synchronization, there is k (i)-k (i-1)=n, is by time t (i)-t (i-1)=k (i) Δ t- (k (i)-n) Δ t, t (i)
The total time passed through to the current i moment, then:
In above formula, Δ Ri-1,iFor from the i moment to the spin matrix at i-1 moment, Δ vi-1,iBecome for the speed at i-1 moment to i moment
Change amount, Δ pi-1,iFor from the i-1 moment to the location variation at i moment, RiFor i moment slave system to the spin moment of navigation system
Battle array,Spin matrix for the i-1 moment from navigation system to body system, viFor speed of the i moment in navigation system, vi-1For i-1
Speed of the moment in navigation system, Δ tk(i-1),iFor the time cycle from k (i-1) moment to the i moment, Δ Rk(i-1),kWhen for from k
It is carved into the spin matrix at k (i-1) moment, piFor position of the i moment in navigation system, pi-1For position of the i-1 moment in navigation system
It sets；
Represent index projection
Mapping, by an attitude vectorsThe spin matrix being projected as in Lie group；Wherein,For any three-dimensional vector, ‖ ‖ expression is limited
It makes within the scope of any one other continuous 2 π,For vectorAntisymmetric matrix, I be unit battle array, R3For algebraic space,
SO (3) is Lie group space.
5. the inertia pre-integration pedestrian navigation method of map auxiliary according to claim 4, which is characterized in that in step (3)
In, the process of initialization is as follows:
(301) start time at static k (i) moment obtains all inertia measurement information at the i-th=1 moment and carries out the pre- product of inertia
Point, and be using the body system of start time as navigation；
(302) using the zero-speed of Still time it is assumed that calculating gyro by the prediction of optimization pre-integration and static hypothesis rotational difference
The measured value of gyroscope is carried out pre-integration in initial zero-speed section by the initial deviation of instrument:
Wherein, the quiet continuous change of error ignored in the only moment, bg(1)=bg(2)=...=bg(k (1) -1),The error for indicating gyroscope is bg(1) k (1) moment to 1 moment when
Spin matrixPartial derivative,For k moment spin matrixRelative to gyro error
bg(1) derivative；
Due to for stationary state, so it is unit battle array I that attitudes vibration, which measures, optimizing above-mentioned function, to obtain initialization gyroscope inclined
Difference；
(303) estimate to determine component of the current roll pitching in navigation system using gravity:
Due to zero-speed it is assumed that obtaining weight component when all gravity of measuring value of accelerometer is in the projection in being of navigating
Initial valuegbFor projection of the gravity under body system；
(304) gravity is expressed are as follows:
Wherein, RbiFor the variation relation of inertial system and body system, ()×To negate symmetrical matrix, δ θ=[δ θx δθyIt 0] is appearance
The disturbance of state, δ θxFor the angular disturbance rotated around x axis, δ θyFor the angular disturbance rotated around y-axis, giFor the gravity under inertial system,
Navigation system is aligned with inertial system, so that weight component is vertical with local level in navigation system；
Spin matrix is converted into attitude quaternion, conversion process is as follows:
Wherein, q0、q1、q2、q3For attitude quaternion, rijThe element arranged for the i-th row jth in spin matrix.
6. the inertia pre-integration pedestrian navigation method of map auxiliary according to claim 4, which is characterized in that in step (4)
In, the method for map match detection is as follows:
Using the current scene two-dimensional map obtained in advance, spot level point is set, and coordinate is in setting initial position and course
It will project afterwards to inertial coodinate system, elevation of the carrier under inertial system is changed and is detected with angle change, equation is detected
It is as follows:
In previous movement section, if its elevation variation peak is more than the threshold epsilon h that is set in advance, i.e., if there is k when
The height z at quarterkVariation relative to the k-1 moment | zk-zk-1| > ε h then moves first point of zero velocity behind section, position herein
ForPoint is corrected as map match, and utilizes the speed v at current timekX, y-component compensate position deviation,To mend
Repay rear position, vi-1(x, y, 0) is the speed at i-1 moment (x, y, 0), vi(x, y, 0) is the speed at i moment (x, y, 0)
Degree, eliminating elevation speed influences, that is, is shown below:
7. the inertia pre-integration pedestrian navigation method of map auxiliary according to claim 6, which is characterized in that in step (4)
In, the method that joint map, inertial error carry out pose global optimization is as follows:
Global error is added in the carrier navigation position information at current time and the error of the matched location information of point map to carry out
Optimization, to carry out global pose optimization；Optimization aim is to detect till now from the last point map Jing Guo successful match
Map match point between all q zero-speeds interval in inertia pre-integration predicted state value χ:Wherein,For q moment slave system to navigation system
Spin matrix,The position in navigation system is tied up to for q moment body,The speed in navigation system is tied up to for q moment body,
Gyro error when for q,For q brief acceleration meter error, EIMU(q, q+1) is to detect the successfully moment and the pose at lower a moment is estimated
Count majorized function:
Wherein:
ev=0
eb=b (q+1)-b (q)
In above formula,For the position of current detection matching value, ΣIWith ΣRFor information matrix, for describing between inertial error
Relationship, ρ be robust kernel function, for resisting erroneous point bring error, eRThe error for being inertia in posture, evFor inertia
Error in speed, epThe error for being inertia in position, ebThe variation for being inertia on error transfer, Δ Rq,q+1(bg(q))
It is b for gyro errorg(q) from the q+1 moment to q moment spin matrix, Δ v underq,q+1(bg(q),baIt (q)) is gyro error
For bg(q), accelerometer error ba(q) from the q moment to q+1 moment velocity variations under,For the q+1 moment from inertial system to
The spin matrix of body system, δ bgIt (q) is q moment gyro error variable quantity, δ ba(q) change for q moment accelerometer error
Amount；
Logarithm projection mapping is represented,
Spin matrix is transformed to a rotation angleIt is any spin matrix, subscript " ∨ " table with rotary shaft unit vector an a, R
Show and takes converse symmetrical matrix；
eROnly optimize the flow pattern integral measured by gyroscope and the roll of gravity estimation and the difference of pitch angle in optimization process；
After carrying out joint map matching optimization, recursion again is carried out to position and yaw using speed,To global position and yaw angle into
Row optimization；
It carries out solving above-mentioned majorized function using newton-column Weinberg alternative manner, when obtaining optimal solution or the number of iterations reaches
The solving result of current global state amount is exported when maximum number of iterations.
8. the inertia pre-integration pedestrian navigation method of map auxiliary according to claim 7, which is characterized in that in step (5)
In, the method for zero-speed detection is as follows:
Zero-speed detection is carried out using the Zero velocity Updating of generalized likelihood-ratio test method, the probability for phase of standing mutually and walk is in pedestrian
Estimated, thus to zero-speed state T (zn) estimated:
Wherein, N is sliding window section, Ωn={ k ∈ N:n≤k≤N-1 },It is defeated for accelerometer in sliding window section
Average value out,For secondly norm mould is long,WithFor the noise variance of accelerometer and gyroscope, g indicates local
Weight component；
When detecting T (zn) be less than threshold gamma ', then it is assumed that into standing static phase be then zero-speed state, wherein γ '=- 2ln
(γ)/N, γ are set according to actual conditions.
9. the inertia pre-integration pedestrian navigation method of map auxiliary according to claim 7, which is characterized in that in step (5)
In, the method that joint inertial error, zero-speed constrained optimization solve pose is as follows:
After detecting zero-speed, then present carrier pre-integration attitude value is compared with zero-speed and obtains error, thus optimization pair
The estimation of inertial error；
To be divided into q between current zero-speed, increase zero-speed constraint between current zero-speed q and upper zero-speed q-1, optimization aim isMajorized function is as follows:
It carries out solving above-mentioned majorized function using newton-column Weinberg alternative manner, when obtaining optimal solution or the number of iterations reaches
The solving result of current quantity of state is exported when maximum number of iterations.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910393891.4A CN110207692B (en) | 2019-05-13 | 2019-05-13 | Map-assisted inertial pre-integration pedestrian navigation method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910393891.4A CN110207692B (en) | 2019-05-13 | 2019-05-13 | Map-assisted inertial pre-integration pedestrian navigation method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110207692A true CN110207692A (en) | 2019-09-06 |
| CN110207692B CN110207692B (en) | 2021-03-30 |
Family
ID=67787129
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910393891.4A Active CN110207692B (en) | 2019-05-13 | 2019-05-13 | Map-assisted inertial pre-integration pedestrian navigation method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110207692B (en) |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111241983A (en) * | 2020-01-07 | 2020-06-05 | 北京海益同展信息科技有限公司 | Posture detection method, device and system, electronic equipment and storage medium |
| CN111487973A (en) * | 2020-04-26 | 2020-08-04 | 河南科技大学 | Navigation method and system for low-frequency refreshing of navigation signal |
| CN112729301A (en) * | 2020-12-10 | 2021-04-30 | 深圳大学 | Indoor positioning method based on multi-source data fusion |
| CN112985392A (en) * | 2021-04-19 | 2021-06-18 | 中国人民解放军国防科技大学 | Pedestrian inertial navigation method and device based on graph optimization framework |
| CN115685292A (en) * | 2023-01-05 | 2023-02-03 | 中国人民解放军国防科技大学 | Navigation method and device of multi-source fusion navigation system |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104061934A (en) * | 2014-06-10 | 2014-09-24 | 哈尔滨工业大学 | Pedestrian indoor position tracking method based on inertial sensor |
| CN107869989A (en) * | 2017-11-06 | 2018-04-03 | 东北大学 | A kind of localization method and system of the fusion of view-based access control model inertial navigation information |
| CN108007456A (en) * | 2017-12-06 | 2018-05-08 | 深圳市致趣科技有限公司 | A kind of indoor navigation method, apparatus and system |
| CN108492316A (en) * | 2018-02-13 | 2018-09-04 | 视辰信息科技(上海)有限公司 | A kind of localization method and device of terminal |
| CN108827315A (en) * | 2018-08-17 | 2018-11-16 | 华南理工大学 | Vision inertia odometer position and orientation estimation method and device based on manifold pre-integration |
| CN109029448A (en) * | 2018-06-28 | 2018-12-18 | 东南大学 | The IMU of monocular vision inertial positioning assists trace model |
| CN109465832A (en) * | 2018-12-18 | 2019-03-15 | 哈尔滨工业大学（深圳） | High-precision vision and the tight fusion and positioning method of IMU and system |
| CN109631894A (en) * | 2018-12-11 | 2019-04-16 | 智灵飞（北京）科技有限公司 | A kind of monocular vision inertia close coupling method based on sliding window |
| US20190113347A1 (en) * | 2017-10-12 | 2019-04-18 | Hanwha Land Systems Co., Ltd. | Inertia-based navigation apparatus and inertia-based navigation method based on relative preintegration |
-
2019
- 2019-05-13 CN CN201910393891.4A patent/CN110207692B/en active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104061934A (en) * | 2014-06-10 | 2014-09-24 | 哈尔滨工业大学 | Pedestrian indoor position tracking method based on inertial sensor |
| US20190113347A1 (en) * | 2017-10-12 | 2019-04-18 | Hanwha Land Systems Co., Ltd. | Inertia-based navigation apparatus and inertia-based navigation method based on relative preintegration |
| CN107869989A (en) * | 2017-11-06 | 2018-04-03 | 东北大学 | A kind of localization method and system of the fusion of view-based access control model inertial navigation information |
| CN108007456A (en) * | 2017-12-06 | 2018-05-08 | 深圳市致趣科技有限公司 | A kind of indoor navigation method, apparatus and system |
| CN108492316A (en) * | 2018-02-13 | 2018-09-04 | 视辰信息科技(上海)有限公司 | A kind of localization method and device of terminal |
| CN109029448A (en) * | 2018-06-28 | 2018-12-18 | 东南大学 | The IMU of monocular vision inertial positioning assists trace model |
| CN108827315A (en) * | 2018-08-17 | 2018-11-16 | 华南理工大学 | Vision inertia odometer position and orientation estimation method and device based on manifold pre-integration |
| CN109631894A (en) * | 2018-12-11 | 2019-04-16 | 智灵飞（北京）科技有限公司 | A kind of monocular vision inertia close coupling method based on sliding window |
| CN109465832A (en) * | 2018-12-18 | 2019-03-15 | 哈尔滨工业大学（深圳） | High-precision vision and the tight fusion and positioning method of IMU and system |
Non-Patent Citations (2)
| Title |
|---|
| DINESH ATCHUTHAN等: "Odometry Based on Auto-Calibrating Inertial Measurement Unit Attached to the Feet", 《2018 EUROPEAN CONTROL CONFERENCE (ECC)》 * |
| 李建禹: "基于单目视觉与IMU结合的SLAM技术研究", 《中国优秀硕士学位论文全文数据库信息科技辑》 * |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111241983A (en) * | 2020-01-07 | 2020-06-05 | 北京海益同展信息科技有限公司 | Posture detection method, device and system, electronic equipment and storage medium |
| CN111241983B (en) * | 2020-01-07 | 2023-09-26 | 京东科技信息技术有限公司 | Gesture detection method, device and system, electronic equipment and storage medium |
| CN111487973A (en) * | 2020-04-26 | 2020-08-04 | 河南科技大学 | Navigation method and system for low-frequency refreshing of navigation signal |
| CN111487973B (en) * | 2020-04-26 | 2023-09-05 | 河南科技大学 | Navigation method and system for low-frequency refreshing of navigation signals |
| CN112729301A (en) * | 2020-12-10 | 2021-04-30 | 深圳大学 | Indoor positioning method based on multi-source data fusion |
| CN112985392A (en) * | 2021-04-19 | 2021-06-18 | 中国人民解放军国防科技大学 | Pedestrian inertial navigation method and device based on graph optimization framework |
| CN115685292A (en) * | 2023-01-05 | 2023-02-03 | 中国人民解放军国防科技大学 | Navigation method and device of multi-source fusion navigation system |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110207692B (en) | 2021-03-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110207692A (en) | A kind of inertia pre-integration pedestrian navigation method of map auxiliary | |
| CN109991636A (en) | Map constructing method and system based on GPS, IMU and binocular vision | |
| CN110095116A (en) | A kind of localization method of vision positioning and inertial navigation combination based on LIFT | |
| CN108731670A (en) | Inertia/visual odometry combined navigation locating method based on measurement model optimization | |
| CN104121905B (en) | Course angle obtaining method based on inertial sensor | |
| CN109540126A (en) | A kind of inertia visual combination air navigation aid based on optical flow method | |
| US10337884B2 (en) | Method and apparatus for fast magnetometer calibration | |
| CN103776446B (en) | A kind of pedestrian's independent navigation computation based on double MEMS-IMU | |
| CN107655493B (en) | SINS six-position system-level calibration method for fiber-optic gyroscope | |
| CN103674021B (en) | Integrated navigation system based on inertial navigation and star sensor and method | |
| CN109827577A (en) | High-precision inertial navigation location algorithm based on motion state detection | |
| CN108362282A (en) | A kind of inertia pedestrian's localization method based on the adjustment of adaptive zero-speed section | |
| CN106052691B (en) | Close ring error correction method in the mobile drawing of laser ranging | |
| CN110398257A (en) | The quick initial alignment on moving base method of SINS system of GPS auxiliary | |
| CN108253963A (en) | A kind of robot active disturbance rejection localization method and alignment system based on Multi-sensor Fusion | |
| CN111024070A (en) | Inertial foot binding type pedestrian positioning method based on course self-observation | |
| CN109099913B (en) | Wearable navigation device and method based on MEMS inertial device | |
| CN109916394A (en) | A kind of Integrated Navigation Algorithm merging optical flow position and velocity information | |
| CN107270893A (en) | Lever arm, time in-synchronization error estimation and the compensation method measured towards real estate | |
| CN109764880A (en) | The vision inertia ranging method and system of close coupling vehicle wheel encoder data | |
| CN107941217A (en) | A kind of robot localization method, electronic equipment, storage medium, device | |
| CN106662443A (en) | Methods and systems for vertical trajectory determination | |
| CN109931955A (en) | Strapdown inertial navigation system Initial Alignment Method based on the filtering of state correlation Lie group | |
| CN107728182A (en) | Flexible more base line measurement method and apparatus based on camera auxiliary | |
| CN113175933B (en) | Factor graph integrated navigation method based on high-precision inertial pre-integration |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |