CN109507677A - A kind of SLAM method of combination GPS and radar odometer - Google Patents

A kind of SLAM method of combination GPS and radar odometer Download PDF

Info

Publication number
CN109507677A
CN109507677A CN201811306455.0A CN201811306455A CN109507677A CN 109507677 A CN109507677 A CN 109507677A CN 201811306455 A CN201811306455 A CN 201811306455A CN 109507677 A CN109507677 A CN 109507677A
Authority
CN
China
Prior art keywords
data
gps
pose
lidar
point cloud
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
Application number
CN201811306455.0A
Other languages
Chinese (zh)
Other versions
CN109507677B (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.)
Zhejiang University of Technology ZJUT
Original Assignee
Zhejiang University of Technology ZJUT
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 Zhejiang University of Technology ZJUT filed Critical Zhejiang University of Technology ZJUT
Priority to CN201811306455.0A priority Critical patent/CN109507677B/en
Publication of CN109507677A publication Critical patent/CN109507677A/en
Application granted granted Critical
Publication of CN109507677B publication Critical patent/CN109507677B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • 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
    • 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/53—Determining attitude
    • G—PHYSICS
    • G09—EDUCATION; CRYPTOGRAPHY; DISPLAY; ADVERTISING; SEALS
    • G09B—EDUCATIONAL OR DEMONSTRATION APPLIANCES; APPLIANCES FOR TEACHING, OR COMMUNICATING WITH, THE BLIND, DEAF OR MUTE; MODELS; PLANETARIA; GLOBES; MAPS; DIAGRAMS
    • G09B29/00—Maps; Plans; Charts; Diagrams, e.g. route diagram
    • G09B29/003—Maps
    • G09B29/005—Map projections or methods associated specifically therewith

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Theoretical Computer Science (AREA)
  • Business, Economics & Management (AREA)
  • Educational Administration (AREA)
  • Educational Technology (AREA)
  • Mathematical Physics (AREA)
  • Optical Radar Systems And Details Thereof (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)
  • Length Measuring Devices By Optical Means (AREA)
  • Navigation (AREA)
  • Traffic Control Systems (AREA)

Abstract

一种结合GPS和雷达里程计的SLAM方法，包括如下步骤：1)采集差分GPS数据和来自激光雷达的点云数据；2)处理GPS数据获得位移(X,Y,Z)和姿态RPY角；3)匹配GPS数据和LiDAR的点云数据，通过时间戳对齐的方式实现数据匹配；4)结合步骤2)处理GPS得到的位姿数据和LiDAR的点云数据检验GPS数据的可靠性；5)使用雷达里程计算法LOAM获取(X,Y,Z)和RPY角；6)在GPS数据可靠的地方，使用GPS获取的位姿作为最终的位姿；在GPS数据不可靠的路段，利用该路段起点和终点的GPS位姿优化LOAM算法的位姿来获取最终的位姿；7)使用步骤6)输出的位姿转换激光雷达的点云数据到世界坐标系下，获取最终的全局地图。本发明适用于大范围城市三维地图的构建。A SLAM method combining GPS and radar odometer, comprising the following steps: 1) collecting differential GPS data and point cloud data from lidar; 2) processing GPS data to obtain displacement (X, Y, Z) and attitude RPY angle; 3) Matching GPS data and LiDAR point cloud data, and realizing data matching by timestamp alignment; 4) In combination with step 2) processing the pose data obtained by GPS and LiDAR point cloud data to verify the reliability of GPS data; 5) Use the radar mileage calculation method LOAM to obtain (X, Y, Z) and RPY angles; 6) In places where GPS data is reliable, use the pose obtained by GPS as the final pose; in sections with unreliable GPS data, use this section The GPS pose of the starting point and the end point optimizes the pose of the LOAM algorithm to obtain the final pose; 7) Use the pose output in step 6) to convert the point cloud data of the lidar to the world coordinate system to obtain the final global map. The invention is suitable for the construction of a large-scale city three-dimensional map.

Description

