CN106679660B - Vehicle navigation method based on M estimation incomplete constraint - Google Patents

Vehicle navigation method based on M estimation incomplete constraint Download PDF

Info

Publication number
CN106679660B
CN106679660B CN201710076668.8A CN201710076668A CN106679660B CN 106679660 B CN106679660 B CN 106679660B CN 201710076668 A CN201710076668 A CN 201710076668A CN 106679660 B CN106679660 B CN 106679660B
Authority
CN
China
Prior art keywords
vehicle
navigation
incomplete constraint
estimation
mimu
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
CN201710076668.8A
Other languages
Chinese (zh)
Other versions
CN106679660A (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.)
Harbin Institute of Technology
Original Assignee
Harbin Institute of Technology
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 Harbin Institute of Technology filed Critical Harbin Institute of Technology
Priority to CN201710076668.8A priority Critical patent/CN106679660B/en
Publication of CN106679660A publication Critical patent/CN106679660A/en
Application granted granted Critical
Publication of CN106679660B publication Critical patent/CN106679660B/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
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

A vehicle navigation method based on M estimation incomplete constraint is realized by a vehicle navigation system based on an MIMU sensor. The Kalman filter is constructed by using the incomplete constraint algorithm, the navigation information of the MIMU is corrected in the running process of the vehicle, the navigation precision under pure inertial navigation is improved, and meanwhile, the universality of the incomplete constraint algorithm is enhanced by combining the M estimation algorithm, so that the problem of navigation positioning of a combat vehicle under the conditions that the combat environment is severe and a GPS (global positioning system) and other satellite navigation systems cannot be used is solved.

Description

