合作环境下基于惯性/里程计/Lidar融合的UGV导航方法

付相可，赖际舟，刘 建，何洪磊，岑益挺，姚义心

(1.南京航空航天大学，南京 211106； 2.北京空间飞行器总体设计部， 北京 100094)

0 引言

随着科技的进步，地面无人车得到了空前的发展。地面无人车(Unmanned Ground Vehicle，UGV)也在社会生活中起到越来越重要的作用，在巡检、探测等领域扮演着重要的角色。UGV能够完成这些任务的前提是进行准确的导航定位。

目前在室外环境中，无人车的定位主要采用GPS/惯导融合的方法。但是在室内、桥洞等GPS不可用的环境中，需要采用新的自主方式实现自身的导航定位。在GPS拒止环境下常用的传感器主要包括激光雷达[1-3]、视觉[4-5]、UWB[6-7]等。其中，视觉对光照的依赖性强，在无光条件下基本不可用；UWB则需要提前对基站进行标定，使用复杂，而且基站的位置精度对其定位精度有较大的影响。激光雷达是一种主动式的感知传感器，不依赖外界的光照条件，且测距精度高，更适合用于UGV在复杂环境下的导航定位。

激光雷达分为二维激光雷达和三维激光雷达。受成本等因素的限制，二维激光雷达在无人车上更为常用。通常使用同步定位与构图(Simultaneous Localization and Mapping，SLAM)技术进行定位。

二维激光雷达SLAM算法目前通常采用扫描匹配的方法估算无人车的位姿，代表的算法有Hector SLAM[8-9]和MRPT SLAM[10]等。扫描匹配的过程为通过变换帧间位姿关系将两帧点云数据对齐，在对齐过程中的旋转和平移变化就是两帧之间的位姿变化[11]。但是在实际巡检过程中，UGV有时会处于结构特征稀少、点云信息不丰富的空旷环境中，此时得到的有效点数量稀少，通过扫描匹配的方法估计无人车的位姿，会出现匹配误差大、精度低的问题，不能满足定位的精度需求。

针对该环境,本文提出了一种合作环境下基于惯性/里程计/激光雷达融合的UGV导航方法，合作环境的主要特点是结构特征已知。本文主要贡献可总结为以下两点：

1)提出了一种基于几何结构特征的激光雷达定位方法，实现了在点云稀少环境下的精确定位，并且在动态环境下的鲁棒性较高；

2)通过惯性/里程计/激光雷达的融合，提高了导航解算的准确性及可靠性。

1 合作环境下激光雷达定位方法

本文提出的激光雷达定位方法流程图如图1所示，该方法主要利用空间中已知结构体的位置、尺寸和形状信息，对UGV进行定位。主要包括以下几个步骤：

图1 本文提出的激光雷达定位方法Fig.1 The proposed positioning method based on the lidar

1)点云数据预处理。完成对当前激光雷达获取的扫描点云数据聚类，并进行角点的判断，为下一步的匹配奠定基础。

2)特征匹配。将分割完成的点云数据与已知的几何结构体信息进行匹配。

3)求解中心点位置。求解出各个结构体的中心点，计算UGV相对每个几何结构体中心点的距离。

4)估计UGV位姿。通过最小二乘法求解UGV当前所处的位置。

1.1 点云数据预处理

点云数据预处理是激光雷达定位方法的基础，其准确性对后续导航解算起到决定性作用。该部分主要包括有效点提取、点云数据聚类以及角点判断3个部分。

提取有效点的目的是为了从点云数据中提取处于激光雷达扫描范围内的数据，即提取一定范围内的点云数据。为了去除无人车自身带来的影响，选择的有效距离范围为0.4～30m。

点云的聚类方法主要有K-means聚类[12-13]、DBSCAN聚类算法[14]以及最近邻聚类[15-16],这三种聚类方法的优缺点对比如表1所示。

表1 聚类算法优缺点比较

由于本文的任务是实现巡检时的UGV自主定位，实验环境具有以下特点：1)合作环境中，各个结构体的位置是固定的；2)结构体的尺寸和形状是固定的。所以本文使用最近邻聚类算法进行点云分割。最近邻聚类算法的基本原理如式(1)所示

(1)

式中，ρ(i)和ρ(i+1)是激光雷达第i个和第i+1个有效点；D(ρ(i),ρ(i+1))是相邻两点之间的距离；Dth是设置的阈值，其大小根据结构体之间的距离进行确定。