A kind of SLAM method of combination GPS and radar odometer
Technical field
The present invention relates to the SLAM of computer vision technique, especially one (simultaneous localization and Mapping, while positioning and building figure) method.
Background technique
SLAM technology refers to that robot in a strange environment, can construct map and the positioning of ambient enviroment simultaneously The technology of oneself position in the map out.SALM technology is by many applications, such as the positioning and navigation of automatic Pilot, robot Deng.
The SLAM technology of view-based access control model odometer or radar odometer cannot achieve big due to the presence of accumulated error The building of the city three-dimensional map of range.Although the SLAM technology based on GPS is without accumulated error, in urban area, by It is blocked in building, the reasons such as signal interference, some areas can not obtain reliable GPS data, thus also cannot achieve big model The building of the city three-dimensional map enclosed.
Summary of the invention
In order to realize the building of large-scale city three-dimensional map, the invention proposes one combine radar odometer and The SLAM method of GPS.
The technical solution adopted by the present invention to solve the technical problems is:
A kind of combination GPS and radar odometer SLAM (simultaneous localization and mapping, Position and build figure simultaneously) method, the SLAM method includes the following steps:
1) data acquire
Using differential GPS acquisition longitude and latitude height, the angle RPY, timestamp (times of acquisition data) data, the angle RPY includes Roll- Roll angle, Pitch- pitch angle and Yaw- yaw angle；Point cloud data and timestamp are acquired using laser radar LiDAR；
2) processing GPS data is displaced (X, Y, Z) and the angle posture RPY
(X, Y, Z) is the displacement of the initial position LiDAR to the current location LiDAR, and what the angle RPY indicated is LiDAR present bit The posture set, wherein Z, that is, difference in height obtains the high degree of the current location LiDAR and initial position directly from DGPS data According to and ask difference to obtain, the value of X, Y is sat by the way that the longitude and latitude data of the current location LiDAR and initial position are transformed into UTM plane Mark system is lower and asks poor acquisition；The angle RPY is directly obtained from DGPS data；The calculating of (X, Y, Z) is as follows:
(X, Y, Z)=(XEnd,YEnd,ZEnd)-(XJust,YJust,ZJust)
Wherein, (XEnd,YEnd,ZEnd) that represent is the position of current radar, (XJust,YJust,ZJust) what is represented is the position of initial radar It sets；
3) point cloud data of GPS data and LiDAR is matched
We realize Data Matching in such a way that timestamp is aligned.
The time stamp T ime of the data of GPS gathersGPSIt is all seconds, the time stamp T ime of the data of laser radar acquisitionLiDARIt is Number of seconds apart from nearest integral point；In addition, TimeGPSAnd TimeLiDARThere are 18 seconds leap second is poor；In order to realize timestamp The unification of format, to TimeGPSIt pre-processes, remembers that the timestamp of pretreated GPS is TimeGPSL:
TimeGPSL=TimeGPS%3600-18
4) point cloud data of step 2) treated GPS data and LiDAR is combined to examine the reliability of GPS data
Collected for LiDAR continuous two frame (LiDAR from the collected data that turn around) data F1, F2, first use GPS data after reason converts it under world coordinate system, remembers the point cloud after converting into FW1, FW2, then, use LOAM The Feature Points Extraction of (Lidar Odometry and Mapping in Real-time) algorithm extracts FW1, FW2Angle point And millet cake, remember FW1The quantity of angle point and millet cake is C2, S2；Using the corresponding relationship matching process in LOAM algorithm in FW2Feature F is looked in point1Characteristic point corresponding relationship, note find corresponding relationship characteristic point quantity be C1, S1, calculate and find corresponding relationship Angle point and millet cake quantity accounting R1, R2:
If R1, R2, C2, S2Both greater than given threshold value, it is considered that GPS data is reliable；
5) method of radar odometer obtains the angle RPY and displacement (X, Y, Z)
Using laser radar obtain point cloud data, using high-precision radar odometer LOAM algorithm obtain the angle RPY and (X, Y,Z)；
6) pose merges
It is reliably local in GPS data, the pose for using GPS the to obtain pose final as system；GPS data can not The pose data of the section starting point obtained by GPS are first passed to LOAM algorithm as initial value, are calculated using LOAM by the section leaned on Method obtains the pose T of each frame point cloud in the section1(transformation matrix being calculated by the angle RPY and displacement (X, Y, Z) information), so Afterwards, we are input to ELCH (An using the pose data of above-mentioned pose and the road segment end obtained by GPS as input Explicit Loop Closing Technique for 6D SLAM) in algorithm, the accumulation for obtaining each frame point cloud pose misses Poor T2, finally use T2Optimize the pose from radar odometer, the pose after note optimization is T3；
T3=T2*T1
7) final global three-dimensional map is obtained
Use the fused pose T 6) obtained4Under the point cloud data to world coordinate system of switched laser radar, obtain most Whole global map；
Some coordinate of point under radar fix system is P in note point cloud1, the coordinate being transformed under world coordinate system is P2；
P2=T4*P1。
Technical concept of the invention are as follows: when GPS data is reliable, the pose of laser radar is obtained by GPS；? When GPS data is insecure, the pose of GPS is obtained by radar odometer, and using the pose of GPS to radar odometer Pose optimizes.And then realize the SLAM system being applicable in the building of a wide range of city map.
Particularly, GPS data and point cloud data are acquired first, and obtain us to the necessary processing of GPS data progress to need The pose (displacement and posture) wanted.Then, the point cloud data of GPS data and laser radar is matched.Next, in conjunction with point cloud data With the reliability of the data judging GPS data from GPS.Then, pose is obtained by radar odometer.Finally, according to GPS number Pose fusion is carried out to the pose of pose and radar odometer from GPS according to the judgement result of reliability and using fused Pose transfer point cloud obtains global map.
Beneficial effects of the present invention are mainly manifested in: effectively having been merged GPS and radar odometer, and then realized one It can be realized the SLAM system of a wide range of city map building.
Specific embodiment
The invention will be further described below.
A kind of combination GPS and radar odometer SLAM (simultaneous localization and mapping, Position and build simultaneously figure) method, include the following steps:
1) data acquire
The relative position of fixed XW-GI5651 (differential GPS mobile terminal) and VLP-16 LiDAR (a laser radar), leads to It crosses XW-GI5651 and exports GPRMC data to VLP-16 LiDAR, realize that timestamp of the two on hardware is synchronous, then acquire Data；
Using differential GPS acquisition longitude, latitude, height, (Roll- roll angle, Pitch- pitch angle and Yaw- are inclined at the angle RPY Navigate angle, expression be laser radar posture), the timestamp times of data (acquisition)；Use the collection point laser radar LiDAR cloud Data and timestamp.
2) processing GPS data is displaced (X, Y, Z) and the angle posture RPY
The GPS data returned are handled, the displacement and posture needed.
(X, Y, Z) is the displacement of the initial position LiDAR to the current location LiDAR, and what the angle RPY indicated is LiDAR present bit The posture set；The value of X, Y are by being transformed into UTM plane coordinate system for the longitude and latitude data of the current location LiDAR and initial position It descends and asks poor acquisition；Z, that is, difference in height obtains the high degree of the current location LiDAR and initial position directly from DGPS data According to and ask difference to obtain, the calculating of (X, Y, Z) is as follows:
(X, Y, Z)=(XEnd,YEnd,ZEnd)-(XJust,YJust,ZJust)
Wherein, (XEnd,YEnd,ZEnd) that represent is the position of current radar, (XJust,YJust,ZJust) what is represented is the position of initial radar It sets；
The angle RPY is directly obtained from DGPS data；
3) point cloud data of GPS data and LiDAR is matched
We realize Data Matching in such a way that timestamp is aligned
The time stamp T ime of the data of GPS gathersGPSIt is all seconds, the time stamp T ime of the data of laser radar acquisitionLiDARIt is Number of seconds apart from nearest integral point；In addition, TimeGPSAnd TimeLiDARThere are 18 seconds leap second is poor；In order to realize timestamp The unification of format, to TimeGPSIt pre-processes, remembers that the timestamp of pretreated GPS is TimeGPSL:
TimeGPSL=TimeGPS%3600-18
For TimeLiDAR=Time1Moment collected point cloud data, TimeGPSL=Time1Corresponding GPS data is exactly The data to match with it；
4) step 2) treated the GPS data matched in conjunction with the point cloud data of LiDAR examines the reliability of GPS data
Collected for LiDAR continuous two frame (LiDAR from the collected data that turn around) data F1, F2, first use GPS data after reason converts it under world coordinate system, remembers the point cloud after converting into FW1, FW2, then extract FW1, FW2Spy Point is levied, then in FW2Middle searching FW1Characteristic point corresponding relationship, if FW1, FW2There are enough characteristic points and has enough FW1Characteristic point have found corresponding relationship and then think that GPS data is reliable, otherwise, GPS data is unreliable；
Use the Feature Points Extraction of LOAM (Lidar Odometry and Mapping in Real-time) algorithm Extract FW1, FW2Angle point and millet cake, remember FW1The quantity of angle point and millet cake is C2, S2；Use the corresponding relationship in LOAM algorithm Method of completing the square is in FW2Characteristic point in look for F1Characteristic point corresponding relationship, note find corresponding relationship characteristic point quantity be C1, S1。 Calculate the quantity accounting R of the angle point and millet cake that find corresponding relationship1, R2:
If R1, R2, C2, S2Both greater than given threshold value, it is considered that GPS data is reliable, C2, S2Sufficiently large explanation FW1, FW2There are enough characteristic points, R1, R2It is sufficiently large, illustrate enough FW1Characteristic point have found corresponding relationship；
5) method of radar odometer obtains the angle RPY and displacement (X, Y, Z)
The angle RPY and displacement (X, Y, Z) are obtained using radar odometer LOAM；
Only using the point cloud data of laser radar as input, RPY is obtained using high-precision radar odometer LOAM algorithm Angle and (X, Y, Z)；
6) pose merges
It is reliably local in GPS data, the pose for using GPS the to obtain pose final as system；GPS data can not The section leaned on, Optimization Steps 5) obtained pose is as final pose；
In the insecure section of GPS data, the pose data of the section starting point obtained by GPS are first passed into LOAM and are calculated Method obtains the pose T of each frame point cloud in the section using LOAM algorithm as initial value1(by the angle RPY and displacement (X, Y, Z) information The transformation matrix being calculated), then, using the pose data of above-mentioned pose and the road segment end obtained by GPS as inputting, It is input in ELCH (An Explicit Loop Closing Technique for 6D SLAM) algorithm, obtains each frame point The accumulated error T of cloud pose2, finally use T2Optimize the pose from radar odometer, the pose after note optimization is T3；
T3=T2*T1
7) final global three-dimensional map is obtained
The fused pose T obtained using step 6)4Under the point cloud data to world coordinate system of switched laser radar, obtain Take final global map；
Some coordinate of point under radar fix system is P in note point cloud1, the coordinate being transformed under world coordinate system is P2；
P2=T4*P1。