Vehicle navigation method based on M estimation incomplete constraint
Technical Field
The invention belongs to a pure inertial navigation mode, and particularly relates to a vehicle navigation method based on M estimation incomplete constraint.
Background
For a combat vehicle, due to the consideration of concealment, when navigation is performed, satellite navigation is not relied on, and only inertial navigation, doppler navigation and other modes are adopted, but the adoption of inertial navigation has two disadvantages, firstly, inertial navigation errors are dispersed along with time, so that the navigation positioning accuracy by adopting a pure inertial navigation mode is poor, the stability is low, secondly, the cost of inertial devices is high, and if a tactical-level precision inertial navigation system is adopted for navigation, one set of inertial navigation system can reach tens of thousands of yuan, which obviously cannot bear the combat vehicle.
In order to solve the above problems, there is a solution to correct navigation information using an incomplete constraint algorithm, which can suppress error divergence in an inertial navigation system and ensure positioning accuracy even when a low-cost Micro Inertial Measurement Unit (MIMU) is used. However, the method has a great problem that it is necessary to ensure that the vehicle does not have sideslip, jump and other activities when running, that is, it is ensured that the information on the carrier system output by the MIMU is theoretically zero in the sky-direction velocity information and the lateral-direction velocity information, but for the combat vehicle, because of the limit of the working environment, incomplete constraint conditions can not be met frequently, which leads to the great reduction of the accuracy of the navigation result.
In order to solve the problems, the M estimation algorithm and the incomplete constraint algorithm are combined to construct a vehicle navigation method based on the M estimation incomplete constraint algorithm, the problem that the incomplete constraint condition is not satisfied is solved, and the vehicle positioning accuracy under the pure inertial navigation condition is improved.
Disclosure of Invention
The invention aims to provide a vehicle navigation method based on M estimation incomplete constraint, which aims to solve the problems of error divergence and high cost of a combat vehicle only depending on pure inertial navigation positioning, and improve an incomplete constraint algorithm to solve the problems of poor universality and low stability of the combat vehicle in various occasions.
In order to solve the problems existing in the background technology, the invention adopts the following technical scheme:
a vehicle navigation method based on M estimation incomplete constraint comprises the following steps:
the MIMU sensor is arranged: fixing the MIMU on the vehicle to prevent the MIMU from sliding, thereby outputting navigation information relative to a vehicle carrier system;
when the vehicle normally runs, resolving the MIMU output navigation information by adopting an incomplete constraint algorithm to obtain vehicle navigation information;
when the vehicle has sideslip, jump and other actions in the driving process, the incomplete constraint condition is not met any more, the MIMU information is corrected by using the M estimation algorithm, and the corrected information is used for carrying out the computation of the incomplete constraint algorithm to obtain the vehicle navigation information.
In the step (2), when the vehicle normally runs, the pure inertial navigation information output by the MIMU can be directly utilized to carry out vehicle position calculation through an incomplete constraint algorithm, and the positioning error is not dispersed along with time, so that the navigation capability of the pure inertial navigation system is greatly improved.
In the step (3), because the working environment of the combat vehicle is severe, phenomena such as vehicle sideslip and take-off frequently occur, so that incomplete constraint conditions are not met, and the algorithm calculation effect is deteriorated.
In the step (3), for identifying gross errors, the navigation information output by the MIMU can be pre-judged, and the identification is carried out by utilizing a probability statistics method, namely, according to the normal distribution principle, gross error data are mainly concentrated on two sides of a normal distribution curve, and the gross errors can be identified by dividing the probability density.
In step (3), if the vehicle is equipped with a GPS, the GPS can be assisted by the navigation information estimated by the present invention for navigation.
Compared with the prior art, the invention has the following beneficial effects: when the vehicle navigation method based on M estimation incomplete constraint is adopted, accurate vehicle position information is provided in real time in the vehicle running process, and the positioning error is not dispersed; secondly, the MIMU is used as a sensor, so that the navigation cost is greatly reduced; the algorithm provided by the invention has universality, can be applied to various land vehicle navigation systems, has high positioning precision and good stability, and finally, if the vehicle is provided with the GPS, the navigation information estimated by the invention can be used for assisting the GPS to carry out navigation, so that the navigation effect is enhanced.
Drawings
Fig. 1 is a schematic view of the MIMU mounted on a vehicle.
Fig. 2 is a schematic diagram of incomplete constraints.
FIG. 3 shows MIMU model ADIS16490, produced by Noada corporation, used in the present invention.
Fig. 4 shows a GPS receiver model ET02A, manufactured by zeugo corporation, used in the present invention.
FIG. 5 is a comparison graph of vehicle navigation speed error under simulation conditions using an M-based estimation incomplete constraint and an incomplete constraint algorithm.
FIG. 6 is a vehicle navigation speed error plot under on-board test conditions with incomplete constraints based on M estimation.
FIG. 7 is a flow chart of the present invention.
Detailed Description
The method comprises the following steps: mathematical model building
Selecting a northeast (E, N, U) geographic coordinate system as a navigation coordinate system (N system), taking the gravity center of the vehicle as an origin, taking the right direction of a transverse axis as the positive direction of an x axis, taking the positive direction of a y axis as the positive direction of the x axis, taking the positive direction of the y axis as the positive direction of the y axis along the longitudinal axis of the vehicle, and establishing a carrier coordinate system (b system) along the upward direction of the vertical axis of the vehicle. Accordingly, a vehicle navigation model is established.
Step two: state equation establishment
Establishing a state equation according to an inertial navigation system error equation:
Figure GDA0002711281680000031
wherein, a is a state matrix, B is a system noise matrix, W is system noise, and X is a state vector, that is:
Figure GDA0002711281680000032
step three: incomplete constraint equation establishment
Compared with the traditional Kalman filtering, the incomplete constraint algorithm is different from the establishment of a measurement equation.
The incomplete constraint conditions of the vehicle are:
based on the characteristics of the ground vehicle movement, the following two assumptions are satisfied:
a) no direction-finding slip when the vehicle is moving;
b) when the vehicle moves, the vehicle does not leave the ground all the time, namely, no jump-up movement exists;
that is, as shown in FIG. 2, the velocities of the x-axis and z-axis should be theoretically zero in the b-system, i.e., the following equation holds
Figure GDA0002711281680000033
However, the inertial navigation system has the problem of navigation error accumulation, and the speed of the x axis and the speed of the z axis on the actual resolving b system are not zero, so that the speed error calculated on the two axes can be used as the observed quantity to carry out auxiliary navigation.
When the incomplete constraint condition is used as external auxiliary information, the observed quantity information is established as follows:
Figure GDA0002711281680000034
wherein the content of the first and second substances,
Figure GDA0002711281680000035
is the speed error of x-axis and z-axis in b, which can be obtained from the formula (2)
Figure GDA0002711281680000036
Wherein the content of the first and second substances,
Figure GDA0002711281680000037
the two-axis speed on the b system is obtained by strapdown calculation.
Using strapdown matrices
Figure GDA0002711281680000038
Determining the relation between the n line and the b line:
Figure GDA0002711281680000039
adding error disturbance to equation (6) in order:
Figure GDA00027112816800000310
the measurement equation obtained according to equation (7) is as follows
Figure GDA00027112816800000311
Wherein the measurement noise e is zero-mean white noise, and the measurement matrix H is as follows
Figure GDA0002711281680000041
Wherein the content of the first and second substances,
Figure GDA0002711281680000042
Vnx represents VnThe anti-symmetric matrix of (a) is,
Figure GDA0002711281680000043
express get
Figure GDA0002711281680000044
The first row of the matrix, the remaining symbols are similar.
Step four: m estimation incomplete constraint equation establishment
When the vehicle has sideslip, jump and other actions in the driving process and the incomplete constraint condition is not met, an M estimation incomplete constraint equation is adopted, and the equation is established as follows:
according to the idea of M estimation, for making the state variable XkObtaining optimal estimation by only letting the error parameters
Figure GDA0002711281680000045
Taking the minimum value, therefore, the likelihood function equation is constructed by the equation (8) as follows
Figure GDA0002711281680000046
The derivation of the formula (10) is made zero
Figure GDA0002711281680000047
The formula (11) is finished by:
Figure GDA0002711281680000048
wherein the content of the first and second substances,
Figure GDA0002711281680000049
called an extremum function, is
Figure GDA00027112816800000410
Balance
Figure GDA00027112816800000411
As a function of the score.
By rewriting formula (12) to
Figure GDA00027112816800000412
Wherein the content of the first and second substances,
Figure GDA00027112816800000413
referred to as weight functions.
Writing equation (13) in matrix form
Figure GDA00027112816800000414
Wherein the content of the first and second substances,
Figure GDA00027112816800000415
solving the equation by using a Newton method to obtain
Figure GDA0002711281680000051
Wherein the content of the first and second substances,
Figure GDA0002711281680000052
solving the equation by using a Newton method to obtain
Figure GDA0002711281680000053
Where j is the number of iterations.
The iteration is carried out by the formula (16) to obtain
Figure GDA0002711281680000054
At this time, the covariance matrix is
Figure GDA0002711281680000055
For the selection of the extreme value function rho, the IGG method is selected as the extreme value function and is expressed as follows:
Figure GDA0002711281680000056
wherein, b, c and d are constants, and the value of each constant is generally obtained by the knowledge of normal distribution according to experience.
Step five: feedback correction procedure
And obtaining the vehicle navigation information error by using the algorithm of the third step and the algorithm of the fourth step, and performing feedback correction on the MIMU output parameter by using the error to obtain correct vehicle navigation information.
The effect of the invention is verified by the following method:
in order to verify the effectiveness of the invention, Matlab is used for simulation analysis. The simulation initial setting position is [125.6705 DEG E,45.7796 DEG N,1.2m]The initial course angle is [0 °,0 °]Simulation time is 1500s, gyro constant drift is 0.01 degree/h, and accelerometer zero offset is 10-4g, sampling period 1 s. The simulation process is shown in Table 1 and includes the steps of vehicle stationary, uniform speed and turningThe driving state, on the basis of which, the artificial adding acceleration in the x direction of the b system within the time of 400s-403s is 2m/s2Adding acceleration of-2 m/s within the time of 403s-405s2To simulate the vehicle sideslip condition; the artificial added acceleration in the z direction of the b system within the time of 900s-903s is 2m/s2Adding acceleration of-2 m/s within the time of 903s-905s2The longitudinal speed of the vehicle is used for simulating the jumping condition of the vehicle; thereby simulating the motion pattern of the vehicle in the actual state.
During simulation, the MIMU navigation information is respectively processed by adopting an incomplete constraint algorithm and the algorithm of the invention, and a navigation error result is obtained.
TABLE 1 vehicle simulation Process setup
Figure GDA0002711281680000057
Figure GDA0002711281680000061
As can be seen from FIG. 4, both algorithms can provide more accurate navigation information when the vehicle normally runs, but when the vehicle jumps, sideslips and the like, the algorithm provided by the invention can effectively solve the problem of divergence of the navigation information, and the traditional method is invalid.

