无人驾驶车GPS自主导航与障碍规避系统设计与实现

方海洋，赵瑞妮，涂市委



无人驾驶车GPS自主导航与障碍规避系统设计与实现

方海洋，赵瑞妮，涂市委

（长安大学汽车学院，陕西 西安 710064）

提出一种基于Trimble BD982定位传感器和ZED双目视觉传感器的无人车GPS自主导航与障碍规避系统设计与实现方案。利用RTK-GPS系统进行高精度定位，当ZED双目相机检测前方出现出现正负障碍物时，借助于ZED建立的三维障碍物模型对原来GPS采集的路网文件中的一部分路段进行点位整体偏移建立局部GPS路网，最终将局部GPS路网与整体GPS路网融合实现无人驾驶车GPS自主导航。

BD982；双目视觉传感器；RTK-GPS；路网融合

1 概述

本文所研究的无人驾驶车是基于高精度GPS定位系统实现自主导航，配合ZED双目相机进行避障。由人工操作无人驾驶车提前进行GPS点位的采集，利用NURBS曲线插值法构建车辆运行的二维轨迹路径。车辆的自主驾驶运行过程中采用NURBS曲线算法中寻找最近点的方法，实时寻找车辆运行的目标点，结合车辆的航向角计算出车辆所需的舵量以及适宜速度，利用中值滤波方法处理数据后，实时控制车辆线控系统，从而让无人驾驶车沿着该轨迹运行。在行进的过程中当ZED双目相机检测到前方出现障碍物或坑洼时，制作局部GPS路网，并且根据车辆航向角和路网信息进行局部GPS路网和整体路网的融合。

2 高精度GPS地图

2.1 GPS点位采集

本文采取载波相位差技术获得无人车的准确位置，将BD982采集的WGS-84坐标通过高斯投影转换为平面坐标。

图1 系统整体工作流程图

图2 高斯坐标投影原理图

高斯投影后转换的平面坐标系是以是以子午线与赤道的交点为原点，x轴为中央子午线的投影，y轴为赤道的投影，大地坐标点（B,L）经高斯投影转换后的平面坐标(x,y)如下：

式中D是指赤道至纬度B的子午线弧长，N为卯酉圈曲率半径，为椭球的长轴半径。

图3 坐标系转换

通过高斯投影变换后的坐标系x轴指向正北方，y轴指向正东方，不符合操作习惯，所以将坐标系进行矩阵变换如下：

2.2 路网制作

在本文中我们利用NURBS曲线插值法将离散的点位数据处理成平滑的曲线：

式中E()是m阶B样条基函数，q是权重因子，p是控制点。基函数E()通常由递推公式定义如下：

2.3 无人驾驶GPS导航算法

根据实际道路情况在软件中提前设定打舵参考行n，相当于选取距离车辆一定距离的GPS点位作为车辆打舵的目标点。

图4 GPS导航示意图

车辆前后自身安装了2个GPS天线，根据这两个天线位置计算出车辆与参考行GPS点位的连线与地球正北的夹角为：

由和车辆的航向角可以求得打舵角度为：

式中n为倍率系数。

对打舵角度进行中值滤波处理：

计算机将打舵角度δ发送给下位机，计算出车辆打舵的信号电压二次多项式为：

无人驾驶车速度的控制采用设定一个和，反复试验确定车速和打舵角度△θ之间的关系如下：

3 避障控制策略

3.1 障碍物建模

为了配合GPS导航，需要将障碍物的相机坐标系转化为世界坐标系。

式中Xw代表世界坐标系，代cmn表旋转矩阵，Xc代表相机坐标系，Dx代表平移矩阵。相机坐标系向世界坐标系的转换过程可视为坐标系先绕Z轴旋转，然后绕Y轴旋转，最后绕X轴旋转的过程，但最终还需要做出适当平移达到相互统一。

绕Z轴、Y轴、X轴旋转即等同于将原来的坐标矩阵经过三次矩阵旋转变换，三个旋转矩阵按照顺序如下：

无人车主要行驶在平坦道路，所以可认为车辆的俯仰角和侧倾角为0，即=90°，=90°。此时车辆的航向角为，当＜90°，=90°-，即旋转矩阵为=321。

3.2 局部GPS路网重建

当ZED双目相机探测到前方出现障碍物时，本文采用两步判断法进行路网重建。首先基于可通过性判断，当障碍物的一侧仍然存在其他障碍物或坑洼，排除从该侧避障的可能性；其次从左右侧避障的路径长度来判断，分别计算车辆与左右角点的距离为d，d为：

当d-d≥3时选择从右侧避障；当d-d＞3时选择从左侧避障。