Claims (1)

1.一种结合GPS和雷达里程计的SLAM方法，其特征在于，所述SLAM方法包括如下步骤：1. a SLAM method in conjunction with GPS and radar odometer, is characterized in that, described SLAM method comprises the steps: 1)数据采集1) Data collection 使用差分GPS采集经纬高、RPY角、时间戳数据，RPY角包括Roll-滚转角、Pitch-俯仰角和Yaw-偏航角；使用激光雷达LiDAR采集点云数据和时间戳；Use differential GPS to collect latitude and longitude, RPY angle, timestamp data, RPY angle includes Roll-roll angle, Pitch-pitch angle and Yaw-yaw angle; use LiDAR to collect point cloud data and timestamp; 2)处理GPS数据获得位移(X,Y,Z)和姿态RPY角2) Process GPS data to obtain displacement (X, Y, Z) and attitude RPY angle (X,Y,Z)为LiDAR起始位置到LiDAR当前位置的位移,RPY角表示的是LiDAR当前位置的姿态，其中，Z即高度差直接从差分GPS数据中获取LiDAR当前位置和初始位置的高度数据并求差得到，X，Y的值通过将LiDAR当前位置和初始位置的经纬度数据转换到UTM平面坐标系下并求差获得，RPY角直接从差分GPS数据中获取；(X,Y,Z)的计算如下：(X, Y, Z) is the displacement from the starting position of the LiDAR to the current position of the LiDAR, and the RPY angle represents the attitude of the current position of the LiDAR, where Z is the height difference. The difference between the current position and the initial position of the LiDAR is directly obtained from the differential GPS data. The altitude data is obtained by calculating the difference. The values of X and Y are obtained by converting the latitude and longitude data of the current position and initial position of LiDAR to the UTM plane coordinate system and calculating the difference. The RPY angle is obtained directly from the differential GPS data; (X, Y, Z) is calculated as follows: (X,Y,Z)＝(X末,Y末,Z末)-(X初,Y初,Z初)(X, Y, Z) = (X end , Y end , Z end ) - (X beginning , Y beginning , Z beginning ) 其中，(X末,Y末,Z末)代表的是当前雷达的位置，(X初,Y初,Z初)代表的是初始雷达的位置；Among them, (X end , Y end , Z end ) represents the current radar position, (X beginning , Y beginning , Z beginning ) represents the initial radar position; 3)匹配GPS数据和LiDAR的点云数据3) Match GPS data and LiDAR point cloud data 通过时间戳对齐的方式实现数据匹配；Data matching is achieved by timestamp alignment; GPS采集的数据的时间戳TimeGPS是周秒，激光雷达采集的数据的时间戳TimeLiDAR是距离最近的整点的秒数；此外，TimeGPS和TimeLiDAR存在一个18秒的闰秒差；为了实现时间戳格式的统一，对TimeGPS做预处理，记预处理后的GPS的时间戳为TimeGPSL:The time stamp of the data collected by GPS Time GPS is weekly seconds, and the time stamp of data collected by lidar Time LiDAR is the number of seconds from the nearest whole point; in addition, there is a leap second difference of 18 seconds between Time GPS and Time LiDAR ; in order to To unify the timestamp format, preprocess Time GPS , and record the timestamp of the preprocessed GPS as Time GPSL : TimeGPSL＝TimeGPS％3600-18Time GPSL = Time GPS %3600-18 4)结合步骤2)处理后的GPS数据和LiDAR的点云数据检验GPS数据的可靠性4) Combine the GPS data processed in step 2) and the LiDAR point cloud data to check the reliability of the GPS data 对于LiDAR采集到的连续的两帧数据F1，F2，先用处理后的GPS数据将其转换到世界坐标系下，记转换后的点云为FW1，FW2，接着，使用LOAM算法的特征点提取方法提取FW1，FW2的角点和面点，记FW1角点和面点的数量为C2，S2；使用LOAM算法中的对应关系匹配方法在FW2的特征点中找F1的特征点的对应关系，记找到对应关系的特征点数量为C1，S1；计算找到对应关系的角点和面点的数量占比R1，R2:For the two consecutive frames of data F 1 , F 2 collected by LiDAR, first convert the processed GPS data to the world coordinate system, and denote the converted point cloud as F W1 , F W2 , and then use the LOAM algorithm The feature point extraction method extracts the corner points and surface points of F W1 and F W2 , and denote the number of corner points and surface points of F W1 as C 2 , S 2 ; use the corresponding relationship matching method in the LOAM algorithm in the feature points of F W2 Find the corresponding relationship of the feature points of F 1 , record the number of feature points that find the corresponding relationship as C 1 , S 1 ; calculate the ratio of the number of corner points and surface points for finding the corresponding relationship R 1 , R 2 : 如果R1，R2，C2，S2都大于给定的阈值，那么认为GPS数据是可靠的；If R 1 , R 2 , C 2 , S 2 are all greater than a given threshold, then the GPS data is considered reliable; 5)雷达里程计的方法获取RPY角和位移(X,Y,Z)5) Radar odometry method to obtain RPY angle and displacement (X, Y, Z) 利用激光雷达获取点云数据，使用高精度的雷达里程计LOAM算法获取RPY角和(X,Y,Z)；Use lidar to obtain point cloud data, and use high-precision radar odometry LOAM algorithm to obtain RPY angle and (X, Y, Z); 6)位姿融合6) Pose fusion 在GPS数据可靠的地方，使用GPS获取的位姿作为系统最终的位姿；在GPS数据不可靠的路段，先将该路段起点的由GPS得到的位姿数据传递给LOAM算法作为初值，使用LOAM算法获得该路段每一帧点云的位姿T1，由RPY角和位移(X,Y,Z)信息计算得到的变换矩阵，然后，将上述位姿和该路段终点的由GPS得到的位姿数据作为输入，输入到ELCH算法中，获得每一帧点云位姿的累积误差T2，最后使用T2优化来自雷达里程计的位姿，记优化后的位姿为T3；In places where GPS data is reliable, the pose obtained by GPS is used as the final pose of the system; in sections with unreliable GPS data, the pose data obtained by GPS at the starting point of the section is first passed to the LOAM algorithm as the initial value, using The LOAM algorithm obtains the pose T 1 of the point cloud of each frame of the road section, and the transformation matrix calculated from the RPY angle and displacement (X, Y, Z) information, and then combines the above pose and the end point of the road section obtained by GPS. The pose data is input into the ELCH algorithm to obtain the cumulative error T 2 of the pose of each frame of the point cloud. Finally, T 2 is used to optimize the pose from the radar odometer, and the optimized pose is denoted as T 3 ; T3＝T2*T1 T 3 =T 2 *T 1 7)获取最终的全局三维地图7) Get the final global 3D map 使用6)得到的融合后的位姿T4转换激光雷达的点云数据到世界坐标系下，获取最终的全局地图；Use the fused pose T4 obtained in 6 ) to convert the point cloud data of the lidar to the world coordinate system to obtain the final global map; 记点云中某个点在雷达坐标系下的坐标为P1，转换到世界坐标系下的坐标为P2；Note that the coordinate of a point in the point cloud in the radar coordinate system is P 1 , and the coordinate converted to the world coordinate system is P 2 ; P2＝T4*P1。P 2 =T 4 *P 1 .
CN201811306455.0A 2018-11-05 2018-11-05 SLAM method combining GPS and radar odometer Active CN109507677B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811306455.0A CN109507677B (en) 2018-11-05 2018-11-05 SLAM method combining GPS and radar odometer

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811306455.0A CN109507677B (en) 2018-11-05 2018-11-05 SLAM method combining GPS and radar odometer

