CN108917761B - 一种无人车在地下车库中的精确定位方法 - Google Patents
一种无人车在地下车库中的精确定位方法 Download PDFInfo
- Publication number
- CN108917761B CN108917761B CN201810427773.6A CN201810427773A CN108917761B CN 108917761 B CN108917761 B CN 108917761B CN 201810427773 A CN201810427773 A CN 201810427773A CN 108917761 B CN108917761 B CN 108917761B
- Authority
- CN
- China
- Prior art keywords
- module
- ground
- point cloud
- points
- feature
- 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.)
- Expired - Fee Related
Links
- 238000000034 method Methods 0.000 title claims abstract description 60
- 238000000605 extraction Methods 0.000 claims abstract description 19
- 230000011218 segmentation Effects 0.000 claims abstract description 19
- 238000005516 engineering process Methods 0.000 claims abstract description 14
- 238000005070 sampling Methods 0.000 claims abstract description 10
- 230000009466 transformation Effects 0.000 claims description 44
- 238000004364 calculation method Methods 0.000 claims description 15
- 230000006870 function Effects 0.000 claims description 11
- 238000012545 processing Methods 0.000 claims description 10
- 238000011156 evaluation Methods 0.000 claims description 9
- 238000000513 principal component analysis Methods 0.000 claims description 7
- 238000001514 detection method Methods 0.000 claims description 4
- 238000010276 construction Methods 0.000 claims description 3
- 238000006073 displacement reaction Methods 0.000 claims description 3
- 238000010845 search algorithm Methods 0.000 claims description 3
- 238000010187 selection method Methods 0.000 claims description 3
- 238000011524 similarity measure Methods 0.000 claims description 3
- 125000003118 aryl group Chemical group 0.000 claims description 2
- 238000006243 chemical reaction Methods 0.000 claims description 2
- 230000004888 barrier function Effects 0.000 claims 3
- 230000035945 sensitivity Effects 0.000 abstract description 2
- 238000010586 diagram Methods 0.000 description 11
- 230000000694 effects Effects 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 238000013473 artificial intelligence Methods 0.000 description 2
- 238000012790 confirmation Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 230000018109 developmental process Effects 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 238000013461 design Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000000977 initiatory effect Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 230000002035 prolonged effect Effects 0.000 description 1
- 230000000717 retained effect Effects 0.000 description 1
- 230000002441 reversible effect Effects 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
- 230000000007 visual effect Effects 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
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C3/00—Measuring distances in line of sight; Optical rangefinders
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Electromagnetism (AREA)
- Image Analysis (AREA)
- Traffic Control Systems (AREA)
Abstract
本发明公开了一种无人车在地下车库中的精确定位方法,该方法包括:激光点云地面分割模块、圆柱特征提取与特征匹配,以及非地面有效点选择模块、激光有效点合并与降采样模块、栅格地图产生与三维地图匹配模块、初始位姿模块。该方法的思路是利用圆柱特征,剔除激光点云场景中的动态障碍物,选择属于地下车库本身结构的激光有效点,然后利用地图匹配技术实现车辆精确定位。该方法对初始位姿的敏感性要远远低于传统方法,且具有较高的定位精度,适用于无人车在地下车库中的定位与导航,尤其适合在地下车库中含有大量动态障碍物的场景。
Description
技术领域
本发明属于无人驾驶汽车研究领域,特别涉及一种无人车在地下车库中的精确定位方法。
背景技术
近年来,随着新一代人工智能技术的兴起,无人驾驶汽车作为人工智能技术的重要算法验证平台应运而生,并呈现出井喷式的发展,尤其是在国内涌现出了一大批创业公司,直接推动了这一研究领域的快速发展。无人驾驶汽车是一种特殊的轮式移动机器人,其是依靠自身所携带的各类传感器,融合感知道路环境,自动获得车辆定位信息,规划行车路线,控制车辆运动,从而实现车辆的自主行驶功能。
定位技术在无人车研究领域内具有很重要的作用,涉及车辆感知、规划、控制等功能的准确实现。鉴于RTK-GPS具有高精度、高可靠性的特点,绝大部分无人驾驶车辆选择用GPS来进行定位,然而在当前的城市道路中存在着很多隧道、桥梁、较高的树木等遮挡物,GPS信号在这种环境下很容易受到干扰和阻挡,车辆定位精度严重不足,为了解决这一问题,人们提出利用GPS+IMU的组合定位方法,但其缺点是在长时间的隧道等GPS信号受到阻挡的环境中,很容易失效,或者车辆在GPS信号受到遮挡的环境中启动,无法确定车辆位姿。
为此,研究者提出要利用车辆自身携带的各类传感器进行感知定位,该方法首先要建立先验地图,然后利用车辆运动模型和惯性导航器件进行定位,这种方法在无GPS信号的环境中效果明显,但唯一不足的是这种方法容易受到感知环境数据中噪声或者动态障碍物的影响,从而影响定位精度。
发明内容
本发明的目的在于提供一种无人车在地下车库中的精确定位方法,以解决上述问题。
为实现上述目的,本发明采用以下技术方案:
一种无人车在地下车库中的精确定位方法,该方法包括无人车在地下车库中的精确定位***,所述无人车在地下车库中的精确定位***包括地面分割模块M1、特征提取与特征匹配,以及非地面有效点选择模块M2、有效点合并与降采样模块M3、栅格地图产生与三维地图匹配模块M4和初始位姿模块M5;地面分割模块M1连接特征提取与特征匹配,以及非地面有效点选择模块M2和有效点合并与降采样模块M3,特征提取与特征匹配,以及非地面有效点选择模块M2连接有效点合并与降采样模块M3,有效点合并与降采样模块M3连接栅格地图产生与三维地图匹配模块M4,初始位姿模块M5连接栅格地图产生与三维地图匹配模块M4;
地面分割模块M1包括地面分割模块M11、地面点云M13和非地面点云M12;
特征提取与特征匹配,以及非地面有效点选择模块M2包括特征提取M21、特征匹配M22和特征地图M23,
有效点合并与降采样模块M3包括点云合并M31、降采样M33和最终参与匹配点云M34;
栅格地图产生与三维地图匹配模块M4包括格栅地图生成模块M42和地图匹配模块M41,用于确定车辆在地下车库中的精确位姿;
初始位姿模块M5,用于为栅格地图产生与三维地图匹配模块M4提供车辆的初始位姿;
具体包括以下步骤:
步骤1,地面分割模块M11采用高斯过程回归算法实现对完整的三维激光扫描场景的分割,得到地面点云M13和非地面点云M12,地面点云M13和非地面点云M12分别表示为PGrd和PNoGrd;地面点云是场景结构中的点,属于有效点云;
步骤2,提取非地面点云的特征,并和事先建立的特征地图进行匹配,选择出场景中的有效特征,进而反推获得非地面有效点云,记为PNoGrdValid;
步骤3,模块M2产生的非地面有效点云PNoGrdValid与模块M1产生的地面点云PGrd参与的合并模块M31,得到全部的有效点云M32,表示为PValid,再经过降采样处理模块M33,得到最终参与地图匹配的部分有效点云M34,表示为PLastValid。该模块可表示为:
PLastValid=Downsample(PValid);
步骤4,基于Graph-SLAM技术实现栅格地图生成,通过点到面测度的配准算法实现地图匹配;假设两个点集为和在本发明中分别对应实时点云和栅格占据地图点云,Cpq(P,Q)表示点集P和点集Q之间的对应关系,该关系通过最近邻搜索实现,Np和Nq表示两个点集中点的个数,T=(R,t)为两个点集之间的变换关系,包括旋转变换R和位移变换t,满足R∈SO(3),t∈R3,则有:
其中,{pi,qi}∈Cpq为两个点集之间的一对对应点;
步骤5,初始位姿模块M5为地图匹配模块M41提供初始位姿信息,匹配得到精确位姿。
进一步的,地面分割模块M11利用高斯回归过程对采集到的激光点云进行分割处理,得到地面点云和包含有墙面、柱子、车辆、行人障碍物的非地面点云,其中地面点云能够在定位过程中提供丰富的俯仰角和横滚角信息,属于有效点云。
进一步的,特征提取M21功能是提取非地面点云的圆柱特征;特征地图M23是在地图创建过程中所标注的车库内部柱子、墙面、以及部分车辆的圆柱特征,为特征匹配M22提供模版;特征匹配M22是选择距离测度,度量特征之间的距离,采用余弦相似性测度:设两个圆柱特征F1和F2的距离为d(F1,F2),则有
以此来区分点云是否属于车库本身几何结构信息,如果属于则保留,否则删除,得到场景特征M24,为立柱和墙面点云的圆柱特征,最后根据场景特征,反推得到非地面有效点云M25。
进一步的,所述初始位姿模块M5用于为地图匹配模块M41提供足够精确的初始位姿;分为两种情况:第一,当车辆运行正常时,用前一帧定位的结果作为当前车辆定位的初始位姿,第二,当车辆在地下车库中启动时,或者车辆定位失败时,需要重新确定车辆的初始位姿,此时,启动全局重定位机制,确保车辆定位的鲁棒性。
进一步的,提取点云特征时,设计为圆柱体空间搜索,以便尽可能将整个立柱包含进来;搜索空间利用最近邻搜索算法快速实现,圆柱体采用二维欧式距离与高度阈值结合实现。
进一步的,圆柱特征提取流程,针对非地面点云进行处理,最后的输出为圆柱特征;首先寻找关键点,根据相关的一些显著性特点选择关键点,采用随机选取的方法;找到关键点之后,利用最近邻搜索和高度阈值限制的方法找出以关键点为中心的圆柱体内所有点,以这些点为基础,利用主成份分析技术建立局部坐标系;随后进行坐标变换,将圆柱内部所有的点通过坐标变换,转换到激光坐标下,以此来实现圆柱特征的旋转不变性;后续通过每次沿圆柱体中轴旋转固定角度,分别计算圆柱内所有点投影到x-y、x-z、y-z三个平面的统计量,该统计量为图像的高阶统计矩和香农熵,最后通过先后顺序组合成圆柱特征。
进一步的,圆柱特征的计算过程如下:首先分别计算圆柱内所有点Sc在x-y、x-z、y-z平面上的投影,投影图像分别表示为Ixy(i,j)、Ixz(i,j)、Iyz(i,j),然后计算投影图像的中心矩和香农熵,如下公式所示:
接下来,将圆柱体内部所有点Sc绕z轴以固定角度θ旋转,计算投影图像,以及图像的中心矩和香农熵,得到本次旋转后的圆柱特征Fk={Fxy,Fxz,Fyz},以此类推,旋转K次之后结束;最后将每次旋转之后计算得到的投影图像统计信息按照一定的排列规律合并起来,构成完整的圆柱特征向量F=F0∪…∪FK。
在特征计算完毕之后,判断此特征是否属于车库本身结构,以此来保留或是删除该圆柱体内部的点云;
其特征选择策略如下:假设创建的特征地图为FMAP={Fv,Fc,Fw},其中Fv表示车辆的圆柱特征,Fc表示车库中立柱的特征,Fw表示墙面的特征,在特征相似性计算过程中,得到当前特征Fx与特征地图之间的相似性关系D={d(Fx,Fv),d(Fx,Fc),d(Fx,Fw)},由此,根据如下公式获得圆柱体内的点属性:
<l,dmax>=argmaxD
其中,l表示特征的标签,1表示立柱,2表示墙面,3表示车辆,dmax表示Fx与特征地图的最大相似距离,则最终当前的圆柱特征Fx的属性为:
如果条件不满足则为未知特征属性,在后续处理中直接剔除;在地下车库定位中,只有属于车库本身结构的点才会准确定位,车辆、行人等不属于车库本身结构的动态障碍物会造成定位偏差,为此,我们将保留墙面和立柱上的点云,删除车辆等其他动态障碍物上的点;非地面有效点云PNoGrdValid为:
PNoGrdValid=(Pc∪Pw)-(Pv∪Pothers)
其中,Pc表示所有立柱上的点,Pw表示所有墙面上的点,Pv表示所有车辆上的点,Pothers表示其他未知物体上的点。
进一步的,坐标变换示意,Fuvw表示圆柱内所有点构建的局部坐标,Fxyz表示经过坐标转换后的激光坐标系,表示坐标转换关系;在圆柱特征构建过程中,为了得到特征的旋转不变性,坐标变换非常关键,它将局部坐标转换到全局坐标下进行计算;全局坐标已知,为车辆激光传感器坐标,局部坐标Fuvw需要计算确认,典型的方法为PCA(Principal Component Analysis)技术,鉴于圆柱特征的特点,所创建的坐标系为:
其中,ns为关键点ps的法向量。
进一步的,在重定位机制中选择FPFH特征作为全局匹配,启用RANSAC算法,找出对应关系确定粗定位变换,再根据场景几何一致性检测确定是否有效,以此类推,对每一关键帧做同样的处理,得到一组初始变换结果,最后根据每个初始变换的评估结果,选择最佳的变换作为初始位姿;其中评估函数为:
其中,是一个二值函数,用于统计匹配成功的点的个数,表示当两帧点云通过初始变换之后,对应点的最小距离;评估函数说明了当最小距离小于阈值时,认为匹配成功,否则失败,通过计算所有点的匹配情况,对比总的点数得到匹配的成功率。
与现有技术相比,本发明有以下技术效果:
本发明与常用的差分GPS定位***相比,在城区环境中不容易受到高楼、隧道等高大物体的遮挡而被干扰。该方法能够在完全丧失GPS信号的环境中实现鲁棒的定位,唯一的要求是提前生成三维激光占据栅格地图。
本发明与其他地图匹配方法相比较,该方法实时性较好,能够快速的得到精确定位结果。
本发明与其他地图匹配方法相比较,该方法对初始位姿的敏感度较低,能够在复杂的无GPS信号的环境中,鲁棒的实现精确定位。
附图说明
图1为本发明的***整体框架示意图。
图2为本发明的典型搜索空间示意图。
图3为本发明的典型三维激光扫描图。
图4为本发明的圆柱特征提取流程图。
图5为本发明的圆柱特征细节示意图。
图6为本发明的坐标变换示意图。
图7为本发明的三种形状模型示意图。
图8为本发明的三种形状模型的圆柱特征示意图。
图9为本发明的获取初始定位结果示意图。
图10为本发明的地下车库三维占据栅格地图。
图11为本发明的剔除障碍物点云结果图一。
图12为本发明的剔除障碍物点云结果图二。
图13为本发明的车辆定位结果图。
图14为本发明的x、y、z三个方向定位结果变化情况。
具体实施方式
以下结合附图对本发明作详细的说明:
参见图1,为本发明的***整体框架示意图,包括五个功能模块,分别是:
地面分割模块M1,其目的是通过地面分割模块M11,得到地面点云M13和非地面点云M12,其中地面分割模块采用高斯过程回归算法实现。地面点云和非地面点云分别表示为PGrd和PNoGrd;
特征提取与特征匹配,以及非地面有效点选择模块M2,其包括特征提取模块M21,特征匹配模块M22,得到场景特征M24,再反推得到非地面有效点云M25,另外还包括提前制作的特征地图M23,其中特征提取模块M21是在非地面有效点云中提取圆柱特征,该圆柱特征对检测地下车库中的立柱、墙面具有很高的效率,特征匹配模块M22是根据所提取的圆柱特征的特点,找到合适的距离测度对各个特征进行距离度量,在本发明中采用余弦相似性测度:设两个圆柱特征F1和F2的距离为d(F1,F2),则有
有效点合并与降采样模块M3,包括模块M2产生的非地面有效点云PNoGrdValid与模块M1产生的地面点云PGrd参与的合并模块M31,得到全部的有效点云M32,表示为PValid,再经过降采样处理模块M33,得到最终参与地图匹配的部分有效点云M34,表示为PLastValid。该模块可表示为:
PLastValid=Downsample(PValid)
栅格地图产生与三维地图匹配模块M4,包括地图匹配模块M41和栅格地图产生模块M42,其中栅格地图产生模块M42采用Graph-SLAM技术实现,地图匹配模块M41是基于点到面测度的配准算法实现。假设两个点集为和在本发明中分别对应实时点云和栅格占据地图点云,Cpq(P,Q)表示点集P和点集Q之间的对应关系,该关系通过最近邻搜索实现,Np和Nq表示两个点集中点的个数,T=(R,t)为两个点集之间的变换关系,包括旋转变换R和位移变换t,满足R∈SO(3),t∈R3,则有:
其中,{pi,qi}∈Cpq为两个点集之间的一对对应点。
初始位姿模块M5为地图匹配模块M41提供初始位姿信息,以加快算法的收敛性,提高定位效率,重定位机制见图9的说明。
参见图2,为本发明的典型搜索空间示意图。在计算提取点云特征时,常用的搜索空间为立方体和球形,立方体是划分三维空间的基本单元,较为直观,方便理解,球形设计将距离为半径以内都视为内点,本发明为了检测地下车库中的立柱和墙面,设计为圆柱体空间搜索,以便尽可能将整个立柱包含进来。以上三种搜索空间均利用最近邻搜索算法快速实现,其中立方体采用曼哈顿距离,球形采用欧式距离,圆柱体采用二维欧式距离与高度阈值结合实现。
参见图3,为本发明的典型三维激光扫描图,该图为Velodyne公司HDL-64E-S3激光传感器所采集的激光点云,场景为典型城区十字路口。此传感器有效探测距离可达100m,能够获取车辆周围360度范围内的场景三维信息,具有内密外疏的扫描特点,距离较远时误差较大。
参见图4,为本发明的圆柱特征提取流程,针对非地面点云进行处理,最后的输出为圆柱特征。首先寻找关键点,通常的做法是根据相关的一些显著性特点选择关键点,本发明采用随机选取的方法,简单快捷,不足的是某些点会被计算两次或者多次,经过实验对比发现,这种选取方法对时间消耗影响不大。找到关键点之后,利用最近邻搜索和高度阈值限制的方法找出以关键点为中心的圆柱体内所有点,以这些点为基础,利用主成份分析技术(PCA)建立局部坐标系。随后进行坐标变换,将圆柱内部所有的点通过坐标变换,转换到激光坐标下,以此来实现圆柱特征的旋转不变性。后续的步骤用来计算旋转统计不变特征,通过每次沿圆柱体中轴旋转固定角度,分别计算圆柱内所有点投影到x-y、x-z、y-z三个平面的统计量,该统计量为图像的高阶统计矩和香农熵,最后通过先后顺序组合成圆柱特征。
参见图5,为本发明的圆柱特征细节示意,图中符号说明:
ps为关键点;
po为圆柱体底部中心点;
pi为圆柱体内部任意一点;
r为圆柱体半径;
Z为圆柱体高度;
W为x-z、y-z平面投影图像的宽度,也是x-y平面投影图像的高度;
H为x-z、y-z平面投影图像的高度;
dz为pi点到圆柱底平面的距离,满足0<dz≤Z;
dxy为pi点在圆柱底部平面的投影到底部中心点的距离,满足0<dxy≤r;
假设Sc为圆柱体内所有点构成的集合,则有pi∈Sc。
圆柱特征的计算过程如下:首先分别计算圆柱内所有点Sc在x-y、x-z、y-z平面上的投影,投影图像分别表示为Ixy(i,j)、Ixz(i,j)、Iyz(i,j),然后计算投影图像的中心矩和香农熵,如下公式所示:
接下来,将圆柱体内部所有点Sc绕z轴以固定角度θ旋转,计算投影图像,以及图像的中心矩和香农熵,得到本次旋转后的圆柱特征Fk={Fxy,Fxz,Fyz},以此类推,旋转K次之后结束。最后将每次旋转之后计算得到的投影图像统计信息按照一定的排列规律合并起来,构成完整的圆柱特征向量F=F0∪…∪FK。本发明公开的方法中,Sc绕局部坐标的z轴共旋转三次,每次的角度为30度,为了更加丰富圆柱特征,将旋转的角度适当调节。
在特征计算完毕之后,需要及时的判断此特征是否属于车库本身结构,以此来保留或是删除该圆柱体内部的点云。其特征选择策略如下:假设创建的特征地图为FMAP={Fv,Fc,Fw},其中Fv表示车辆的圆柱特征,Fc表示车库中立柱的特征,Fw表示墙面的特征,在特征相似性计算过程中,得到当前特征Fx与特征地图之间的相似性关系D={d(Fx,Fv),d(Fx,Fc),d(Fx,Fw)},由此,根据如下公式获得圆柱体内的点属性:
<l,dmax>=argmaxD
其中,l表示特征的标签,1表示立柱,2表示墙面,3表示车辆,dmax表示Fx与特征地图的最大相似距离,则最终当前的圆柱特征Fx的属性为:
如果条件不满足则为未知特征属性,在后续处理中直接剔除即可。在地下车库定位中,只有属于车库本身结构的点才会准确定位,车辆、行人等不属于车库本身结构的动态障碍物会造成定位偏差,为此,我们将保留墙面和立柱上的点云,删除车辆等其他动态障碍物上的点。非地面有效点云PNoGrdValid为:
PNoGrdValid=(Pc∪Pw)-(Pv∪Pothers)
其中,Pc表示所有立柱上的点,Pw表示所有墙面上的点,Pv表示所有车辆上的点,Pothers表示其他未知物体上的点。
参见图6,为本发明的坐标变换示意,Fuvw表示圆柱内所有点构建的局部坐标,Fxyz表示经过坐标转换后的激光坐标系,表示坐标转换关系。在圆柱特征构建过程中,为了得到特征的旋转不变性,坐标变换非常关键,它将局部坐标转换到全局坐标下进行计算。本发明中全局坐标已知,为车辆激光传感器坐标,局部坐标Fuvw需要计算确认,典型的方法为PCA(Principal Component Analysis)技术,鉴于圆柱特征的特点,所创建的坐标系为:
其中,ns为关键点ps的法向量。
参见图7,为本发明的三种典型形状模型以及图8为三种典型形状模型的圆柱特征。此模型只针对常见地下车库中的结构,通常认为包含有墙面、立柱、车辆,从图7中看到三种典型形状模型的圆柱特征都是在全局坐标系下计算,图8中是图7中对应形状的圆柱特征,从图中看到,圆柱特征具有很强的鉴别能力,将立柱和墙面从复杂的场景中分离出来。
参见图9,为本发明的获取初始定位结果示意,此图描述了当无人车在地下车库中启动时,或者在定位识别时,启动重定位机制。根据图示,图中关键帧为创建地图时的点云帧,它包含有场景点云和其对应的位姿信息。输入信息为实时点云和地图关键帧,在本发明中该关键帧选为7帧,其根据场景大小,以及每帧点云所覆盖的范围来确定,选择的越多,得到的初始化位姿会越准确,相应的计算时间会变长。在重定位机制中选择FPFH特征作为全局匹配,启用RANSAC算法,找出对应关系确定粗定位变换,再根据场景几何一致性检测确定是否有效。以此类推,对每一关键帧做同样的处理,得到一组初始变换结果,最后根据每个初始变换的评估结果,选择最佳的变换作为初始位姿。其中评估函数为:
其中,是一个二值函数,用于统计匹配成功的点的个数,表示当两帧点云通过初始变换之后,对应点的最小距离。评估函数说明了当最小距离小于阈值时,认为匹配成功,否则失败,通过计算所有点的匹配情况,对比总的点数得到匹配的成功率,实验证明了此方法有效的评估初始变换的效果。
参见图10,为本发明的地下车库三维占据栅格地图,该地图通过Graph-SLAM技术实现。
参见图11和图12,为本发明的剔除障碍物点云的结果图,从图中看到本发明通过提取圆柱特征的方法剔除动态障碍物,保留能够提高定位精度的场景本身结果的点。
参见图13和图14,分别为本发明的车辆定位结果图和x、y、z三个方向定位结果变化情况,从图13中看到车辆定位轨迹光滑准确,结合图14,x和y方向是的位姿变换非常光滑,只有z方向上存在一些波动,但波动范围在0.1m以内,满足定位要求。
以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明,不能认定本发明的具体实施方式仅限于此,对于本发明所属技术领域的普通技术人员来说,在不脱离本发明构思的前提下,还做出若干简单的推演或替换,都应当视为属于本发明由所提交的权利要求书确定专利保护范围。
Claims (9)
1.一种无人车在地下车库中的精确定位方法,其特征在于,该方法基于无人车在地下车库中的精确定位***,所述无人车在地下车库中的精确定位***包括地面分割模块(M1)、特征提取与特征匹配,以及非地面有效点选择模块(M2)、有效点合并与降采样模块(M3)、栅格地图产生与三维地图匹配模块(M4)和初始位姿模块(M5);地面分割模块(M1)连接特征提取与特征匹配,以及非地面有效点选择模块(M2)和有效点合并与降采样模块(M3),特征提取与特征匹配,以及非地面有效点选择模块(M2)连接有效点合并与降采样模块(M3),有效点合并与降采样模块(M3)连接栅格地图产生与三维地图匹配模块(M4),初始位姿模块(M5)连接栅格地图产生与三维地图匹配模块(M4);
地面分割模块(M1)包括地面分割模块(M11)、地面点云(M13)和非地面点云(M12);
特征提取与特征匹配,以及非地面有效点选择模块(M2)包括特征提取(M21)、特征匹配(M22)、场景特征(M24)、非地面有效云点(M25)和特征地图(M23),
有效点合并与降采样模块(M3)包括点云合并(M31)、降采样(M33)、全部有效点云(M32)和最终参与匹配点云(M34);
栅格地图产生与三维地图匹配模块(M4)包括格栅地图生成模块(M42)和地图匹配模块(M41),用于确定车辆在地下车库中的精确位姿;
初始位姿模块(M5),用于为栅格地图产生与三维地图匹配模块(M4)提供车辆的初始位姿;
具体包括以下步骤:
步骤1,地面分割模块(M11)采用高斯过程回归算法实现对完整的三维激光扫描场景的分割,得到地面点云(M13)和非地面点云(M12),地面点云(M13)和非地面点云(M12)分别表示为PGrd和PNoGrd;地面点云是场景结构中的点,属于有效点云;
步骤2,提取非地面点云的特征,并和事先建立的特征地图进行匹配,选择出场景中的有效特征,进而反推获得非地面有效点云,记为PNoGrdValid;
步骤3,非地面有效点选择模块(M2)产生的非地面有效点云PNoGrdValid与地面分割模块(M1)产生的地面点云PGrd参与的点云合并(M31),得到全部有效点云(M32),表示为PValid,再经过降采样(M33),得到最终参与匹配点云(M34),表示为PLastValid;PLastValid模块表示为:
PLastValid=Downsample(PValid);
步骤4,基于Graph-SLAM技术实现栅格地图生成,通过点到面测度的配准算法实现地图匹配;假设两个点集为和在分别对应实时点云和栅格占据地图点云,Cpq(P,Q)表示点集P和点集Q之间的对应关系,该关系通过最近邻搜索实现,Np和Nq表示两个点集中点的个数,T=(R,t)为两个点集之间的变换关系,包括旋转变换R和位移变换t,满足R∈SO(3),t∈R3,则有:
其中,{pi,qi}∈Cpq为两个点集之间的一对对应点;
步骤5,初始位姿模块(M5)为地图匹配模块(M41)提供初始位姿信息,匹配得到精确位姿。
2.根据权利要求1所述的一种无人车在地下车库中的精确定位方法,其特征在于,地面分割模块(M11)利用高斯回归过程对采集到的激光点云进行分割处理,得到地面点云和包含有墙面、柱子、车辆、行人障碍物的非地面点云,其中地面点云能够在定位过程中提供丰富的俯仰角和横滚角信息,属于有效点云。
4.根据权利要求1所述的一种无人车在地下车库中的精确定位方法,其特征在于,所述初始位姿模块(M5)用于为地图匹配模块(M41)提供精确的初始位姿;分为两种情况:第一,当车辆运行正常时,用前一帧定位的结果作为当前车辆定位的初始位姿,第二,当车辆在地下车库中启动时,或者车辆定位失败时,需要重新确定车辆的初始位姿,此时,启动全局重定位机制,确保车辆定位的鲁棒性。
5.根据权利要求1所述的一种无人车在地下车库中的精确定位方法,其特征在于,提取点云特征时,设计为圆柱体空间搜索,以便将整个立柱包含进来;搜索空间利用最近邻搜索算法快速实现,圆柱体采用二维欧式距离与高度阈值结合实现。
6.根据权利要求5所述的一种无人车在地下车库中的精确定位方法,其特征在于,圆柱特征提取流程,针对非地面点云进行处理,最后的输出为圆柱特征;首先寻找关键点,采用随机选取的方法,根据相关的一些显著性特点选择关键点;找到关键点之后,利用最近邻搜索和高度阈值限制的方法找出以关键点为中心的圆柱体内所有点,以这些点为基础,利用主成份分析技术建立局部坐标系;随后进行坐标变换,将圆柱内部所有的点通过坐标变换,转换到激光坐标下,以此来实现圆柱特征的旋转不变性;后续通过每次沿圆柱体中轴旋转固定角度,分别计算圆柱内所有点投影到x-y、x-z、y-z三个平面的统计量,该统计量为图像的高阶统计矩和香农熵,最后通过先后顺序组合成圆柱特征。
7.根据权利要求6所述的一种无人车在地下车库中的精确定位方法,其特征在于,圆柱特征的计算过程如下:首先分别计算圆柱内所有点Sc在x-y、x-z、y-z平面上的投影,投影图像分别表示为Ixy(i,j)、Ixz(i,j)、Iyz(i,j),然后计算投影图像的中心矩和香农熵,如下公式所示:
将圆柱体内部所有点Sc绕z轴以固定角度θ旋转,计算投影图像,以及图像的中心矩和香农熵,得到本次旋转后的圆柱特征Fk={Fxy,Fxz,Fyz},以此 类推,旋转K次之后结束;最后将每次旋转之后计算得到的投影图像统计信息按照一定的排列规律合并起来,构成完整的圆柱特征向量F=F0∪…∪FK;
在特征计算完毕之后,判断此特征是否属于车库本身结构,以此来保留或是删除该圆柱体内部的点云;
其特征选择策略如下:假设创建的特征地图为FMAP={Fv,Fc,Fw},其中Fv表示车辆的圆柱特征,Fc表示车库中立柱的特征,Fw表示墙面的特征,在特征相似性计算过程中,得到当前特征Fx与特征地图之间的相似性关系D={d(Fx,Fv),d(Fx,Fc),d(Fx,Fw)},由此,根据如下公式获得圆柱体内的点属性:
<l,dmax>=arg max D
其中,l表示特征的标签,1表示立柱,2表示墙面,3表示车辆,dmax表示Fx与特征地图的最大相似距离,则最终当前的圆柱特征Fx的属性为:
如果条件不满足则为未知特征属性,在后续处理中直接剔除;在地下车库定位中,只有属于车库本身结构的点才会准确定位,车辆、行人不属于车库本身结构的动态障碍物会造成定位偏差,为此,将保留墙面和立柱上的点云,删除其他动态障碍物上的点,其他动态障碍物包括车辆;非地面有效点云PNoGrdValid为:
PNoGrdValid=(Pc∪Pw)-(Pv∪Pothers)
其中,Pc表示所有立柱上的点,Pw表示所有墙面上的点,Pv表示所有车辆上的点,Pothers表示其他未知物体上的点。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810427773.6A CN108917761B (zh) | 2018-05-07 | 2018-05-07 | 一种无人车在地下车库中的精确定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810427773.6A CN108917761B (zh) | 2018-05-07 | 2018-05-07 | 一种无人车在地下车库中的精确定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108917761A CN108917761A (zh) | 2018-11-30 |
CN108917761B true CN108917761B (zh) | 2021-01-19 |
Family
ID=64403608
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810427773.6A Expired - Fee Related CN108917761B (zh) | 2018-05-07 | 2018-05-07 | 一种无人车在地下车库中的精确定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108917761B (zh) |
Families Citing this family (24)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111323027A (zh) * | 2018-12-17 | 2020-06-23 | 兰州大学 | 一种基于激光雷达与环视相机融合制作高精度地图方法及装置 |
CN111469781B (zh) * | 2019-01-24 | 2023-06-23 | 北京京东乾石科技有限公司 | 用于输出信息的方法和装置 |
CN109889977B (zh) * | 2019-02-25 | 2021-01-12 | 广州市香港科大***研究院 | 一种基于高斯回归的蓝牙定位方法、装置、设备和*** |
CN109903383B (zh) * | 2019-04-11 | 2020-11-10 | 中国矿业大学 | 一种采煤机在工作面煤层三维模型中精确定位方法 |
CN110031825B (zh) * | 2019-04-17 | 2021-03-16 | 北京智行者科技有限公司 | 激光定位初始化方法 |
CN110349192B (zh) * | 2019-06-10 | 2021-07-13 | 西安交通大学 | 一种基于三维激光点云的在线目标跟踪***的跟踪方法 |
CN112634181B (zh) * | 2019-09-24 | 2024-06-14 | 阿波罗智能技术(北京)有限公司 | 用于检测地面点云点的方法和装置 |
CN112923933A (zh) * | 2019-12-06 | 2021-06-08 | 北理慧动(常熟)车辆科技有限公司 | 一种激光雷达slam算法与惯导融合定位的方法 |
CN112991440B (zh) * | 2019-12-12 | 2024-04-12 | 纳恩博(北京)科技有限公司 | 车辆的定位方法和装置、存储介质和电子装置 |
CN111239763A (zh) * | 2020-03-06 | 2020-06-05 | 广州视源电子科技股份有限公司 | 对象的定位方法、装置、存储介质和处理器 |
CN112017219B (zh) * | 2020-03-17 | 2022-04-19 | 湖北亿咖通科技有限公司 | 一种激光点云配准方法 |
CN111968179B (zh) * | 2020-08-13 | 2022-07-19 | 厦门大学 | 地下停车场自动驾驶车辆定位方法 |
WO2022087916A1 (zh) * | 2020-10-28 | 2022-05-05 | 华为技术有限公司 | 定位方法、装置、电子设备和存储介质 |
CN112382116A (zh) * | 2020-11-12 | 2021-02-19 | 浙江吉利控股集团有限公司 | 一种用于获取车辆的点云地图的方法和*** |
CN112710318B (zh) * | 2020-12-14 | 2024-05-17 | 深圳市商汤科技有限公司 | 地图生成方法、路径规划方法、电子设备以及存储介质 |
CN112700479B (zh) * | 2020-12-23 | 2024-02-23 | 北京超星未来科技有限公司 | 一种基于cnn点云目标检测的配准方法 |
CN112329749B (zh) * | 2021-01-05 | 2021-04-27 | 新石器慧通(北京)科技有限公司 | 点云的标注方法及标注设备 |
CN113778077B (zh) * | 2021-02-09 | 2024-04-16 | 贵州京邦达供应链科技有限公司 | 移动平台的定位方法、设备以及存储介质 |
CN113593021B (zh) * | 2021-06-22 | 2023-06-09 | 天津大学 | 一种基于轮廓分割的车库点云地图构建方法 |
CN113465607A (zh) * | 2021-06-30 | 2021-10-01 | 上海西井信息科技有限公司 | 港口车辆定位方法、装置、电子设备、存储介质 |
CN113568003B (zh) * | 2021-07-26 | 2022-11-01 | 奥特酷智能科技(南京)有限公司 | 一种用于机场地勤车的防撞预警***及方法 |
CN114280583B (zh) * | 2022-03-02 | 2022-06-17 | 武汉理工大学 | 无gps信号下激光雷达定位精度验证方法及*** |
CN114577215B (zh) * | 2022-03-10 | 2023-10-27 | 山东新一代信息产业技术研究院有限公司 | 一种移动机器人的特征地图更新方法、设备及介质 |
CN116719067B (zh) * | 2023-08-08 | 2023-10-17 | 科沃斯家用机器人有限公司 | 基准站位置变动的检测方法、设备及可读存储介质 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103389103A (zh) * | 2013-07-03 | 2013-11-13 | 北京理工大学 | 一种基于数据挖掘的地理环境特征地图构建与导航方法 |
CN104897161A (zh) * | 2015-06-02 | 2015-09-09 | 武汉大学 | 基于激光测距的室内平面地图制图方法 |
CN106896353A (zh) * | 2017-03-21 | 2017-06-27 | 同济大学 | 一种基于三维激光雷达的无人车路口检测方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR102139667B1 (ko) * | 2014-02-14 | 2020-07-31 | 삼성전자주식회사 | 정보 획득 방법 및 디바이스. |
-
2018
- 2018-05-07 CN CN201810427773.6A patent/CN108917761B/zh not_active Expired - Fee Related
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103389103A (zh) * | 2013-07-03 | 2013-11-13 | 北京理工大学 | 一种基于数据挖掘的地理环境特征地图构建与导航方法 |
CN104897161A (zh) * | 2015-06-02 | 2015-09-09 | 武汉大学 | 基于激光测距的室内平面地图制图方法 |
CN106896353A (zh) * | 2017-03-21 | 2017-06-27 | 同济大学 | 一种基于三维激光雷达的无人车路口检测方法 |
Non-Patent Citations (2)
Title |
---|
基于三维激光雷达的动态障碍实时检测与跟踪;杨飞等;《浙江大学学报(工学版)》;20120930;第46卷(第9期);全文 * |
基于多线激光雷达的非结构化道路感知技术研究;李宁等;《车辆与动力技术》;20171231(第3期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN108917761A (zh) | 2018-11-30 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108917761B (zh) | 一种无人车在地下车库中的精确定位方法 | |
Wang et al. | Torontocity: Seeing the world with a million eyes | |
Nagy et al. | Real-time point cloud alignment for vehicle localization in a high resolution 3D map | |
Yu et al. | Automated extraction of urban road facilities using mobile laser scanning data | |
CN109631855A (zh) | 基于orb-slam的高精度车辆定位方法 | |
Sohn et al. | Using a binary space partitioning tree for reconstructing polyhedral building models from airborne lidar data | |
CN112785643A (zh) | 一种基于机器人平台的室内墙角二维语义地图构建方法 | |
CN111242041A (zh) | 基于伪图像技术的激光雷达三维目标快速检测方法 | |
Wang et al. | Window detection from mobile LiDAR data | |
CN112362072A (zh) | 一种复杂城区环境中的高精度点云地图创建***及方法 | |
Nagy et al. | 3D CNN-based semantic labeling approach for mobile laser scanning data | |
CN112184736A (zh) | 一种基于欧式聚类的多平面提取方法 | |
Zhang et al. | 3D highway curve reconstruction from mobile laser scanning point clouds | |
Wang et al. | A method for detecting windows from mobile LiDAR data | |
Alidoost et al. | Y-shaped convolutional neural network for 3d roof elements extraction to reconstruct building models from a single aerial image | |
CN115690138A (zh) | 一种融合车载影像与点云的道路边界提取与矢量化方法 | |
Gálai et al. | Crossmodal point cloud registration in the Hough space for mobile laser scanning data | |
Lu et al. | Pole-based localization for autonomous vehicles in urban scenarios using local grid map-based method | |
KR102534031B1 (ko) | 도로 기호 및 문자 추출방법 | |
Wu et al. | A stepwise minimum spanning tree matching method for registering vehicle-borne and backpack LiDAR point clouds | |
Nagy et al. | 3D CNN based phantom object removing from mobile laser scanning data | |
CN112435336B (zh) | 一种弯道类型识别方法、装置、电子设备及存储介质 | |
CN117036733A (zh) | 一种城市道路场景特征线提取的方法 | |
CN111323026B (zh) | 一种基于高精度点云地图的地面过滤方法 | |
CN113781639B (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 | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20210119 |
|
CF01 | Termination of patent right due to non-payment of annual fee |