针对低纹理车库的智能网联汽车激光雷达定位系统及方法与流程

1.本发明涉及无人驾驶车辆定位技术领域，特别是涉及一种针对车库的智能网联汽车激光雷达定位系统。


背景技术：

2.智能网联汽车定位解决了“车辆在哪”的基本问题，是诸如导航、避障和路径规划等应用的基础。随着智能网联汽车的不断发展，安全驾驶、车路协同和智慧交通等落地均需要实时精准的车辆定位。在空旷的室外环境之中，智能网联汽车可以依赖于gnss信号与rtk的差分定位获取实时的精准位置信息。然而，在仍需高精定位的室内车库环境中，卫星信号受到遮挡，需要选用其他的定位方法。参照于室内机器人的定位方法，现有的技术方案应用较多的有基于imu惯性导航的定位、wifi信号定位、rfid标签定位和激光雷达定位等。这些现有技术分别存在以下的优缺点：
3.1、基于imu性导航的定位可以在进行高频定位，同时结合车辆本身的轮速计与陀螺仪等设备知晓车辆当前状态的变换信息。然而，这种方法存在累积误差，并且出现误差后无法进行校准。由于车辆的行驶距离通常较远并具有一定速度，短距离的精准定位应用于长时间行驶会造成无法进行校正的漂移问题。
4.2、rfid标签定位通过电磁感应原理可以无线激发近距离无线标签，基于贴在不同位置的多个标签进行信息读取，但是rfid无法做到实时的定位，只能确定在一定的区域之内，标签的定位范围平均在4
‑
6m，同时在车库内通信信号会受到不同的干扰，导致定位的精度变差，无法进行精准定位。
5.3、wifi定位是一种应用于室内定位研究较为广泛的一种方法，主要包括两种定位方法，信号强度差定位和指纹定位，但是这些方法需要多个近距离的ap得到接收的信号强度以及角度，但是车库空间较大，进行大量ap安装会有一定的成本并且实时性和精度并不足以达到汽车驾驶的要求。
6.4、激光slam定位已经有了丰富的研究成果，可以应用于智能网联车辆定位，激光雷达到达范围广，测量精度高，不受光线影响，抗干扰能力强。然而，通过以往的实验结果表明，精准定位需要较为明显的几何特征信息作为基础进行比较，对低纹理车库环境的定位效果误差较大。经过对多个车库的场景进行问题分析，我们提出了一种面向智能网联汽车的应用于低纹理车库环境中的新型定位方法。


技术实现要素：

