基于激光雷达和环境特征的水平运输车辆定位系统

刘春明 王全宇 欧阳乐 黄旭东

随着人工智能技术的发展和应用，自动化集装箱码头（以下简称“自动化码头”）建设蓬勃兴起。青岛港自动化码头和上海港洋山深水港区自动化码头相继投产运营，在港口行业内产生良好的示范效应，很多码头对半自动化或全自动化方案表示出浓厚兴趣。作为自动化码头重要的水平运输设备，基于磁钉定位系统的自动导引车虽然已成功应用于自动化码头生产作业，但也存在一定局限性：首先，磁钉定位系统的初期投资成本和后期维护成本较高，很多新建码头受制于资金压力而无法应用该系统;其次，磁钉定位系统的施工过程复杂，大部分老旧码头因场地布局、基础设施和作业模式等难以改变而无法应用该系统。为此，本文提出基于激光雷达和环境特征的水平运输车辆定位系统（以下简称“激光雷达定位系统”），从而以较低的投资和维护成本、较小的施工难度、较高的精度满足自动化码头对水平运输车辆定位和导航的要求。

1 激光雷达定位系统基本原理

自动化码头水平运输车辆的自动驾驶系统通过人工智能算法规划车道和路径，并向制动、转向、加速等控制器下达指令，从而控制车辆开往目的地，实现车辆自动驾驶。在此过程中，高精度地图的精确定位、辅助环境感知、路径规划和决策等功能[1]发挥着重要作用，其中：精确定位功能是实现路径规划和决策功能的前提，其通过对比车辆传感器感知的环境信息与高精度地图标注的环境信息，得到车辆在地图上的精确位置;辅助环境感知功能通过在高精度地图上标注详细的道路信息，辅助汽车在环境感知的过程中验证相关信息;路径规划和决策功能则利用云平台采集传感器感知区域以外的路况信息，控制车辆提前避让。

激光雷达是通过发射激光束探测目标位置和速度等特征量的雷达探测装置，其工作原理如下：激光雷达向目标发射探测信号（激光束），并通过比较目标反射回来的信号与发射信号，获得目标距离、方位、高度、速度、姿态、形状等参数。激光雷达具有测距精度和分辨率较高、体积较小、质量较轻、隐蔽性较好、抗有源干扰能力较强等优点。

本文结合高精度地图的功能和激光雷达的优点，提出激光雷达定位系统，其基本原理如下：首先，在道路上布置一定数量的标志物，并建立这些标志物的点云地图;然后，利用激光雷达点云数据和聚类算法识别标志物，并将识别出的标志物点云数据与点云地图进行匹配;最后，将车辆码盘的里程计和惯性测量单元、激光雷达里程计、标志物点云匹配结果进行融合，输出车辆位置信息。

2 激光雷達定位系统构成

如图1所示，激光雷达定位系统主要由车载多线激光雷达、车载控制器和环境特征组成。

（1）车载多线激光雷达 车载多线激光雷达每秒发射上万个激光点，并接收其反射光，然后采用飞行时间测距法计算多线激光雷达与探测点之间的距离，并获得激光反射强度信息。

（2）车载控制器 车载控制器利用多线激光雷达收集的目标表面大量密集点的三维坐标、反射率和纹理等信息，得到被测目标的三维模型以及线、面、体等数据，据此构建三维点云地图并绘制环境地图。在车辆行驶过程中，车载控制器对激光雷达获取的实时扫描数据与预存的高精度地图进行连续匹配，从而达到环境感知和车辆定位的目的。由于光速极快，激光飞行时间极短，测量设备必须具备非常高的精度。从效果上来看，激光雷达的维度（线束）越多，测量精度就越高，安全性也就越高。

（3）环境特征 环境特征的设计选用反光柱。反光柱的特殊材质具有高反光率，使激光雷达能够在环境中迅速获取激光反射强度信息，从而实现精准定位。激光反射强度的可控性使其即使在雨、雾、雪等恶劣天气下也有很强的鲁棒性，定位精度达到。反光柱立杆可以利用码头灯杆为载体，其布置效果见图2。

3 激光雷达定位系统定位方法

3.1 定位方法架构

激光雷达定位系统定位方法架构如图3所示：首先，在车辆行驶的道路两旁布置一定数量的反光柱立杆作为标志物，并建立标志物点云地图;然后，利用激光雷达点云数据和聚类算法识别标志物，并将识别出的标志物点云数据与点云地图进行匹配;最后，将车辆码盘的里程计和惯性测量单元、激光雷达里程计、标志物点云匹配结果进行融合，输出车辆位置信息。