本论文按照障碍物横向轮廓线方向平移，将靠近车辆一侧的离散点位用最小二乘法回归：

假设车辆从右侧避障，由二次回归直线的斜率可以求得直线与大地坐标系X轴正向的角度为：

计算GPS路网与障碍物第一个交点为(4,4)，根据该交点与右侧角点(R,R)的距离计算向右偏移距离4。

偏移后点的坐标1为：

当车辆行驶至右侧1点时，如果车辆的航向偏向右方，由于ZED双目相机水平视场角有限，所以可能捕捉不到障碍物，因此必须预先按照障碍物右侧边缘与前侧边缘垂直的假设情形来偏移一段距离的GPS点位。系统遍历E点之后50个GPS点位信息，并向右按照与当前航向垂直方向寻找障碍物右边缘点坐标，计算每个GPS点位所需要偏移的距离d为：

当车辆通过1在障碍物右侧的行进过程中,当双目相机拍摄到障碍物的图像时，按照上文提到的方法对障碍物建模，利用坐标系转换求出该段所有GPS点位的平移距离，然后计算出更新后的GPS点位。如果障碍物在路径方向的距离很长，ZED相机无法捕捉全景，所以要对障碍物的模型进行更新。考虑到车速和计算机性能，我们将障碍物模型个更新频率设置为1HZ。采用算术平均值滤波方法对大量离散的GPS坐标数据进行滤波处理，具体做法是：选取若干个样本框，对各个样本框里点位的横纵坐标进行平均后得到一个新的坐标。单个样本框的确定取决于点位的数量，当GPS点位达到预设数量时就形成了一个样本框。样本框自身的个数由双目相机前方的盲区区域大小确定，因为无法对盲区建模，所以不会有新的离散数据干扰滤波。在车辆前进时，盲区不断向前扩展，系统根据新增的GPS点位生成样本框，然后对样本框的数据进行算术平均值滤波处理。

在车辆前进的过程中，当双目相机检测到前方没有障碍物时，可能障碍物右侧边缘线向左侧偏移，还有可能车辆已经通过障碍物区域。首先我们需要确定车辆是否通过障碍物，具体做法是：选取一个适宜的△，将此时车辆航向角由调整为△，使车辆向左侧行驶，可以增大双目相机左侧的视场。将原来的坐标系顺时针旋转，相当于GPS点位逆时针旋转，车辆航向向左侧偏移。

当车辆向左侧偏移后，双目相机检测到障碍物边缘后，按照上述方法继续建模，GPS偏移。如果车辆向左偏移后仍无法检测到障碍物，此时系统认为车辆已经通过障碍物区域。

3.3 局部GPS路网和全局GPS路网的融合

首先根据偏移距离d确定车辆在距离E点d时就开始离开全局GPS路网（d正相关于d）。根据距离d遍历E点之前的点位，寻找离开点F，统计F点和E点之间的GPS点位个数为n，假设第一个点位的偏移量为0.05m，按照等比数列设计其他点位的偏移量，等比数列的比例系数q为：

根据该比例系数计算每一个点位的偏移距离d,计算所有点位的偏移距离d=0.05(m-1)，然后再次使用NURBS曲线插值法构造该段路网曲线，通过障碍物之后的局部GPS路网偏移方法与此相同。

[1] 李陆浩,面向无人驾驶汽车的车道级导航研究[D].吉林大学.2014.

[2] 夏天,智能车速度规划及路径跟踪控制方法研究[D].北京工业大学.2017.

[3] 王晨,无人驾驶智能车控制与规划系统的设计与实现[D].上海交通大学.2009.

Design and implementation of GPS and Binocular camera for driverless car

Fang Haiyang, Zhao Ruini, Tu Shiwei

( Automobile College of Chang’an University, Shaanxi Xi’an 710064 )

This paper presents a design and implementation of driverless car based on Trimble BD982 positioning sensor and ZED binocular vision sensor. The difference with RTK-GPS system base station for high precision positioning. When obstacles or eliminate appears in front of the road is detected by ZED binocular camera, With the help of the 3D obstacle model established by ZED, the local GPS road network has been established by the point bit overall migration of some sections of the road network files which were collected by the GPS originally. Finally we integrate the local GPS road network with the overall GPS network to realize the GPS autonomous navigation of the driverless car.

BD982; binocular vision sensor; RTK-GPS; road network fusion

B

1671-7988(2018)24-30-04

U462

B

1671-7988(2018)24-30-04

U462

方海洋，长安大学汽车学院硕士研究生，研究方向：无人驾驶汽车，线控地盘改造，高精度GPS导航。

10.16638/j.cnki.1671-7988.2018.24.009