Summary of the invention
Moving alignment method alignment precision in order to overcome the shortcomings of existing vehicle movement mathematical model auxiliary is poor, this hair
Alignment methods between a kind of bright backtracking formula for providing vehicle kinematics constraint auxiliary is advanced.This method carries out in vehicle travel process
Real-time coarse alignment, while extracting and storing key sensor information;Recalled after obtaining initial attitude battle array using storing data
Navigation calculation;It is laterally and vertical using vehicle using the installation deviation angle between inertial navigation and vehicle as error state in trace-back process
Kalman filtering fine alignment is carried out to the constraint condition that speed is zero.The method of the present invention does not need special quiescent time, can
Effectively improve alignment precision between advancing.
A kind of the technical solution adopted by the present invention to solve the technical problems: backtracking formula row of vehicle kinematics constraint auxiliary
Into an alignment methods, its main feature is that the following steps are included:
(a) after inertial navigation system obtains initial position and enters alignment, starting vehicle is travelled by pre-determined route.
(b) in driving process, real-time coarse alignment is carried out using the double vector method for determining posture of inertial system quadratic integral.Definition resolves
Required reference frame is as follows:
N-navigational coordinate system, X, Y, Z axis are respectively directed to east-north-day direction of carrier geographic location;
B-IMU coordinate system, X, Y, Z axis are respectively directed to preceding-upper direction in the right side-of Inertial Measurement Unit;
M- bodywork reference frame, X, Y, Z axis are respectively directed to preceding-upper direction in the right side-of car body;
n0Navigational coordinate system n relative inertness space is solidified institute in alignment initial time by initial navigation inertial coodinate system
The inertial coodinate system obtained;
b0Initial IMU inertial coodinate system, alignment initial time will be obtained by the solidification of IMU coordinate system b relative inertness space
Inertial coodinate system.
The influence for ignoring vehicle movement solves initial matrix using inertial system quadratic integral TRIAD method
Subscript t in formula1、t2Indicate different time points,For the n obtained to gravity acceleration information quadratic integral0It is position
Set increment;For according to the b of gyro and the calculating of accelerometer data quadratic integral0It is positional increment.
(c) during entire alignment, the key message of gyro and accelerometer is extracted, every 1s stores 6 floating numbers: posture four
First numberAnd speed increment
The quaternary number of kth secondIt calculates
Period and this period, recursion initial value before subscript k-1 and k are indicated in formulaAccording to q (Φ)
The rotation quaternary number of each cycle gyro output construction.
The speed increment of kth secondIt calculates
In formulaIt is resolved by formula (2)It is converted to,For the specific force letter of accelerometer output
Breath.
(d) after coarse alignment, backtracking navigation calculation is carried out according to storage information, including speed updates, posture renewal and
Location updating
T in formulak=1, it is the update cycle;vnFor navigation speed, and vn=[vE vN vU]T;[λ, L, h] be position longitude,
Latitude and height;For current pose quaternary number;WithTo read storing data;MatrixAccording to
Earth rotation angular speedApparent motion angular speedLocal latitude L and alignment time obtain;For coarse alignment acquisition
Initial attitude battle array,For corresponding quaternary number;gnFor gravity acceleration;For matrixCorresponding quaternary number;
RM、RNFor meridian circle and prime vertical radius, calculated according to current location.
(e) while recalling navigation calculation, Select Error state are as follows: velocity errorThe misaligned angle of the platform φn, gyro zero
Inclined εb, accelerometer zero, the installation deviation angle α of the spaced winding X of IMU coordinate system b and bodywork reference frame m, Z axisxAnd αz.It establishes
17 dimension kalman filter state variables
Establish corresponding state equation
F in formulaI15 × 15 to establish by classical inertial navigation error equation tie up state-transition matrixes,For top
Spiral shell and the corresponding system noise of accelerometer data.
According to vehicle kinematics Construction of A Model nonholonomic restriction, every 1s carries out measurement update
Measuring value in formulaSymbol M1(1 :) it indicates to take 3 × 3 matrix M1The 1st row,
Remaining symbol is defined by same rule, andwvFor speed amount
Survey noise.
The beneficial effects of the present invention are: this method carries out real-time coarse alignment in vehicle travel process, while extracting and depositing
Store up key sensor information;Backtracking navigation calculation is carried out using storing data after acquisition initial attitude battle array;It will be used in trace-back process
Installation deviation angle between vehicle is led as error state, using vehicle laterally and vertical velocity be zero constraint condition progress
Kalman filtering fine alignment.The method of the present invention does not need special quiescent time, effectively increases alignment precision between traveling.Through surveying
Examination, alignment of orientation result reach 2.148 '.Meanwhile alignment procedures do not lose position and speed information, positioning accuracy is better than 20m.
It elaborates with reference to the accompanying drawings and detailed description to the present invention.
Specific embodiment
Referring to Fig.1-6.Specific step is as follows for alignment methods between the backtracking formula of vehicle kinematics constraint auxiliary of the present invention is advanced:
1, initial position is obtained.
In t0Moment stationary vehicle, inertial navigation system enter alignment after obtaining initial position, and vehicle is allowed to go immediately
It sails, does not need special quiescent time.
2, real-time quadratic integral inertial system coarse alignment.
In [the t of vehicle driving0,tc] the real-time coarse alignment of time interval progress.It is auxiliary without speed and location information during traveling
It helps, ignores the motion process of vehicle, carry out coarse alignment resolving by quiet pedestal environment.In order to reduce the influence of motion artifacts, introduce
The inertial system coarse alignment method of quadratic integral.Definition resolves required reference frame first:
N-navigational coordinate system, X, Y, Z axis are respectively directed to east-north-day direction of carrier geographic location;
B-IMU coordinate system, X, Y, Z axis are respectively directed to preceding-upper direction in the right side-of Inertial Measurement Unit;
M- bodywork reference frame, X, Y, Z axis are respectively directed to preceding-upper direction in the right side-of car body;
n0Navigational coordinate system n relative inertness space is solidified institute in alignment initial time by initial navigation inertial coodinate system
The inertial coodinate system obtained;
b0Initial IMU inertial coodinate system, alignment initial time will be obtained by the solidification of IMU coordinate system b relative inertness space
Inertial coodinate system.
Inertial system quadratic integral TRIAD method Real-time solution initial matrix is used in driving process
Subscript t in formula1、t2Corresponding different moments, take t1=t/2, t2=t, t are the alignment time;To accelerate to gravity
Spend the n that information quadratic integral obtains0It is positional increment;For according to the b of gyro and the calculating of accelerometer data quadratic integral0
It is positional increment.
Method for solving are as follows:
WhereinIt is obtained by inertial system posture renewal,For the ratio force information of accelerometer output.
Method for solving are as follows:
G in formulanFor gravity acceleration;Transition matrixCalculating ignore vehicle movement, according to earth rotation angular speed
ωie, initial latitude L0With alignment time t analytical Calculation.
3, data are extracted and are stored.
In entire alignment procedures [t0,tf] during, every 1s period stores 6 floating numbers: attitude quaternionSpeed increases
AmountThe calculation method of each parameter is as follows:
The quaternary number of kth secondIt is calculated using inertial system posture renewal method
Period and this period, recursion initial value before subscript k-1 and k are indicated in formulaAccording to q (Φ)
The rotation quaternary number of each cycle gyro output construction.
The speed increment of kth secondIt calculates
Attitude matrix in formulaPass through the posture renewal quaternary number of formula (11)It is converted to,To add
The ratio force information of speedometer output.
4, recall navigation calculation.
Coarse alignment is in tcAfter moment, continues to extract and store [tc,tf] section data, while utilize free time
To [t0,tf] storing data in section carries out backtracking navigation, the resolving period is 1s.Backtracking navigation calculation process include speed update,
Posture renewal and position updating process, specific calculation method are as follows:
1. recalling speed to update.
Period and this period before subscript k-1 and k are indicated in formula, and cycle Tk=1s;vn=[vE vN vU]T, for navigation speed
Degree;For coarse alignment result;For the speed increment per second for reading storage;For earth rotation angular speed;For around ground
Ball surface moves angular speed;For rotating vectorCorresponding transition matrix;Transition matrixSolution be considered as the influence of change in location, according to earth rotation angular speed ωie, initial position longitude and latitude [λ0,L0], when
Front position resolves longitude and latitude [λ, L] and time t analytical Calculation.
2. recalling posture renewal.
WhereinFor the quaternary number currently updated,For matrixCorresponding quaternary number,For coarse alignment resultCorresponding quaternary number,For the quaternary number for reading storage.
3. recalling location updating.
[λ, L, h] is position longitude, latitude and height, R in formulaM、RNFor meridian circle and prime vertical radius, according to present bit
Set calculating.
5, vehicle kinematics constrain fine alignment.
During the backtracking navigation calculation of step 4, every 1s period carries out Kalman using vehicle kinematics model-aided
Fine alignment is filtered, estimate every error and is modified.
1. establishing state equation.
Select Error state are as follows: velocity error δ vI n, the misaligned angle of the platform φn, gyro zero bias εb, accelerometer zero,
The installation deviation angle α of the spaced winding X of IMU coordinate system b and bodywork reference frame m, Z axisxAnd αz.Establish 17 dimension kalman filter states
Variable
Establish corresponding state equation
F in formulaI15 × 15 to establish by classical inertial navigation error equation tie up state-transition matrixes,For top
Spiral shell and the corresponding system noise of accelerometer data.
2. establishing measurement equation.
According to vehicle kinematics restricted model, there is no sideslips and jump in normal driving process for vehicle, so laterally
It is zero with vertical velocity, i.e.,WhereinFor the X under bodywork reference frame m, Z axis speed.
Consider installation deviation angle αxAnd αzFor low-angle, nonholonomic restriction measuring value is constructedWhereinAndvnThe respectively attitude matrix of trace-back process and velocity calculated result.
Corresponding measurement equation is
Symbol M in formula1(1 :) it indicates to take 3 × 3 matrix M1The 1st row, remaining symbol by same rule definition, andwvFor velocity measurement noise.
Inertial navigation system is in [tc,tf] period interior processing [t0,tf] section compression storing data, complete backtracking navigational solution
It calculates and kinematical constraint fine alignment, the every error of estimation is simultaneously modified.
It applies the inventive method in specific embodiment below.
Pilot system is used to group using Laser strapdown, and laser gyro precision is 0.008 °/h, and accelerometer precision is 40 μ g,
GPS location precision is horizontal 3m, elevation 5m.It is examined using inertia/GPS integrated navigation result as posture and position in test
Benchmark does not use GPS data in alignment methods between traveling.
Fig. 2 is the driving path figure of alignment test between 3 travelings, is predominantly travelled from east to west, and road conditions are country road,
The operating range tested every time is in 10km or so.
Fig. 3 gives the travel speed curve of alignment test between 3 travelings.As can be seen that the alignment time of each test
In 10min or so, and vehicle travels immediately after entering alignment, and the rest time is less than 5s.
Fig. 4 is attitude error curve of the embodiment of the present invention in real-time coarse alignment stage.After the influence for ignoring vehicle movement,
Gradually convergent posture and alignment of orientation result can be obtained by quadratic integral inertial system coarse alignment method.Take tc=580s,
Horizontal coarse alignment precision is better than 1 °, and orientation coarse alignment precision is better than 5 °, meets the requirement of subsequent linearisation fine alignment.
Fig. 5 is the attitude error curve that the embodiment of the present invention recalls the fine alignment stage in kinematical constraint.As can be seen that each
It is gradually restrained at any time to misalignment angle error, is [0.370 in the statistical accuracy of alignment finish time;0.225;2.148] ', it is right
Quasi- precision is high.
Fig. 6 is location error curve of the embodiment of the present invention in the backtracking fine alignment stage.As can be seen that method had been aligned
Location information is not lost in journey;In alignment finish time, horizontal position statistical accuracy is 16.58m, height accuracy 6.93m.