3.2 激光雷达里程计的实现

从长期来看，环境处于动态变化的过程中;但在很短的时间（如100 ms）内，环境是固定不变的。基于环境的这一特点，可采用迭代最近点（iterative closest point，ICP）算法匹配激光雷达前后帧数据，计算水平运输车辆在短时间内的位移和旋转情况，从而实现激光雷达里程计的定位功能。

ICP算法的基本原理是：按照一定的约束条件，分别在待匹配的目标点云P和源点云Q中，找到最邻近点（pi，qi），然后计算最优匹配参数R和t，使得误差函数E（R，t）最小。[2]误差函数

式中：n为最邻近点对的数量;pi为目标点云P中的一点;qi为源点云Q中与pi对应的最近点;R为旋转矩阵;t为平移向量。ICP算法的计算步骤如下。

（1）在目标点云P中取点集pi∈P。

（2）找出源点云Q中的对应点集qi，qi∈Q，使得最小。

（3）计算旋转矩阵R和平移向量t，使得误差函数E（R，t）最小。

（4）用上一步求得的旋转矩阵R和平移向量t对pi进行旋转和平移变换，得到新的对应点集=Rpi + t，pi∈P。

（5）计算与对应点集qi的平均距离d，即

（6）如果d小于某一给定的阈值或大于预设的最大迭代次数，则停止迭代计算;否则，返回第二步，直到满足收敛条件为止。

（7）采用ICP算法不断匹配激光雷达前后帧数据，并累计平移和旋转数据，最后输出激光雷达里程计的定位结果。

3.3 立杆点云匹配

首先，采用全站型电子测距仪测量立杆位置，获取立杆在全局坐标系下的坐标，并形成全局坐标系下的立杆点云格式地图（见图4）;然后，通过筛选强度信息（只有反光柱区域才能产生密集反光点）并采用点云聚类和识别算法识别立杆（见图5）;最后，采用ICP算法完成立杆点云匹配，从而获得车辆定位结果。

3.4 定位结果融合

激光雷达定位结果的融合方法是：通過对车辆码盘的里程计和惯性测量单元进行预积分得到里程计数据，再利用扩展卡尔曼滤波（extended Kalman filter，EKF）算法融合激光雷达里程计、立杆点云匹配定位、车辆里程计，最终得到激光雷达定位结果。EKF是标准卡尔曼滤波在非线性情形下的扩展形式。作为一种高效率的递归滤波器（自回归滤波器），EKF的基本思想是利用泰勒级数展开算法将非线性系统线性化，然后采用卡尔曼滤波框架对信号进行滤波处理;因此，EKF是一种次优滤波，也是目前智能驾驶框架中常见的数据融合算法。

4 激光雷达定位系统测试

（1）静态测试 在静态测试中，水平运输车辆停在相应的坐标点处，记录车辆静止状态下激光雷达定位系统和磁钉定位系统的定位结果。经比较，在车辆静止状态下，激光雷达定位系统与磁钉定位系统的定位误差不超过10 cm。

（2）动态测试 动态测试分为直行测试和转弯测试：在直行测试中，水平运输车辆分别以2 m/s和的速度在直行车道上行驶，记录车辆行驶过程中激光雷达定位系统和磁钉定位系统的定位结果;在转弯测试中，水平运输车辆在直行车道上沿直角转入与当前车道垂直的另一条直行车道，记录车辆行驶过程中激光雷达定位系统和磁钉定位系统的定位结果。经比较，在车辆行驶过程中，激光雷达定位系统与磁钉定位系统的定位误差不超过25 cm。

5 结束语

测试结果显示：激光雷达定位系统与磁钉定位系统的静态定位误差不超过10 cm，动态定位误差不超过25 cm，基本能够满足自动化码头对水平运输车辆定位精度的要求。除了定位精度满足要求外，激光雷达定位系统还具有投资和维护成本较低、施工难度较小、适用范围较广等优点，可作为磁钉定位系统的替代方案应用于新建码头和老旧码头。

参考文献：

[1] 宫慧琪，牛芳. 自动驾驶关键技术与产业发展态势研究[J]. 信息通信技术与政策，2018（8）：45-50.

[2] 孙宇，项志宇，刘济林. 未知室外环境下移动机器人的三维场景重建[J]. 浙江大学学报（工学版），2007，41（12）：1949-1954.

（编辑：张敏 收稿日期：2020-03-09）