CN113447949A - 一种基于激光雷达和先验地图的实时定位系统及方法 - Google Patents

一种基于激光雷达和先验地图的实时定位系统及方法 Download PDF

Info

Publication number
CN113447949A
CN113447949A CN202110653480.1A CN202110653480A CN113447949A CN 113447949 A CN113447949 A CN 113447949A CN 202110653480 A CN202110653480 A CN 202110653480A CN 113447949 A CN113447949 A CN 113447949A
Authority
CN
China
Prior art keywords
positioning
point cloud
map
registration
laser radar
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
CN202110653480.1A
Other languages
English (en)
Other versions
CN113447949B (zh
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.)
Tianjin University
Original Assignee
Tianjin University
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 Tianjin University filed Critical Tianjin University
Priority to CN202110653480.1A priority Critical patent/CN113447949B/zh
Publication of CN113447949A publication Critical patent/CN113447949A/zh
Application granted granted Critical
Publication of CN113447949B publication Critical patent/CN113447949B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching

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)
  • Electromagnetism (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种基于激光雷达和先验地图的实时定位系统及方法，该系统包括点云注册模块(100)、激光雷达里程计模块(200)、地图配准定位模块(300)和定位融合模块(400)；提取为特征点云数据；进行第t个时间戳激光雷达里程计；进行地图配准定位，在第k周期，获取地图片段
Figure DDA0003112782240000011
关键帧特征点云
Figure DDA0003112782240000012
和里程计的航迹推算结果
Figure DDA0003112782240000013
后，通过NDT配准得出车辆相对准确的定位
Figure DDA0003112782240000014
在第t个时间戳将10Hz的航迹推算结果
Figure DDA0003112782240000015
与1Hz的地图定位结果
Figure DDA0003112782240000016
融合计算，得到10Hz的车辆定位
Figure DDA0003112782240000017
与现有技术相比，本发明能够在无卫星信号和基站信号的封闭环境中，实现车道级的车辆定位；满足自动驾驶实时性的要求；实现了长距离和长时间行驶环境下仍维持在车道级以内的漂移误差。

Description

一种基于激光雷达和先验地图的实时定位系统及方法
技术领域
本发明涉及无人驾驶车辆定位技术领域，特别是涉及一种基于激光雷达和先验地图的实时定位系统及方法。
背景技术
智能网联汽车及自动驾驶技术是《中国制造2025》的重要领域，近年来其相关领域和技术得到了快速发展，其中就包括车辆定位技术。作为车辆环境感知的重要组成部分，同时也是一切自动驾驶技术的出发点，车辆定位技术面临着越来越高的要求。
现有车辆可选的定位方案包括：全球卫星导航系统GNSS、地面站定位系统、车辆惯性导航系统IMU、车轮定位技术、二维码定位技术、WiFi指纹定位技术、基于激光雷达的建图和定位技术(LiDAR-SLAM)、地图配准(MM)定位技术等方案。各种方法的优缺点描述如下：
(1)全球卫星导航系统GNSS、地面站定位系统在开阔地带可以实现车辆的厘米级的高精度实时定位；但是不适用在高楼群、隧道、地下停车场等相对封闭的环境中，因信号遮挡问题。
(2)车载惯性导航系统IMU依靠内置的加速计和陀螺仪得出车辆的实时位置，但IMU比较容易受到外界环境干扰，如磁场、温度，此外IMU的漂移误差问题也同样严重。
(3)车轮定位技术可以依靠车轮转速和行驶中的车轮角度变化来计算车辆位置，对路面平整情况要求严格，在凹凸不平的地面环境中误差较大，此外还存在车轮打滑、漂移误差等难以解决的难题。
(4)二维码定位技术依靠二维码图片来实现车辆定位，技术简单快速，车辆扫描到二维码的同时，即可快速计算车辆的真实位置；但如果要满足车辆定位实时性的要求，则需要保证驾驶场景中有足够的二维码图片密度，同时，二维码图片的识别也不是完美可靠的，易受障碍物遮挡、环境光照因素影响。
(5)WiFi指纹定位技术同样需要大量的前期布置计算，并且同时为所有非开阔地带布置WiFi是不切实际的。
(6)W基于激光雷达的建图和定位技术(LiDAR-SLAM)由机器人领域扩展而来，可以同时实现车辆定位和周围地图构建，但是缺点在车辆领域十分致命：它只能保证短距离或短时间内定位的实时性和定位精度，系统运行的时间越长其漂移误差越大，通常需要轨迹闭环来进行矫正；
(7)地图配准(MM)定位可以保证车辆长时间内的定位精度，但是其短时间内的实时性和定位精度无法保证。
面向未来智能网联汽车和自动驾驶技术应用需求达到“车辆在封闭环境下定位保证10Hz及以上的定位频度以及至少车道级以内的定位精度”这一技术问题，目前尚不存在有效的解决方案。
发明内容
针对上述现有技术存在的问题，本发明提出了一种基于激光雷达和先验地图的实时定位系统及方法，实现了基于激光雷达里程计和先验地图就能车道级的车辆定位，从而改善了在封闭环境下的车辆定位问题。
本发明的一种基于激光雷达和先验地图的实时定位系统，该系统包括点云注册模块100、激光雷达里程计模块200、地图配准定位模块300和定位融合模块400；其中：
所述点云注册模块100，该模块用于提取激光雷达点云帧中的特征点云；
所述激光雷达里程计模块200，该模块利用激光雷达计算两帧特征点云之间的变换关系进行车辆航迹推算，得到10Hz频率的航迹推算结果；
所述地图配准定位模块300，该模块用于从点云注册模块100产生的特征点云中选取特征点最多的一帧作为关键点云帧；利用将该关键点云帧与已建好的先验地图进行配准，得到配准频率为1Hz的矫正定位结果；
定位融合模块400，该模块将定位频率为10Hz的激光雷达里程计航迹推算结果与1Hz的地图配准定位结果进行融合计算，得到定位频率为10Hz以及车道级定位精度的车辆定位结果。
本发明的一种基于激光雷达和先验地图的实时定位方法，该方法包括以下步骤：
步骤1：进行点云注册，具体为在第t个时间戳将激光雷达的实时点云帧
Figure BDA0003112782220000031
注册，提取为特征点云数据；
步骤2：进行第t个时间戳激光雷达里程计，具体为在第t个时间戳激光雷达里程计模块接收到特征点云，之后，计算连续两帧特征点的变换关系
Figure BDA0003112782220000032
进而计算得到相对于初始位置的航迹推算
Figure BDA0003112782220000033
步骤3：进行地图配准定位，具体为以连续帧周期进行地图配准定位，在第k周期，获取地图片段
Figure BDA0003112782220000034
关键帧特征点云
Figure BDA0003112782220000035
和里程计的航迹推算结果
Figure BDA0003112782220000036
后，通过NDT配准得出车辆相对准确的定位
Figure BDA0003112782220000037
步骤4：进行定位融合，即在第t个时间戳将定位频率为10Hz的航迹推算结果
Figure BDA0003112782220000038
与1Hz的地图定位结果
Figure BDA0003112782220000039
融合计算，得到定位频率为10Hz的车辆定位
Figure BDA00031127822200000310
与现有技术相比，本发明能够达成以下技术效果：
1)在无卫星信号和基站信号的封闭环境中，实现车道级的车辆定位；
2)车辆定位的频率在10Hz左右，与激光雷达固有频率持平，满足自动驾驶实时性的要求；
3)实现长距离和长时间行驶环境下仍维持在车道级以内的漂移误差；
4)前期工作只需制作一份先验地图即可，无需环境布置工作。
附图说明
图1为本发明的基于激光雷达和先验地图的实时定位系统架构示意图；
图2为第t个时间戳时的点云注册流程示意图；
图3为第t个时间戳激光雷达里程计流程示意图；
图4为通过kd-tree寻找相邻帧间的特征点对应示意图；
图5为点线距离与点面距离示意图；
图6为第t个时间戳地图配准定位流程示意图；
图7为第t个时间戳定位融合流程示意图。
具体实施方式
下面结合附图和具体实施例对本发明技术方案作进一步详细描述。
如图1所示，为本发明的基于激光雷达和先验地图的实时定位系统架构示意图。该系统包括四个节点，分别是点云注册模块100、激光雷达里程计模块200、地图配准定位模块300和定位融合模块400。其中：
点云注册模块100，该模块用于提取激光雷达点云帧中的特征点云，所述特征点云用于近似表示车辆周围环境，降低原始点云帧中的点云规模以及减少计算量；每秒产生的10帧特征点云，特征点云的产生频率为10Hz；
激光雷达里程计模块200，该模块利用激光雷达计算两帧特征点云之间的变换关系进行车辆航迹推算，得到基于10Hz频率的航迹推算结果(该航迹推算结果的漂移误差会不断增大)；10Hz的频率代表此模块1秒产生10次航迹推算结果；
地图配准定位模块300，该模块用于从点云注册模块100每秒产生的10帧特征点云中选取特征点最多的一帧作为关键点云帧；然后将该关键点云帧与已建好的先验地图通过正态分布变换(NDT)进行配准，得到基于配准频率为1Hz的地图配准定位结果，该定位结果具有车道级的定位精度；1Hz的频率代表此模块1秒只产生1次地图配准定位结果；
定位融合模块400，该模块将10Hz的激光雷达里程计航迹推算结果与1Hz的地图配准定位结果进行融合计算，得到频率为10Hz以及车道级定位精度的车辆定位结果。
本发明的基于激光雷达和先验地图的实时定位方法根据激光雷达传感器数据和先验地图数据，获取频率为10Hz以及车道级精度的车辆定位，具体实现包括以下流程：
步骤1：进行点云注册，具体为在第t个时间戳将激光雷达的实时点云帧
Figure BDA0003112782220000051
提取为特征点云数据，如图2所示，为第t个时间戳时的点云注册过程示意图。该步骤具体包括以下处理：
步骤1.1：去除无效点，即去除激光雷达传感器产生的点云帧
Figure BDA0003112782220000052
中的无效点，记录原始数据时间戳t(t＝1,2,…)，以此时间戳作为航迹推算结果和最终车辆定位结果的时间戳；
步骤1.2：划分点云层ID，即根据点云中每个点的俯仰角α，将点云中具有相同俯仰角α的点划分到同一个层ID中；设激光雷达的垂直视角为±θ，垂直分辨率为β，线数为n，它们满足关系
Figure BDA0003112782220000053
对其中任意一点对应的坐标值(x,y，z)，计算其俯仰角度α以及对应的层ID，表达式如下：
Figure BDA0003112782220000054
Figure BDA0003112782220000055
如果求得一个点的ID不为正整数，视其为无效点予以丢弃；
步骤1.3：计算曲率，以曲率作为提取特征点云的依据，即为降低计算复杂度，只利用当前点pi(xi，yi，zi)，及在同一层ID中其前后各5个点(pi-5，…，pi-1,pi+1,…,pi+5)的坐标差的平方和来近似估计曲率k，表达式如下：
kx＝xi-5+…+xi-1+xi+1+…+xi+5-10xi
ky＝yi-5+…+yi-1+yi+1+…+yi+5-10yi
kz＝zi-5+…+zi-1+zi+1+…+zi+5-10zi
Figure BDA0003112782220000056
其中，kx,kykz分别表示x,y,z方向的曲率，xi-5,…,xi-1,xi+1,…,xi+5分别表示当前点前后各5个点的x坐标，yi-5,…,yi-1,yi+1,…,yi+5分别表示当前点前后各5个点的y坐标，zi-5,…,zi-1,zi+1,…,zi+5分别表示当前点前后各5个点的z坐标；
步骤1.4：提取特征点云，即根据每个点对应曲率k的大小，提取四类点云包括轮廓点云
Figure BDA0003112782220000061
次级轮廓点云
Figure BDA0003112782220000062
平面点云
Figure BDA0003112782220000063
和次级平面点云
Figure BDA0003112782220000064
每一个层ID分组中，曲率最大的前12个点标记为轮廓点，曲率最大的前120个点标记为次级轮廓点，曲率最小的24个点标记为平面点，剩下的点经过下采样降低到原点云数量的20％，标记为次级平面点。之后将所有层ID分组中的特征点分别合并，形成对应的特征点云，发布给激光雷达里程计节点和地图配准定位节点，用于后续计算。
步骤2：进行第t个时间戳激光雷达里程计，即在第t个时间戳激光雷达里程计模块接收到特征点云，之后，根据连续两帧特征点的对应关系计算连续两帧特征点的变换关系
Figure BDA0003112782220000065
进而计算得到相对于初始位置的航迹推算
Figure BDA0003112782220000066
如图3所示，为第t个时间戳激光雷达里程计流程示意图。该步骤具体包括以下处理：
步骤2.1：构建kd-tree(kd-tree是一种分割k维数据空间的数据结构，在本发明中用于最近点的查找)。kd-tree-ic和kd-tree-if分别为由第t-1个时间戳的次级轮廓点云
Figure BDA0003112782220000067
和次级平面点云
Figure BDA0003112782220000068
构建成的kd-tree结构；
步骤2.2：依靠kd-tree寻找不同时间戳的特征点的对应关系，再求得它们之间的几何距离；
如图4所示，为通过kd-tree寻找相邻帧间的特征点对应点示例图。通过该示例表明：
通过kd-tree-ic找到第t个时间戳轮廓点云
Figure BDA0003112782220000069
中的每一个点(如轮廓点A)在第t-1个时间戳的次级轮廓点云
Figure BDA00031127822200000610
中的2个对应点(最近点I、相邻层ID的最近点J)；
通过kd-tree-if找到第t个时间戳平面点云
Figure BDA00031127822200000611
中的每一个点(如平面点B)在第t-1个时间戳次级平面点云
Figure BDA00031127822200000612
中的3个对应点(最近点K、次近点L、相邻层ID的最近点M)。
然后分别计算特征点A、B分别到最近点K组成的线IJ、面KLM的距离d1及d2。如图5所示，为点线距离与点面距离示意图。
距离d1的计算公式如下：
Figure BDA0003112782220000071
其中，
Figure BDA0003112782220000072
和
Figure BDA0003112782220000073
为特征点A到两个对应点I、J的向量，
Figure BDA0003112782220000074
为两个对应点形成的向量。
距离d2的计算公式如下：
Figure BDA0003112782220000075
其中，
Figure BDA0003112782220000076
为特征点B到对应点K的向量，
Figure BDA0003112782220000077
为三个对应点间形成的向量；在时间戳序列中，重复进行步骤2.1和2.2，获取对应时间戳t的
Figure BDA0003112782220000078
和
Figure BDA0003112782220000079
步骤2.3：计算相邻时间戳t-1和t之间车辆位姿的变换关系
Figure BDA00031127822200000710
通过牛顿梯度下降法来迭代最小化距离函数
Figure BDA00031127822200000711
和
Figure BDA00031127822200000712
的值，求出变换关系
Figure BDA00031127822200000713
具体推导过程如下：
首先构建雅可比矩阵
Figure BDA00031127822200000714
列出高斯牛顿方程：
JTJΔTl＝-JTd
求得
Figure BDA00031127822200000715
的增量ΔTl后，更新
Figure BDA00031127822200000716
及A、B的坐标值(XA,YA,ZA)、(XB,YB,ZB)，迭代过程如下：
Figure BDA00031127822200000717
Figure BDA00031127822200000718
Figure BDA00031127822200000719
之后，使用步骤2.2中距离公式重新计算
Figure BDA00031127822200000720
和
Figure BDA00031127822200000721
构建新的雅可比矩阵J和高斯牛顿方程，迭代进行一定次数后便得到第t个时间戳的位姿变换关系
Figure BDA00031127822200000722
步骤2.4：计算第t个时间戳的航迹推算结果
Figure BDA00031127822200000723
表达式如下：
Figure BDA00031127822200000724
其中，
Figure BDA00031127822200000725
代表在时间戳序列中，第i(i＝1,2,…,t)个车辆位姿变换关系。
步骤3：进行地图配准定位，即该定位系统以10个连续帧为一个周期进行地图配准定位。在第k周期，获取地图片段
Figure BDA0003112782220000081
关键帧特征点云
Figure BDA0003112782220000082
和里程计的航迹推算结果
Figure BDA0003112782220000083
后，通过NDT配准得出车辆相对准确的定位
Figure BDA0003112782220000084
如图6所示，为第t个时间戳地图配准定位流程示意图。
步骤3.1：预测车辆位置
Figure BDA0003112782220000085
根据k-1周期的地图定位结果
Figure BDA0003112782220000086
以及两次地图配准定位之间航迹推算结果的几何变换过程
Figure BDA0003112782220000087
推算关键帧被采集时车辆所在的预测位置，作为NDT配准的原初变换矩阵。表达式为：
Figure BDA0003112782220000088
其中，
Figure BDA0003112782220000089
表示第k-1周期所选关键帧对应的航迹推算结果，
Figure BDA00031127822200000810
表示第k周期所选关键帧对应的航迹推算结果。
步骤3.2：获取地图片段
Figure BDA00031127822200000811
根据预测的车辆在地图中的位置，截取地图内的点云片段作为参与NDT配准的地图片段
Figure BDA00031127822200000812
步骤3.3：合并特征点云。将关键帧中的轮廓点云、次级轮廓点云、平面点云和次级平面点云合并，得到一个新的特征点云
Figure BDA00031127822200000813
作为NDT配准的源点云。
步骤3.4：NDT配准。根据特征点云、地图点云和预测的车辆位置，使用NDT配准算法得出矫正定位结果
Figure BDA00031127822200000814
即为地图配准定位结果。NDT算法会借助牛顿梯度下降法，最小化特征点云与地图间的差异，同时将
Figure BDA00031127822200000815
更新为
Figure BDA00031127822200000816
此结果与此时刻的里程计结果
Figure BDA00031127822200000817
会被发布给定位融合节点。
步骤4：进行定位融合，即在第t个时间戳，将10Hz的航迹推算结果
Figure BDA00031127822200000818
与1Hz的地图定位结果
Figure BDA00031127822200000819
融合计算，得到10Hz的车辆定位
Figure BDA00031127822200000820
如图7所示，为第t个时间戳定位融合流程示意图。
接收里程计模块得到的第t个时间戳的航迹推算结果
Figure BDA00031127822200000821
接收地图配准定位模块得到的第k个周期地图定位结果
Figure BDA00031127822200000822
和航迹推算
Figure BDA00031127822200000823
采取推定的方式计算车辆定位结果
Figure BDA00031127822200000824
以地图定位节点的结果
Figure BDA00031127822200000825
为基准，计算地图定位节点的航迹推算数据
Figure BDA00031127822200000826
和激光雷达里程计节点的航迹推算数据
Figure BDA00031127822200000827
之间的几何变换过程
Figure BDA00031127822200000828
再推算得出车辆的实际位置
Figure BDA0003112782220000091
表达式如下：
Figure BDA0003112782220000092

