CN111721298A - A SLAM accurate positioning method for large outdoor scenes - Google Patents
A SLAM accurate positioning method for large outdoor scenes Download PDFInfo
- Publication number
- CN111721298A CN111721298A CN202010587786.7A CN202010587786A CN111721298A CN 111721298 A CN111721298 A CN 111721298A CN 202010587786 A CN202010587786 A CN 202010587786A CN 111721298 A CN111721298 A CN 111721298A
- Authority
- CN
- China
- Prior art keywords
- lidar
- gnss
- imu
- wheel
- slam
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 39
- 238000005259 measurement Methods 0.000 claims abstract description 15
- 238000001914 filtration Methods 0.000 claims description 3
- 230000001902 propagating effect Effects 0.000 claims description 3
- XEEYBQQBJWHFJM-UHFFFAOYSA-N Iron Chemical compound [Fe] XEEYBQQBJWHFJM-UHFFFAOYSA-N 0.000 description 2
- 229910000831 Steel Inorganic materials 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 229910052742 iron Inorganic materials 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 239000010959 steel Substances 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/20—Instruments for performing navigational calculations
-
- 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
- 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
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
本发明公开了一种SLAM室外大场景精准定位方法，包括以下步骤：使用IMU和Wheel Odemetry更新的状态，传播IMU和Wheel Odemetry的不确定性，当有GNSS或者LIDAR测量结果到达时计算GNSS/Lidar的卡尔曼增益，计算误差状态Error State，校正预测状态Predicted State，计算校正协方差，计算当前位置，当GNSS或者LIDAR测量结果没有到达时，继续使用IMU和Wheel Odemetry更新的状态，传播IMU和Wheel Odemetry的不确定性。本发明提高了该定位系统和方法的准确性。
The invention discloses a SLAM accurate positioning method for large outdoor scenes, comprising the following steps: using the updated state of IMU and Wheel Odemetry, disseminating the uncertainty of IMU and Wheel Odemetry, and calculating GNSS/Lidar when GNSS or LIDAR measurement results arrive Kalman gain, calculate the error state Error State, correct the predicted state Predicted State, calculate the correction covariance, calculate the current position, when the GNSS or LIDAR measurement results do not arrive, continue to use the IMU and Wheel Odemetry to update the state, propagate IMU and Wheel Uncertainty of Odemetry. The present invention improves the accuracy of the positioning system and method.
Description
Technical Field
The invention relates to the technical field of SLAM outdoor large scene accurate positioning, in particular to an SLAM outdoor large scene accurate positioning method.
Background
The scene characteristics of molten iron transportation are as follows: the coverage is large, steel structure buildings are many, the shielding is serious, and the scene is complex.
The reliability of the GPS is greatly reduced under the scene; integral errors inevitably exist in IMU, Lidar and WheelOdmetry; the single sensor is used for accurate positioning, and the effect cannot be achieved.
Disclosure of Invention
Based on the technical problems in the background art, the invention provides an accurate positioning method for an outdoor large scene of an SLAM.
The invention provides an accurate positioning method for an outdoor large scene of an SLAM (simultaneous localization and mapping), which comprises the following steps of:
s1: status updated using IMU and WheelOdetry;
s2: propagating uncertainty of IMU and WheelOdemetry;
s3: entering S4 when a GNSS or LIDAR measurement result arrives, or entering S1;
s4: calculating a Kalman gain (Kalman gain) of the GNSS/Lidar;
s5: calculating an error state ErrorState;
s6: correcting the prediction state PredictedState;
s7: calculating a correction covariance;
s8: the current position is calculated.
Preferably, each sensor needs to be calibrated individually and calibrated jointly.
Preferably, the IMU and WheelOdemetry acquire high frequency state tracking information, and the GNSS and LIDAR acquire low frequency measurements for calibration.
Preferably, when the kalman gain is calculated, kalman filtering based on iteration is used, the iterative kalman filter mainly aims to overcome the inaccuracy of estimation caused by discarding the high-order error in the linearization process of the EKF, and the iteration mainly means that one step of iteration is added in the measurement updating process until the state is converged.
Preferably, S6 is corrected by the keyframe and Lidar of the high-precision map, in addition to the calculation of ErrorState.
The beneficial effects of the invention are as follows:
1. the sensors required by the positioning system and method are as follows: the IMU, the GNSS, the Lidar and the Wheel Odmetry are the most common sensors in the market, the cost of the positioning system and the positioning method is reduced, and the positioning method is accurate and has low error, so that the accuracy of the positioning system and the positioning method is improved.
2. The positioning system and the method not only can carry out correction through ErrorState calculation, but also can carry out correction through a key frame and Lidar of a high-precision map, and can enter S4 when a GNSS or LIDAR measurement result arrives, otherwise, the positioning system and the method need to return to S1, thereby further reducing the positioning error and further improving the accuracy of the positioning system and the method,
3. the positioning system and the positioning method avoid the existence of integral errors and the condition that a single sensor cannot be accurately positioned, each sensor needs to be calibrated and calibrated jointly, the influence of the sensor on positioning is avoided, in addition, the IMU and the WheelOdetry acquire high-frequency state tracking information, and the GNSS and the LIDAR acquire low-frequency measurement results and further correct, so that the accuracy of the positioning system and the positioning method is further improved.
Drawings
Fig. 1 is a system diagram of an accurate positioning method for a large outdoor SLAM scene 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 only a part of the embodiments of the present invention, and not all of the embodiments.
Referring to fig. 1, an accurate positioning method for a large outdoor SLAM scene includes the following steps:
s1: status updated using IMU and WheelOdetry;
s2: propagating uncertainty of IMU and WheelOdemetry;
s3: entering S4 when a GNSS or LIDAR measurement result arrives, or entering S1;
s4: calculating a Kalman gain (Kalman gain) of the GNSS/Lidar;
s5: calculating an error state ErrorState;
s6: correcting the prediction state PredictedState;
s7: calculating a correction covariance;
s8: the current position is calculated.
According to the method, respective calibration and combined calibration are required to be carried out on IMU, GNSS, Lidar and WheelOdmetry sensors, the IMU and WheelOdmetry obtain high-frequency state following new information, the GNSS and LIDAR obtain low-frequency measurement results, further correction is carried out, when Kalman gain of the GNSS/Lidar is calculated, Kalman filtering based on iteration is used, the iteration Kalman filter mainly aims to overcome the defect that high-order errors are discarded in an EKF linearization process to cause inaccurate estimation, iteration mainly means that one-step iteration is added in a measurement updating process until the state is converged, S6 not only carries out correction through ErrorState calculation, but also carries out correction through a key frame of a high-precision map and the Lidar, and in addition, HDmap drawn by the high-precision Lidar is required.
In the invention, the sensors required by the positioning system and the method are as follows: the IMU, the GNSS, the Lidar and the WheelOdmetry are the most common sensors in the market, the cost of the positioning system and the positioning method is reduced, the positioning method is not only accurate, but also has low error, so that the accuracy of the positioning system and the positioning method is improved, the positioning system and the positioning method can be corrected by calculation of ErrorState, also can be corrected by a key frame of a high-precision map and the Lidar, and can enter S4 when a GNSS or LIDAR measurement result arrives, otherwise, the positioning system and the positioning method need to return to S1, so that the positioning error is further reduced, the accuracy of the positioning system and the positioning method is improved, the situations that the integral error exists and the single sensor cannot be accurately positioned are avoided by the positioning system and the positioning method, and each sensor needs to be calibrated and jointly calibrated, so that the influence of the sensor on the positioning is avoided, and in addition, the IMU and the WheelOdmetry obtain high-frequency state and new information, the GNSS and the LIDAR acquire low-frequency measurement results and further correct, so that the accuracy of the positioning system and the positioning method is further improved.
The above description is only for the preferred embodiment of the present invention, but the scope of the present invention is not limited thereto, and any person skilled in the art should be considered to be within the technical scope of the present invention, and the technical solutions and the inventive concepts thereof according to the present invention should be equivalent or changed within the scope of the present invention.
Claims (5)
1. An SLAM outdoor large scene accurate positioning method is characterized by comprising the following steps:
s1: status updated using IMU and Wheel Odetry;
s2: propagating uncertainty of IMU and Wheel Odemetry;
s3: entering S4 when a GNSS or LIDAR measurement result arrives, or entering S1;
s4: calculating a Kalman Gain (Kalman Gain) of the GNSS/Lidar;
s5: calculating an Error State;
s6: correcting the Predicted State;
s7: calculating a correction covariance;
s8: the current position is calculated.
2. The method as claimed in claim 1, wherein the IMU, GNSS, Lidar and Wheel Odmetry sensors need respective calibration and joint calibration.
3. The method as claimed in claim 1, wherein the IMU and the Wheel Odetry acquire high frequency state and new information, and the GNSS and the LIDAR acquire low frequency measurement results for further correction.
4. The SLAM outdoor large scene accurate positioning method according to claim 1, wherein when calculating the Kalman gain of GNSS/Lidar, iterative Kalman filtering is used, the iterative Kalman filter is mainly aimed at overcoming the inaccuracy of estimation caused by discarding high order errors in the EKF linearization process, and the iteration mainly means that one iteration is added in the measurement updating process until the state converges.
5. The SLAM outdoor large scene accurate positioning method as claimed in claim 1, wherein the S6 can be corrected by the key frame and Lidar of the high precision map in addition to the Error State calculation.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010587786.7A CN111721298A (en) | 2020-06-24 | 2020-06-24 | A SLAM accurate positioning method for large outdoor scenes |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010587786.7A CN111721298A (en) | 2020-06-24 | 2020-06-24 | A SLAM accurate positioning method for large outdoor scenes |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN111721298A true CN111721298A (en) | 2020-09-29 |
Family
ID=72568695
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010587786.7A Pending CN111721298A (en) | 2020-06-24 | 2020-06-24 | A SLAM accurate positioning method for large outdoor scenes |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111721298A (en) |
Citations (18)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1948910A (en) * | 2006-11-09 | 2007-04-18 | 复旦大学 | Combined positioning method and apparatus using GPS, gyroscope, speedometer |
| CN102608641A (en) * | 2012-03-30 | 2012-07-25 | 江苏物联网研究发展中心 | Vehicle-mounted combined navigation system based on single-axis gyroscope and single-axis accelerometer and method |
| CN103207634A (en) * | 2013-03-20 | 2013-07-17 | 北京工业大学 | Data fusion system and method of differential GPS (Global Position System) and inertial navigation in intelligent vehicle |
| CN105628026A (en) * | 2016-03-04 | 2016-06-01 | 深圳大学 | Positioning and posture determining method and system of mobile object |
| CN105824039A (en) * | 2016-03-17 | 2016-08-03 | 孙红星 | Odometer-based GNSS/INS (Global Navigation Satellite System/Inertial Navigation System) vehicle-mounted combined positioning and orientation algorithm for overcoming satellite locking loss |
| CN106780699A (en) * | 2017-01-09 | 2017-05-31 | 东南大学 | A kind of vision SLAM methods aided in based on SINS/GPS and odometer |
| CN107765242A (en) * | 2017-09-16 | 2018-03-06 | 太原理工大学 | System state estimation method based on state augmentation iterative extended Kalman filter |
| CN109781118A (en) * | 2019-03-08 | 2019-05-21 | 兰州交通大学 | A method for location tracking of unmanned vehicles |
| CN109781096A (en) * | 2017-11-15 | 2019-05-21 | 洛阳中科晶上智能装备科技有限公司 | A kind of integrated navigation and location system and method for intelligent agricultural machinery |
| CN110017850A (en) * | 2019-04-19 | 2019-07-16 | 小狗电器互联网科技（北京）股份有限公司 | A kind of gyroscopic drift estimation method, device and positioning system |
| CN110243358A (en) * | 2019-04-29 | 2019-09-17 | 武汉理工大学 | The unmanned vehicle indoor and outdoor localization method and system of multi-source fusion |
| CN110430534A (en) * | 2019-09-12 | 2019-11-08 | 北京云迹科技有限公司 | A positioning selection method, device, electronic equipment and storage medium |
| CN110567467A (en) * | 2019-09-11 | 2019-12-13 | 北京云迹科技有限公司 | Map construction method, device and storage medium based on multi-sensor |
| CN110988951A (en) * | 2019-12-06 | 2020-04-10 | 中国地质大学（北京） | Multi-source data fusion real-time navigation positioning method and system |
| CN111006655A (en) * | 2019-10-21 | 2020-04-14 | 南京理工大学 | Multi-scene autonomous navigation positioning method for airport inspection robot |
| CN111102978A (en) * | 2019-12-05 | 2020-05-05 | 深兰科技（上海）有限公司 | Method and device for determining vehicle motion state and electronic equipment |
| CN111156994A (en) * | 2019-12-31 | 2020-05-15 | 西安航天华迅科技有限公司 | INS/DR & GNSS loose integrated navigation method based on MEMS inertial component |
| CN111272165A (en) * | 2020-02-27 | 2020-06-12 | 清华大学 | Intelligent vehicle positioning method based on characteristic point calibration |
-
2020
- 2020-06-24 CN CN202010587786.7A patent/CN111721298A/en active Pending
Patent Citations (18)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1948910A (en) * | 2006-11-09 | 2007-04-18 | 复旦大学 | Combined positioning method and apparatus using GPS, gyroscope, speedometer |
| CN102608641A (en) * | 2012-03-30 | 2012-07-25 | 江苏物联网研究发展中心 | Vehicle-mounted combined navigation system based on single-axis gyroscope and single-axis accelerometer and method |
| CN103207634A (en) * | 2013-03-20 | 2013-07-17 | 北京工业大学 | Data fusion system and method of differential GPS (Global Position System) and inertial navigation in intelligent vehicle |
| CN105628026A (en) * | 2016-03-04 | 2016-06-01 | 深圳大学 | Positioning and posture determining method and system of mobile object |
| CN105824039A (en) * | 2016-03-17 | 2016-08-03 | 孙红星 | Odometer-based GNSS/INS (Global Navigation Satellite System/Inertial Navigation System) vehicle-mounted combined positioning and orientation algorithm for overcoming satellite locking loss |
| CN106780699A (en) * | 2017-01-09 | 2017-05-31 | 东南大学 | A kind of vision SLAM methods aided in based on SINS/GPS and odometer |
| CN107765242A (en) * | 2017-09-16 | 2018-03-06 | 太原理工大学 | System state estimation method based on state augmentation iterative extended Kalman filter |
| CN109781096A (en) * | 2017-11-15 | 2019-05-21 | 洛阳中科晶上智能装备科技有限公司 | A kind of integrated navigation and location system and method for intelligent agricultural machinery |
| CN109781118A (en) * | 2019-03-08 | 2019-05-21 | 兰州交通大学 | A method for location tracking of unmanned vehicles |
| CN110017850A (en) * | 2019-04-19 | 2019-07-16 | 小狗电器互联网科技（北京）股份有限公司 | A kind of gyroscopic drift estimation method, device and positioning system |
| CN110243358A (en) * | 2019-04-29 | 2019-09-17 | 武汉理工大学 | The unmanned vehicle indoor and outdoor localization method and system of multi-source fusion |
| CN110567467A (en) * | 2019-09-11 | 2019-12-13 | 北京云迹科技有限公司 | Map construction method, device and storage medium based on multi-sensor |
| CN110430534A (en) * | 2019-09-12 | 2019-11-08 | 北京云迹科技有限公司 | A positioning selection method, device, electronic equipment and storage medium |
| CN111006655A (en) * | 2019-10-21 | 2020-04-14 | 南京理工大学 | Multi-scene autonomous navigation positioning method for airport inspection robot |
| CN111102978A (en) * | 2019-12-05 | 2020-05-05 | 深兰科技（上海）有限公司 | Method and device for determining vehicle motion state and electronic equipment |
| CN110988951A (en) * | 2019-12-06 | 2020-04-10 | 中国地质大学（北京） | Multi-source data fusion real-time navigation positioning method and system |
| CN111156994A (en) * | 2019-12-31 | 2020-05-15 | 西安航天华迅科技有限公司 | INS/DR & GNSS loose integrated navigation method based on MEMS inertial component |
| CN111272165A (en) * | 2020-02-27 | 2020-06-12 | 清华大学 | Intelligent vehicle positioning method based on characteristic point calibration |
Non-Patent Citations (4)
| Title |
|---|
| 张佳乐;赵敏;: "基于BDS/SINS的空投翼伞降落轨迹还原", 机械制造与自动化, no. 02, 20 April 2019 (2019-04-20), pages 119 - 122 * |
| 徐元;陈熙源;: "面向室内行人的Range-only UWB/INS紧组合导航方法", 仪器仪表学报, no. 09, 15 September 2016 (2016-09-15) * |
| 李炳荣;马强;丁善荣;: "固定观测站对运动辐射源定位的IEKF算法及仿真", 战术导弹技术, no. 05, 15 September 2012 (2012-09-15), pages 94 - 98 * |
| 钟乐;胡延霖;陈永明;王翌臣;: "一种基于DSP的SINS/GPS组合导航系统实现", 舰船电子工程, no. 02, 28 February 2011 (2011-02-28), pages 55 - 57 * |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP5046687B2 (en) | Method and apparatus for implementing an iterative extended Kalman filter in a navigation system | |
| US8843340B2 (en) | Track information generating device, track information generating method, and computer-readable storage medium | |
| CN107884800B (en) | Combined navigation data resolving method and device for observation time-lag system and navigation equipment | |
| CN108759833A (en) | A kind of intelligent vehicle localization method based on priori map | |
| CN109141427B (en) | EKF positioning method based on distance and angle probability model under non-line-of-sight environment | |
| US6407701B2 (en) | GPS receiver capable of calculating accurate 2DRMS | |
| CN110542438B (en) | SINS/DVL-based integrated navigation error calibration method | |
| CN114234969B (en) | Navigation positioning method and device and electronic equipment | |
| CN106772517A (en) | Agricultural machinery roll angle method of testing based on double antenna GNSS receiver/gyroscope information fusion | |
| US20180245916A1 (en) | Precise altitude estimation for indoor positioning | |
| CN110657806B (en) | Position resolving method based on CKF, chan resolving and Savitzky-Golay smooth filtering | |
| CN118276131B (en) | High-precision single Beidou satellite positioning delay deviation correction method and system | |
| CN109782240A (en) | One kind being based on the modified multisensor syste error registration method of recursion and system | |
| CN114019954A (en) | Course installation angle calibration method and device, computer equipment and storage medium | |
| CN113933798B (en) | A global sensor system error partition registration algorithm based on similarity principle | |
| CN112147656B (en) | GNSS double-antenna course installation angle offset estimation method | |
| CN110426011A (en) | Vehicle Steering Angle Measuring System and Method | |
| CN112945266B (en) | Laser navigation robot and robot odometer calibration method | |
| CN105652292B (en) | GPS multipath effect correction positioning method and system | |
| CN114527500A (en) | Indoor and outdoor integrated positioning method, equipment, medium and product | |
| CN107289973A (en) | A kind of gravitational field suitability determination methods in Gravity Matching navigation | |
| US9258679B1 (en) | Modifying a history of geographic locations of a computing device | |
| CN114199236A (en) | Positioning data processing method and device, electronic equipment and automatic driving vehicle | |
| CN114111767A (en) | Method for optimizing line design line type based on multi-information fusion | |
| CN117930294A (en) | GNSS robust positioning method based on nuclear density estimation in non-line-of-sight transmission environment |
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 | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20200929 |
|
| RJ01 | Rejection of invention patent application after publication |