本文所使用的结构体为长方体，在激光雷达扫描得到的点云数据中，每一部分最多只存在一个角点，可以根据点到直线的距离来判断该部分是否存在角点。

1.2 特征匹配

特征匹配是该激光雷达导航定位方法的关键，它的准确性对后续步骤起到决定性作用。它依据无人车的位姿以及物体彼此间的距离关系进行判断匹配，其具体流程如图2所示。

图2 特征匹配流程图Fig.2 Flow chart of feature matching

图2中,C_pre的值是由上一时刻识别的结构体数量决定的,是当前时刻选择特征匹配模式的标志位；C的值是由当前时刻识别的结构体数量决定的，是下一时刻选择特征匹配模式的标志位。

模式一的计算流程如下：

1)根据无人车在导航系下的位置筛选出可能是已知几何结构的点云数据geari_m(j)(geari_m为第i个物体，其中j=1,2,3,)；

2)计算geari_m(j)之间的距离，并与已知的信息进行比较，分别从geari_m(j)中选出符合条件次数最多的点geari_m；

3)计算geari_m之间的距离，确定是否符合要求；

4)根据确定已知物体的位置信息，推算其他物体的位置信息，确认在点云数据中是否存在其他物体。

模式二的计算流程如下：

1)计算各类的平均值与上一时刻结构体在机体系下的欧式距离，如果小于阈值，则认为是同一结构体geari_m；

2)计算geari_m之间的距离，确定是否符合要求；

3)根据确定结构体的位置信息，推算其他结构体的位置信息，确认在点云数据中是否存在其他结构体。

模式三的计算流程如下：

1)根据无人车在导航系下的位置筛选出可能是结构体的点云数据geari_m(j)；

2)计算各类的平均值与上一时刻结构体在机体系下坐标的距离，如果小于阈值，则认为是同一结构体gear_known；

3)将geari_m(j)与geari_known进行比较,确定其他结构体的位置信息；

4)根据确定结构体的位置信息，推算其他结构体的位置信息，确认在点云数据中是否存在其他结构体。

同时，由于本文所考虑的巡检环境是对称结构，使用距离进行判断时，有时会将后方的2个结构体位置判断错误，因此需要叉乘法对其进行辅助判断。

1.3 中心点确定

结构体中心点的位置估计精度对导航定位的精度有着直接影响。该部分主要包括直线拟合、直线方向的判定和中心点的坐标解算3个步骤，其流程图如图3所示。

图3 中心点确定流程图Fig.3 Flow chart for determining the center point

本文是通过最小二乘法进行直线拟合的，其目标函数为ax+by-1=0。因为在对结构体中心点进行确定时需要进行直线平移，而结构体的不同面平移量并不相同，所以要进行直线方向的判定。

本文使用的直线方向判定方法有两种，分别如下：

1)当可匹配的结构体有2个或者3个时，根据不同结构体上的直线方程，确定平行的直线，再根据平行直线间的距离判断这条直线属于结构体的哪一侧。

2)根据结构体不同方向的长度进行判断。

之所以采用两种方法进行直线方向的判定，是由于无人车在行驶过程中会存在以下两种情况：1)激光雷达只能扫描到一个结构体；2)根据点云拟合的线段只是结构体一侧的部分信息。

2 惯性/里程计/激光雷达融合算法设计

单独使用激光雷达估计无人车的位姿容易受到周围环境的影响，导致解算出现误差甚至无法解算，所以本文引入惯性与里程计进行辅助解算。单独惯性或里程计，虽然其短时间内精度高，但是其精度会随着距离而发散。因此，本文采用扩展卡尔曼滤波(Extended Kalman Filter，EKF)算法将里程计、激光雷达与惯性传感器进行容错组合，融合算法结构图如图4所示。

图4 惯性/里程计/激光雷达融合算法结构图Fig.4 Structure of inertial/odometer/lidar fusion algorithm

2.1 状态预测

本文以UGV初始时刻所在位置为原点，以UGV机体系为三轴方向构建导航坐标系，EKF所构造的16维状态量为

(2)

姿态四元数的预测采用如下公式

(3)

其中，Δt为离散采样周期；Q(k)和Q(k-1)分别为k时刻和k-1时刻的姿态四元数；Ω(k)通过式(4)进行计算

(4)

(5)