Claims (6)

1.一种基于激光雷达和先验地图的实时定位系统，其特征在于，该系统包括点云注册模块(100)、激光雷达里程计模块(200)、地图配准定位模块(300)和定位融合模块(400)；其中：
所述点云注册模块(100)，该模块用于提取激光雷达点云帧中的特征点云；
所述激光雷达里程计模块(200)，该模块利用激光雷达计算两帧特征点云之间的变换关系进行车辆航迹推算，得到10Hz频率的航迹推算结果；
所述地图配准定位模块(300)，该模块用于从点云注册模块(100)产生的特征点云中选取特征点最多的一帧作为关键点云帧；利用将该关键点云帧与已建好的先验地图进行配准，得到配准频率为1Hz的矫正定位结果；
定位融合模块(400)，该模块将定位频率为10Hz的激光雷达里程计航迹推算结果与1Hz的地图配准定位结果进行融合计算，得到定位频率为10Hz以及车道级定位精度的车辆定位结果。
2.一种基于激光雷达和先验地图的实时定位方法，其特征在于，该方法包括以下步骤：
步骤1：进行点云注册，具体为在第t个时间戳将激光雷达的实时点云帧
Figure FDA0003112782210000011
注册，提取为特征点云数据；
步骤2：进行第t个时间戳激光雷达里程计，具体为在第t个时间戳激光雷达里程计模块接收到特征点云，之后，计算连续两帧特征点的变换关系
Figure FDA0003112782210000012
进而计算得到相对于初始位置的航迹推算
Figure FDA0003112782210000013
步骤3：进行地图配准定位，具体为以连续帧周期进行地图配准定位，在第k周期，获取地图片段
Figure FDA0003112782210000014
关键帧特征点云
Figure FDA0003112782210000015
和里程计的航迹推算结果
Figure FDA0003112782210000016
后，通过NDT配准得出车辆相对准确的定位
Figure FDA0003112782210000017
步骤4：进行定位融合，即在第t个时间戳将定位频率为10Hz的航迹推算结果
Figure FDA0003112782210000018
与1Hz的地图定位结果
Figure FDA0003112782210000019
融合计算，得到定位频率为10Hz的车辆定位
Figure FDA00031127822100000110
3.如权利要求1所述的基于激光雷达和先验地图的实时定位方法，其特征在于，所述步骤1中，为第t个时间戳时的点云注册过程具体包括以下处理：
步骤1.1：去除无效点，即去除激光雷达传感器产生的点云帧
Figure FDA0003112782210000021
中的无效点，记录原始数据时间戳t(t＝1，2，...)，以此时间戳作为航迹推算结果和最终车辆定位结果的时间戳；
步骤1.2：划分点云层ID，即根据点云中每个点的俯仰角α，将点云中具有相同俯仰角α的点划分到同一个层ID中；计算其俯仰角度α以及对应的层ID，表达式如下：
Figure FDA0003112782210000022
Figure FDA0003112782210000023
其中，±θ表示激光雷达的垂直视角，β表示垂直分辨率，n表示线数，x，y，z分别为任意一点对应的坐标值，
点云层ID不为正整数的视其为无效点，予以丢弃；
步骤1.3：计算曲率，以曲率作为提取特征点云的依据降低计算复杂度，只利用当前点pi(xi，yi，zi)及在同一层ID中其前后各5个点(pi-5，...，pi-1，pi+1，...，pi+5)的坐标差的平方和来近似估计曲率k，表达式如下：
kx＝xi-5+...+xi-1+xi+1+...+xi+5-10xi
ky＝yi-5+...+yi-1+yi+1+...+yi+5-10yi
kz＝zi-5+...+zi-1+zi+1+...+zi+5-10zi
Figure FDA0003112782210000024
其中，kx，kykz分别表示x，y，z方向的曲率，xi-5，...，xi-1，xi+1，...，xi+5分别表示当前点前后各5个点的x坐标，yi-5，...，yi-1，yi+1，...，yi+5分别表示当前点前后各5个点的y坐标，zi-5，...，zi-1，zi+1，...，zi+5分别表示当前点前后各5个点的z坐标；
步骤1.4：提取特征点云，即根据每个点对应曲率k的大小，提取四类点云包括轮廓点云
Figure FDA0003112782210000025
次级轮廓点云
Figure FDA0003112782210000026
平面点云
Figure FDA0003112782210000027
和次级平面点云
Figure FDA0003112782210000028
之后，将所有层ID分组中的特征点分别合并，形成对应的特征点云，发布给激光雷达里程计节点和地图配准定位节点。
4.如权利要求1所述的基于激光雷达和先验地图的实时定位方法，其特征在于，所述步骤2中，第t个时间戳激光雷达里程计流程具体包括以下处理：
步骤2.1：构建kd-tree；
步骤2.2：构建依靠kd-tree所寻找到得不同时间戳的特征点A、B分别到最近点K组成的线IJ、面KLM的距离函数d1及d2；
距离d1的计算公式如下：
Figure FDA0003112782210000031
其中，
Figure FDA0003112782210000032
和
Figure FDA0003112782210000033
为特征点A到两个对应点I、J的向量，
Figure FDA0003112782210000034
为两个对应点形成的向量；
距离d2的计算公式如下：
Figure FDA0003112782210000035
其中，
Figure FDA0003112782210000036
为特征点B到对应点K的向量，
Figure FDA0003112782210000037
为三个对应点间形成的向量；在时间戳序列中，重复进行步骤2.1和2.2，获取对应时间戳t的
Figure FDA0003112782210000038
和
Figure FDA0003112782210000039
步骤2.3：计算相邻时间戳t-1和t之间车辆位姿的变换关系
Figure FDA00031127822100000310
通过牛顿梯度下降法来迭代最小化距离函数
Figure FDA00031127822100000311
和
Figure FDA00031127822100000312
的值，求出第t个时间戳变换关系
Figure FDA00031127822100000313
表达式如下：
首先构建雅可比矩阵J，表达式如下：
Figure FDA00031127822100000314
列出高斯牛顿方程：
JTJΔTl＝-JTd
其中，
Figure FDA00031127822100000315
表示第t个时间戳的距离d1，
Figure FDA00031127822100000316
表示第t个时间戳的距离d2，JT为雅可比矩阵的J转置矩阵，ΔTl为两次迭代之间
Figure FDA00031127822100000317
的增量；
求得
Figure FDA00031127822100000318
的增量ΔTl后，更新
Figure FDA00031127822100000319
及特征点A、B的坐标值(XA，YA，ZA)、(XB，YB，ZB)，迭代过程如下：
Figure FDA00031127822100000320
Figure FDA0003112782210000041
Figure FDA0003112782210000042
之后，使用步骤2.2中距离公式重新计算
Figure FDA0003112782210000043
和
Figure FDA0003112782210000044
构建新的雅可比矩阵J和高斯牛顿方程，迭代进行一定次数后便得到第t个时间戳的位姿变换关系
Figure FDA0003112782210000045
步骤2.4：计算第t个时间戳的航迹推算结果
Figure FDA0003112782210000046
表达式如下：
Figure FDA0003112782210000047
其中，
Figure FDA0003112782210000048
代表在时间戳序列中，第i(i＝1，2，...，t)个车辆位姿变换关系。
5.如权利要求1所述的基于激光雷达和先验地图的实时定位方法，其特征在于，所述步骤2中，第t个时间戳的地图配准定位流程具体包括以下处理：
步骤3.1：预测车辆位置
Figure FDA0003112782210000049
即根据k-1周期的地图定位结果
Figure FDA00031127822100000410
以及两次地图配准定位之间航迹推算结果的几何变换过程
Figure FDA00031127822100000411
推算关键帧被采集时车辆所在的预测位置，作为NDT配准的原初变换矩阵，表达式如下：
Figure FDA00031127822100000412
其中，
Figure FDA00031127822100000413
表示第k-1周期所选关键帧对应的航迹推算结果，
Figure FDA00031127822100000414
表示第k周期所选关键帧对应的航迹推算结果；
步骤3.2：获取地图片段
Figure FDA00031127822100000415
即根据预测车辆位置
Figure FDA00031127822100000416
截取地图内的点云片段作为参与NDT配准的地图片段
Figure FDA00031127822100000417
步骤3.3：合并特征点云，具体为将关键帧中的轮廓点云、次级轮廓点云、平面点云和次级平面点云合并，得到一个新的特征点云
Figure FDA00031127822100000418
作为NDT配准的源点云；
步骤3.4：进行NDT配准，即根据特征点云、地图点云和预测的车辆位置，使用NDT配准算法得出矫正定位结果
Figure FDA00031127822100000419
作为地图配准定位结果，得到的
Figure FDA00031127822100000420
与此时刻的里程计结果
Figure FDA00031127822100000421
会被发布给定位融合节点；
步骤4：进行定位融合，即在第t个时间戳，将10Hz的航迹推算结果
Figure FDA00031127822100000422
与1Hz的地图定位结果
Figure FDA00031127822100000423
融合计算，得到10Hz的车辆定位
Figure FDA00031127822100000424
6.如权利要求1所述的基于激光雷达和先验地图的实时定位方法，其特征在于，所述步骤4中，第t个时间戳的定位融合流程具体包括以下处理：
接收里程计模块得到的第t个时间戳的航迹推算结果
Figure FDA0003112782210000051
接收地图配准定位模块得到的第k个周期地图定位结果
Figure FDA0003112782210000052
和航迹推算
Figure FDA0003112782210000053
采取推定的方式计算车辆定位结果
Figure FDA0003112782210000054
以地图定位节点的结果
Figure FDA0003112782210000055
为基准，计算地图定位节点的航迹推算数据
Figure FDA0003112782210000056
和激光雷达里程计节点的航迹推算数据
Figure FDA0003112782210000057
之间的几何变换过程
Figure FDA0003112782210000058
再推算得出车辆的实际位置
Figure FDA0003112782210000059
表达式如下：
Figure FDA00031127822100000512
CN202110653480.1A 2021-06-11 2021-06-11 一种基于激光雷达和先验地图的实时定位系统及方法 Active CN113447949B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110653480.1A CN113447949B (zh) 2021-06-11 2021-06-11 一种基于激光雷达和先验地图的实时定位系统及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110653480.1A CN113447949B (zh) 2021-06-11 2021-06-11 一种基于激光雷达和先验地图的实时定位系统及方法

