基于 GPS/INS/Lidar 的无人车导航 | 厚势汽车

杨森森、张伟军等厚势汽车 2024-04-13

厚势按：为了实现无人车的自主导航提出了一种基于和的组合导航并辅以激光雷达实现避障功能：

首先确立整车的控制架构，确定基于协议的整车总线通信网络；
然后研究了导航，利用的载波差分获得较高的位置精度同时整合惯导的输出数据；
最后通过激光雷达的不断扫描获取周围物体的角度和距离，实现周边环境建模，有效规避障碍。

实验验证了此导航的可行性和实时性。本文来自 2013 年 2 月 20 日出版的《机电一体化》，作者是来自上海交通大学机械与动力工程学院的杨森森、张伟军教授和谢一峰。

0 引言

在汽车工业的发展过程中，汽车的智能化导航一直是大众所追求的目标，特别是在军用方面，由于危险性和不确定性的增加，为了减少不必要的人员伤亡，无人导航已经成为武器车载平台的重要发展方向。

自主导航系统一般包括 GPS 导航、惯性导航（INS）、激光雷达导航等。GPS 和 INS 是目前较为先进的导航统二者各有优缺点：

GPS 导航具有全球性、全天候、高精度和三维定位等优点，但也存在数据易丢失、可靠性差之类的缺点；
INS 完全依靠载体自身设备独立导航，不与外界发生联系，所以数据不会受到干扰，但是 INS 的误差会随着时间累积，不适合长距离导航。

所以，两者互补，可以有效提高导航精度和可靠性。

车辆在行进过程中，将不可避免地遇到障碍或者出现紧急情况而丢失 GPS/INS 信息，此时需要感知车辆周围的环境，因此激光雷达由于其测距精度高、可靠性好而得到了广泛应用。

1. 系统构架

本系统由 GPS 导航和 INS 导航实现组合导航 [1]。

GPS 接收机包括基站和移动站，其中移动站安装在车上。通过载波差分 [2] 获得相对高精度的定位数据（数量级为 10 cm)。GPS 数据通过 RS232 串口，以 NMEA0183 协议形式输出，数据头字段为 $GPGCA。

惯性导航系统固定在车身上，可以提供车辆运行时的 (x, y, z) 三相加速度和方向角，据此可以根据当前点推断下一个时间点的位置。将惯性导航系统推算的位置和 GPS 推算的数据按权重比融合，可以得到相对精确的位置。

激光雷达通过实时扫描周边物体，获得 361 个障碍物的数据点，每个数据点包含被扫描物体的角度和距离。通过对数据点的处理，完成点的聚类合集和基于最小二乘法的直线化处理，实现周围障碍物的直线轮廓建模 [3]，形成避障的依据。

图 1 车身控制系统

车身系统的 CAN 网络使用 CAN2.0B 协议，波特率设为 500Kbps，采用扩展帧方式通信。上下位机的 CAN 总线通信协议的构架是基于商用车控制系统局域网络通信协议（SAE1939）标准。整车的车身通信控制构架如图 1 所示。

2. GPS/INS 组合算法

CPS 接收是基于 NMEA-0183-GCA 协议，得到的数据是当前点的地球经纬度坐标 ( B, L )，此坐标是基于 WGS-84 地球坐标系，而我国的地图测绘成果是基于北京坐标，车辆也是在 ( x, y ) 平面上行走，故需要进行高斯投影坐标转换 [4]： ( B, L ) → ( x, y ) 。

对于整个系统，要事先规划好导航路径。车辆在行进过程中，会出现偏离既定轨道的状况。任选一段路径，建立车身偏离轨道时的控制模型，如图 2 所示。

图 2 导航算法示意图

图 2 中，由 ( X_i, Y_i ) 与 ( X_(i+1), Y_(i+1) ) 构成的是理想行走路线。当前车的中心点位于 ( x, y )处，其 α、β、γ、θ 逆时针方向为正。α 为实际航向与当前期望航向的夹角；β 为 INS 输出的当前航向；γ 为当前期望航向；φ 为期望角。L 为无人车与目标点的直线距离；S 为无人车与路径的垂直距离。其中

同理，可推断其他位置时的状态变量。

图 3 控制系统反馈图

控制变量为 α、θ、L、S，控制策略为使 α、θ、L、S 4个量尽可能最小最优。由控制器解算出 4 个量后，运用算法判断小车位置，发 CAN 命令给无人车，并执行命令。控制框图如图 3 所示。GPS 接收机作为位置反馈器，使系统成为一个闭环统。

3. 激光雷达环境建模