Publications (2)

Publication Number Publication Date
CN109507677A true CN109507677A (en) 2019-03-22
CN109507677B CN109507677B (en) 2020-08-18

Family

ID=65747565

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811306455.0A Active CN109507677B (en) 2018-11-05 2018-11-05 SLAM method combining GPS and radar odometer

Country Status (1)

Country Link
CN (1) CN109507677B (en)

Cited By (28)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110163968A (en) * 2019-05-28 2019-08-23 山东大学 RGBD camera large-scale three dimensional scenario building method and system
CN110211228A (en) * 2019-04-30 2019-09-06 北京云迹科技有限公司 For building the data processing method and device of figure
CN110261870A (en) * 2019-04-15 2019-09-20 浙江工业大学 It is a kind of to synchronize positioning for vision-inertia-laser fusion and build drawing method
CN110262504A (en) * 2019-07-02 2019-09-20 吉林大学 A kind of adjustable multilasered optical radar coupled system of structure and its control method
CN110411464A (en) * 2019-07-12 2019-11-05 中南大学 Three-dimensional point cloud map generation method, device, equipment and storage medium
CN110570449A (en) * 2019-09-16 2019-12-13 电子科技大学 A positioning and mapping method based on millimeter-wave radar and visual SLAM
CN110673115A (en) * 2019-09-25 2020-01-10 杭州飞步科技有限公司 Combined calibration method, device, equipment and medium for radar and integrated navigation system
CN110849374A (en) * 2019-12-03 2020-02-28 中南大学 Underground environment positioning method, device, equipment and storage medium
CN110906941A (en) * 2019-12-03 2020-03-24 苏州智加科技有限公司 Construction method and system of automatic driving map for long-distance tunnel
CN110988949A (en) * 2019-12-02 2020-04-10 北京京东乾石科技有限公司 Positioning method, positioning device, computer readable storage medium and mobile device
CN111080682A (en) * 2019-12-05 2020-04-28 北京京东乾石科技有限公司 Point cloud data registration method and device
CN111272181A (en) * 2020-02-06 2020-06-12 北京京东乾石科技有限公司 Method, device, equipment and computer readable medium for constructing map
CN111308490A (en) * 2020-02-05 2020-06-19 浙江工业大学 Indoor positioning and navigation system of self-balancing vehicle based on single-line lidar
CN111812668A (en) * 2020-07-16 2020-10-23 南京航空航天大学 Machine winding inspection device, positioning method and storage medium thereof
CN111829515A (en) * 2020-07-09 2020-10-27 新石器慧通（北京）科技有限公司 Time synchronization method, device, vehicle and storage medium
CN111862215A (en) * 2020-07-29 2020-10-30 上海高仙自动化科技发展有限公司 Computer equipment positioning method and device, computer equipment and storage medium
CN112034438A (en) * 2020-08-27 2020-12-04 江苏智能网联汽车创新中心有限公司 Radar calibration method and device, electronic equipment and storage medium
CN112241016A (en) * 2019-07-19 2021-01-19 北京初速度科技有限公司 Method and device for determining geographic coordinates of parking map
CN112578362A (en) * 2020-12-30 2021-03-30 成都圭目机器人有限公司 Three-dimensional ground penetrating radar data positioning method
WO2021128297A1 (en) * 2019-12-27 2021-07-01 深圳市大疆创新科技有限公司 Method, system and device for constructing three-dimensional point cloud map
CN113296512A (en) * 2021-05-24 2021-08-24 福建盛海智能科技有限公司 Unmanned tracking driving method and terminal based on laser radar and GPS
CN113343061A (en) * 2021-06-24 2021-09-03 广州高新兴机器人有限公司 Dynamic alignment method for coordinate system in GPS laser fusion SLAM
CN113759390A (en) * 2021-07-27 2021-12-07 华能伊敏煤电有限责任公司 Application method of virtual lidar technology based on autonomous mining trucks
CN113917547A (en) * 2021-10-08 2022-01-11 深圳安德空间技术有限公司 Ground penetrating radar underground hidden danger positioning method and system based on fusion positioning
CN114187359A (en) * 2021-12-13 2022-03-15 哈尔滨工业大学芜湖机器人产业技术研究院 LiDAR fixed pose calibration method and system based on pose incremental constraints
CN114838726A (en) * 2022-04-20 2022-08-02 哈尔滨理工大学 A GPS Data Correction Algorithm Based on Multi-sensor Data Fusion
CN116559928A (en) * 2023-07-11 2023-08-08 新石器慧通（北京）科技有限公司 Pose information determining method, device and equipment of laser radar and storage medium
CN118781817A (en) * 2024-09-06 2024-10-15 杭州交创科技发展有限公司 Three-in-one digital data acquisition and processing equipment

Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103093459A (en) * 2013-01-06 2013-05-08 中国人民解放军信息工程大学 Assisting image matching method by means of airborne lidar point cloud data
US20140028805A1 (en) * 2011-04-15 2014-01-30 Faro Technologies, Inc. System and method of acquiring three-dimensional coordinates using multiple coordinate measurment devices
CN105492985A (en) * 2014-09-05 2016-04-13 深圳市大疆创新科技有限公司 Multi-sensor environment map building
CN105856243A (en) * 2016-06-28 2016-08-17 湖南科瑞特科技股份有限公司 Movable intelligent robot
CN106052691A (en) * 2016-05-17 2016-10-26 武汉大学 Closed ring error correction method in laser ranging mobile drawing
CN106199626A (en) * 2016-06-30 2016-12-07 上海交通大学 Based on the indoor three-dimensional point cloud map generation system and the method that swing laser radar
WO2017132539A1 (en) * 2016-01-29 2017-08-03 Motion Engine Inc. System and method for determining the position of sensor elements in a sensor array
CN107091648A (en) * 2017-05-11 2017-08-25 江苏保千里视像科技集团股份有限公司 A kind of data fusion method and system of laser radar and differential GPS
CN107179086A (en) * 2017-05-24 2017-09-19 北京数字绿土科技有限公司 A kind of drafting method based on laser radar, apparatus and system
CN107577646A (en) * 2017-08-23 2018-01-12 上海莫斐信息技术有限公司 A kind of high-precision track operation method and system
CN107709928A (en) * 2015-04-10 2018-02-16 欧洲原子能共同体由欧洲委员会代表 For building figure and the method and apparatus of positioning in real time
CN107991680A (en) * 2017-11-21 2018-05-04 南京航空航天大学 SLAM methods based on laser radar under dynamic environment
CN108225302A (en) * 2017-12-27 2018-06-29 中国矿业大学 A kind of petrochemical factory's crusing robot alignment system and method
CN108303710A (en) * 2018-06-12 2018-07-20 江苏中科院智能科学技术应用研究院 Drawing method is built in the more scene positioning of unmanned plane based on three-dimensional laser radar