Publications (2)

Publication Number Publication Date
CN113447949A true CN113447949A (zh) 2021-09-28
CN113447949B CN113447949B (zh) 2022-12-09

Family

ID=77811475

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110653480.1A Active CN113447949B (zh) 2021-06-11 2021-06-11 一种基于激光雷达和先验地图的实时定位系统及方法

Country Status (1)

Country Link
CN (1) CN113447949B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114199240A (zh) * 2022-02-18 2022-03-18 武汉理工大学 无gps信号下二维码、激光雷达与imu融合定位系统及方法
CN114296096A (zh) * 2021-12-23 2022-04-08 深圳优地科技有限公司 一种机器人定位方法、系统以及终端
CN114563000A (zh) * 2022-02-23 2022-05-31 南京理工大学 一种基于改进型激光雷达里程计的室内外slam方法
CN115631314A (zh) * 2022-12-19 2023-01-20 中汽研（天津）汽车工程研究院有限公司 一种基于多特征和自适应关键帧的点云地图构建方法

Citations (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104215968A (zh) * 2014-09-01 2014-12-17 中国人民解放军国防科学技术大学 一种云层对敏捷光学卫星的遮挡时间窗口的计算方法
CN109186608A (zh) * 2018-09-27 2019-01-11 大连理工大学 一种面向重定位的稀疏化三维点云地图生成方法
CN109946732A (zh) * 2019-03-18 2019-06-28 李子月 一种基于多传感器数据融合的无人车定位方法
CN110221603A (zh) * 2019-05-13 2019-09-10 浙江大学 一种基于激光雷达多帧点云融合的远距离障碍物检测方法
CN110223379A (zh) * 2019-06-10 2019-09-10 于兴虎 基于激光雷达的三维点云重建方法
US20200025578A1 (en) * 2017-12-12 2020-01-23 Maser Consulting, P.A. Tunnel mapping system and methods
DE102019202178A1 (de) * 2019-02-19 2020-06-10 Zf Friedrichshafen Ag Verfahren zur Bestimmung von Geschwindigkeit über Grund und Richtungswinkel eines Fahrzeugs
CN111288984A (zh) * 2020-03-04 2020-06-16 武汉大学 一种基于车联网的多车联合绝对定位的方法
CN111707272A (zh) * 2020-06-28 2020-09-25 湖南大学 一种地下车库自动驾驶激光定位系统
CN111854732A (zh) * 2020-07-27 2020-10-30 天津大学 一种基于数据融合和宽度学习的室内指纹定位方法
CN112180396A (zh) * 2020-10-21 2021-01-05 航天科工智能机器人有限责任公司 一种激光雷达定位及地图创建方法
CN112489186A (zh) * 2020-10-28 2021-03-12 中汽数据(天津)有限公司 一种自动驾驶双目数据感知算法
CN112561998A (zh) * 2020-12-16 2021-03-26 国网江苏省电力有限公司检修分公司 一种基于三维点云配准的机器人定位和自主充电方法
CN112767456A (zh) * 2021-01-18 2021-05-07 南京理工大学 一种三维激光点云快速重定位方法
CN112810625A (zh) * 2021-04-19 2021-05-18 北京三快在线科技有限公司 一种轨迹修正的方法及装置
CN112904395A (zh) * 2019-12-03 2021-06-04 青岛慧拓智能机器有限公司 一种矿用车辆定位系统及方法

Patent Citations (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104215968A (zh) * 2014-09-01 2014-12-17 中国人民解放军国防科学技术大学 一种云层对敏捷光学卫星的遮挡时间窗口的计算方法
US20200025578A1 (en) * 2017-12-12 2020-01-23 Maser Consulting, P.A. Tunnel mapping system and methods
CN109186608A (zh) * 2018-09-27 2019-01-11 大连理工大学 一种面向重定位的稀疏化三维点云地图生成方法
DE102019202178A1 (de) * 2019-02-19 2020-06-10 Zf Friedrichshafen Ag Verfahren zur Bestimmung von Geschwindigkeit über Grund und Richtungswinkel eines Fahrzeugs
CN109946732A (zh) * 2019-03-18 2019-06-28 李子月 一种基于多传感器数据融合的无人车定位方法
CN110221603A (zh) * 2019-05-13 2019-09-10 浙江大学 一种基于激光雷达多帧点云融合的远距离障碍物检测方法
CN110223379A (zh) * 2019-06-10 2019-09-10 于兴虎 基于激光雷达的三维点云重建方法
CN112904395A (zh) * 2019-12-03 2021-06-04 青岛慧拓智能机器有限公司 一种矿用车辆定位系统及方法
CN111288984A (zh) * 2020-03-04 2020-06-16 武汉大学 一种基于车联网的多车联合绝对定位的方法
CN111707272A (zh) * 2020-06-28 2020-09-25 湖南大学 一种地下车库自动驾驶激光定位系统
CN111854732A (zh) * 2020-07-27 2020-10-30 天津大学 一种基于数据融合和宽度学习的室内指纹定位方法
CN112180396A (zh) * 2020-10-21 2021-01-05 航天科工智能机器人有限责任公司 一种激光雷达定位及地图创建方法
CN112489186A (zh) * 2020-10-28 2021-03-12 中汽数据(天津)有限公司 一种自动驾驶双目数据感知算法
CN112561998A (zh) * 2020-12-16 2021-03-26 国网江苏省电力有限公司检修分公司 一种基于三维点云配准的机器人定位和自主充电方法
CN112767456A (zh) * 2021-01-18 2021-05-07 南京理工大学 一种三维激光点云快速重定位方法
CN112810625A (zh) * 2021-04-19 2021-05-18 北京三快在线科技有限公司 一种轨迹修正的方法及装置

Non-Patent Citations (10)

* Cited by examiner, † Cited by third party
Title
JI ZHANG: "LOAM: Lidar Odometry and Mapping in Real-time", 《ROBOTICS: SCIENCE AND SYSTEMS》 *
MINHWAN SHIN: "3D LiDAR-Based Point Cloud Map Registration Using Spatial Location of Visual Features", 《2017 2ND INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION ENGINEERING》 *
XIANGLEI ZHU: "Research on Driving Behaviors Based on Machine Learning Algorithms", 《2020 IEEE 4TH INFORMATION TECHNOLOGY,NETWORKING,ELECTRONIC AND AUTOMATION CONTROL CONFERENCE (ITNEC 2020)》 *
包川等: "基于模板特征点提取的立体视觉里程计实现方法", 《传感器与微系统》 *
孙朋朋等: "采用延伸顶点的地面点云实时提取算法", 《计算机工程与应用》 *
游安清等: "激光雷达辅助驾驶道路参数计算方法研究", 《应用光学》 *
王晓原等: "基于32线激光雷达的道路边界识别算法", 《科技通报》 *
谢磊等: "基于ICP算法的内河岸线点云数据配准方法", 《水运工程》 *
赵梓乔等: "基于三维激光扫描仪的室内移动设备定位与建图", 《计算机与数字工程》 *
邹北骥等: "一种精确匹配的全景图自动拼接算法", 《计算机工程与科学》 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114296096A (zh) * 2021-12-23 2022-04-08 深圳优地科技有限公司 一种机器人定位方法、系统以及终端
CN114199240A (zh) * 2022-02-18 2022-03-18 武汉理工大学 无gps信号下二维码、激光雷达与imu融合定位系统及方法
CN114563000A (zh) * 2022-02-23 2022-05-31 南京理工大学 一种基于改进型激光雷达里程计的室内外slam方法
CN114563000B (zh) * 2022-02-23 2024-05-07 南京理工大学 一种基于改进型激光雷达里程计的室内外slam方法
CN115631314A (zh) * 2022-12-19 2023-01-20 中汽研（天津）汽车工程研究院有限公司 一种基于多特征和自适应关键帧的点云地图构建方法
CN115631314B (zh) * 2022-12-19 2023-06-09 中汽研（天津）汽车工程研究院有限公司 一种基于多特征和自适应关键帧的点云地图构建方法

Also Published As

Publication number Publication date
CN113447949B (zh) 2022-12-09

Similar Documents

Publication Publication Date Title
CN113447949B (zh) 一种基于激光雷达和先验地图的实时定位系统及方法
Wang et al. Map-based localization method for autonomous vehicles using 3D-LIDAR
US7868821B2 (en) Method and apparatus to estimate vehicle position and recognized landmark positions using GPS and camera
CN110873570B (zh) 用于位置信息的定源、生成并且更新表示位置的地图的方法和装置
CN103411609B (zh) 一种基于在线构图的飞行器返航路线规划方法
Sim et al. Integrated position estimation using aerial image sequences
CN110345937A (zh) 一种基于二维码的导航定姿定位方法和系统
JP5389964B2 (ja) 地図情報生成装置
CN113985429B (zh) 基于三维激光雷达的无人机环境扫描与重构方法
CN111426320B (zh) 一种基于图像匹配/惯导/里程计的车辆自主导航方法
CN105352509A (zh) 地理信息时空约束下的无人机运动目标跟踪与定位方法
CN114526745A (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
JP7431320B2 (ja) デジタル地図データを使用する方法およびシステム
Wen et al. TM 3 Loc: Tightly-coupled monocular map matching for high precision vehicle localization
CN114636414A (zh) 高清晰度城市地图绘制
JP2002532770A (ja) 映像に関連してカメラポーズを決定する方法及びシステム
CN115272596A (zh) 一种面向单调无纹理大场景的多传感器融合slam方法
CN113959437A (zh) 一种用于移动测量设备的测量方法及系统
CN115371662B (zh) 一种基于概率栅格移除动态对象的静态地图构建方法
Khoshelham et al. Vehicle positioning in the absence of GNSS signals: Potential of visual-inertial odometry
CN114690229A (zh) 一种融合gps的移动机器人视觉惯性导航方法
CN117782063A (zh) 一种基于图优化的可变滑动窗口的多传感器融合定位方法
CN114323038B (zh) 融合双目视觉和2d激光雷达的室外定位方法
CN113227713A (zh) 生成用于定位的环境模型的方法和系统
CN110849349A (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
GR01 Patent grant
GR01 Patent grant