激光雷达三角定位技术在AGV车上的应用

郭景红　赵璐　蒋鹏

摘要：AGV技术在多自由度无轨车上的应用。本文总结了工程应用中基于反光板的激光雷达三角定位技术的使用经验，用于项目上的实时跟踪。

关键字：AGV;激光导航;三角定位;全局建图;卡尔曼滤波

1 前言

本文研究了一种无轨多自由度车，该无轨车是由AGV+多自由度Stewart平台组成，具有路线随机、行进速度实时需调整、场景内光线明暗变化较大等特点。

2 AGV导航技术分类

AGV导航技术根据导航方式分为电磁引导、激光导航、视觉导航等。激光导引AGV行驶路劲的周围安装位置精确的激光反射板，AGV通过发射激光束并采集由不同角度的反射板反射回来的信号，根据三角几何运算来确定其当前的位置和方向，实现AGV的导引。其主要优点是定位精度高，地面无需其他定位设施，能够适应复杂的路径条件及工作环境，可快速变更行驶路径和修改运行参数。

由于应用场景对路线及光线的要求，采用激光导航是较为稳妥可靠的方式。项目应用中对于激光导航采用了三角定位及SLAM两种不同的方案，这里研究三角定位算法。

3 系统组成

该AGV三角定位系统组成：倍福运动控制器5130、倍加福R2000激光雷达（2D）、埃莫伺服驱动器及电机（多摩川编码器）。倍福运动控制器主要完成定位算法、路径规划、轨迹规划、逻辑控制等。激光雷达提供雷达点云数据、埃莫伺服电机编码器提供里程计信息用于AGV位姿数据融合。

4 AGV模型分析

4.1运动模型

该种无轨车采用差速方式，两个驱动轮及4个万向从动轮。

根据模型可以得到车辆中心线速度、角速度与驱动轮速度的计算模型，该计算公式会用于之后的姿态数据融合及路径规划控制算法中。

瞬时认为左轮、中心点、右轮绕一点O做圆周运动，则很容易推出中心点的运动参数与左右轮速度的关系：线速度为 ，角速度为 。

5 三角定位的两种计算方式

雷达扫描出标准点后获取的信息是有冗余的，我们可以通过三角法和角边法计算雷达在世界坐标系里面的位置及角度。

5.1三角法

三角法需要已知三个标准点，以雷达与各个点之间的夹角为条件计算自身的位置和姿态。

如图P点为雷达，已知∠a1，∠a2，并且知道a点，b点，c点的坐标，计算p点的坐标。

该计算方法是通过几何学的方式进行计算。

1）由于已知abc三点可以直接得到角abc的值

2）由三角形PBA，三角形PBC，公垂线可以得到如下计算公式：

P2*SIN（a1）=d1*SIN（a3）;

P2*SIN（a2）=d2*SIN（a4）;

同时很明显a3+a4=2*PI-a1-a2-abc;

则通过以上关系式可以计算∠a3及∠a4的值，进而得到∠a5、∠a6数值，

由h/tan（a3）+h/tan（a4）=d1;得到h的高度，进而可以算出P1的长度，

由于已知向量ab及夹角∠a3，可以直接计算向量pa，进而得到P的坐标值。

由于激光雷达的角度信息非常精准，距离信息具有比较大的波动，如果只使用角度信息的话，可以得到相对高的精度，但是该计算方法需要3个标准点，且计算方法相对麻烦，实际使用中还是使用边角法进行定位计算最后通过卡尔曼滤波使得位姿数据平滑变化。

5.2边角法

该方法只需要已知两个标准点，因为已经知道它们的夹角及两边信息，可以很快计算出AGV的向量信息，进而得到AGV的位姿，該方法缺点是由于距离信息的误差跳动，带来定位具有一定误差，需要通过里程计信息数据融合平滑。

6 激光反射板地图列表校准

反光板地图的建立有两个问题：特征信息建立及全局反光板精度

该种定位方式需要通过激光雷达扫描到点的特征信息与地图列表中的标准点进行匹配，寻找到符合特征的组合，进而反算激光雷达的位姿。因此激光反光板的安装至关重要，某个区域内的反光板需要具有特定的信息，使得可以匹配出来并当前区域内唯一。

首先需要在圆柱上环绕粘贴反光贴，使得在任意位置扫描反光板得到的结果一致。从激光点云数据的0°到360°，搜索超过能量阈值的点，计算开始角度。起始角度与结束角度中间值即为扫描到的角度信息。实际测试精度在正负15mm。接下来转动或者移动AGV通过雷达3个以上标准点校准等待点。7 AGV定位计算

保证雷达每次都可以扫描到3个以上点后，我们可以通过5节中的计算方式，计算出雷达的位姿，所以问题的关键是雷达扫描出来的标准点的匹配算法，即标准点的特征信息设定。

目前设定的最小扫描点个数为3个，则关键的特征信息可以以三角形的特征为单位。三角形的特征分析如：长度信息、中间点信息、顺序信息;在特征点匹配过程中，雷达扫描点中同一个点会多次出现匹配成功现象，如果某个点出现与多个标准点匹配成功，则需要将该点排除，不纳入计算中。

AGV定位的流程总结如下图：

8 卡尔曼滤波

在动态运动状态下，由于通讯、反光板安装环境变化等原因会导致雷达扫描数据偶尔会出现上传延时、数据跳变等现象，影响到AGV车辆的定位的准确性、平滑性，进而影响到路径规划的偏差控制。因此我们采用卡尔曼滤波将左右车轮的实际速度来预测AGV坐标并与雷达计算出的观测值进行数据融合得到更为平滑准确的数据用于路径跟踪控制。

该项目中为了方便计算，采用了三个卡尔曼滤波器分别对X、Y、A进行滤波操作，可以得到较好的滤波效果。

图4中蓝色为滤波前跳动，红色为滤波后效果，该动态滤波效果收敛明显。

9 总结

该种类型设备的关键是AGV设备的轨迹运动控制，所以定位技术是必须重点解决的问题。通过一种雷达反光贴的三角定位及AGV车轮速度的数据融合技术，可以精确得到AGV实时的运动轨迹，可以为AGV的实时曲线跟踪提供支撑。

参考文献：

[1]钟钜斌. 基于多种导航技术混合的AGV系统设计. 浙江大学， 2016.

[2]伍舜喜. 基于激光雷达的智能车定位技术研究. 上海交通大学，2008.

[3]候学勇. 基于二维激光测距的移动机器人道路可行区域提取. 杭州电子科技大学，2012