速度预测采用如下公式

(6)

(7)

位置预测采用如下公式

(8)

其中，Pn(k)和Pn(k-1)是k和k-1时刻UGV在导航系的坐标。

陀螺仪零偏和加速度计零偏预测采用如下公式

(9)

(10)

2.2 滤波器更新

本文中通过EKF融合IMU预测的状态、激光雷达解算的位姿和里程计的速度信息，从而获取更精准的导航信息。

一步预测均方误差Pk|k-1的计算公式如下

(11)

式中，Φ是状态转移矩阵。

Φk,k-1=

(12)

式中，M4×4、U4×3、N3×4的计算方法如下所示

(13)

(14)

(15)

滤波器的噪声矩阵Γ的计算方法如下所示

(16)

系统的噪声矩阵为

W=[εwxεwyεwzεaxεayεaz

(17)

系统的滤波增益方程、状态估值方程和估计均方误差方程如下

(18)

(19)

Pk|k=(I-KkHk)Pk|k-1

(20)

其中，H是量测矩阵，根据量测信息的不同选择不同的量测模型。

1)激光雷达的量测量为

(21)

其中，x_lidar、y_lidar、ψL为激光雷达估计的无人车位姿。量测方程为

z_lidark=H_lidark·Xk+uk

(22)

(23)

式中，uk为量测噪声，偏航角ψL与四元数的关系如下

(24)

2)里程计的量测量为

(25)

其中，v_odomx和v_odomy为里程计得到的机体系下速度通过滤波后的航向角转换到导航系下x、y方向的速度。量测方程为

z_odomk=H_odomkXk+uk

(26)

(27)

2.3 激光雷达故障检测

在UGV巡检过程中，激光雷达会出现得不到足够的有效信息的情况，此时解算误差大甚至不能进行位置解算，也不能够将激光雷达的信息作为量测带入滤波框架。针对此问题，本文提出了一种激光雷达的故障检测方法，根据融合导航算法的输出结果以及可识别的结构体的数量对雷达的解算结果进行判别，故障检测具体框架如图5所示。thr是判定雷达解算结果是否正确的一个阈值，其大小由无人车的行驶速度和惯性器件的输出频率决定。

图5 激光雷达故障检测方法Fig.5 Fault detection method for lidar

3 仿真验证及分析

3.1 仿真环境设计

为了验证本文算法在UGV巡检中的定位精度，在Gazebo仿真环境中搭建无人车模型和仿真环境，同时将本文所提算法与激光雷达SLAM进行对比。仿真平台如图6所示。

图6 仿真平台Fig.6 Simulation platform

仿真环境中各传感器参数设置如下：

1)激光雷达的探测距离是25m，扫描角度是-180°～180°，角度分辨率是0.25°；

2)加速度计的精度为 0.002g，陀螺仪的精度为0.02(°)/s；

3)里程计的精度为0.6%。

3.2 仿真结果分析

仿真实验结果如图7和图8所示，黑色实线为本文算法结果，蓝色点线为Hector-SLAM结果，红色虚线为真值。图7为无人车在仿真环境中的行走轨迹，图8为x、y方向的位置误差。本文算法和2D激光雷达SLAM的定位均方根误差(m)分别为[σx,σy]=[0.123,0.062]和[σx,σy]=[2.864,6.594]。可见本文算法相对于传统2D激光雷达SLAM方法能够实现较为精准的导航。

图7 UGV的行驶轨迹Fig.7 Trajectory of UGV

(a)X轴方向位置误差

(b)Y轴方向位置误差图8 位置误差Fig.8 Position estimation errors

综上所述，在特征稀少的合作环境中，使用本文算法能够实现无人车在巡检过程中的精准可靠导航。

4 结论

本文针对合作环境下，无人车巡检中的自主导航问题展开了研究，提出了一种基于惯性/里程计/激光雷达的UGV导航方法，解决了因有效点数量稀少而导致定位误差大的问题。主要内容为以下几点：

1)提出了一种基于已知结构特征的激光雷达定位方法；

2)进行了惯性/里程计/激光雷达的融合算法研究，相较于2D激光雷达SLAM，其定位精度及鲁棒性均得到提高。

仿真结果表明，本文提出的方法能够有效实现无人车在巡检过程中的精准可靠导航。在后续工作中会针对激光雷达对运动物体的检测展开研究，进一步提高算法的鲁棒性和精度。