Patent Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20140028805A1 (en) * 2011-04-15 2014-01-30 Faro Technologies, Inc. System and method of acquiring three-dimensional coordinates using multiple coordinate measurment devices
CN103093459A (en) * 2013-01-06 2013-05-08 中国人民解放军信息工程大学 Assisting image matching method by means of airborne lidar point cloud data
CN105492985A (en) * 2014-09-05 2016-04-13 深圳市大疆创新科技有限公司 Multi-sensor environment map building
CN107709928A (en) * 2015-04-10 2018-02-16 欧洲原子能共同体由欧洲委员会代表 For building figure and the method and apparatus of positioning in real time
WO2017132539A1 (en) * 2016-01-29 2017-08-03 Motion Engine Inc. System and method for determining the position of sensor elements in a sensor array
CN106052691A (en) * 2016-05-17 2016-10-26 武汉大学 Closed ring error correction method in laser ranging mobile drawing
CN105856243A (en) * 2016-06-28 2016-08-17 湖南科瑞特科技股份有限公司 Movable intelligent robot
CN106199626A (en) * 2016-06-30 2016-12-07 上海交通大学 Based on the indoor three-dimensional point cloud map generation system and the method that swing laser radar
CN107091648A (en) * 2017-05-11 2017-08-25 江苏保千里视像科技集团股份有限公司 A kind of data fusion method and system of laser radar and differential GPS
CN107179086A (en) * 2017-05-24 2017-09-19 北京数字绿土科技有限公司 A kind of drafting method based on laser radar, apparatus and system
CN107577646A (en) * 2017-08-23 2018-01-12 上海莫斐信息技术有限公司 A kind of high-precision track operation method and system
CN107991680A (en) * 2017-11-21 2018-05-04 南京航空航天大学 SLAM methods based on laser radar under dynamic environment
CN108225302A (en) * 2017-12-27 2018-06-29 中国矿业大学 A kind of petrochemical factory's crusing robot alignment system and method
CN108303710A (en) * 2018-06-12 2018-07-20 江苏中科院智能科学技术应用研究院 Drawing method is built in the more scene positioning of unmanned plane based on three-dimensional laser radar

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
张剑华，王燕燕: "单目同时定位与建图中的地图恢复融合技术", 《中国图像图形学报》 *
李鑫: "基于激光雷达的同时定位与建图方法研究", 《中国优秀硕士学位论文全文数据库信息科技辑》 *
韩明瑞: "基于激光LIDAR的室外移动机器人三维定位与建图", 《中国优秀硕士学位论文全文数据库信息科技辑》 *

