CN113740875A - 一种基于激光里程计和点云描述符匹配的自动驾驶车辆定位方法 - Google Patents
一种基于激光里程计和点云描述符匹配的自动驾驶车辆定位方法 Download PDFInfo
- Publication number
- CN113740875A CN113740875A CN202110886022.2A CN202110886022A CN113740875A CN 113740875 A CN113740875 A CN 113740875A CN 202110886022 A CN202110886022 A CN 202110886022A CN 113740875 A CN113740875 A CN 113740875A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- laser
- descriptor
- positioning
- matching
- 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 61
- 230000004927 fusion Effects 0.000 claims abstract description 23
- 239000011159 matrix material Substances 0.000 claims description 56
- 230000004807 localization Effects 0.000 claims description 21
- 238000004364 calculation method Methods 0.000 claims description 14
- 230000033001 locomotion Effects 0.000 claims description 14
- 238000013519 translation Methods 0.000 claims description 14
- 230000008569 process Effects 0.000 claims description 10
- 238000012545 processing Methods 0.000 claims description 9
- 230000008859 change Effects 0.000 claims description 7
- 238000005516 engineering process Methods 0.000 claims description 7
- 230000009466 transformation Effects 0.000 claims description 5
- 238000000546 chi-square test Methods 0.000 claims description 4
- 238000005070 sampling Methods 0.000 claims description 4
- FGUUSXIOTUKUDN-IBGZPJMESA-N C1(=CC=CC=C1)N1C2=C(NC([C@H](C1)NC=1OC(=NN=1)C1=CC=CC=C1)=O)C=CC=C2 Chemical compound C1(=CC=CC=C1)N1C2=C(NC([C@H](C1)NC=1OC(=NN=1)C1=CC=CC=C1)=O)C=CC=C2 FGUUSXIOTUKUDN-IBGZPJMESA-N 0.000 claims description 3
- 238000001514 detection method Methods 0.000 claims description 3
- 238000007781 pre-processing Methods 0.000 claims description 3
- 230000007613 environmental effect Effects 0.000 description 5
- 238000013507 mapping Methods 0.000 description 3
- 238000010276 construction Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 238000000844 transformation Methods 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Images
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/88—Lidar systems specially adapted for specific applications
- G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
Landscapes
- Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Length Measuring Devices By Optical Means (AREA)
Abstract
本发明涉及一种基于激光里程计和点云描述符匹配的自动驾驶车辆定位方法。本发明设计了一个基于激光里程计定位和点云描述符匹配定位的融合定位方法，它将实时的里程计定位结果和全局匹配定位结果相融合，以提高匹配定位对环境的适应性。本发明解决了匹配信息位姿区分度低和环境发生变化时，自动驾驶车辆匹配定位精度降低的问题，相比于其他基于先验信息匹配的定位方法，本发明将实时的激光里程计定位结果和基于点云描述符匹配定位结果相融合，在环境信息发生较大变化时依然能保持较高定位精度。
Description
技术领域
本发明涉及自动驾驶汽车自主定位技术领域，具体涉及一种基于激光里程计和点云描述符匹配的自动驾驶车辆定位方法。
背景技术
随着信息技术的发展，汽车逐渐走向智能化、无人化，自动驾驶是汽车智能化的一个重要特征。车辆的定位是自动驾驶系统关键技术之一，定位对于车辆行驶安全的重要性不言而喻，对于自动驾驶汽车而言，精确的定位结果是安全、合理的路径规划和运动规划的前提。目前大多数自动驾驶定位方案都采用基于先验信息的匹配定位，然而如果当前信息和数据库中多个位置信息相似度较高或者环境信息发生较大变化时，车辆无法准确将当前信息和数据库中的历史信息进行匹配从而导致定位精度降低，甚至定位失效。
发明内容
为了解决现有技术问题，本发明的目的在于克服已有技术存在的不足，提供一种基于激光里程计和点云描述符匹配的自动驾驶车辆定位方法，解决自动驾驶车辆在先验环境发生较大变化等情况下定位精度降低或定位失效的问题，提高自动驾驶车辆定位精度和系统鲁棒性。
为达到上述发明创造目的，本发明采用如下技术方案：
一种基于激光里程计定位和点云描述符匹配的自动驾驶车辆定位方法，操作步骤如下：
a.安装设备与激光点云数据采集：
将三维激光雷达安装在车辆顶部，在车辆运动过程中通过三维激光雷达扫描周围环境，将获取到的激光点云传输到计算机存储后进行处理；
b.通过同时定位与地图构建(Simultaneous localization and mapping,SLAM)技术获取匹配定位的先验信息：
在构建先验地图阶段通过SLAM在获取车辆位姿的同时记录周围环境的先验信息；
c.构建点云描述符数据库：
在通过步骤b获取到环境的三维点云后，离线计算在每一个位姿获取到的激光点云的点云描述符；
d.基于点云描述符匹配的车辆位姿计算：
在定位阶段，实时计算三维激光雷达当前帧点云的描述符，通过和步骤c中构建的描述符数据库匹配，计算描述符相似性，对车辆运动位姿进行估算；
e.基于激光里程计的车辆位姿计算：
在定位阶段，通过激光里程计计算车辆两个相邻位姿的相对变化，估计车辆当前的位姿；
f.基于激光里程计定位和点云描述符匹配定位的融合位姿计算：
将激光里程计定位结果和点云描述符匹配定位结果融合，最终实现车辆的实时定位。
优选地，在所述的步骤a中，三维激光雷达安装在车辆的顶部，三维激光雷达将采集到的三维点云传输到车载计算机中，同时计算机和自动驾驶车辆通过机器人操作系统(Robot Operating System)进行数据传输和指令发送，以此获得三维激光点云数据输入。
优选地，在所述的步骤b中，并不局限于某一种具体的SLAM方案，同时还能在空旷环境中使用GPS，在系统中加入回环检测方法来提高构建的先验地图的精度。
优选地，在所述的步骤c中，首先对采集到的点云进行去除运动畸变处理，然后对点云进行下采样预处理，以减少激光点数量，降低计算复杂度；然后计算每一帧点云的描述符，点云描述符生成的具体方法分为如下步骤：
首先，将激光雷达扫描的每帧激光点云的空间进行划分；将三维空间沿径向将激光的测距范围Rmax划分为Nr个区间，沿着激光雷达的旋转方向将整个圆周划分为Nc个区间，则整个空间被划分为Nr×Nc个区间，其中每一个区间为一个柱状空间Vij，将Vij在高度范围Z均匀划分为8个区间，即
其中i∈(0,Nr),j∈(0,Nc)。
优选地，在圆柱坐标系中，将划分的空间
表示出来，其中k表示第k个区间。公式表示如下：
式中(ρ,α,z)为空间中的任意一个激光点在圆柱坐标系下的表示，ρ表示该激光点极坐标下的极径，α表示该激光点极坐标下的极角，z表示该激光点的高度值，i，j，p分别表示径向，轴向，高度方向的第i，j，p个区间，Rmax表示设定的径向范围，Z表示高度范围。
优选地，确定了
以后，将下采样后的激光点依次划分到所属的
中；用二进制的方法来编码空间中的结构信息指的是：
设
为
中所有点的集合，如果
则
否则
在具体的实施中，由于噪声点的不可避免，并不严格的在空间中没有点时才将集合视为空集，而是当空间中的激光点少于三个时，将集合
视为空集。由此，就得到了若干个8位二进制数编码
每个二进制编码描述了对应柱状空间内的点云结构信息。二进制编码
所表示的十进制数为
矩阵E为描述符的矩阵表示，eij为矩阵E的元素。将沿着点云空间半径方向划分的每一个环，用矩阵的一行表示，每个扇形区间用矩阵的一个列表示，则
并且eij∈[0，255]，这个矩阵即为点云描述符。在构建先验地图阶段，将每个位姿获取到的激光点云构建为点云描述符，并生成包含所有先验点云描述符的数据库。
优选地，在所述的步骤d中，匹配定位过程就是建立当前位置描述符和历史描述符位置的关联；匹配过程的输入是三维激光雷达实时采集的激光点云，在接收到点云之后，首先计算该帧点云的描述符，再和预先构建的描述符数据库进行匹配；具体匹配步骤为：
首先，在全局描述符数据库中进行搜索，利用描述符数据分布直方图找到小于设定相似度阈值的匹配候选帧；设当前激光帧和历史数据帧分别为：p和q，则这两帧点云的描述符数据分布直方图分别为：Hp，Hq；使用卡方检验来评价两个直方图的相似性；两个点云描述符相似性计算公式为：
式中
和
分别为直方图Hp，Hq中的第k组，m表示直方图区间总数；
进一步地，通过计算描述符矩阵间的距离对候选帧进行确认，找到最终的匹配帧；由于描述符矩阵的每一个元素本质都是一组八位二进制代码，用汉明距离来表示两帧点云的描述符矩阵之间的距离；依旧用p和q来表示当前帧激光点云和历史数据帧，对应的描述符矩阵分别为：Ep，Eq。这两个描述符矩阵之间的距离为：
最终通过和描述符数据库中的描述符匹配，得到车辆匹配定位位姿，i表示描述符矩阵的第i行，j表示描述符矩阵的第j列，p表示当前第p帧激光点云，q表示历史第q帧激光点，
表示第p帧激光点云描述符矩阵的第i行，第j列元素为汉明距离与第q帧激光点云描述符矩阵的第i行，第j列元素之间的汉明距离。
优选地，在所述的步骤e中，通过两个位姿激光点云数据的配准，求得这两个位姿的旋转平移矩阵(R,T)，用Xk来表示车辆在k时刻的位姿，则：
Xk＝R·Xk-1+T
通过连续位姿之间的相对变换，通过激光里程计得到车辆位姿估计，R表示旋转矩阵，T表示平移矩阵。
优选地，在所述的步骤f中，设
为车辆在空间中的六自由度状态；
分别为车辆在空间中的俯仰角、横滚角以及航向角，本发明中车辆状态角的分析中仅考虑航向角；(Tx,Ty,Tz)为车辆当前位置沿着各个轴相对于全局坐标系原点的平移量；最终车辆在空间中的状态为：
用一个变换矩阵来表示车辆在空间中的状态：
T＝[R,t]
其中,R为航向角在空间中对应的标准旋转矩阵，表示公式为：
在步骤e的基于里程计的定位中，将两个连续位姿，第一个位姿称为起始位置，第二个位姿称为终点位置；起始位置的状态为：
其旋转平移矩阵为：Ts＝[Rs,ts]；同样的，终点位置状态为：
旋转平移矩阵：Te＝[Re,te]；设To为起点与终点之间的状态转移矩阵，则状态转移矩阵满足如下公式关系：
TsTo＝Te
To可由下式求解得到：
To＝((Ts)TTs)-1(Ts)TTe
步骤d中基于匹配的定位是在全局坐标系下进行匹配的，可直接得到匹配定位结果的旋转平移矩阵：TM＝[RM,tM]，M表示该矩阵为匹配定位得到的平移矩阵；则最终输出的基于里程计的定位与匹配定位的融合定位的结果：
T＝TMTo
最终融合的定位结果作为这个周期融合定位结果的输出，同时又作为下一个周期激光里程计定位的起点，既能高频输出定位结果，又能周期性的不断校正累积误差，实现高精度实时定位。
本发明与现有技术相比较，具有如下显而易见的突出实质性特点和显著优点：
1.本发明解决了环境发生变化时，自动驾驶车辆匹配定位精度降低的问题，相比于其他基于先验信息匹配的定位方法，本发明将实时的激光里程计定位结果和基于点云描述符匹配定位结果相融合，在环境信息发生较大变化时依然能保持较高定位精度；
2.本发明仅使用三维激光雷达即可实现自动驾驶车辆的高精度定位，相对其他定位方案，设备成本低且操作简单易上手；
3.本发明方法简单易行，成本低，适合推广使用。
附图说明
图1为本发明的基于激光里程计定位和点云描述符匹配定位融合的定位方法操作步骤流程图。
图2为本发明的点云描述符构建方法示意图。
图3为本发明的点云描述符的匹配流程图。
图4为本发明的激光里程计定位结果和点云描述符匹配定位结果融合示意图。
具体实施方式
以下就优选实施例结合附图对上述方案做进一步说明，本发明的优选实施例详述如下：
实施例一：
参见图1，一种基于激光里程计定位和点云描述符匹配定位的融合定位方法，操作步骤如下：
a.安装设备与激光点云数据采集：
将三维激光雷达安装在车辆顶部，在车辆运动过程中通过三维激光雷达扫描周围环境，将获取到的激光点云传输到计算机存储后进行处理；
b.通过同时定位与地图构建SLAM技术获取匹配定位的先验信息：
在构建先验地图阶段通过SLAM在获取车辆位姿的同时记录周围环境的先验信息；
c.构建点云描述符数据库：
在通过步骤b获取到环境的三维点云后，离线计算在每一个位姿获取到的激光点云的点云描述符；
d.基于点云描述符匹配的车辆位姿计算：
在定位阶段，实时计算三维激光雷达当前帧点云的描述符，通过和步骤c中构建的描述符数据库匹配，计算描述符相似性，对车辆运动位姿进行估算；
e.基于激光里程计的车辆位姿计算：
在定位阶段，通过激光里程计计算车辆两个相邻位姿的相对变化，估计车辆当前的位姿；
f.基于激光里程计定位和点云描述符匹配定位的融合位姿计算：
将激光里程计定位结果和点云描述符匹配定位结果融合，最终实现车辆的实时定位。
本实施例基于激光里程计和点云描述符匹配的自动驾驶车辆定位方法，解决自动驾驶车辆在先验环境发生较大变化等情况下定位精度降低或定位失效的问题，提高自动驾驶车辆定位精度和系统鲁棒性。
实施例二：
本实施例与实施例一基本相同，特别之处如下：
一种基于激光里程计定位和点云描述符匹配定位的融合定位方法，操作步骤如下：
在步骤a.中，安装设备与激光点云数据采集：将三维激光雷达安装在车辆顶部，在车辆运动过程中通过三维激光雷达扫描周围环境，将获取到的激光点云传输到计算机存储后进行处理；
在所述步骤b中，并不局限于某一种具体的SLAM方案，同时还可在空旷环境中使用GPS，在系统中加入回环检测方法来提高构建的先验地图的精度。
在所述步骤c中，首先对采集到的点云进行去除运动畸变处理，然后对点云进行下采样预处理，以减少激光点数量，降低计算复杂度；然后计算每一帧点云的描述符，点云描述符生成的具体方法分为如下步骤：
首先将激光雷达扫描的每帧激光点云的空间进行划分；将三维空间沿径向将激光的测距范围Rmax划分为Nr个区间，沿着激光雷达的旋转方向将整个圆周划分为Nc个区间，则整个空间被划分为Nr×Nc个区间，其中每一个区间为一个柱状空间Vij，将Vij在高度范围Z均匀划分为8个区间，即
其中i∈(0,Nr),j∈(0,Nc)；其中k表示第k个区间。
在圆柱坐标系中，将划分的空间
表示出来，公式表示如下：
式中(ρ,α,z)为空间中的任意一个激光点在圆柱坐标系下的表示，ρ表示该激光点极坐标下的极径，α表示该激光点极坐标下的极角，z表示该激光点的高度值，i，j，p分别表示径向，轴向，高度方向的第i，j，p个区间，Rmax表示设定的径向范围，Z表示高度范围。
确定了
以后，将下采样后的激光点依次划分到所属的
中；用二进制的方法来编码空间中的结构信息指的是：设
为
中所有点的集合，如果
则
否则
在具体的实施中，由于噪声点的不可避免，并不严格的在空间中没有点时才将集合视为空集，而是当空间中的激光点少于三个时，将集合
视为空集；由此，就得到了若干个8位二进制数编码
每个二进制编码描述了对应柱状空间内的点云结构信息；二进制编码
所表示的十进制数为
矩阵E为描述符的矩阵表示，eij为矩阵E的元素；将沿着点云空间半径方向划分的每一个环，用矩阵的一行表示，每个扇形区间用矩阵的一个列表示，则
并且eij∈[0，255]，这个矩阵即为点云描述符；在构建先验地图阶段，将每个位姿获取到的激光点云构建为点云描述符，并生成包含所有先验点云描述符的数据库。
在所述步骤d中，在匹配定位阶段，匹配过程的输入是三维激光雷达实时采集的激光点云，在接收到点云之后，首先计算该帧点云的描述符，再和预先构建的描述符数据库进行匹配；具体匹配步骤为：
首先，在全局描述符数据库中进行搜索，利用描述符数据分布直方图找到小于设定相似度阈值的匹配候选帧；设当前激光帧和历史数据帧分别为：p和q，则这两帧点云的描述符数据分布直方图分别为：Hp，Hq。使用卡方检验来评价两个直方图的相似性；两个点云描述符相似性计算公式为：
式中
和
分别为直方图Hp，Hq中的第k组，m表示直方图区间总数；
进一步的通过计算描述符矩阵间的距离对候选帧进行确认，找到最终的匹配帧；由于描述符矩阵的每一个元素本质都是一组八位二进制代码，所以用汉明距离来表示两帧点云的描述符矩阵之间的距离；依旧用p和q来表示当前帧激光点云和历史数据帧，它们对应的描述符矩阵分别为：Ep，Eq。这两个描述符矩阵之间的距离为：
最终通过和描述符数据库中的描述符匹配，得到车辆匹配定位位姿，i表示描述符矩阵的第i行，j表示描述符矩阵的第j列，p表示当前第p帧激光点云，q表示历史第q帧激光点，
表示第p帧激光点云描述符矩阵的第i行，第j列元素为汉明距离与第q帧激光点云描述符矩阵的第i行，第j列元素之间的汉明距离。
在所述步骤e中，通过两个位姿激光点云数据的配准，求得这两个位姿的旋转平移矩阵(R,T)，用Xk来表示车辆在k时刻的位姿，则：
Xk＝R·Xk-1+T
通过连续位姿之间的相对变换，通过激光里程计得到车辆位姿估计，其中R表示旋转矩阵，T表示平移矩阵。
所述步骤f中，将激光里程计定位得到的车辆位姿To和描述符匹配定位得到的车辆位姿Tm进行融合：
T＝TmTo
最终融合的定位结果作为这个周期融合定位结果的输出，同时又作为下一个周期激光里程计定位的起点，既能高频输出定位结果，又能周期性的不断校正累积误差，实现高精度实时定位。
本实施例方法解决了环境发生变化时，自动驾驶车辆匹配定位精度降低的问题，相比于其他基于先验信息匹配的定位方法，本发明将实时的激光里程计定位结果和基于点云描述符匹配定位结果相融合，在环境信息发生较大变化时依然能保持较高定位精度；本实施例方法仅使用三维激光雷达即可实现自动驾驶车辆的高精度定位，相对其他定位方案，设备成本低且操作简单易上手。
实施例三：
本实施例与实施例二基本相同，特别之处如下：
本实施例为具体实例，图1示意性地显示了本发明所述地基于激光里程计定位和点云描述符匹配定位的融合定位方法在一种实施方式下的步骤流程图。
在本实施例中，参见图1，一种基于激光里程计定位和点云描述符匹配定位的融合定位方法，操作步骤如下：
a.安装设备与激光点云数据采集：
将Velodyne VLP-16三维激光雷达安装在车辆的顶部，为了后续位姿计算方便，将激光雷达安装在车辆中轴线的位置上。Velodyne VLP-16三维激光扫描范围为100米，在30°的视角范围内有16条三维激光雷达，垂直方向的分辨率为2°，10Hz扫描频率下，水平分辨率为0.2°。将采集到的三维点云传输到车载计算机中，同时计算机和自动驾驶车辆通过机器人操作系统(Robot Operating System)进行数据传输和指令发送，以此获得三维激光点云数据输入。
b.通过同时定位与地图构建(Simultaneous localization and mapping,SLAM)技术获取匹配定位的先验信息：
使用IMLS-SLAM系统构建环境的先验点云描述，该方法实时性较低，但离线构建高精度点云先验地图。
c.构建点云描述符数据库：
在通过步骤b获取到环境的三维点云后，离线计算在每一个位姿获取到的激光点云的点云描述符；首先对采集到的点云进行去除运动畸变处理，然后对点云进行下采样预处理，以减少激光点数量，降低计算复杂度；然后计算每一帧点云的描述符，点云描述符生成的具体方法分为如图2所示。
首先将激光雷达扫描的每帧激光点云的空间进行划分。将三维空间沿径向将激光的测距范围Rmax＝80m划分为Nr＝40个区间，沿着激光雷达的旋转方向将整个圆周划分为Nc＝120个区间，则整个空间被划分为Nr×Nc＝4800个区间，其中每一个区间为一个柱状空间Vij，将Vij在高度范围Z均匀划分为8个区间，即
其中i∈(0,40),j∈(0,120)。
确定了
以后，将下采样后的激光点依次划分到所属的
中。用二进制的方法来编码空间中的结构信息指的是：设
为
中所有点的集合，如果
则
否则
在具体的实施中，由于噪声点的不可避免，并不严格的在空间中没有点时才将集合视为空集，而是当空间中的激光点少于三个时，将集合
视为空集。由此就得到了若干个8位二进制数编码
每个二进制编码描述了对应柱状空间内的点云结构信息。二进制编码
所表示的十进制数为
矩阵E为描述符的矩阵表示，eij为矩阵E的元素。将沿着点云空间半径方向划分的每一个环，用矩阵的一行表示，每个扇形区间用矩阵的一个列表示，则
并且eij∈[0，255]，这个矩阵即为点云描述符。将每个位姿获取到的激光点云构建为点云描述符，并以KD树的形式生成包含所有先验点云描述符的数据库。
d.基于点云描述符匹配的车辆位姿计算：
在匹配定位阶段，匹配过程的输入是三维激光雷达实时采集的激光点云，在接收到点云之后，首先计算该帧点云的描述符，再和预先构建的描述符数据库进行匹配，如图3所示，具体匹配步骤为：
首先在全局描述符数据库中进行搜索，利用描述符数据分布直方图找到小于设定相似度阈值的匹配候选帧。设当前激光帧和历史数据帧分别为：p和q，则这两帧点云的描述符数据分布直方图分别为：Hp，Hq。使用卡方检验来评价两个直方图的相似性。两个点云描述符相似性计算公式为：
式中
和
分别为直方图Hp，Hq中的第k组，m表示直方图区间总数。
进一步的通过计算描述符矩阵间的距离对候选帧进行确认，找到最终的匹配帧。由于描述符矩阵的每一个元素本质都是一组八位二进制代码，所以用汉明距离来表示两帧点云的描述符矩阵之间的距离。依旧用p和q来表示当前帧激光点云和历史数据帧，它们对应的描述符矩阵分别为：Ep，Eq。这两个描述符矩阵之间的距离为：
最终通过和描述符数据库中的描述符匹配，得到车辆匹配定位位姿。
e.基于激光里程计的车辆位姿计算：
在定位阶段，通过激光里程计计算车辆两个相邻位姿的相对变化，求得这两个位姿的旋转平移矩阵(R,T)，用Xk来表示车辆在k时刻的位姿，则：
Xk＝R·Xk-1+T
通过连续位姿之间的相对变换，通过激光里程计得到车辆位姿估计。
f.基于激光里程计定位和点云描述符匹配定位的融合位姿计算：
将激光里程计定位得到的车辆位姿To和描述符匹配定位得到的车辆位姿Tm进行融合，融合的流程如图4所示：
T＝TMTo
最终融合的定位结果作为这个周期融合定位结果的输出，同时又作为下一个周期激光里程计定位的起点，既能高频输出定位结果，又能周期性的不断校正累积误差，实现高精度实时定位。
本实施例基于激光里程计和点云描述符的定位方法，解决车辆在环境发生较大变化和匹配信息位姿区分度低等情况下定位精度低的问题，提高自动驾驶车辆定位精度和系统鲁棒性。
实施例四：
本实施例与上述实施例三基本相同，特别之处在于步骤b中，通过GPS定位信息，矫正SLAM系统的位姿图，以提供更高的先验地图构建精度，具体步骤为：
a.安装设备与激光点云数据采集：
将三维激光雷达安装在车辆顶部，在车辆运动过程中通过三维激光雷达扫描周围环境，将获取到的激光点云传输到计算机存储后进行处理；
b.通过同时定位与地图构建(Simultaneous localization and mapping,SLAM)技术获取匹配定位的先验信息：
在构建先验地图阶段通过SLAM的前端构建位姿图，并周期性的加入GPS定位信息，将GPS定位信息同样作为约束，为后端优化提供更多信息，得到更加精确的位姿估计以及先验点云地图；
c.构建点云描述符数据库：
在通过步骤b获取到环境的三维点云后，离线计算在每一个位姿获取到的激光点云的点云描述符；
d.基于点云描述符匹配的车辆位姿计算：
在定位阶段，实时计算三维激光雷达当前帧点云的描述符，通过和步骤c中构建的描述符数据库匹配，计算描述符相似性，对车辆运动位姿进行估算；
e.基于激光里程计的车辆位姿计算：
在定位阶段，通过激光里程计计算车辆两个相邻位姿的相对变化，估计车辆当前的位姿；
f.基于激光里程计定位和点云描述符匹配定位的融合位姿计算：
将激光里程计定位结果和点云描述符匹配定位结果融合，最终实现车辆的实时定位。
综上所述，上述实施例基于激光里程计和点云描述符匹配的自动驾驶车辆定位方法，将实时的里程计定位结果和全局匹配定位结果相融合，以提高匹配定位对环境的适应性。本发明解决了匹配信息位姿区分度低和环境发生变化时，自动驾驶车辆匹配定位精度降低的问题，相比于其他基于先验信息匹配的定位方法，上述实施例方法将实时的激光里程计定位结果和基于点云描述符匹配定位结果相融合，在环境信息发生较大变化时依然能保持较高定位精度。
上面对本发明实施例结合附图进行了说明，但本发明不限于上述实施例，还可以根据本发明的发明创造的目的做出多种变化，凡依据本发明技术方案的精神实质和原理下做的改变、修饰、替代、组合或简化，均应为等效的置换方式，只要符合本发明的发明目的，只要不背离本发明的技术原理和发明构思，都属于本发明的保护范围。
Claims (7)
1.一种基于激光里程计定位和点云描述符匹配定位的融合定位方法，其特征在于，操作步骤如下：
a.安装设备与激光点云数据采集：
将三维激光雷达安装在车辆顶部，在车辆运动过程中通过三维激光雷达扫描周围环境，将获取到的激光点云传输到计算机存储后进行处理；
b.通过同时定位与地图构建SLAM技术获取匹配定位的先验信息：
在构建先验地图阶段通过SLAM在获取车辆位姿的同时记录周围环境的先验信息；
c.构建点云描述符数据库：
在通过步骤b获取到环境的三维点云后，离线计算在每一个位姿获取到的激光点云的点云描述符；
d.基于点云描述符匹配的车辆位姿计算：
在定位阶段，实时计算三维激光雷达当前帧点云的描述符，通过和步骤c中构建的描述符数据库匹配，计算描述符相似性，对车辆运动位姿进行估算；
e.基于激光里程计的车辆位姿计算：
在定位阶段，通过激光里程计计算车辆两个相邻位姿的相对变化，估计车辆当前的位姿；
f.基于激光里程计定位和点云描述符匹配定位的融合位姿计算：
将激光里程计定位结果和点云描述符匹配定位结果融合，最终实现车辆的实时定位。
2.根据权利要求1所述的基于激光里程计定位和点云描述符匹配定位的融合定位方法，其特征在于：在所述步骤b中，并不局限于某一种具体的SLAM方案，同时还可在空旷环境中使用GPS，在系统中加入回环检测方法，来提高构建的先验地图的精度。
3.根据权利要求1所述的基于激光里程计定位和点云描述符匹配定位的融合定位方法，其特征在于：在所述步骤c中，首先对采集到的点云进行去除运动畸变处理，然后对点云进行下采样预处理，以减少激光点数量，降低计算复杂度；然后计算每一帧点云的描述符，点云描述符生成的具体方法分为如下步骤：
首先，将激光雷达扫描的每帧激光点云的空间进行划分；将三维空间沿径向将激光的测距范围Rmax划分为Nr个区间，沿着激光雷达的旋转方向将整个圆周划分为Nc个区间，则整个空间被划分为Nr×Nc个区间，其中每一个区间为一个柱状空间Vij，将Vij在高度范围Z均匀划分为8个区间，即
其中i∈(0,Nr),j∈(0,Nc)；其中k表示第k个区间。
确定了
以后，将下采样后的激光点依次划分到所属的
中，其中k表示第k个区间；用二进制的方法来编码空间中的结构信息指的是：
设
为
中所有点的集合，如果
则
否则
在具体的实施中，由于噪声点的不可避免，并不严格的在空间中没有点时才将集合视为空集，而是当空间中的激光点少于三个时，将集合
视为空集；由此，就得到了若干个8位二进制数编码
每个二进制编码描述了对应柱状空间内的点云结构信息；二进制编码
所表示的十进制数为
矩阵E为描述符的矩阵表示，eij为矩阵E的元素；将沿着点云空间半径方向划分的每一个环，用矩阵的一行表示，每个扇形区间用矩阵的一个列表示，则
并且eij∈[0，255]，这个矩阵即为点云描述符；在构建先验地图阶段，将每个位姿获取到的激光点云构建为点云描述符，并生成包含所有先验点云描述符的数据库。
4.根据权利要求3所述的基于激光里程计定位和点云描述符匹配定位的融合定位方法，其特征在于：在所述步骤c中，
在圆柱坐标系中，将划分的空间
表示出来，公式表示如下：
式中(ρ,α,z)为空间中的任意一个激光点在圆柱坐标系下的表示，ρ表示该激光点极坐标下的极径，α表示该激光点极坐标下的极角，z表示该激光点的高度值，i，j，p分别表示径向，轴向，高度方向的第i，j，p个区间，Rmax表示设定的径向范围，Z表示高度范围。
5.根据权利要求1所述的基于激光里程计定位和点云描述符匹配定位的融合定位方法，其特征在于：在所述步骤d中，在匹配定位阶段，匹配过程的输入是三维激光雷达实时采集的激光点云，在接收到点云之后，首先计算该帧点云的描述符，再和预先构建的描述符数据库进行匹配；具体匹配步骤为：
首先，在全局描述符数据库中进行搜索，利用描述符数据分布直方图找到小于设定相似度阈值的匹配候选帧；设当前激光帧和历史数据帧分别为：p和q，则这两帧点云的描述符数据分布直方图分别为：Hp，Hq；使用卡方检验来评价两个直方图的相似性；两个点云描述符相似性计算公式为：
式中
和
分别为直方图Hp，Hq中的第k组，m表示直方图区间总数；
通过计算描述符矩阵间的距离对候选帧进行确认，找到最终的匹配帧；由于描述符矩阵的每一个元素本质都是一组八位二进制代码，用汉明距离来表示两帧点云的描述符矩阵之间的距离；依旧用p和q来表示当前帧激光点云和历史数据帧，对应的描述符矩阵分别为：Ep，Eq；这两个描述符矩阵之间的距离为：
最终通过和描述符数据库中的描述符匹配，得到车辆匹配定位位姿，i表示描述符矩阵的第i行，j表示描述符矩阵的第j列，p表示当前第p帧激光点云，q表示历史第q帧激光点，
表示第p帧激光点云描述符矩阵的第i行，第j列元素为汉明距离与第q帧激光点云描述符矩阵的第i行，第j列元素之间的汉明距离。
6.根据权利要求1所述的基于激光里程计定位和点云描述符匹配定位的融合定位方法，其特征在于：在所述步骤e中，通过两个位姿激光点云数据的配准，求得这两个位姿的旋转平移矩阵(R,T)，用Xk来表示车辆在k时刻的位姿，则：
Xk＝R·Xk-1+T
通过连续位姿之间的相对变换，通过激光里程计得到车辆位姿估计，R表示旋转矩阵，T表示平移矩阵。
7.根据权利要求1所述的基于激光里程计定位和点云描述符匹配定位的融合定位方法，其特征在于：在所述步骤f中，将激光里程计定位得到的车辆位姿To和描述符匹配定位得到的车辆位姿Tm进行融合：
T＝TmTo
最终融合的定位结果作为这个周期融合定位结果的输出，同时又作为下一个周期激光里程计定位的起点，高频输出定位结果，又能周期性地不断校正累积误差，实现实时定位。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110886022.2A CN113740875A (zh) | 2021-08-03 | 2021-08-03 | 一种基于激光里程计和点云描述符匹配的自动驾驶车辆定位方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110886022.2A CN113740875A (zh) | 2021-08-03 | 2021-08-03 | 一种基于激光里程计和点云描述符匹配的自动驾驶车辆定位方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN113740875A true CN113740875A (zh) | 2021-12-03 |
Family
ID=78729968
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110886022.2A Pending CN113740875A (zh) | 2021-08-03 | 2021-08-03 | 一种基于激光里程计和点云描述符匹配的自动驾驶车辆定位方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113740875A (zh) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115685133A (zh) * | 2022-12-30 | 2023-02-03 | 安徽蔚来智驾科技有限公司 | 自动驾驶车辆的定位方法、控制装置、存储介质及车辆 |
| CN116425088A (zh) * | 2023-06-09 | 2023-07-14 | 未来机器人（深圳）有限公司 | 货物搬运方法、装置及机器人 |
| CN116452422A (zh) * | 2023-04-24 | 2023-07-18 | 上海几何伙伴智能驾驶有限公司 | 一种4d成像毫米波雷达的回环检测方法 |
| CN116500638A (zh) * | 2023-06-25 | 2023-07-28 | 江苏大学 | 一种基于slam技术的收割机机耕道自动导航方法及系统 |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20180075643A1 (en) * | 2015-04-10 | 2018-03-15 | The European Atomic Energy Community (Euratom), Represented By The European Commission | Method and device for real-time mapping and localization |
| WO2020155616A1 (zh) * | 2019-01-29 | 2020-08-06 | 浙江省北大信息技术高等研究院 | 一种基于数字视网膜的拍摄装置的定位方法 |
| CN111707272A (zh) * | 2020-06-28 | 2020-09-25 | 湖南大学 | 一种地下车库自动驾驶激光定位系统 |
-
2021
- 2021-08-03 CN CN202110886022.2A patent/CN113740875A/zh active Pending
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20180075643A1 (en) * | 2015-04-10 | 2018-03-15 | The European Atomic Energy Community (Euratom), Represented By The European Commission | Method and device for real-time mapping and localization |
| WO2020155616A1 (zh) * | 2019-01-29 | 2020-08-06 | 浙江省北大信息技术高等研究院 | 一种基于数字视网膜的拍摄装置的定位方法 |
| CN111707272A (zh) * | 2020-06-28 | 2020-09-25 | 湖南大学 | 一种地下车库自动驾驶激光定位系统 |
Non-Patent Citations (1)
| Title |
|---|
| 刘子明;陈庆盈;李阳;彭文飞;: "可变高度激光里程计在室内不平整地面环境下的地图构建", 宁波大学学报(理工版), no. 04, 2 July 2020 (2020-07-02) * |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115685133A (zh) * | 2022-12-30 | 2023-02-03 | 安徽蔚来智驾科技有限公司 | 自动驾驶车辆的定位方法、控制装置、存储介质及车辆 |
| CN116452422A (zh) * | 2023-04-24 | 2023-07-18 | 上海几何伙伴智能驾驶有限公司 | 一种4d成像毫米波雷达的回环检测方法 |
| CN116452422B (zh) * | 2023-04-24 | 2024-02-20 | 上海几何伙伴智能驾驶有限公司 | 一种4d成像毫米波雷达的回环检测方法 |
| CN116425088A (zh) * | 2023-06-09 | 2023-07-14 | 未来机器人（深圳）有限公司 | 货物搬运方法、装置及机器人 |
| CN116425088B (zh) * | 2023-06-09 | 2023-10-24 | 未来机器人（深圳）有限公司 | 货物搬运方法、装置及机器人 |
| CN116500638A (zh) * | 2023-06-25 | 2023-07-28 | 江苏大学 | 一种基于slam技术的收割机机耕道自动导航方法及系统 |
| CN116500638B (zh) * | 2023-06-25 | 2023-10-10 | 江苏大学 | 一种基于slam技术的收割机耕道自动导航方法及系统 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113740875A (zh) | 一种基于激光里程计和点云描述符匹配的自动驾驶车辆定位方法 | |
| CN108253958B (zh) | 一种稀疏环境下的机器人实时定位方法 | |
| CN109755995B (zh) | 基于ros机器人操作系统的机器人自动充电对接方法 | |
| CN111693968B (zh) | 一种车载三维激光雷达系统外参的系统化标定方法 | |
| CN110009029B (zh) | 基于点云分割的特征匹配方法 | |
| CN110515055B (zh) | 利用半径搜索优化激光雷达定位的方法 | |
| CN110243380A (zh) | 一种基于多传感器数据与角度特征识别的地图匹配方法 | |
| CN112965063B (zh) | 一种机器人建图定位方法 | |
| CN114005280A (zh) | 一种基于不确定性估计的车辆轨迹预测方法 | |
| CN110501006B (zh) | 一种异类传感器联合航迹关联与跟踪方法 | |
| CN110910440B (zh) | 一种基于电力影像数据的输电线路长度确定方法和系统 | |
| CN114565674B (zh) | 自动驾驶车辆城市结构化场景纯视觉定位方法及装置 | |
| CN111316328A (zh) | 车道线地图的维护方法、电子设备与存储介质 | |
| CN111160447A (zh) | 一种基于DSmT理论的自主泊车定位系统多传感器感知融合方法 | |
| CN113406659A (zh) | 一种基于激光雷达信息的移动机器人位置重识别方法 | |
| CN112986982B (zh) | 环境地图参照定位方法、装置和移动机器人 | |
| CN114565726A (zh) | 一种非结构化动态环境下的同时定位与建图方法 | |
| CN112965076B (zh) | 一种用于机器人的多雷达定位系统及方法 | |
| CN116299525A (zh) | 一种基于点云区域相关性的动态环境rgb-d视觉slam方法 | |
| CN116124144A (zh) | 融合注意力的目标检测与几何约束的视觉惯性室内动态环境定位系统 | |
| CN115436968A (zh) | 一种基于激光雷达的位图化重定位方法 | |
| Gadd et al. | Open-RadVLAD: Fast and Robust Radar Place Recognition | |
| CN113554705A (zh) | 一种变化场景下的激光雷达鲁棒定位方法 | |
| CN114511080A (zh) | 一种模型构建方法、装置以及异常轨迹点实时检测方法 | |
| CN109883434B (zh) | 限制场景下辅助无人车全局定位的场端及全局定位方法 |
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 |