随着自动驾驶技术的发展,在未知环境中智能汽车的定位技术成为该领域研究的核心。目前定位技术主要的解决方案是基于全球定位系统(GPS),但是在某些特殊的环境中如下车库,没有 GPS 信号如何解决定位问题就是本文研究的关键所在。
1.自动驾驶系统概述
自动驾驶汽车即无人驾驶智能汽车,在没有人为参与的情况下,依靠车内的控制系统与智能算法,通过多重传感器数据融合控制汽车底层协议完成正常的车辆行驶功能。智能驾驶汽车是一个综合的集成系统,包括了自动泊车系统、自动驾驶系统、障碍物停障系统等,又分为了感知层、决策层和控制层三个部分如图 1 所示。
其中感知层包括各路传感器的数据采集、处理与融合等,更加精确和全面的感知周围环境信息。决策层的输入包括感知层的信息、路径的规划以及控制层反馈回来的数据,通过增强学习算法下发决策指令。决策指令包括了循迹、跟车、超车、刹车、转向、调头等等;最终通过控制层下发 CAN 总线下发指令完成智能驾驶汽车的自动驾驶任务,包括油门与刹车的控制、方向盘与挡位的控制等等。
自动驾驶汽车发展与研发中的核心技术是车辆线控技术和车辆精确定位技术。本论文主要分析车辆的精确定位,目前最常用的解决方法就是使用 GPS,可以让汽车实时地得到自身的位置坐标。本文则基于 SLAM 技术研究了一种新的定位方法。
结合 SLAM 技术,自动驾驶汽车的传感器分别由摄像头、激光雷达、车载毫米波雷达、车载超声波雷达、惯导组合、工业控制器等组成,具体的安装位置如图 2 所示。
使用的自动驾驶控制流程图如图 3 所示。
2.SLAM技术
表示观测量,表示当前环境量,表示当前的位姿状态,表示当前运动传感器的读数,都是噪声值。
SLAM 技术是一个比较庞大的系统,其中包括前端里程计、后端优化、回环检测、建图四大部分。本文主要介绍基于视觉激光融合的 SLAM 前端里程计的位姿估计与定位技术。
3.位姿估计算
4.线性代数法(SVD)
定义两组点的质心为:
对应的平方项展开并去掉交叉项为零的部分,优化目标函数可以简化为:
同样的方法展开只含有 R 的左侧式子的平方和,并消去与 R 无关的项,最终化简为需要优化的式子:
先定义矩阵:
W 是一个三维矩阵,对其进行 SVD 分解:
其中 U 和 V 为对角阵,当 W 满秩时,
求解 R 后,代回优化目标函数可求出 t。
5.非线性优化法
求解 ICP 用非线性优化的方法,是根据迭代思想寻找最优解,与 PnP 算法类似。优化过程中为了消除旋转矩阵的自身约束,使用了李代数表示位姿,构建的目标函数为:
式子中只有一个未知量,所以可以采用李代数的扰动模型求解:
因为 ICP 算法中特征点已经匹配好,所以构建的最小二乘方程一定有解析解,就可以不进行迭代优化。为了方便计算,可以采用两种算法放在一起考虑,例如:如果深度信息未知,可以采用 3D-2D 的 PNP 重投影误差;若深度信息已经被测出,就可以用3D-3D 的 ICP 算法完成建模与求解。
6.基于滤波的定位算法
化简可得:
贝叶斯滤波的核心就是通过 t-1 时刻的后验概率推导在 t 时刻的先验概率,在通过 t 时刻的先验概率更新 t 时刻的后验概率。
(2)粒子滤波算法
粒子滤波是基于蒙特卡洛的方法,用粒子集表示概率,寻找随机样本用来近似的描述概率密度函数,用样本均值代替积分运算,进而获得系统状态的最小方差估计的过程。由于粒子滤波在非高斯、非线性系统和多峰分布表现出来的优势,可以用来估计带噪声的观测数据中自动驾驶汽车的位姿和运动状态。
粒子滤波可表示为:
其中表示自动驾驶汽车的位姿,表示当前位姿所处环境跟地图的匹配度的权重。如图 4 所示为粒子近似分布示意图。
(3) 实现流程
本论文中实现自动驾驶系统中粒子滤波算法的流程分为以下三步:
1. 用粒子进行状态传播:
2. 评价每一个粒子的权重:
3. 根据权重进行重采样:
以概率接受,目的是去除低权重的粒子,其中粒子滤波的状态传播模型为:
因为在没有 GPS 信号的情况下,无法知道自动驾驶汽车位姿的实际发布,从它的预测分布进行定位采样如图 5 所示,并用观测模型进行计算权重,联合权重再重新采样来近似自动驾驶汽车的后验概率分布。
概率越大,权重越大,定位越准确,重定位需要复制的粒子越多,粒子权重就更符合后验概率分布如图 6,图 7 所示。
7.基于ROS的定位与建图仿真实验
进行了理论分析与实验算法仿真以后,用自动驾驶车辆进行测试与实验,选择的环境为没有 GPS 信号的地下车库如图 10所示。
首先通过自动驾驶车辆的相对位姿估计算法进行求解当前车的空间刚体运动矩阵,得到的位姿关系进行了整个地库地图拼接与构建并在 Rviz 中进行显示如图 11 所示,转换为 PCD 格式的点云地图如图 12 所示。
将构建好的整个车库地图选取部分路径进行定位与导航试验,通过 VL-SLAM 算法可以得到自动驾驶的位姿估计以及精度地图。为了精确定位自动驾驶汽车在精度地图中的位置关系,将 velodyne32 线激光雷达和单目视觉相机采集的数据融合后与精度地图进行匹配,根据匹配的权重大小确定当前的采集数据是在精度地图中的哪个位置。
已知自身定位,然后再设定目标位置,结合 ROS 中的gmapping 导航功能包就可以实现地图环境中的路径规划任务。在 RVIZ 中的实验结果如图 13 所示,用红色正方形标出自动驾驶车辆正在初始化,加载汽车模型。
如图 14 所示,汽车模型加载成功,并且定位准确作为初始位置出发点。
随后自动驾驶汽车开启自动驾驶功能,如图 15所示为运动过程中自动驾驶汽车的精确定位。
摘自:https://mp.weixin.qq.com/s/DB0bHNXbfpiz8oVoCKbdUg