Cited By (47)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110261870A (en) * 2019-04-15 2019-09-20 浙江工业大学 It is a kind of to synchronize positioning for vision-inertia-laser fusion and build drawing method
CN110211228A (en) * 2019-04-30 2019-09-06 北京云迹科技有限公司 For building the data processing method and device of figure
CN110163968B (en) * 2019-05-28 2020-08-25 山东大学 RGBD camera large three-dimensional scene construction method and system
CN110163968A (en) * 2019-05-28 2019-08-23 山东大学 RGBD camera large-scale three dimensional scenario building method and system
CN110262504B (en) * 2019-07-02 2021-06-01 吉林大学 Multi-laser radar coupling system with adjustable structure and control method thereof
CN110262504A (en) * 2019-07-02 2019-09-20 吉林大学 A kind of adjustable multilasered optical radar coupled system of structure and its control method
CN110411464B (en) * 2019-07-12 2023-04-07 中南大学 Three-dimensional point cloud map generation method, device, equipment and storage medium
CN110411464A (en) * 2019-07-12 2019-11-05 中南大学 Three-dimensional point cloud map generation method, device, equipment and storage medium
CN112241016A (en) * 2019-07-19 2021-01-19 北京初速度科技有限公司 Method and device for determining geographic coordinates of parking map
CN110570449A (en) * 2019-09-16 2019-12-13 电子科技大学 A positioning and mapping method based on millimeter-wave radar and visual SLAM
CN110673115A (en) * 2019-09-25 2020-01-10 杭州飞步科技有限公司 Combined calibration method, device, equipment and medium for radar and integrated navigation system
CN110988949A (en) * 2019-12-02 2020-04-10 北京京东乾石科技有限公司 Positioning method, positioning device, computer readable storage medium and mobile device
CN110849374A (en) * 2019-12-03 2020-02-28 中南大学 Underground environment positioning method, device, equipment and storage medium
CN110906941A (en) * 2019-12-03 2020-03-24 苏州智加科技有限公司 Construction method and system of automatic driving map for long-distance tunnel
CN110906941B (en) * 2019-12-03 2023-06-23 苏州智加科技有限公司 Automatic driving map construction method and system for long-distance tunnel
CN110849374B (en) * 2019-12-03 2023-04-18 中南大学 Underground environment positioning method, device, equipment and storage medium
CN111080682A (en) * 2019-12-05 2020-04-28 北京京东乾石科技有限公司 Point cloud data registration method and device
CN111080682B (en) * 2019-12-05 2023-09-01 北京京东乾石科技有限公司 Registration method and device for point cloud data
CN113424232B (en) * 2019-12-27 2024-03-15 深圳市大疆创新科技有限公司 Three-dimensional point cloud map construction method, system and equipment
CN113424232A (en) * 2019-12-27 2021-09-21 深圳市大疆创新科技有限公司 Three-dimensional point cloud map construction method, system and equipment
WO2021128297A1 (en) * 2019-12-27 2021-07-01 深圳市大疆创新科技有限公司 Method, system and device for constructing three-dimensional point cloud map
CN111308490A (en) * 2020-02-05 2020-06-19 浙江工业大学 Indoor positioning and navigation system of self-balancing vehicle based on single-line lidar
CN111308490B (en) * 2020-02-05 2021-11-19 浙江工业大学 Balance car indoor positioning and navigation system based on single-line laser radar
CN111272181A (en) * 2020-02-06 2020-06-12 北京京东乾石科技有限公司 Method, device, equipment and computer readable medium for constructing map
CN111272181B (en) * 2020-02-06 2022-04-26 北京京东乾石科技有限公司 Method, device, equipment and computer readable medium for constructing map
CN111829515A (en) * 2020-07-09 2020-10-27 新石器慧通（北京）科技有限公司 Time synchronization method, device, vehicle and storage medium
CN111812668A (en) * 2020-07-16 2020-10-23 南京航空航天大学 Machine winding inspection device, positioning method and storage medium thereof
CN111862215A (en) * 2020-07-29 2020-10-30 上海高仙自动化科技发展有限公司 Computer equipment positioning method and device, computer equipment and storage medium
CN111862215B (en) * 2020-07-29 2023-10-03 上海高仙自动化科技发展有限公司 Computer equipment positioning method and device, computer equipment and storage medium
CN112034438A (en) * 2020-08-27 2020-12-04 江苏智能网联汽车创新中心有限公司 Radar calibration method and device, electronic equipment and storage medium
CN112034438B (en) * 2020-08-27 2024-04-09 江苏智能网联汽车创新中心有限公司 Radar calibration method and device, electronic equipment and storage medium
CN112578362A (en) * 2020-12-30 2021-03-30 成都圭目机器人有限公司 Three-dimensional ground penetrating radar data positioning method
CN112578362B (en) * 2020-12-30 2023-08-29 成都圭目机器人有限公司 Three-dimensional ground penetrating radar data positioning method
CN113296512B (en) * 2021-05-24 2022-10-04 福建盛海智能科技有限公司 Unmanned tracking driving method and terminal based on laser radar and GPS
CN113296512A (en) * 2021-05-24 2021-08-24 福建盛海智能科技有限公司 Unmanned tracking driving method and terminal based on laser radar and GPS
CN113343061A (en) * 2021-06-24 2021-09-03 广州高新兴机器人有限公司 Dynamic alignment method for coordinate system in GPS laser fusion SLAM
CN113759390B (en) * 2021-07-27 2023-07-14 华能伊敏煤电有限责任公司 Application method of virtual lidar technology based on self-driving mining truck vehicle
CN113759390A (en) * 2021-07-27 2021-12-07 华能伊敏煤电有限责任公司 Application method of virtual lidar technology based on autonomous mining trucks
CN113917547A (en) * 2021-10-08 2022-01-11 深圳安德空间技术有限公司 Ground penetrating radar underground hidden danger positioning method and system based on fusion positioning
CN114187359A (en) * 2021-12-13 2022-03-15 哈尔滨工业大学芜湖机器人产业技术研究院 LiDAR fixed pose calibration method and system based on pose incremental constraints
CN114187359B (en) * 2021-12-13 2024-11-15 长三角哈特机器人产业技术研究院 LiDAR fixed pose calibration method and system based on pose increment constraint
CN114838726A (en) * 2022-04-20 2022-08-02 哈尔滨理工大学 A GPS Data Correction Algorithm Based on Multi-sensor Data Fusion
CN114838726B (en) * 2022-04-20 2024-07-30 哈尔滨理工大学 GPS data correction method based on multi-sensor data fusion
CN116559928A (en) * 2023-07-11 2023-08-08 新石器慧通（北京）科技有限公司 Pose information determining method, device and equipment of laser radar and storage medium
CN116559928B (en) * 2023-07-11 2023-09-22 新石器慧通（北京）科技有限公司 Pose information determining method, device and equipment of laser radar and storage medium
CN118781817A (en) * 2024-09-06 2024-10-15 杭州交创科技发展有限公司 Three-in-one digital data acquisition and processing equipment
CN118781817B (en) * 2024-09-06 2024-12-24 杭州交创科技发展有限公司 Three-in-one digital data acquisition and processing equipment

