CN110501024B - Measurement error compensation method for vehicle-mounted INS/laser radar integrated navigation system - Google Patents

Measurement error compensation method for vehicle-mounted INS/laser radar integrated navigation system Download PDF

Info

Publication number
CN110501024B
CN110501024B CN201910288820.8A CN201910288820A CN110501024B CN 110501024 B CN110501024 B CN 110501024B CN 201910288820 A CN201910288820 A CN 201910288820A CN 110501024 B CN110501024 B CN 110501024B
Authority
CN
China
Prior art keywords
measurement
error
laser radar
ins
inertial
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.)
Active
Application number
CN201910288820.8A
Other languages
Chinese (zh)
Other versions
CN110501024A (en
Inventor
陈辛波
熊璐
韩燕群
夏新
陆逸适
高乐天
胡英杰
魏琰超
宋舜辉
刘伟
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Tongji University
Original Assignee
Tongji University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Tongji University filed Critical Tongji University
Priority to CN201910288820.8A priority Critical patent/CN110501024B/en
Publication of CN110501024A publication Critical patent/CN110501024A/en
Application granted granted Critical
Publication of CN110501024B publication Critical patent/CN110501024B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—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 combined with non-inertial navigation instruments
    • 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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Abstract

The invention relates to a measurement error compensation method of a vehicle-mounted INS/laser radar integrated navigation system, which solves the technical problems that: the method comprises the steps of carrying out vehicle integrated navigation by using a laser radar auxiliary inertial navigation system, taking the mounting offset angle and lever arm errors of an INS system and a laser radar system into consideration, correcting the mounting offset angle and lever arm errors as measurement quantities in a measurement equation of the integrated navigation system, constructing a filtering system taking inertial navigation position, speed and attitude errors and inertial device random constant errors as state quantities, carrying out feedback correction, and improving integrated navigation positioning accuracy based on measurement error compensation.

Description

Measurement error compensation method of vehicle-mounted INS/laser radar integrated navigation system
Technical Field
The invention relates to the technical field of vehicle navigation and positioning, in particular to a measurement error compensation method of a vehicle-mounted INS/laser radar integrated navigation system.
Background
An Inertial Navigation System (INS) has the characteristics of high autonomy, anti-interference performance, high short-term precision, high data output rate, complete Navigation information, wide application range and the like, but the System error has the characteristic of periodic oscillation, and certain Navigation parameter errors have the characteristic of accumulation along with time and the time required by initial alignment is longer; the LiDAR (Light Detection and Ranging) is widely used by the middle and outer scholars for assisting inertial navigation due to its advantages of high sampling frequency, high precision, low computation, no influence of ambient Light, no need of modifying the environment, and the like. However, in practical applications, due to limitations of the volume of the device, installation errors, and the like, the coordinate systems of the device often cannot be physically overlapped, that is, there are installation offset angle and lever arm errors between the laser radar system and the inertial navigation system, and there is rotation/translation transformation of the coordinate systems between the two coordinate systems, that is, the problem of measurement consistency. To accurately acquire external environment data in navigation, rotation/translation transformation parameters must be known, and the parameters can be solved by two modes of post correction and pre calibration. In a laser radar/inertia combined navigation system, a pre-calibration method better meets the actual requirement, one pre-calibration method is to carry out accurate measurement, the other pre-calibration method is to estimate parameters after a running experiment, and the estimation method comprises parameter estimation by an average control method (ACS), a common least square method (NLS) and a generalized least square method (GLS).
There are three main modes of lidar/inertial integrated navigation: 1) The terrain of the no-load system is matched with a navigation mode; 2) Scanning and matching laser radar of the ground system to assist navigation; 3) Geometric feature (landmark) based lidar aided navigation, i.e., a feature landmark based filtered estimation mode. The INS is mainly used in the relative positioning of the vehicle navigation, and other navigation positioning modes are used for assisting, namely the laser radar scanning matching assisted navigation in the second mode. Meanwhile, the navigation parameter error feedback correction scheme of the combined navigation system is divided into the following steps according to the correction method and the corrected state parameters: hybrid correction (initially using output correction and later using feedback correction), incomplete feedback (feedback correction for only position, velocity, attitude error) and complete feedback correction (feedback correction for position, velocity, attitude error and random constant error of inertial device). Because the correction of the random constant error of the inertial device has obvious influence on the output of the system, a feedback correction scheme of the random constant error of the inertial device must be considered at the moment, and because the measurement error is not considered in the conventional vehicle-mounted INS/laser radar combined navigation system, the difference between the output pose estimation of the laser radar and the corresponding output quantity of the INS is mostly used as the measurement quantity for feedback, but the influence and compensation of the measurement error caused by the difference of the installation positions of the INS and the laser radar are not considered in the vehicle application, so that the precision of the combined navigation system is reduced.
A combined inertial/visual odometer/lidar navigation method as disclosed in patent application No. 201510727853 as follows: the invention uses the autonomous navigation technology of machine vision, and the monocular camera can measure the speed of the carrier under the condition of known distance through the difference of the front frame image and the rear frame image; the laser radar can accurately measure the distance to an observation point, then measure the speed of the carrier, and finally realize high-precision navigation under the condition of no external reference information input by utilizing the speed obtained by measurement and the inertial navigation speed for combined navigation. The patent does not take into account the measurement errors of the different navigation subsystems and compensate in the measurement equations.
The INS/laser radar integrated navigation system scheme has the following defects:
calibration and estimation of measurement consistency parameters of an inertial navigation system and a laser radar test system installed on a vehicle are lacked;
there is no error compensation for errors due to rotation and translation between metrology coordinate systems.
Disclosure of Invention
The invention aims to overcome the defects in the prior art and provide a measurement error compensation method for a vehicle-mounted INS/laser radar integrated navigation system, which is used for a vehicle integrated navigation system by using a laser radar auxiliary inertial navigation system, takes the installation offset angle and lever arm error of the INS and the laser radar into consideration, corrects the measurement error as a measurement quantity in a measurement equation of the integrated navigation system, constructs a filtering system taking inertial navigation position, velocity, attitude error and random constant error of an inertial device as state quantities, and performs feedback correction, thereby realizing the improvement of the integrated navigation positioning accuracy based on measurement error compensation.
The purpose of the invention can be realized by the following technical scheme:
a measurement error compensation method of a vehicle-mounted INS/laser radar integrated navigation system comprises the following steps:
step 1: initializing an INS when the vehicle is static, and measuring and calibrating errors of a lever arm by using a measuring instrument and a related drawing to obtain an initial value;
and 2, step: setting a reference point, acquiring the observation of the laser radar on the reference point and the position of an inertial navigation system, constructing space vector measurement under different coordinate systems, and determining the optimal estimation of rotation and translation parameters by means of GPS derivation by utilizing a known laser radar measurement geometric model construction equation;
and step 3: acquiring INS original navigation data and laser radar data in the vehicle running process;
and 4, step 4: performing device compensation, attitude calculation and navigation calculation on INS original navigation data, and inputting the obtained speed increment attitude, speed and position into a combined filter;
and 5: removing motion distortion, extracting and matching features and tracking feature points of the laser radar data, and inputting the speed, displacement and attitude variables of the obtained pose estimation into a combined filter;
and 6: after the data input of the combined filter is finished, a state equation of a combined navigation system is established, 15-dimensional error state quantities of error state vectors including position, speed, attitude, gyro random constant drift epsilon and accelerometer random constant zero v are adopted to estimate the state equation, the speed of settlement of the two systems, the difference value of the position and the attitude are used for compensating a measurement error, and the result is used as a measurement quantity, and after each time of filtering, the position error estimated by filtering is utilized
Figure BDA0002024223780000031
Speed error>
Figure BDA0002024223780000032
Misalignment angle error>
Figure BDA0002024223780000033
Gyro random constant drift/>
Figure BDA0002024223780000034
Accelerometer random constant zero offset>
Figure BDA0002024223780000035
And the result carries out feedback correction on the INS resolving result.
Further, the lidar geometric measurement model in step 2 is described by the following formula:
Figure BDA0002024223780000036
in the formula, L is a laser radar measurement geometric model, ρ is a measurement distance, and α and β are laser measurement angles.
Further, the space vector measurement in step 2 is described by the formula:
Figure BDA0002024223780000037
in the formula, P i For space vector measurement, P i n Is a measurement of a space vector under a geographic system,
Figure BDA0002024223780000038
for a coordinate transformation matrix from carrier system to geographic system, based on the comparison of the value of the reference value>
Figure BDA0002024223780000039
And δ l b Respectively representing a rotation parameter and a lever arm value, L i The geometric model is measured for the ith lidar.
Further, the objective function corresponding to the optimal estimation in step 2 is:
Figure BDA00020242237800000310
where k is the number of reference points.
Further, the velocity increment attitude, the velocity and the position in the step 4 are calculated by adopting a two-subsample cone error compensation algorithm, and a corresponding calculation equation system is as follows:
Figure BDA00020242237800000311
Figure BDA0002024223780000041
Figure BDA0002024223780000042
in the formula,. DELTA.theta. m1 And Δ θ m2 Corresponding angle increment is sampled for the gyro at two equal intervals, T is sampling time,
Figure BDA0002024223780000043
for reference by the inertial frame, the carrier is moved from t m-1 Time to t m A change in the time of rotation>
Figure BDA0002024223780000044
For reference by the inertial frame, the geographic system is from t m Time t m-1 The rotation change of the moment, subscript i represents the inertial navigation system calculation value, upper subscript b represents the load system, upper subscript n represents the geography system, and (m) represents t m Time, (m-1) represents t m-1 Time phi with subscript represents corresponding gesture, I represents unit matrix, and->
Figure BDA0002024223780000045
Is a constant value.
Further, the step 6 comprises the following sub-steps:
step 61: establishing a system equation;
step 62: establishing a measurement equation;
and step 63: establishing a kalman filtering system equation and discretizing a measurement equation;
step 64: and performing feedback correction by using a kalman filtering system equation.
Further, the system equation in step 61 describes the formula:
X＝[φ E φ N φ U δv E δv N δv U δL δλ δh ε x ε y ε z ▽ x ▽ y ▽ z ] T
wherein X is a state vector, phi E 、φ N And phi U Respectively, attitude error, δ v, in east-north-sky geographic coordinate system E 、δv N And δ v U Respectively, velocity errors in an east-north-sky geographic coordinate system, δ L, δ λ and δ h are position errors of longitude, latitude and altitude, ε x 、ε y And ε z Is a zero bias of three coordinate axes of the gyroscope respectively x 、▽ y And & z Zero offset for three coordinate axes of the accelerometer respectively;
Figure BDA0002024223780000046
Figure BDA0002024223780000047
/>
Figure BDA0002024223780000048
Figure BDA0002024223780000049
Figure BDA00020242237800000410
in the formula (I), the compound is shown in the specification,
Figure BDA00020242237800000411
angular velocity for geographical relative to inertial system>
Figure BDA00020242237800000412
For the angular velocity error of the earth system relative to the inertial system>
Figure BDA0002024223780000051
For the angular velocity error of the geographical system relative to the terrestrial system>
Figure BDA0002024223780000052
For a coordinate transformation matrix from carrier system to geographic system, based on the comparison of the value of the reference value>
Figure BDA0002024223780000053
For the angular speed error of the carrier system relative to the inertial system>
Figure BDA0002024223780000054
Is the output specific force, v, of the carrier system relative to the inertial navigation system accelerometer under the geographic system n Is the speed of the vector in geographical system>
Figure BDA0002024223780000055
Is the angular velocity of the earth system relative to the inertial system,
Figure BDA0002024223780000056
is the angular velocity, δ v, of the geographic system relative to the Earth's system n For a speed error of a carrier under geographic system>
Figure BDA0002024223780000057
Is the output specific force error, delta g, of the carrier system relative to the inertial navigation system accelerometer under the geographic system n As error of gravitational acceleration, R M Radius of the mortise, h local altitude, L local latitude and R N And for the meridian radius, the single phi represents a mathematical platform error angle in the strapdown inertial navigation system.
Further, the measurement equation in step 62 describes the formula:
Figure BDA0002024223780000058
Figure BDA0002024223780000059
Figure BDA00020242237800000510
Figure BDA00020242237800000511
Figure BDA00020242237800000512
in the formula, the superscript n represents a geography system, Z represents a measurement equation, the subscript INS represents an inertial system, the subscript L represents a laser radar, the superscript represents an actual value, v represents a speed, p represents a position,
Figure BDA00020242237800000513
representing the angular velocity, R, of the carrier system relative to the earth system Mh ＝R M +h，R Nh ＝R N +h。
Further, the step 6 further includes: feeding back the kalman filtered gyroscope and the acceleration zero offset to a device compensation position for correction, feeding back the attitude to an attitude updating compensation position, and feeding back the speed and position errors to the output value calculated by the INS for correction, namely: by modified
Figure BDA00020242237800000514
The course angle psi, the pitch angle theta and the roll angle gamma can be obtained through solution, and after the primary filtering feedback, the error state returns to 0.
Further, said modified
Figure BDA00020242237800000515
The heading angle psi, the pitch angle theta and the roll angle gamma can be obtained through solution, and the corresponding description formulas are as follows:
Figure BDA0002024223780000061
in the formula, (numeral 1, numeral 2) represents a specific corresponding matrix element in the matrix.
The principle of the invention is as follows:
before a vehicle runs, an inertial navigation system is initialized, then a measuring instrument is used for preliminarily measuring the mounting offset angle and the lever arm error of an INS system and a laser radar system, the mounting offset angle and the lever arm error are used as measurement quantities in a measurement equation of the integrated navigation system for correction, a filtering system taking inertial navigation position, speed, attitude error and inertial device random constant value error as state quantities is constructed, and feedback correction is carried out. The invention is divided into four stages, the first stage is an INS initialization stage: this stage employs the use of external heading information to assist in initializing navigation attitude angles and positions. And in the second stage, the initial values of the installation angle error and the lever arm error are utilized to set the reference point and the observation of the laser radar to the reference point and the position of an inertial navigation system, so as to form space vector measurement under different coordinate systems, and the known geometric model construction equation for laser radar measurement is utilized and the GPS is utilized to perform nonlinear optimal estimation on rotation/translation parameters. And the third stage is a data acquisition and processing stage, and comprises the motion distortion correction, the feature identification and matching, the pose estimation, the device error compensation of the INS, the attitude calculation and the navigation calculation of the laser radar. And the fourth stage is a combined filtering and feedback correction stage, namely, the estimated position error, the estimated speed error, the estimated attitude error and the estimated random constant error of the inertial device are fed back to the INS for feedback compensation.
Compared with the prior art, the invention has the following advantages:
(1) In the invention, the measurement consistency of the mounting offset angle and the lever arm error of the INS and the laser radar systems mounted on the vehicle is considered, the measurement error parameters are calibrated and estimated and are used as the measurement in the measurement equation of the integrated navigation system for correction, and the integrated navigation positioning accuracy based on the measurement error compensation is improved.
Drawings
FIG. 1 is a lever arm schematic diagram of the relative positions of the INS inertial measurement unit center and the camera assembly center according to the present invention;
FIG. 2 is a block diagram of an integrated navigation system according to the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are some, not all, embodiments of the present invention. All other embodiments, which can be obtained by a person skilled in the art without any inventive step based on the embodiments of the present invention, shall fall within the scope of protection of the present invention.
Examples
The method comprises four stages, wherein the first stage comprises an INS initialization stage and measurement error initial value measurement, and the second stage utilizes the installation angle error and the lever arm error initial value to carry out nonlinear optimal estimation on rotation/translation parameters. The third stage is the data acquisition and processing stage of the sensor, and the fourth stage is the combined filtering and feedback correction stage.
The specific implementation steps of the invention are shown in fig. 2:
1) Initialization of INS and measurement of initial value of error (R) are carried out under the static state of vehicle ini ,t ini ) Wherein R is ini Representing the relative rotation of the lidar coordinate system (denoted as L-system) with respect to the INS coordinate system (carrier system) (denoted as b-system)
Figure BDA0002024223780000071
t ini Values of the lever arms δ l expressed in a carrier coordinate system b Error, as shown in FIG. 1;
2) Calibration of laser radar system and INS measurement error by using mounting angle error and rodInitial values of arm error, setting a series of reference points (control points) P 1 ,P 2 ,…P k (the number k of the reference points can be selected according to actual conditions), and simultaneously the coordinate system observation value L of the laser radar relative to the reference points is obtained 1 ,L 2 ,…L k And position of inertial navigation system
Figure BDA0002024223780000072
The space vector measurement under different coordinate systems is formed, the known laser radar measurement geometric model is used for constructing an equation, and the rotation and translation parameters are determined by means of GPS through derivation or calculation, namely, the nonlinear optimal estimation of the rotation/translation parameters is carried out.
Laser radar measures geometric model:
Figure BDA0002024223780000073
wherein ρ is a measurement distance, α and β are laser measurement angles, respectively.
Measuring a space vector:
Figure BDA0002024223780000074
through simultaneous k equation, the following indexes are satisfied to obtain nonlinear optimal estimation
Figure BDA0002024223780000075
Figure BDA0002024223780000076
3) Data are collected in the running process of the vehicle, and inertia measurement data are as follows: three axis gyroscope data
Figure BDA0002024223780000077
Three-axis accelerometer data->
Figure BDA0002024223780000078
4) And (3) selecting an east-north-sky (E-N-U) geographic coordinate system (g system) as a navigation reference coordinate system of the strapdown inertial navigation system for attitude calculation, and recording the geographic coordinate system as an N system again, wherein an attitude differential equation taking the N system as the reference system is as follows:
Figure BDA0002024223780000081
Figure BDA0002024223780000082
wherein the matrix
Figure BDA0002024223780000083
Denotes the reference i system (inertial coordinate system) and b system from t m-1 Time t m A change in the moment of rotation, and>
Figure BDA0002024223780000084
can be selected by the gyro angular speed->
Figure BDA0002024223780000085
Determining; />
Figure BDA0002024223780000086
Denotes i as a reference, n is from t m Time t m-1 A change in the moment of rotation, and>
Figure BDA0002024223780000087
can be determined by calculating angular speed>
Figure BDA0002024223780000088
Is determined and/or is taken up>
Figure BDA0002024223780000089
And &>
Figure BDA00020242237800000810
Respectively represent t m-1 And t m A strapdown attitude matrix of the time of day. If the gyro is in the time period t m-1 ,t m ]Inner (T = T) m -t m-1 ) Two times of equal interval sampling are carried out, and the angular increment is respectively delta theta m1 And Δ θ m2 A two-subsample cone error compensation algorithm is adopted, and comprises the following steps:
Figure BDA00020242237800000811
taking fourth order truncation and taking approximation:
Figure BDA00020242237800000812
Figure BDA00020242237800000813
navigation update period [ t ] m-1 ,t m ]In the interior, it can be considered that the velocity and position are caused
Figure BDA00020242237800000814
Has small variation, can be viewed>
Figure BDA00020242237800000815
Is constant value and is recorded as>
Figure BDA00020242237800000816
Then there are:
Figure BDA00020242237800000817
Figure BDA00020242237800000818
5) Carrying out motion distortion correction including laser radar on the laser radar data in the step (3), identifying and matching features, and inputting pose estimation into a combined filter;
6) And (5) after the data in the step (4) and the step (5) are input into a filter, establishing a state equation of the integrated navigation system, and estimating the 15-dimensional error state vector by adopting the 15-dimensional error state vector which specifically comprises the position, the speed, the attitude, the gyro random constant drift epsilon and the accelerometer random constant zero bias V. After each filtering, the position error estimated by the filtering
Figure BDA00020242237800000819
Speed error->
Figure BDA00020242237800000820
Misalignment angle error pick>
Figure BDA00020242237800000821
Gyro random constant value drift->
Figure BDA00020242237800000822
Accelerometer random constant zero offset->
Figure BDA00020242237800000823
And performing feedback correction on the INS calculation result.
1. Filtering and resolving:
establishing a system equation
Figure BDA0002024223780000091
Wherein: x: an error state vector;
f: a system matrix;
g: a noise distribution matrix;
w: a zero-mean gaussian white noise vector;
z: measuring the vector;
h: measuring a matrix;
v: measuring a noise state vector;
b at the relevant subscript positions denotes the carrier system, n denotes the geographic system, e denotes the earth system, and i denotes the inertial system.
X＝[φ E φ N φ U δv E δv N δv U δL δλ δh ε x ε y ε z ▽ x ▽ y ▽ z ] T
Wherein X is a state vector, phi E 、φ N And phi U Respectively, attitude error, δ v, in east-north-sky geographic coordinate system E 、δv N And δ v U Respectively, velocity errors in an east-north-sky geographic coordinate system, δ L, δ λ and δ h are position errors of longitude, latitude and altitude, ε x 、ε y And epsilon z Is a zero bias of three coordinate axes of the gyroscope respectively x 、▽ y And + z Zero offset for three coordinate axes of the accelerometer respectively;
Figure BDA0002024223780000092
Figure BDA0002024223780000093
Figure BDA0002024223780000094
Figure BDA0002024223780000095
Figure BDA0002024223780000096
in the formula (I), the compound is shown in the specification,
Figure BDA0002024223780000097
based on the angular velocity of the geographical system relative to the inertial system>
Figure BDA0002024223780000098
For the angular velocity error of the earth system relative to the inertial system>
Figure BDA0002024223780000099
For the angular velocity error of the geographical system relative to the terrestrial system>
Figure BDA00020242237800000910
For a coordinate transformation matrix from carrier system to geographical system, in conjunction with a coordinate transformation matrix for a geographical system>
Figure BDA00020242237800000911
For the angular speed error of the carrier system relative to the inertial system>
Figure BDA00020242237800000912
Is the output specific force, v, of the carrier system relative to the inertial navigation system accelerometer under the geographic system n For the speed of a vehicle in geographical system>
Figure BDA00020242237800000913
Is the angular velocity of the earth system relative to the inertial system,
Figure BDA00020242237800000914
is the angular velocity, δ v, of the geographic system relative to the Earth's system n For a speed error of a carrier under geographic system>
Figure BDA00020242237800000915
Is the output specific force error, delta g, of the carrier system relative to the inertial navigation system accelerometer under the geographic system n As error of gravitational acceleration, R M Radius of the mortise, h local altitude, L local latitude and R N And for the meridian radius, the single phi represents a mathematical platform error angle in the strapdown inertial navigation system.
Gyro zero bias under carrier system:
Figure BDA0002024223780000101
carrier systemAccelerometer zero offset of:
Figure BDA0002024223780000102
the following develops the equations (attitude-velocity-position) in order:
Figure BDA0002024223780000103
wherein
Figure BDA0002024223780000104
Figure BDA0002024223780000105
Figure BDA0002024223780000106
Figure BDA0002024223780000111
/>
Figure BDA0002024223780000112
Wherein:
Figure BDA0002024223780000113
for gyro measurement errors, m-band different a, x, y and z subscripts are expressed as cross coupling coefficients between two axes in the gyro measurement, and s-band a, x and z subscripts are expressed as scale factor errors in the gyro measurement.
Figure BDA0002024223780000114
Figure BDA0002024223780000115
Wherein:
Figure BDA0002024223780000116
for accelerometer measurement errors, the m-band different g, x, y, z subscripts are expressed as cross-coupling coefficients in the accelerometer measurement, and the s-band g, x, z subscripts are expressed as scale factor errors in the accelerometer measurement.
Figure BDA0002024223780000121
/>
Figure BDA0002024223780000122
The earth parameters given by the WGS-84 (World Geodetic System 1984) Earth series are: semi-major axis: r is e =6378137m, flattish ratio: f =1/298.257223563,
gravitational constant (including atmosphere): μ =3.986004418 × 10 14 m 3 /s 2 ，
Angular rate of rotation of the earth: omega ie ＝7.2921151467×10 -5 rad/s
g e And g p Equator gravity and pole gravity respectively, and the earth gravity oblateness is as follows:
Figure BDA0002024223780000123
β 1 represents the ratio to the equatorial gravity:
Figure BDA0002024223780000124
β 2 represents the gradient of gravity falling with height:
Figure BDA0002024223780000125
setting the geographic information under the local coordinate system to be kept unchanged, h is approximately equal to 0,
the finishing formula is as follows:
Figure BDA0002024223780000126
Figure BDA0002024223780000127
Figure BDA0002024223780000128
F 15 ＝0 3×3
Figure BDA0002024223780000129
/>
Figure BDA0002024223780000131
Figure BDA0002024223780000132
F 34 ＝0 3×3 ，F 35 ＝0 3×3 ，F 41 ＝F 42 ＝F 43 ＝F 44 ＝F 45 ＝F 51 ＝F 52 ＝F 53 ＝F 54 ＝F 55 ＝0 3×3
2. establishing a measurement equation:
Figure BDA0002024223780000133
Figure BDA0002024223780000134
Figure BDA0002024223780000135
Figure BDA0002024223780000136
Figure BDA0002024223780000137
in the formula, the superscript n represents a geography system, Z represents a measurement equation, the subscript INS represents an inertial system, the subscript L represents a laser radar, the superscript represents an actual value, v represents a speed, p represents a position,
Figure BDA0002024223780000138
representing the angular velocity, R, of the carrier system relative to the earth system Mh ＝R M +h，R Nh ＝R N +h。
The finishing process comprises the following steps:
Figure BDA0002024223780000141
discretization of Kalman filtering system equation and measurement equation
Making approximate discretization equivalence:
X k ＝Φ k/k-1 X k-1 +Γ k-1 W k-1
in which a discretized time interval T is set s ＝t k -t k-1 Then the state transition matrix takes a first order truncation, having:
Figure BDA0002024223780000142
Figure BDA0002024223780000143
W k-1 is a system noise vector, V k For measuring the noise vector, both are zero-mean gaussian white noise vector sequences (obeying normal distribution), and they are not correlated with each other, i.e. they satisfy:
Figure BDA0002024223780000144
the basic assumption for noise requirements in a Kalman Filter State-space model, generally requires Q k Is semi-positive and R k Is positive, i.e. Q k Not less than 0 and R k Is greater than 0. The Kalman filtering complete set algorithm can be divided into five basic formulas as follows:
(1) State one-step prediction
Figure BDA0002024223780000145
(2) State one-step prediction mean square error
Figure BDA0002024223780000146
(3) Filter gain
Figure BDA0002024223780000147
(4) State estimation
Figure BDA0002024223780000148
(5) State estimation mean square error
P k ＝(I-K k H k )P k/k-1
4. Feedback correction
And feeding back the Kalman filtered gyroscope and acceleration zero offset to a device compensation position for correction, feeding back the attitude to an attitude updating compensation position, feeding back the speed and position errors to the output value calculated by the INS for correction, and returning the error state to 0 after feedback.
While the invention has been described with reference to specific embodiments, the invention is not limited thereto, and various equivalent modifications and substitutions can be easily made by those skilled in the art within the technical scope of the invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the claims.