Claims (5)

1. A vehicle navigation method based on M estimation incomplete constraint is characterized by comprising the following steps:
(1) the MIMU sensor is arranged: fixing the MIMU on the vehicle to prevent the MIMU from sliding, thereby outputting navigation information relative to a vehicle carrier system;
(2) when the vehicle normally runs, resolving the MIMU output navigation information by adopting an incomplete constraint algorithm to obtain vehicle navigation information;
(3) when the vehicle sideslips and jumps in the driving process, the incomplete constraint condition is not met any more, the MIMU information is corrected by using an M estimation algorithm, and the corrected information is used for carrying out incomplete constraint algorithm calculation to obtain vehicle navigation information;
when the vehicle sideslips and jumps in the driving process and the incomplete constraint condition is not met, an M estimation incomplete constraint equation is adopted, and the equation is established as follows:
Figure 365490DEST_PATH_IMAGE001
Figure 914283DEST_PATH_IMAGE002
wherein the content of the first and second substances,b, c, dall constants are constant values, and the values of all constants are obtained by utilizing the knowledge of normal distribution according to experience.
2. The vehicle navigation method based on M estimation incomplete constraint of claim 1, wherein in step (2), when the vehicle is running normally, the pure inertial navigation information output by the MIMU can be directly utilized to carry out vehicle position calculation through an incomplete constraint algorithm, and the positioning error does not diverge with time, so that the navigation capability of the pure inertial navigation system is greatly improved.
3. The vehicle navigation method based on M estimation incomplete constraint of claim 1, wherein in step (3), as the working environment of the combat vehicle is severe, the phenomena of vehicle sideslip and take-off frequently occur, so that the incomplete constraint condition is not satisfied any more, and the algorithm resolving effect is deteriorated.
4. The method as claimed in claim 1, wherein in step (3), the navigation information outputted from the MIMU is pre-determined for the identification of the gross error, and the identification is performed by using a probabilistic method, that is, according to the normal distribution principle, the gross error data is mainly concentrated on two sides of the normal distribution curve, and the gross error identification can be realized by dividing the probability density.
5. The vehicle navigation method based on M estimation incomplete constraint of claim 1, wherein in step (3), if the vehicle is equipped with GPS, the GPS is assisted by the estimated navigation information for navigation.
CN201710076668.8A 2017-02-13 2017-02-13 Vehicle navigation method based on M estimation incomplete constraint Active CN106679660B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710076668.8A CN106679660B (en) 2017-02-13 2017-02-13 Vehicle navigation method based on M estimation incomplete constraint

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710076668.8A CN106679660B (en) 2017-02-13 2017-02-13 Vehicle navigation method based on M estimation incomplete constraint