7.本发明针对上述大量车库普遍存在的问题，提出了针对低纹理车库的智能网联汽车激光雷达定位系统及方法，基于激光雷达传感器和先验地图实现了适用于具有低纹理特点的室内车库智能网联汽车定位系统。
8.本发明利用以下技术方案实现：
9.一种针对低纹理车库的智能网联汽车激光雷达定位系统，该系统包括初始位姿确
定模块100、静态物体滤波模块200、特征提取模块300、基于点云强度的里程计匹配模块400、雷帧与先验地图匹配模块500以及融合定位模块600；其中：
10.所述初始位姿确定模块100，在车辆初次进入车库时生效，用于捕捉到车辆初次进入车库时的位姿；
11.所述静态物体滤波模块200，用于，滤除从激光雷达获取到的点云帧中的动态物体和地面点，获得静态物体；
12.所述特征提取模块300，用于在进行特征点提取时结合强度差和几何空间差选取静态物体骨架点；
13.所述基于点云强度的里程计匹配模块400，用于进行激光雷达里程计采集到的相邻帧间的匹配；
14.所述雷达点云帧与先验地图匹配模块500，用于将雷达点云帧与在先验地图之中的里程计记录的累计位姿进行匹配；
15.所述融合定位模块600，用于将初始位姿进行雷达点云帧与先验地图进行匹配所得到的车辆位姿和将滤波后得到的静态物体点进行骨架特征进行基于强度的里程计匹配所得到的车辆位姿进行融合，得到车辆的定位结果。
16.一种针对低纹理车库的智能网联汽车激光雷达定位方法，该流程具体步骤如下：
17.步骤1：判断此次是否为首次调用系统，如果是，则转至步骤2，进行初始位姿确定；如果否，则转至步骤4，进行静态物体滤波；
18.步骤2：确定车辆初始位姿；
19.步骤3：将初始位姿发送给雷达点云帧与先验地图进行匹配，其中所述先验地图采用实时更新与加载的地图流；
20.步骤4：进行静态物体滤波：通过对于点云库中的数据统计进行合适的动态阈值滤波，在当前t时刻，从激光雷达获取到的点云帧中进行静态物体的提取，具体包括：通过强度过滤有效高速的去除动态物体和地面点；
21.步骤5：对滤波后得到的静态物体点进行骨架特征提取；对当前情况是否为长廊进行判断，如果为长廊情况则进行分视角的特征提取；
22.步骤6：将滤波后得到的静态物体点进行骨架特征进行基于强度的里程计匹配，即利用激光雷达里程计对连续两帧点云进行匹配，获得局部位姿变换；通过预测估计的车辆变换对两帧点云进行对齐，然后对两帧点云中的最近点作为对应点，之后对变换不断进行地图下降迭代调整，直到误差最小为止；
23.步骤7：将初始位姿进行雷达点云帧与先验地图进行匹配和将滤波后得到的静态物体点进行骨架特征进行基于强度的里程计匹配。
24.与现有技术相比，本发明能够得到高频高精度的车辆定位结果。
附图说明
25.图1为本发明的一种针对低纹理车库的智能网联汽车激光雷达定位系统模块示意图；
26.图2为本发明的一种针对低纹理车库的智能网联汽车激光雷达定位方法整体流程图；
27.图3为初始帧与初始地图点面距离示意图；
28.图4为点云中点的强度分布示意图；
29.图5为车库数据包中的阈值分布示图；
30.图6为按照强度进行静态物体滤波的结果图；
31.图7为几何差距不明显但强度差距明显的车库内的门示意图；
32.图8为长廊情况下的距离分布与角度的关系图；
33.图9为根据角度进行切分的点云图。
具体实施方式
34.下面结合附图和具体实施例对本发明技术方案作进一步详细描述。
35.如图1所示，为本发明的一种针对低纹理车库的智能网联汽车激光雷达定位系统模块示意图。其中低纹理是指车库中主要存在的平面较多，纹理不明显的场景较多的问题，该系统分为五个部分，分别是初始位姿确定模块100、静态物体滤波模块200、特征提取模块300、基于点云强度的里程计匹配模块400、雷帧与先验地图匹配模块500以及融合定位模块600。其中：
36.所述初始位姿确定模块100，在车辆初次进入车库时生效，用于捕捉到车辆初次进入车库时的位姿；
37.所述静态物体滤波模块200，用于，滤除从激光雷达获取到的点云帧中的动态物体和地面点，获得静态物体；
38.所述特征提取模块300，用于在进行特征点提取时结合强度差和几何空间差选取静态物体骨架点；
39.所述基于点云强度的里程计匹配模块400，用于进行激光雷达里程计采集到的相邻帧间的匹配；
40.所述雷达点云帧与先验地图匹配模块500，用于将雷达点云帧与在先验地图之中的里程计记录的累计位姿进行匹配；
41.所述融合定位模块600，用于将初始位姿进行雷达点云帧与先验地图进行匹配所得到的车辆位姿和将滤波后得到的静态物体点进行骨架特征进行基于强度的里程计匹配所得到的车辆位姿进行融合，得到车辆的定位结果。
42.如图2所示，为本发明的一种针对低纹理车库的智能网联汽车激光雷达定位方法整体流程图。该流程具体步骤如下：
43.步骤1：判断此次是否为首次调用系统，如果是，则转至步骤2，进行初始位姿确定；如果否，则转至步骤4，进行静态物体滤波；
44.步骤2：初始位姿确定；
45.步骤3：将初始位姿发送给雷达点云帧与先验地图进行匹配；与其他基于先验地图的激光雷达定位方法相比，不同之处在于本发明不使用全局地图，而是采用实时更新与加载的地图流。因为在车库中，有很多重复的结构，容易发生错误而无法改正。使用实时更新的地图流进行比较，地图流的建立，有边缘地图和平面地图两种。平面地图由墙壁、地板和天花板组成，用于一般比较。边缘地图是由车库中的边缘点组成的，如墙边界、柱边界等进行更详细的对比；之后通过当前帧与地图进行比较得到更加精准的匹配结果帧与先验地图
匹配，本步骤能够减少累积漂移和匹配时间；在低纹理长廊车库内，基于先验地图可以得到频率在10hz，精度在10cm内的实时定位；
46.步骤3.1：选取车库入口地图，具体操作包括：选定车辆进入时走的是哪一个入口，将从室外进入到车库之中的影像初始帧与车库的一些固定的入口处的局部地图进行比较，选取不同入口的局部地图进行匹配之后取匹配度最高的地图作为当前被比较的车库入口局部地图；
47.如果当前与地图的匹配错误，此时匹配的地图应改为附近局部地图；
48.步骤3.2：之后，将激光雷达扫描得到的点云帧与车库入口先验地图进行配准。为了加速匹配，本发明采用了多线程gicp算法以得到初始位姿。首先对当前t时刻的点云进行降采样得到点云p0＝{p
i
}，i＝1，......，n与地图帧进行比较，其中，表示骨架点形成的地图，表示平面点组成的地图，如图2所示，为初始帧与初始地图点面距离示意图。其中，a为初始帧降采样后的某一水平线a上的点，b1为与a所在线对应线b1上的距离最近的点，b2为同一线b1上距离最近的点，b3为相邻线b2上最近的点，b1、b2和b3形成一个面(灰色的地图帧)。计算点与面的距离d，公式如下：
[0049][0050]
其中，为初始帧a到对应点b1的向量，为地图帧中的相邻三个点形成的向量；进行不断迭代匹配，对于变换不断优化使得距离最小；
[0051]
在得到变换结果后，进行后续帧与地图的比较，进行车辆变换的累计，公式如下：
[0052][0053]
其中，t表示车辆的变换，其右上角标表示在不同坐标下的变换，m表示先验地图的坐标系，l表示激光雷达的坐标系，t表示时刻；
[0054]
通过公式中的变换得到先验地图坐标中的全局坐标。初始位姿有助于将车辆坐标系统合并到先验地图的坐标系统中，从而车辆可以利用先验地图数据提高精度，并获取其他物体在地图上的相对位置。
[0055]
步骤4：进行静态物体滤波：通过对于点云库中的数据统计进行合适的动态阈值滤波，得到相对静态物体；在当前t时刻，从激光雷达获取到的点云帧中进行静态物体的提取，从而减少车库之中的动态物体的干扰，具体做法是通过强度过滤有效高速的去除动态物体和地面点；
[0056]
步骤4.1：对于点云的强度进行转换。点云的强度值反映了被测物体的反射率，可以作为区分物体的标准。点云中每一点返回的信息有(x，y，z，i)，(x，y，z)表示相对于激光雷达的物理坐标，计算每一点的反射强度i，公式如下：
[0057][0058]
其中，ρ为被测物体本身的材质，r为被测物体距离激光雷达的距离，λ为被测物体的反射角，通过公式的推导将点云强度的值转化为被测物体本身的材质直接相关成正比：
[0059]
[0060]
步骤4.2：有效性验证，即抽取点云帧对这种滤波方法是否有效的分割了静态物体和非静态物体进行了验证。
[0061]
如图4所示，为点云中点的强度分布。以图3为示例对强度滤波方法的可信度进行分析。通过对多个车库内收集的数据，得到整帧点云中所有点强度的分布的分析结果。之后，通过传统的较为准确的方法cloth simulation filter(csf)对点云中的地面进行分割，其中地面点集中在点云强度较小的区域，同时还手动分割了车辆点和静态物体，可较为明显地发现车辆点的点云强度也较小，而静态物体的点有小部分分布在点云强度较小的区域，大部分分布在点云强度较大的区域，并且在点云强度较大的区域之中黄色折线与蓝色折线几乎重合，表明此时没有非静态物体的干扰。可以选取合适的阈值进行分割。需要说明的是，虽然消除了一些静态物体的点云，但是相对于整体情况而言并没有很大影响。
[0062]
步骤4.3：计算出一个合适的阈值作为点云强度滤波的基准，该阈值是动态的，可以根据不同的环境选取合适的不同阈值；由于同一个车库内的物体材质一般不会进行很大的改变，所以在进行先验地图构建时对于数据库进行分析，对每一帧点云强度分布中的斜率进行计算，选取点云分布中强度变化的明显凹值。如图5所示，为车库数据包中的阈值分布示例。数据库中提取得到的阈值为正态分布，所以选取正态分布中的μ值作为滤波的阈值。通过这个合适的阈值进行静态物体滤波得到的效果如图所示。其中，不规则的为非静态点，规则的为静态物体点。如图6所示，为静态物体滤波结果示例图；
[0063]
步骤5：对滤波后得到的静态物体点进行骨架特征提取；因为点云之中的点数量较多，而在进行比较时需要高频的比较结果，所以需要对点云进行特征提取，选取特征明显的易于匹配的点作为特征点进行比较；由于车库内长廊问题的存在，对当前情况是否为长廊进行判断，如果为长廊情况则进行分视角的特征提取，使得特征点分布在更有参考价值的区域；如图7所示，为几何差距不明显但强度差距明显的车库内的门示意图。
[0064]
步骤5.1：计算特征描述子。在进行提取时不仅考虑到了大家常用的几何差异，同时也考虑到了的强度差，这样就可以获取点云之中几何差异不明显的但是不同种类物体的轮廓，这样就在低纹理的车库之中获取到了更参考有价值的特征点。
[0065]
在比较计算点p
i
的特征时，每一点与周围前后十个邻点p
j
∈np，计算点云p
i
的特征描述子c，公式如下：
[0066][0067]
其中，n
p
表示当前点p周围的10个点的集合的距离值，r表示当前点p与激光雷达原点的距离，i表示强度，α为权重，r
j
‑
r
i
表示两个相邻点p
i
，p
j
之间的距离差，i
j
‑
i
i
表示两个相邻点p
i
，p
j
之间的反射强度差，α为权重，r
i
表示点p
i
的距离，i
i
表示点p
i
的反射强度；
[0068]
步骤5.2：长廊情况判定。针对于点云的长廊情况进行具体分析，长廊情况代表车辆走在一条长廊之中，前后的场景距离较远并且多为墙面没有较大的参考意义并且匹配不成功的可能性较大，可以将注意力集中在点云的两侧位置进行更加集中的特征提取。选择根据水平角度的分视角进行特征提取，如图8所示，长廊情况下的距离分布与角度的关系示意图；其中包括点云中点的最大距离和点云中点的最小距离。分析可得：距离最大值通过角度不同区域的划分，对长廊情况进行判定并且通过角度进行特征提取。
[0069]
如图9所示，为根据角度进行切分的点云示例图。其中，灰色部分的点云较为稀疏
且不具有很大的参考价值予以舍弃掉，而左右两侧的点云较为稠密并且距离较近，从而点云的特征描述的也更加清晰，这样就可以将注意力更加倾向于有价值的区域，加强特征点的有效性。
[0070]
步骤6：将滤波后得到的静态物体点进行骨架特征进行基于强度的里程计匹配，即利用激光雷达里程计对连续两帧点云进行匹配，获得局部位姿变换。通过预测估计的车辆变换对两帧点云进行对齐，然后对两帧点云中的最近点作为对应点，之后对变换不断进行地图下降迭代调整，直到误差最小为止。由于低纹理车库空间相对狭窄，点云中某些点之间的距离差距很小，导致估计误差不明显。这里仍然考虑添加强度信息进行误差计算，区分点的物体材质类型，从而提高点云匹配的准确性。点到线距离计算的原理是计算当前帧的点到上一帧最近的线的距离。这条线是由雷达中同一直线上最近的点i和相邻直线上最近的点j组成的，所以当前点对应的点在这两点中。误差计算时，将较小的点的强度差绝对值作为误差函数，误差计算公式如下：
[0071][0072][0073]
其中，e表示匹配误差项，ω
r
和ω
i
表示距离误差和强度误差的权重，c表示坐标，坐标相减表示向量，右上角标l表示点在雷达坐标系，每个点的右下角标(t，i)表示在t时刻计算点i的信息，i表示点云强度，r表示当前点到坐标原点的距离；
[0074]
在比较时，误差项为前后帧匹配点的最小几何距离和强度差，进行不断迭代得到激光雷达坐标系下的车辆位姿变换，得到10hz的高频车辆局部坐标变换。
[0075]
步骤7：将初始位姿进行雷达点云帧与先验地图进行匹配所得到的车辆位姿和将滤波后得到的静态物体点进行骨架特征进行基于强度的里程计匹配所得到的车辆位姿进行融合，得到车辆的定位结果。
[0076]
本发明的优点在于：
[0077]
本发明提出了基于先验地图车库内的定位方法，将传统方法中的局部坐标转化为全局坐标，并且应用平面地图，边缘地图和路径形成的地图流；以及，本发明提出了基于强度的滤波算法获取静态物体，并且设计了动态可适应的分割方法，融入了强度差进行静态物体骨架点的提取，增加了可提取的特征点；针对于长廊问题基于分视角提取合理可信的静态目标特征点；通过结合强度的高频里程计匹配和先验地图进行高精度点云匹配，得到精确的位姿变换。
[0078]
以上仅是本技术的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本技术技术原理的前提下，还可以做出若干改进和变型,这些改进和变型也应视为本发明的专利保护范围。