Claims (6)

1. A measurement error compensation method of a vehicle-mounted INS/laser radar integrated navigation system is characterized by comprising the following steps:
step 1: initializing an INS when a vehicle is static, and measuring and calibrating errors of a lever arm by using a measuring instrument and a related drawing to obtain an initial value;
and 2, step: setting a reference point, acquiring the observation of a laser radar on the reference point and the position of an inertial navigation system, constructing space vector measurement under different coordinate systems, and determining the optimal estimation of rotation and translation parameters by GPS derivation by utilizing a known laser radar measurement geometric model construction equation;
and step 3: acquiring INS original navigation data and laser radar data in the vehicle running process;
and 4, step 4: performing device compensation, attitude calculation and navigation calculation on INS original navigation data, and inputting the obtained speed increment attitude, speed and position into a combined filter;
and 5: removing motion distortion, extracting and matching features and tracking feature points of the laser radar data, and inputting the speed, displacement and attitude variables of the obtained pose estimation into a combined filter;
step 6: after the data input of the combined filter is finished, establishing a state equation of the combined navigation system, estimating the state equation, and performing feedback correction on an INS calculation result by using a result estimated by filtering after each filtering;
the laser radar measurement geometric model in the step 2 has a description formula as follows:
Figure FDA0003999318890000011
in the formula, L is a laser radar measurement geometric model, rho is a measurement distance, and alpha and beta are laser measurement angles;
the space vector measurement in step 2 is described by the following formula:
Figure FDA0003999318890000012
in the formula, P i For space vector measurement, P i n Is a measurement of a space vector under a geographic system,
Figure FDA0003999318890000013
for a coordinate transformation matrix from carrier system to geographic system, based on the comparison of the value of the reference value>
Figure FDA0003999318890000014
And δ l b Respectively representing a rotation parameter and a lever arm value, L i Measuring a geometric model for the ith laser radar;
the objective function corresponding to the optimal estimation in the step 2 is as follows:
Figure FDA0003999318890000015
in the formula, k is the number of reference points;
the step 6 comprises the following sub-steps:
step 61: establishing a system equation;
step 62: establishing a measurement equation;
and step 63: establishing a kalman filtering system equation and discretizing a measurement equation;
step 64: and performing feedback correction by using a kalman filtering system equation.
2. The measurement error compensation method of the vehicle-mounted INS/lidar combined navigation system as claimed in claim 1, wherein the velocity increment attitude, the velocity and the position in the step 4 are calculated by using a two-subsample cone error compensation algorithm, and the corresponding calculation equation set is as follows:
Figure FDA0003999318890000021
Figure FDA0003999318890000022
Figure FDA0003999318890000023
Figure FDA0003999318890000024
in the formula,. DELTA.theta. m1 And Δ θ m2 Corresponding angle increment is sampled for the gyro at two equal intervals, T is sampling time,
Figure FDA0003999318890000025
for reference by the inertial frame, the carrier is moved from t m-1 Time t m A change in the moment of rotation, and>
Figure FDA0003999318890000026
for reference by the inertial frame, the geographic system is from t m Time t m-1 The rotation change of the moment, the subscript i represents the solution value of the inertial navigation system, the upper subscript b represents the loading system, and the upper subscript b represents the upper partThe subscript n denotes geographical system, (m) denotes t m Time, (m-1) represents t m-1 Time phi with subscript represents corresponding gesture, I represents unit matrix, and->
Figure FDA0003999318890000027
Is a constant value.
3. The method as claimed in claim 1, wherein the system equation in step 61 is described as follows:
Figure FDA0003999318890000028
wherein X is a state vector, phi E 、φ N And phi U Respectively, attitude error, δ v, in east-north-sky geographic coordinate system E 、δv N And δ v U Respectively, velocity errors in an east-north-sky geographic coordinate system, δ L, δ λ and δ h are position errors of longitude, latitude and altitude, ε x 、ε y And ε z Respectively the zero offset of three coordinate axes of the gyroscope,
Figure FDA0003999318890000029
and &>
Figure FDA00039993188900000210
Zero offset for three coordinate axes of the accelerometer respectively;
Figure FDA0003999318890000031
Figure FDA0003999318890000032
Figure FDA0003999318890000033
Figure FDA0003999318890000034
Figure FDA0003999318890000035
in the formula (I), the compound is shown in the specification,
Figure FDA0003999318890000036
based on the angular velocity of the geographical system relative to the inertial system>
Figure FDA0003999318890000037
For the angular velocity error of the earth system relative to the inertial system>
Figure FDA0003999318890000038
For the angular speed error of the geographical system relative to the earth system>
Figure FDA0003999318890000039
Is a coordinate transformation matrix of carrier system to geographic system,
Figure FDA00039993188900000310
for the angular speed error of the carrier system relative to the inertial system>
Figure FDA00039993188900000311
Is the output specific force, v, of the carrier system relative to the inertial navigation system accelerometer under the geographic system n For the speed of a vehicle in geographical system>
Figure FDA00039993188900000312
Is the angular velocity of the earth system relative to the inertial system,/>
Figure FDA00039993188900000313
is the angular velocity, δ v, of the geographic system relative to the Earth's system n For a geographical speed error of a vehicle>
Figure FDA00039993188900000314
Is the output specific force error, delta g, of the carrier system relative to the inertial navigation system accelerometer under the geographic system n As error of gravitational acceleration, R M Radius of the mortise, h local altitude, L local latitude and R N And for the meridian radius, the single phi represents a mathematical platform error angle in the strapdown inertial navigation system.
4. The method as claimed in claim 3, wherein the measurement equation in step 62 is described as follows:
Figure FDA00039993188900000315
Figure FDA00039993188900000316
Figure FDA00039993188900000317
Figure FDA00039993188900000318
Figure FDA00039993188900000319
in the formula, the superscript n represents a geography system, Z represents a measurement equation, the subscript INS represents an inertia system, the subscript L represents a laser radar, the superscript-represents an actual value, v represents a speed, p represents a position,
Figure FDA00039993188900000320
denotes the angular velocity, R, of the carrier system relative to the earth system Mh ＝R M +h，R Nh ＝R N +h。
5. The method as claimed in claim 1, wherein the step 6 further comprises: feeding back the Kalman filtered gyro and the acceleration zero offset to a device compensation position for correction, feeding back the attitude to an attitude updating compensation position, and feeding back the speed and position errors to an output value calculated by the INS for correction, namely: by modified
Figure FDA0003999318890000041
The heading angle psi, the pitch angle theta and the rolling angle gamma can be obtained through solution, and after the primary filtering feedback, the error state returns to 0.
6. The method as claimed in claim 5, wherein the corrected measurement error of the INS/LIDAR integrated navigation system is compensated
Figure FDA0003999318890000042
The heading angle psi, the pitch angle theta and the roll angle gamma can be obtained through solution, and the corresponding description formula is as follows: />
Figure FDA0003999318890000043
In the formula, (numeral 1, numeral 2) represents a specific corresponding matrix element in the matrix.
CN201910288820.8A 2019-04-11 2019-04-11 Measurement error compensation method for vehicle-mounted INS/laser radar integrated navigation system Active CN110501024B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910288820.8A CN110501024B (en) 2019-04-11 2019-04-11 Measurement error compensation method for vehicle-mounted INS/laser radar integrated navigation system

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910288820.8A CN110501024B (en) 2019-04-11 2019-04-11 Measurement error compensation method for vehicle-mounted INS/laser radar integrated navigation system