激光雷达采用的是 SICK 公司生产的二维激光雷达 LMS511，扫描角度分辨率 0.5°。，扫描角为 180°，扫描所得数据位 361 个。

首先，为了过滤激光雷达中所测数据包含的噪声干扰，需要对激光雷达的测距数据进行动态均值滤波处理。由于激光雷达的扫描周期为 T，在时间 t_0 时刻，对 t_0 - T，t_0 ，t_0 +T 多的数据做均值滤波处理。

其中，i - 1，i + 1 代表数据点；t - T，t，t + T 代表时间点。

首先建立数据点的合集标准，以连续两个数据点距离

作为评判标准。合集的处理规则是：连续两个点的距离小于给定值 Δ；一个合集内的数据点数应大于 N 个（N 为经验值）。起始点的标准是：下一个差值和当前差值小于 Δ，而前一个差值大于 Δ；终止点的标准是：前一个差值和当前差值小于 Δ，而下一个差值大于 Δ。分段合集的线性化处理 [5]，即对每个合集进行最小二乘法拟合，得到近似直线方程

计算合集内点到拟合直线的距离，若有一定数量的数据点到直线距离大于 Δ，则默认为非直线，据此得到主要的障碍线。寻找到原点距离大于 ρ_dis 的连续点，如果连续点的数量大于定值，则默认为寻找到一个出口。

寻找距离原点最近的直线，作为主避障直线，计算距离 d。

其中，α 为安全因子，即为避障算法给出的前进角度；β 为趋向因子，即为出口方向给出的前进角度；μ_α，μ_β 为系数。当 μ_α 更大时，机器人表现为以避障为主；μ_β 当更大时，机器人表现为以导向为主；其中 μ_α + μ_β = 1。

4. 组合导航

图 4 程序流程图

导航程序是基于 Vxworks [5] 嵌入式系统的多任务实时调度。首先建立两个任务，Task1 用于执行数据的接收与处理，完成 GPS/INS 数据融合以定位当前位置。当 GPS 数据丢失时，系统开始启用激光雷达数据，完成对应的信息处理。Task2 用于逻辑判断，实现车身控制算法。在 Vxworks 嵌入式操作系统中，通过多任务运行方式，可以有效合理地利用 CPU。任务运行时，首先需要对任务的优先级进行分配，Task1 用于读取 GPS/INS/Lidar 数据 [6]，应获得更高的优先级。系统程序流程图如图 4 所示。

图 5 导航的 Google 地图显示界面

为了方便系统调试时的可视化操作利用 Visual C++ 编写了基于 Google Map 的 VC 程序运行中，通过坐标转换将 GPS 坐标转为平面坐标，再经过第二次坐标转换使其与界面坐标对应。显示的路径为上海交通大学校内一段马路，从马路的一点导航至另一点，每到达已知点，便将此点的位置在 VC 界面上显示（如图 5 所示）。

5. 结束语

文中针对无人车的导航提出了一个简单可靠、可用于工程实践的 GPS/INS 导航方案，使车辆沿指定路线行走；并辅以激光雷达不间断扫描周围物体，通过获得的数据点实现基本的环境建模和避障功能。通过 VC 建立路径显示界面，方便了调试工作的进行。需要指出的是，本文基本实现了预期效果，可以安全地在指定路线上行走。

参考文献

[1] YEONSIK，CHIWONR OH，SEUNG-BEUMSUH. A Lidar-Based Decision-Making Method for Road Boundary Detection using Multiple Kalman FiltersIEEE Trans [J]. Industrial Electronics，2012，59（11）：4360-4368

[2] ELLIOTT D KAPLAN，CHRISTOPHER J HECARTY. GPS原理与应用[M]. 北京：电子工业出版社，2007

[3] 蔡自兴，郑敏捷，邹小兵. 基于激光雷达的移动机器人实时避障策略[J]. 中南大学学报，26，37（2）：324-329

[4] 张士钰，孙永荣，陈武，等. 基于战术地图的GPS坐标定位[J]. 火力与指挥控制，2004，29（5）：65-67

[5] WINDRIVER，Vxworks程序员指南[M]. 王金刚，译. 北京：清华大学出版社，2003

[6] 刘巍，金文标，肖仙谦. 基于主元分析和聚类的直线检测算法[J]. 计算机应用，2011，31（5）：1202-1208

[7] 张军. 基于Vxworks实时操作系统的串口通信程序设计与实现[J]. 微计算机信息，2006，22（22）：98-99

编辑整理：厚势分析师盖布林

转载请注明来自厚势和厚势公号：iHoushi

-END-

文章精选

企业家

马斯克和贾跃亭｜福特CEO下台｜正道汽车仰融