CN113008229A - Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor - Google Patents
Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor Download PDFInfo
- Publication number
- CN113008229A CN113008229A CN202110217725.6A CN202110217725A CN113008229A CN 113008229 A CN113008229 A CN 113008229A CN 202110217725 A CN202110217725 A CN 202110217725A CN 113008229 A CN113008229 A CN 113008229A
- Authority
- CN
- China
- Prior art keywords
- vehicle
- navigation
- error
- vector
- matrix
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 45
- 238000005259 measurement Methods 0.000 claims abstract description 51
- 239000013598 vector Substances 0.000 claims abstract description 48
- 230000004927 fusion Effects 0.000 claims abstract description 9
- 238000012545 processing Methods 0.000 claims abstract description 4
- 239000011159 matrix material Substances 0.000 claims description 38
- 238000001914 filtration Methods 0.000 claims description 11
- 230000007246 mechanism Effects 0.000 claims description 9
- 230000008569 process Effects 0.000 claims description 8
- 238000004364 calculation method Methods 0.000 claims description 6
- 230000007704 transition Effects 0.000 claims description 6
- 238000012546 transfer Methods 0.000 claims description 5
- 238000004422 calculation algorithm Methods 0.000 claims description 2
- 238000009795 derivation Methods 0.000 claims description 2
- 238000005070 sampling Methods 0.000 claims description 2
- 239000000126 substance Substances 0.000 claims description 2
- 230000009466 transformation Effects 0.000 claims description 2
- 230000003068 static effect Effects 0.000 claims 1
- 230000008901 benefit Effects 0.000 abstract description 4
- 238000012360 testing method Methods 0.000 description 7
- 238000010586 diagram Methods 0.000 description 4
- 230000000694 effects Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000011326 mechanical measurement Methods 0.000 description 1
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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
The invention discloses a distributed autonomous integrated navigation method based on a low-cost vehicle-mounted sensor, which is suitable for a wheeled vehicle, wherein a plurality of rotating speed sensors, a corner sensor and an MEMS (micro-electromechanical systems) inertial measurement unit are carried on the vehicle, the rotating speed sensors are used for respectively obtaining the rotating speeds of four wheels, the corner sensor is used for obtaining the corner of a steering wheel, the MEMS inertial measurement unit fixedly connected with the vehicle is used for obtaining an angular rate vector and a specific force vector, the rotating speeds of the wheels, the corner of the steering wheel, the angular rate vector and the specific force vector are sent to a navigation resolving computer for processing, and a Kalman filter is used for carrying out data fusion so as to. The invention fully utilizes the advantage of the number of low-cost vehicle-mounted sensors and effectively improves the positioning precision and robustness of autonomous navigation.
Description
Technical Field
The invention belongs to the technical field of vehicle autonomous navigation, and particularly relates to a distributed combined navigation method based on various low-cost vehicle-mounted sensors.
Background
At present, however, the navigation and positioning system of the vehicle mainly depends on the GNSS which can provide high-precision speed and position measurement. However, with the progress of urbanization, the working environment of the vehicle becomes more complex, which puts more strict requirements on the vehicle navigation positioning system. For a vehicle equipped with an auxiliary driving system or an automatic driving system, the navigation positioning system is used as a basic functional module thereof, and not only needs to provide accurate vehicle navigation positioning estimation, but also guarantees continuous and stable output. Nowadays, a conventional GNSS/INS integrated navigation system is difficult to maintain accurate navigation and positioning for a long time due to the fast divergence of the INS after the GNSS is shielded.
At present, in order to solve the problem of vehicle navigation and positioning under the condition of GNSS obstruction, a mode of adding an additional sensor to form a standby navigation system is mainly adopted. The vision navigation adds a camera as a sensor, and autonomous navigation can be performed, however, the method is easily affected by external conditions such as illumination and the like, and the robustness is poor in practical application. LIDAR can provide pose information with high accuracy, however the system is costly and loses accuracy advantages in environments without significant structural features. Wheeled vehicles are equipped with various low-cost mechanical measurement sensors, such as turning angle and rotational speed sensors. According to the vehicle motion model, the sensors can measure and solve the related kinematic state of the current vehicle, so that rich kinematic constraints are provided for the INS, and the navigation accuracy is ensured. At present, although such navigation systems, such as an OD/INS combined navigation system, a ris, and the like, have the characteristics of low cost, easy use, and the like, the kinematic constraints of the vehicle are not fully utilized, and the specific expression is that the use of a single sensor is single, so that the precision and robustness of the navigation system are degraded.
Disclosure of Invention
The invention aims to provide a distributed combined navigation method based on various low-cost vehicle-mounted sensors, which utilizes vehicle-mounted various mechanism measurement sensors, provides redundant vehicle kinematics measurement for inertial navigation based on an Ackerman principle, fuses inertial navigation and kinematics constraint by adopting a distributed filtering method, and provides high-precision navigation positioning information for a vehicle when a GNSS is blocked.
The technical solution for realizing the purpose of the invention is as follows: a distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensors is suitable for wheeled vehicles, a plurality of rotating speed sensors, corner sensors and MEMS inertial measurement units are carried on the vehicle, the rotating speed sensors are used for respectively obtaining the rotating speeds of four wheels, the corner sensors are used for obtaining the rotating angles of a steering wheel, the MEMS inertial measurement units fixedly connected with the vehicle are used for obtaining angular rate vectors and specific force vectors, the rotating speeds of the wheels, the rotating angles of the steering wheel, the angular rate vectors and the specific force vectors are sent to a navigation resolving computer for processing, a Kalman filter is used for carrying out data fusion, and integrated navigation resolving is achieved, and the method comprises the following specific steps:
step 1, carrying out initial alignment by using an MEMS inertial measurement unit to obtain an initial posture, initializing parameters of a Kalman filter after the alignment is finished, and then entering a combined navigation state;
step 2, carrying out inertial navigation resolving to obtain the attitude, speed and position of the vehicle, and simultaneously acquiring the rotating speed of the wheels and the rotating angle of a steering wheel;
step 3, the navigation resolving enters a Kalman filtering period, and the navigation state parameter error and the sensor calibration parameter error at the moment and a one-step prediction covariance matrix are predicted according to the autonomous integrated navigation error transfer model;
step 4, according to an Ackerman principle, calculating to obtain a vehicle speed estimation of the vehicle body direction at the inertia measurement unit by utilizing the collected wheel rotating speed and steering wheel rotating angle, so as to obtain a plurality of vehicle kinematic constraints of the same type;
step 6, carrying out measurement updating of the local filter to obtain optimal estimates of a plurality of groups of error state vectors and corresponding optimal estimation covariance matrixes;
step 7, performing global fusion on the optimal estimation obtained by the local filter according to the minimum variance criterion;
and 8, respectively correcting the attitude, the speed and the position calculated by the inertial measurement unit, the mounting parameters among the sensors and the measurement scale coefficients of the speed of each wheel, finally obtaining the estimation of the navigation state of the vehicle, and calibrating the parameters of each sensor in real time.
Compared with the prior art, the invention has the following remarkable advantages: (1) the vehicle navigation positioning system has completely autonomous navigation positioning capability and has better robustness to the external environment on the premise of normal running of the vehicle.
(2) The integrated navigation system is constructed by utilizing the vehicle-mounted mechanism measuring sensor and the MIMU, and is low in cost and easy to use.
(3) An Ackerman principle is introduced for modeling, the number advantage of the vehicle-mounted sensors is fully utilized, redundant kinematic constraints are provided for the inertial navigation system, and the navigation positioning precision is effectively improved.
Drawings
FIG. 1 is a flow chart of the distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensors according to the invention.
FIG. 2 is a schematic diagram of a test track of the distributed autonomous combined navigation method based on the low-cost vehicle-mounted sensor.
FIG. 3 is a schematic diagram of positioning errors of the distributed autonomous integrated navigation method based on the low-cost vehicle-mounted sensor according to the present invention.
Detailed Description
The distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensors of the invention is described in detail below with reference to the accompanying drawings and embodiments.
The invention is suitable for wheeled vehicles, a plurality of low-cost sensors are carried on the vehicles, and the sensors comprise a rotating speed sensor, a corner sensor and an MEMS (micro-electromechanical systems) inertia measurement unit (composed of a three-axis gyroscope and a three-axis accelerometer), the rotating speed sensor is used for respectively obtaining the rotating speeds of four wheels, the corner sensor is used for obtaining the rotating angle of a steering wheel, the MEMS inertia measurement unit fixedly connected with the vehicle is used for obtaining an angular rate vector and a specific force vector, the rotating speeds of the wheels, the rotating angle of the steering wheel, the angular rate vector and the specific force vector are sent to a navigation resolving computer for processing, and according to a kinematic constraint relation, data fusion is carried out on the information of each sensor by.
The coordinate system of the distributed autonomous integrated navigation method based on the low-cost vehicle-mounted sensor is defined as follows:
vehicle navigation coordinate system n: adopting a northeast geographical coordinate system, wherein an x axis indicates east, a y axis indicates north, and a z axis indicates sky;
a carrier coordinate system b: coinciding with the MIMU coordinate system, the x-axis points to the right, the y-axis points forward, and the z-axis points up.
Wheel XX coordinate system ω: XX represents different wheels, which are respectively represented as left front fl, right front fr, left rear rl and right rear rr, the origin o of the wheels is located at the rolling center of the wheels, the x axis and the y axis are parallel to a tangent plane of the wheels and the ground, the x axis points to the right, the y axis points to the front, and the z axis follows the right-hand rule.
Calculating a coordinate system c: the axes of the coordinate are defined to point to the same b system, the position of the origin is the same, but a small misalignment angle error exists between the two.
As shown in FIG. 1, the distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensors of the invention comprises the following steps:
step 1, carrying out initial alignment by using an MEMS inertial measurement unit to obtain an initial attitude, initializing parameters of a Kalman filter after the alignment is finished, and then entering a combined navigation state.
And 2, carrying out inertial navigation calculation to obtain the attitude, speed and position of the vehicle, and simultaneously acquiring the rotating speed of the wheels and the rotating angle of the steering wheel.
Furthermore, the invention adopts a strapdown inertial navigation algorithm to carry out navigation calculation, and can ignore the influence of the earth model according to the use condition so as to simplify the calculation complexity.
And 3, the navigation resolving enters a Kalman filtering cycle, and the navigation state parameter error, the sensor calibration parameter error and the one-step prediction covariance matrix at the moment are predicted according to the autonomous integrated navigation error transfer model, which specifically comprises the following steps:
the navigation resolving enters a Kalman filtering period, and an autonomous integrated navigation error transfer model designed by the method is as follows:
in the formula: represents a derivation operation; delta and each parameter symbol after delta are combined to represent corresponding parameter error amount;
estimating a calibration parameter vector for the vehicle speed measured by each mechanism, wherein k is a scale factor, and beta is an included angle vector between a b system and an omega system; l is a lever arm; phi is the misalignment angle of the inertial measurement unit; omegaieThe rotational angular velocity of the earth; omegaenN is the angular velocity relative to the earth; v ═ Ve,Vn,Vu]Is a velocity estimate of an inertial measurement unit, where VeExpressed as its east component, VnExpressed as its north component, VuExpressed as its antenna component; λ, L, h are longitude, latitude, and altitude estimates for the inertial measurement unit, respectively, and are recorded as position p ═ λ, L, h](ii) a Epsilon is the gyroscope zero bias; baZero bias for the accelerometer; rmIs the meridian plane of the earth, RnThe radius of the prime plane of the earth is the radius of the prime plane of the earth; f is the specific force vector.
Denote the state vector as
The above equation is X ═ Φ X + Γ W, where W denotes the process noise matrix, Φ denotes the state transition matrix, and Γ denotes the noise transition matrix. Solving the state transition matrix phi from a solving step k-1 to a solving step k in the filtering period by discretizationk,k-1And noise transformation matrix gammak-1I.e. by
In the process, high-order terms are abandoned according to use requirements so as to reduce the calculated amount, wherein T is a sampling interval; t is tkRepresents the current time; j represents the order of the series.
And performing prediction updating of Kalman filtering according to the discretized state equation:
obtaining the prediction X of the navigation state parameter error and the sensor calibration parameter error at the momentk|k-1And a one-step prediction covariance matrix Pk|k-1In the formula, Qk-1In order to measure the noise matrix, a noise matrix is measured,
representing the best estimate of the error state vector obtained in the previous filtering cycle.
Step 4, according to an Ackerman principle, by utilizing the collected wheel rotating speed and steering wheel rotating angle, calculating to obtain a vehicle speed estimation of the vehicle body direction at the inertia measurement unit, so as to obtain a plurality of vehicle kinematic constraints of the same type, specifically as follows:
according to steering wheel angle deltaaAngle alpha of rotation with virtual wheelvirtualThe virtual wheel angle is obtained by the linear scale relation of (1):
in the formula SratioThe two front wheel rotation angles are solved according to Ackerman principle after the proportional factor is obtained; calculating the movement speed of the wheel center
Wherein ω isXXAs the wheel speed, dXXIs the wheel diameter.
According to the Ackerman principle, by utilizing the law of motion, calculating to obtain the vehicle speed estimation of the vehicle body direction at the inertia measurement unit
Obtaining a plurality of homomorphic kinematic velocity constraints, wherein C represents a directional cosine matrix, i.e.
Is a direction cosine matrix between b and ω,
representing gyroscope measurements.
constructing an observation vector Z of a local Kalman filterXXComprises the following steps:
the vector is further expanded to represent the error state vector:
this formula is denoted as ZkXX＝HkXXXk+VkXXIn which V iskXXRepresenting the measurement noise from the different mechanism sensors estimating the velocity at inertial navigation.
Step 6, carrying out measurement updating of the local filter to obtain optimal estimates of a plurality of groups of error state vectors and corresponding optimal estimate covariance matrixes, which are as follows:
and (3) performing measurement updating on the daughter Kalman filter to obtain the optimal estimation of a plurality of groups of error state vectors at the current moment, and adopting a standard Kalman measurement updating process:
in the formula, ZkRepresents an observation vector, HkTo convert the matrix, RkFor measuring noise covariance matrix, KkFor the filter gain matrix, I denotes an identity matrix. After the corresponding parameters are brought into each sub-filter to carry out optimal estimation respectively, a plurality of optimal state estimates are obtained
With the corresponding optimum estimated covariance matrix Pkfl,Pkfr,Pkrl,Pkrr。
And 7, performing global fusion on the optimal estimation obtained by the local filter according to the minimum variance criterion, wherein the method specifically comprises the following steps:
respectively extracting front 15-dimensional inertial navigation resolving error sub-vectors in optimal state estimation of each sub-filter
And a sub-matrix P of an optimal estimated covariance matrix corresponding theretokINSXXAnd fusing according to an inverse variance weighting method, wherein the specific process is as follows:
finally obtaining the error state estimation of the vehicle body navigation parameter
In the formula, PkINSDenotes the fusion covariance, and N denotes the number of sub-filters.
Step 8, respectively correcting the attitude, the speed and the position resolved by the inertial measurement unit, the installation parameters among the sensors and the measurement scale coefficients of the speed of each wheel, finally obtaining the estimation of the vehicle navigation state, and calibrating the parameters of each sensor in real time, wherein the method specifically comprises the following steps:
the global estimation of the inertial navigation resolving error is utilized for feedback, inertial navigation resolving parameters are corrected and output, the calibration vector of the vehicle speed estimation measured by each mechanism is corrected by adopting the error parameter estimated by a local filter, and the calculation method is as follows:
wherein the content of the first and second substances,
a vector is estimated for each sub-filter state error.
Vehicle tests were carried out at the university of Nanjing Physician using the method of the present invention, using test vehicle parameters listed in Table 1 and using main sensor parameters listed in Table 2. In the test, the data of the sensor is transmitted to a resolving computer through a CAN bus, and the resolving frequency is 25 Hz. After the alignment is finished, the combined navigation phase is carried out, and the test results are shown in table 3. Fig. 2 is a test trajectory diagram, and fig. 3 is a positioning error comparison diagram, which compares the navigation effects of the conventional odometer and the present invention. In the experiment, the GNSS operates in RTK mode, providing a reference true value with centimeter-level accuracy.
TABLE 1 test vehicle parameters
TABLE 2 sensor parameters
TABLE 3 comparison of positioning Results (RMSE)
In the embodiment, the autonomous navigation positioning of the vehicle is estimated by using the MIMU and the vehicle-mounted mechanism measuring sensor through a distributed integrated navigation method, and compared with the navigation result of the traditional odometer/inertial navigation integrated system, the method has higher precision and robustness.
Claims (10)
1. A distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensors is characterized in that the method is suitable for wheeled vehicles, a plurality of rotating speed sensors, a corner sensor and an MEMS (micro-electromechanical systems) inertial measurement unit are mounted on the method, the rotating speed sensors are used for respectively obtaining the rotating speeds of four wheels, the corner sensor is used for obtaining the rotating angle of a steering wheel, the MEMS inertial measurement unit fixedly connected with the vehicle is used for obtaining an angular rate vector and a specific force vector, the rotating speeds of the wheels, the rotating angle of the steering wheel, the angular rate vector and the specific force vector are sent to a navigation resolving computer for processing, and a Kalman filter is used for carrying out data fusion to realize integrated.
2. The distributed autonomous combined navigation method based on low-cost vehicle-mounted sensors according to claim 1, characterized by comprising the following steps:
step 1, carrying out initial alignment by using an MEMS inertial measurement unit to obtain an initial posture, initializing parameters of a Kalman filter after the alignment is finished, and then entering a combined navigation state;
step 2, carrying out inertial navigation resolving to obtain the attitude, speed and position of the vehicle, and simultaneously acquiring the rotating speed of the wheels and the rotating angle of a steering wheel;
step 3, the navigation resolving enters a Kalman filtering period, and the navigation state parameter error and the sensor calibration parameter error at the moment and a one-step prediction covariance matrix are predicted according to the autonomous integrated navigation error transfer model;
step 4, according to an Ackerman principle, calculating to obtain a vehicle speed estimation of the vehicle body direction at the inertia measurement unit by utilizing the collected wheel rotating speed and steering wheel rotating angle, so as to obtain a plurality of vehicle kinematic constraints of the same type;
step 5, respectively adopting the difference between the speed estimation of the inertia measurement units from different wheels and the speed estimation of the inertia measurement units to construct an observation vector of the local Kalman filter;
step 6, carrying out measurement updating of the local filter to obtain optimal estimates of a plurality of groups of error state vectors and corresponding optimal estimation covariance matrixes;
step 7, performing global fusion on the optimal estimation obtained by the local filter according to the minimum variance criterion;
and 8, respectively correcting the attitude, the speed and the position calculated by the inertial measurement unit, the mounting parameters among the sensors and the measurement scale coefficients of the speed of each wheel, finally obtaining the estimation of the navigation state of the vehicle, and calibrating the parameters of each sensor in real time.
3. The distributed autonomous combined navigation method based on low-cost vehicle-mounted sensors according to claim 2, characterized in that the coordinate system used in the steps 1 to 8 is defined as follows:
vehicle navigation coordinate system n: adopting a northeast geographical coordinate system, wherein an x axis indicates east, a y axis indicates north, and a z axis indicates sky;
a carrier coordinate system b: coinciding with the MIMU coordinate system, with the x-axis pointing to the right, the y-axis pointing to the front, and the z-axis pointing to the upper;
wheel XX coordinate system ω: XX represents different wheels which are respectively represented as left front fl, right front fr, left rear rl and right rear rr, the origin o of the wheels is located at the rolling center of the wheels, the x axis and the y axis are parallel to a tangent plane of the wheels and the ground, the x axis points to the right, the y axis points to the front, and the z axis follows the right-hand rule;
calculating a coordinate system c: the axes of the coordinate are defined to point to the same b system, the position of the origin is the same, but a small misalignment angle error exists between the two.
4. The distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensors according to claim 2, characterized in that in step 1, an inertial measurement unit is used to correspondingly adopt a static base alignment method or a dynamic base alignment method under different conditions.
5. The distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensors according to claim 2, characterized in that a strapdown inertial navigation algorithm is adopted for navigation solution in the step 2.
6. The distributed autonomous combined navigation method based on low-cost vehicle-mounted sensors according to claim 2, characterized in that the steps of predicting the navigation state parameter error and the sensor calibration parameter error at the moment and predicting the covariance matrix in one step in step 3 are as follows:
step 3-1, calculating an error state equation, wherein the autonomous integrated navigation error transfer model comprises the following steps:
ε＝0,
in the formula: represents a derivation operation; delta and each parameter symbol after delta are combined to represent corresponding parameter error amount;
estimating a calibration parameter vector for the vehicle speed measured from each of the mechanisms, in particular, k is a scaling factor, betaIs the angle vector between b and omega; l is a lever arm; phi is the misalignment angle of the inertial measurement unit; omegaieThe rotational angular velocity of the earth; omegaenN is the angular velocity relative to the earth; v ═ Ve,Vn,Vu]Is a velocity estimate of an inertial measurement unit, where VeExpressed as its east component, VnExpressed as its north component, VuExpressed as its antenna component; λ, L, h are longitude, latitude, and altitude estimates for the inertial measurement unit, respectively, and are recorded as position p ═ λ, L, h](ii) a Epsilon is the gyroscope zero bias; baZero bias for the accelerometer; rmIs the meridian plane of the earth, RnThe radius of the prime plane of the earth is the radius of the prime plane of the earth; f is a specific force vector;
denote the state vector as
The above equation is X ═ Φ X + Γ W, where W denotes the process noise matrix, Φ denotes the state transition matrix, and Γ denotes the noise transition matrix. Solving the state transition matrix phi from a solving step k-1 to a solving step k in the filtering period by discretizationk,k-1And noise transformation matrix gammak-1I.e. by
In the process, high-order terms are abandoned according to use requirements, wherein T is a sampling interval; t is tkRepresents the current time; j represents the order of the series;
3-2, performing prediction updating of Kalman filtering according to the discretized state equation:
obtaining the prediction X of the navigation state parameter error and the sensor calibration parameter error at the momentk|k-1And a one-step prediction covariance matrix Pk|k-1In the formula, Qk-1In order to measure the noise matrix, a noise matrix is measured,
representing the best estimate of the error state vector obtained in the previous filtering cycle.
7. The distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensors according to claim 2, characterized in that in step 4, the vehicle speed estimation of the vehicle body direction at the inertial measurement unit is obtained by calculation using the collected wheel rotation speed and steering wheel rotation angle measurements as follows:
step 4-1, according to the steering wheel corner deltaaAngle alpha of rotation with virtual wheelvirtualThe virtual wheel angle is obtained by the linear scale relation of (1):
in the formula SratioThe two front wheel rotation angles are solved according to Ackerman principle after the proportional factor is obtained; calculating the movement speed of the wheel center
Wherein ω isXXAs the wheel speed, dXXIs the wheel diameter;
step 4-2, calculating to obtain vehicle speed estimation of the vehicle body direction at the inertia measurement unit by utilizing the law of motion of involvement according to the Ackerman principle
Obtaining a plurality of homologous transportsKinematic velocity constraints, in which C represents a directional cosine matrix, i.e.
Is a direction cosine matrix between b and ω,
representing gyroscope measurements.
8. The distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensors according to claim 4, characterized in that in step 5, an observation vector Z of a local Kalman filter is constructedXXComprises the following steps:
the vector is further expanded to represent the error state vector:
this formula is denoted as ZkXX＝HkXXXk+VkXXIn which V iskXXRepresenting the measurement noise from the different mechanism sensors estimating the velocity at inertial navigation.
9. The distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensors according to claim 2, characterized in that in step 6, measurement updating of the daughter Kalman filter is performed to obtain the optimal estimation of the current time multi-group error state vectors, and a standard Kalman measurement updating process is adopted:
in the formula, ZkRepresents an observation vector, HkTo convert the matrix, RkFor measuring noise covariance matrix, KkFor the filter gain matrix, I denotes an identity matrix. After the corresponding parameters are brought into each sub-filter to carry out optimal estimation respectively, a plurality of optimal state estimates are obtained
With the corresponding optimum estimated covariance matrix Pkfl,Pkfr,Pkrl,Pkrr。
10. The distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensors according to claim 2, characterized in that in step 7, the first 15-dimensional inertial navigation solution error sub-vectors in the optimal state estimation of each sub-filter are respectively extracted
And a sub-matrix P of an optimal estimated covariance matrix corresponding theretokINSXXAnd fusing according to an inverse variance weighting method, wherein the specific process is as follows:
finally obtaining the error state estimation of the vehicle body navigation parameter
In the formula, PkINSRepresenting the fusion covariance, and N representing the number of sub-filters;
in step 8, the global estimation of the inertial navigation resolving error is used for feedback, the inertial navigation resolving parameters are corrected and output, the calibration vector of the vehicle speed estimation measured by each mechanism is corrected by adopting the error parameter estimated by the local filter, and the calculation method is as follows:
wherein the content of the first and second substances,
a vector is estimated for each sub-filter state error.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110217725.6A CN113008229B (en) | 2021-02-26 | 2021-02-26 | Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110217725.6A CN113008229B (en) | 2021-02-26 | 2021-02-26 | Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113008229A true CN113008229A (en) | 2021-06-22 |
| CN113008229B CN113008229B (en) | 2024-04-05 |
Family
ID=76386606
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110217725.6A Active CN113008229B (en) | 2021-02-26 | 2021-02-26 | Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113008229B (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20220268584A1 (en) * | 2021-02-18 | 2022-08-25 | Trimble Inc. | Range image aided ins with map based localization |
| US11815356B2 (en) | 2021-02-18 | 2023-11-14 | Trimble Inc. | Range image aided INS |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107144284A (en) * | 2017-04-18 | 2017-09-08 | 东南大学 | Inertial navigation combination navigation method is aided in based on the vehicle dynamic model that CKF is filtered |
| CN108128308A (en) * | 2017-12-27 | 2018-06-08 | 长沙理工大学 | A kind of vehicle state estimation system and method for distributed-driving electric automobile |
| CN108254775A (en) * | 2016-12-29 | 2018-07-06 | 联创汽车电子有限公司 | Onboard navigation system and its implementation |
| WO2019228437A1 (en) * | 2018-06-01 | 2019-12-05 | 浙江亚特电器有限公司 | Integrated navigation method for mobile vehicle |
| CN110780326A (en) * | 2019-09-26 | 2020-02-11 | 上海瀚所信息技术有限公司 | Vehicle-mounted integrated navigation system and positioning method |
| WO2020087846A1 (en) * | 2018-10-31 | 2020-05-07 | 东南大学 | Navigation method based on iteratively extended kalman filter fusion inertia and monocular vision |
| CN111845775A (en) * | 2020-07-20 | 2020-10-30 | 上海大学 | Joint estimation method for driving state and inertia parameters of distributed driving electric automobile |
-
2021
- 2021-02-26 CN CN202110217725.6A patent/CN113008229B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108254775A (en) * | 2016-12-29 | 2018-07-06 | 联创汽车电子有限公司 | Onboard navigation system and its implementation |
| CN107144284A (en) * | 2017-04-18 | 2017-09-08 | 东南大学 | Inertial navigation combination navigation method is aided in based on the vehicle dynamic model that CKF is filtered |
| CN108128308A (en) * | 2017-12-27 | 2018-06-08 | 长沙理工大学 | A kind of vehicle state estimation system and method for distributed-driving electric automobile |
| WO2019228437A1 (en) * | 2018-06-01 | 2019-12-05 | 浙江亚特电器有限公司 | Integrated navigation method for mobile vehicle |
| WO2020087846A1 (en) * | 2018-10-31 | 2020-05-07 | 东南大学 | Navigation method based on iteratively extended kalman filter fusion inertia and monocular vision |
| CN110780326A (en) * | 2019-09-26 | 2020-02-11 | 上海瀚所信息技术有限公司 | Vehicle-mounted integrated navigation system and positioning method |
| CN111845775A (en) * | 2020-07-20 | 2020-10-30 | 上海大学 | Joint estimation method for driving state and inertia parameters of distributed driving electric automobile |
Non-Patent Citations (2)
| Title |
|---|
| 张月新;王立辉;汤新华;: "车辆动力学模型辅助的惯性导航系统", 中国惯性技术学报, no. 05 * |
| 阮晓明;程咏梅;程承;潘泉;: "一种改进的低成本车载MIMU/GPS组合导航系统算法", 西北工业大学学报, no. 06 * |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20220268584A1 (en) * | 2021-02-18 | 2022-08-25 | Trimble Inc. | Range image aided ins with map based localization |
| US11815356B2 (en) | 2021-02-18 | 2023-11-14 | Trimble Inc. | Range image aided INS |
| US11874116B2 (en) * | 2021-02-18 | 2024-01-16 | Trimble Inc. | Range image aided inertial navigation system (INS) with map based localization |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113008229B (en) | 2024-04-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107655476B (en) | Pedestrian high-precision foot navigation method based on multi-information fusion compensation | |
| CN111156994B (en) | INS/DR & GNSS loose combination navigation method based on MEMS inertial component | |
| CN103235328B (en) | GNSS (global navigation satellite system) and MEMS (micro-electromechanical systems) integrated navigation method | |
| CN112505737B (en) | GNSS/INS integrated navigation method | |
| CN110702143B (en) | Rapid initial alignment method for SINS strapdown inertial navigation system moving base based on lie group description | |
| CN102519470B (en) | Multi-level embedded integrated navigation system and navigation method | |
| CN112629538A (en) | Ship horizontal attitude measurement method based on fusion complementary filtering and Kalman filtering | |
| CN113203418B (en) | GNSSINS visual fusion positioning method and system based on sequential Kalman filtering | |
| CN109931955B (en) | Initial alignment method of strap-down inertial navigation system based on state-dependent lie group filtering | |
| CN110954102B (en) | Magnetometer-assisted inertial navigation system and method for robot positioning | |
| CN111121766A (en) | Astronomical and inertial integrated navigation method based on starlight vector | |
| CN111024074B (en) | Inertial navigation speed error determination method based on recursive least square parameter identification | |
| CN113008229B (en) | Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor | |
| CN111380516A (en) | Inertial navigation/odometer vehicle combined navigation method and system based on odometer measurement information | |
| CN114526731A (en) | Inertia combination navigation direction positioning method based on moped | |
| CN117053782A (en) | Combined navigation method for amphibious robot | |
| CN112880669A (en) | Spacecraft starlight refraction and uniaxial rotation modulation inertia combined navigation method | |
| CN116222551A (en) | Underwater navigation method and device integrating multiple data | |
| CN111722295A (en) | Underwater strapdown gravity measurement data processing method | |
| CN112525204B (en) | Spacecraft inertia and solar Doppler speed combined navigation method | |
| CN111220151B (en) | Inertia and milemeter combined navigation method considering temperature model under load system | |
| Tang et al. | SINS/GNSS Integrated Navigation Based on Invariant Error Models in Inertial Frame | |
| US20230349699A1 (en) | Absolute heading estimation with constrained motion | |
| Mu et al. | Improved decentralized GNSS/SINS/odometer fusion system for land vehicle navigation applications | |
| CN109596139B (en) | Vehicle-mounted navigation method based on MEMS |
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 |