Publications (2)

Publication Number Publication Date
CN110501024A CN110501024A (en) 2019-11-26
CN110501024B true CN110501024B (en) 2023-03-28

Family

ID=68585265

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910288820.8A Active CN110501024B (en) 2019-04-11 2019-04-11 Measurement error compensation method for vehicle-mounted INS/laser radar integrated navigation system

Country Status (1)

Country Link
CN (1) CN110501024B (en)

Families Citing this family (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110954137B (en) * 2019-12-13 2023-03-24 陕西瑞特测控技术有限公司 Method for correcting assembly error scalar quantity of inertial navigation accelerometer
CN111123280B (en) * 2019-12-31 2023-02-03 武汉万集光电技术有限公司 Laser radar positioning method, device and system, electronic equipment and storage medium
CN111637889A (en) * 2020-06-15 2020-09-08 中南大学 Tunneling machine positioning method and system based on inertial navigation and laser radar three-point distance measurement
CN111721288B (en) * 2020-06-19 2022-03-29 哈尔滨工业大学 Zero offset correction method and device for MEMS device and storage medium
CN111947652B (en) * 2020-08-13 2022-09-20 北京航空航天大学 Inertia/vision/astronomy/laser ranging combined navigation method suitable for lunar lander
CN112197789B (en) * 2020-08-14 2023-09-12 北京自动化控制设备研究所 INS/DVL installation error calibration method based on QUEST
CN112180412B (en) * 2020-09-23 2023-05-02 中国人民解放军空军工程大学 Relative positioning and orientation compensation method based on satellite navigation positioning system
CN112130188B (en) * 2020-11-23 2021-03-02 蘑菇车联信息科技有限公司 Vehicle positioning method and device and cloud server
CN113514863A (en) * 2021-03-23 2021-10-19 重庆兰德适普信息科技有限公司 Multi-sensor fusion positioning method
CN113236363A (en) * 2021-04-23 2021-08-10 陕西陕煤黄陵矿业有限公司 Mining equipment navigation positioning method, system, equipment and readable storage medium
CN113281797B (en) * 2021-05-11 2022-09-13 南京国睿防务系统有限公司 Maneuvering detection and correction radar system based on inertial navigation
CN113340298A (en) * 2021-05-24 2021-09-03 南京航空航天大学 Inertial navigation and dual-antenna GNSS external reference calibration method
CN113295179B (en) * 2021-06-04 2022-07-05 清智汽车科技(苏州)有限公司 Vehicle course angle correction method and device based on laser sensor
CN113534156B (en) * 2021-07-02 2024-04-05 中汽创智科技有限公司 Vehicle positioning method, device and equipment based on vehicle millimeter wave radar
CN113932835B (en) * 2021-12-17 2022-05-17 智道网联科技(北京)有限公司 Calibration method and device for positioning lever arm of automatic driving vehicle and electronic equipment
CN115164886B (en) * 2022-07-22 2023-09-05 吉林大学 Scale factor error compensation method of vehicle-mounted GNSS/INS integrated navigation system
CN116990787B (en) * 2023-09-26 2023-12-15 山东科技大学 Scanning platform coordinate system error correction method based on airborne laser radar system

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106802143A (en) * 2017-03-10 2017-06-06 中国人民解放军国防科学技术大学 A kind of hull deformation angle measuring method based on inertial instruments and Iterative-Filtering Scheme

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6240367B1 (en) * 1998-11-27 2001-05-29 Ching-Fang Lin Full fusion positioning method for vehicle
CN1314945C (en) * 2005-11-04 2007-05-09 北京航空航天大学 Aerial in-flight alignment method for SINS/GPS combined navigation system
CN102393201B (en) * 2011-08-02 2013-05-15 北京航空航天大学 Dynamic lever arm compensating method of position and posture measuring system (POS) for aerial remote sensing
CN102608596B (en) * 2012-02-29 2013-06-05 北京航空航天大学 Information fusion method for airborne inertia/Doppler radar integrated navigation system
CN103487822A (en) * 2013-09-27 2014-01-01 南京理工大学 BD/DNS/IMU autonomous integrated navigation system and method thereof
CN105371840B (en) * 2015-10-30 2019-03-22 北京自动化控制设备研究所 A kind of inertia/visual odometry/laser radar Combinated navigation method
CN107764268B (en) * 2017-10-13 2020-03-24 北京航空航天大学 Method and device for transfer alignment of airborne distributed POS (point of sale)
CN108759845B (en) * 2018-07-05 2021-08-10 华南理工大学 Optimization method based on low-cost multi-sensor combined navigation

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106802143A (en) * 2017-03-10 2017-06-06 中国人民解放军国防科学技术大学 A kind of hull deformation angle measuring method based on inertial instruments and Iterative-Filtering Scheme

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
低成本MIMU/编码器组合的高精度航姿系统;陈述奇等;《压电与声光》;第34卷(第03期);全文 *

Also Published As

Publication number Publication date
CN110501024A (en) 2019-11-26

Similar Documents

Publication Publication Date Title
CN110501024B (en) Measurement error compensation method for vehicle-mounted INS/laser radar integrated navigation system
CN110221332B (en) Dynamic lever arm error estimation and compensation method for vehicle-mounted GNSS/INS integrated navigation
CN110221333B (en) Measurement error compensation method of vehicle-mounted INS/OD integrated navigation system
CN111156994B (en) INS/DR & GNSS loose combination navigation method based on MEMS inertial component
CN107270893B (en) Lever arm and time asynchronous error estimation and compensation method for real estate measurement
Fang et al. Predictive iterated Kalman filter for INS/GPS integration and its application to SAR motion compensation
CN110779521A (en) Multi-source fusion high-precision positioning method and device
CN112629538A (en) Ship horizontal attitude measurement method based on fusion complementary filtering and Kalman filtering
CN112505737B (en) GNSS/INS integrated navigation method
CN113063429B (en) Self-adaptive vehicle-mounted integrated navigation positioning method
CN112504275B (en) Water surface ship horizontal attitude measurement method based on cascade Kalman filtering algorithm
CN110954102B (en) Magnetometer-assisted inertial navigation system and method for robot positioning
US20170074678A1 (en) Positioning and orientation data analysis system and method thereof
CN109612460B (en) Plumb line deviation measuring method based on static correction
CN111121766A (en) Astronomical and inertial integrated navigation method based on starlight vector
CN113203418A (en) GNSSINS visual fusion positioning method and system based on sequential Kalman filtering
CN111288984A (en) Multi-vehicle joint absolute positioning method based on Internet of vehicles
CN113291493B (en) Method and system for determining fusion attitude of multiple sensors of satellite
CN109470276B (en) Odometer calibration method and device based on zero-speed correction
CN108303120B (en) Real-time transfer alignment method and device for airborne distributed POS
CN112880669A (en) Spacecraft starlight refraction and uniaxial rotation modulation inertia combined navigation method
CN114964222A (en) Vehicle-mounted IMU attitude initialization method, and mounting angle estimation method and device
CN115200578A (en) Polynomial optimization-based inertial-based navigation information fusion method and system
CN107764268B (en) Method and device for transfer alignment of airborne distributed POS (point of sale)
CN112197765B (en) Method for realizing fine navigation of underwater robot

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