Publications (2)

Publication Number Publication Date
CN106679660A CN106679660A (en) 2017-05-17
CN106679660B true CN106679660B (en) 2020-12-11

Family

ID=58861776

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710076668.8A Active CN106679660B (en) 2017-02-13 2017-02-13 Vehicle navigation method based on M estimation incomplete constraint

Country Status (1)

Country Link
CN (1) CN106679660B (en)

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1668381A1 (en) * 2003-09-05 2006-06-14 Novatel, Inc. Inertial gps navigation system using injected alignment data for the inertial system
CN101793522A (en) * 2010-03-31 2010-08-04 上海交通大学 Steady filtering method based on robust estimation
CN104501796A (en) * 2014-12-16 2015-04-08 重庆邮电大学 Indoor WLAN/MEMS fusion cross-stair three-dimensional positioning method

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1668381A1 (en) * 2003-09-05 2006-06-14 Novatel, Inc. Inertial gps navigation system using injected alignment data for the inertial system
CN101793522A (en) * 2010-03-31 2010-08-04 上海交通大学 Steady filtering method based on robust estimation
CN104501796A (en) * 2014-12-16 2015-04-08 重庆邮电大学 Indoor WLAN/MEMS fusion cross-stair three-dimensional positioning method

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
GPS/INS 组合导航系统研究及实现;李倩;《中国优秀硕士学位论文全文数据库 信息科技辑》;20101215(第12期);正文第51-52,82-85,95-96页 *
非线性高斯滤波方法研究及其在CNS/SAR/SINS 组合导航中的应用;梁浩;《中国博士学位论文全文数据库 工程科技Ⅱ辑》;20160315(第3期);正文第87-98页 *