Also Published As

Publication number Publication date
CN109507677B (en) 2020-08-18

Similar Documents

Publication Publication Date Title
CN109507677A (en) A kind of SLAM method of combination GPS and radar odometer
CN111880207B (en) Visual inertial satellite tight coupling positioning method based on wavelet neural network
CN113376669B (en) A monocular VIO-GNSS fusion positioning algorithm based on point-line features
CN110411462A (en) A GNSS/inertial/lane line constraint/odometer multi-source fusion method
Wang et al. Vehicle localization at an intersection using a traffic light map
CN110471422A (en) The detection of obstacles and automatic obstacle avoiding method of intelligent wheel chair
CN107728175A (en) The automatic driving vehicle navigation and positioning accuracy antidote merged based on GNSS and VO
CN109901207A (en) A high-precision outdoor positioning method combining Beidou satellite system and image features
CN113269878B (en) Multi-sensor-based mapping method and system
CN105241445A (en) Method and system for acquiring indoor navigation data based on intelligent mobile terminal
CN103149580A (en) Global position system (GPS)/inertial navigation system (INS) combined navigation method based on strong tracking kalman filter (STKF) and wavelet neural network (WNN)
CN105761242A (en) Blind person walking positioning method based on computer binocular vision and inertial measurement
Wen et al. TM 3 Loc: Tightly-coupled monocular map matching for high precision vehicle localization
CN114459471A (en) Positioning information determination method and device, electronic equipment and storage medium
CN115564865A (en) Construction method and system of crowdsourcing high-precision map, electronic equipment and vehicle
Choi et al. Localization using GPS and VISION aided INS with an image database and a network of a ground-based reference station in outdoor environments
CN115291227B (en) Indoor and outdoor seamless positioning and 3D mapping method and system
CN115031744A (en) A cognitive map positioning method and system based on sparse point cloud-texture information
CN113358117B (en) Visual inertial indoor positioning method using map
CN111025364B (en) A satellite-assisted machine vision positioning system and method
CN105806350A (en) Positioning method and device based on pseudorange and linear vector
CN115409910A (en) Semantic map construction method, visual positioning method and related equipment
CN118258378A (en) Multi-mode high-precision robust pose estimation method and system fused with GNSS signals
CN116697981A (en) Method and system for extracting elevation information of automobile front based on multi-sensor fusion
Xie et al. Automatic docking optimization method of bed and chair based on multi-sensor information fusion

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