Also Published As

Publication number Publication date
CN106679660A (en) 2017-05-17

Similar Documents

Publication Publication Date Title
CN107525503B (en) Adaptive cascade Kalman filtering method based on combination of dual-antenna GPS and MIMU
CN105509738B (en) Vehicle positioning orientation method based on inertial navigation/Doppler radar combination
CN105606094B (en) A kind of information condition matched filtering method of estimation based on MEMS/GPS combined systems
CN104344837B (en) Speed observation-based redundant inertial navigation system accelerometer system level calibration method
CN106767787A (en) A kind of close coupling GNSS/INS combined navigation devices
CN105698822B (en) Initial Alignment Method between autonomous type inertial navigation based on reversed Attitude Tracking is advanced
CN105806363B (en) The underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF
CN102322858B (en) Geomagnetic matching navigation method for geomagnetic-strapdown inertial navigation integrated navigation system
CN104019828A (en) On-line calibration method for lever arm effect error of inertial navigation system in high dynamic environment
CN109870173A (en) A kind of track correct method of the submarine pipeline inertial navigation system based on checkpoint
CN103822633A (en) Low-cost attitude estimation method based on second-order measurement update
CN105910606A (en) Direction adjustment method based on angular velocity difference
CN103557864A (en) Initial alignment method for micro electro mechanical system (MEMS) strap-down inertial navigation adaptive square-root cubature Kalman filtering (SCKF)
CN103344260A (en) Initial alignment method of large azimuth misalignment angle of strapdown inertial navigation system based on RBCKF (rao-black-wellised cubature kalman filter)
CN110440830A (en) Vehicle-mounted Strapdown Inertial Navigation System Alignment Method under moving base
CN109959374A (en) A kind of full-time reverse smooth filtering method of whole process of pedestrian's inertial navigation
Gao et al. An integrated land vehicle navigation system based on context awareness
Liu et al. Interacting multiple model UAV navigation algorithm based on a robust cubature Kalman filter
CN106646569B (en) Navigation positioning method and equipment
CN104482942A (en) Inertial system-based optimal alignment method for two positions based on
Ercan et al. Multi-sensor data fusion of DCM based orientation estimation for land vehicles
CN111220151B (en) Inertia and milemeter combined navigation method considering temperature model under load system
CN106352880B (en) A kind of determination method of the inertial navigation algorithm framework based on nonlinear network
Saadeddin et al. Optimization of intelligent-based approach for low-cost INS/GPS navigation system
CN112325878A (en) Ground carrier combined navigation method based on UKF and air unmanned aerial vehicle node assistance

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
CB03 Change of inventor or designer information
CB03 Change of inventor or designer information

Inventor after: Zhang Ya

Inventor after: Wang Guochen

Inventor after: Hao Qiang

Inventor after: Zhang Lin

Inventor after: Xu Dingjie

Inventor after: Li Qian

Inventor before: Hao Qiang

Inventor before: Wang Guochen

Inventor before: Zhang Lin

Inventor before: Xu Dingjie

Inventor before: Li Qian

GR01 Patent grant
GR01 Patent grant