视觉感知下的人工势场设计与避撞路径规划

张宇航，冀 杰，马庆禄，柏 鉴，陈云飞

(1.西南大学 工程技术学院， 重庆 400715； 2.重庆交通大学 交通运输学院， 重庆 400074)

0 引言

路径规划是自动驾驶领域的热点研究内容，在实现自动驾驶功能、保证行驶安全方面发挥着重要作用。目前，用于主动避撞的常见路径规划算法有A*算法[1]、Dijkstra算法[2]、RRT算法[3]和人工势场法等。其中，人工势场法具有结构简单、成熟高效和便于底层控制的特点，在实时避障和平滑轨迹控制方面得到广泛应用，但也存在局部最小值和无法到达目标点的问题。为解决上述问题，学者们开展了大量研究[4-6]，但将其应用于车辆路径规划仍需考虑道路边界和车辆动力学限制。针对结构化道路的车道边界问题，安林芳等[7]考虑道路边界、障碍物和最小安全间距，按正弦曲线构建障碍点；Huang等[8]融合人工势场和阻力网络理论，按车道边界和车辆动力学划分道路网格。为确保能够自适应车辆的速度变化，Liu等[9]根据车辆相对速度构建速度势场，基于模糊推理自适应分配速度势场和人工势场的权值；Lu等[10]和Song等[11]根据障碍物的质量和加速度重新设计了障碍物势场。对于弯道超车路径规划，张家旭等[12]分别利用螺旋下降函数、斜坡正弦函数和指数函数构建弯道下的引力场、斥力场和道路边界场。

分析上述文献可知，国内外学者针对人工势场在结构化直线道路上的应用做了许多积极的探索。然而，对其在弯道方面的研究相对匮乏。同时，目前多数研究只在仿真环境下对算法进行分析，缺乏试验验证环节。为获取势场所需参数，进行实车试验，于振中等[13]利用激光跟踪仪和超声测距传感器返回机器人和障碍物的位置信息，但定位精度和测距范围有限；鲍久圣等[14]利用激光SLAM实现定位与感知；胡习之等[15]利用UWB和GPS实现室内定位，采用毫米波雷达和摄像头检测前方物体，虽具有较高的检测精度，但主要适用于巷道、室内等相对封闭环境，且缺乏道路信息，无法应用于结构化道路。

综上，本文中提出了一种基于单目视觉的人工势场设计和避撞路径规划验证方法。通过单目视觉采集图像，利用传统图像处理和深度学习的方法获取道路和障碍物的参数信息，由此构建人工势场。另外，提出一种改进的人工势场算法，适用于直道和弯道的路径规划。仿真和实车试验证明了算法的可行性。

1 面向主动避撞的路径规划系统框架

为便于研究结构化道路下的人工势场构建，建立如图1所示的道路和车辆坐标系。假设道路包含主车道和超车道，车辆坐标系原点Ov定义为车辆质心，Xv轴指向车辆正前方，Yv轴指向车辆左侧。道路坐标系原点O定义为主车质心右侧车道线上点，X轴、Y轴与车辆坐标系平行。其中，道路坐标系位置固定，车辆坐标系随着车辆的移动而改变。图1(b)中，Oo、R、θ分别为弯道的曲率中心、极径和极角。

图1 直道和弯道下的道路与车辆坐标系

基于单目视觉感知周围环境包括车道线、道路宽度以及前方车辆位置，以此构建主车道引力场、道路边界场和障碍物斥力场，融合得到车辆在行驶过程中的人工势场，利用梯度下降法获得避撞路径。另外，以RTK精确定位信息为参考，利用仿真分析和实车试验对算法进行检验，并以车辆最小间距、航向角和横向距离偏差作为评价参数对所得路径进行定量评估，判断是否符合主动避撞的功能要求。路径规划系统的总体框架如图2所示。

图2 路径规划系统总体框架

2 面向人工势场的车道与车辆信息

2.1 车道线检测和宽度测距

车道线及其宽度是设计人工势场的关键参数。由于透视成像原理，车载相机拍摄的道路图像会使原本平行的车道线相交于消失点[16]，导致不能采用单一模型拟合多条车道线。因此，需要根据逆透视变换模型，把图像中的成像点转化为世界坐标点，从而将图像转换为鸟瞰图(IPM)，使车道线互相平行，其表达式为

(1)

式中：Zc为相机安装高度；u、v为像素坐标点；Xw、Yw、Zw为世界坐标点；R为相机的旋转矩阵；T为相机的平移矩阵；fx、fy为相机焦距；u0、v0为相机的焦点；O为1×3的零矩阵。

针对道路车道变白、变长和路面变暗的特点，采用文献[17]提出的滤波算法进行处理，表达式为

xi′=2xi-(xi-δ+xi+δ)-|xi-δ-xi+δ|

(2)

在实际场景中，阴影、路面杂物和车道损伤等因素都会影响特征的提取，直接对所有特征点拟合的结果准确率较低。因此，利用随机采样一致法(RANSAC)区分车道线特征与噪点，筛选出最符合车道特征的局内点；随后采用最小二乘法对局内点进行拟合，提高检测的准确性，主要步骤如下：

步骤1根据车道线的形状特点，选取抛物线模型进行求解，表达式为：

y=ax2+bx+c

(3)

步骤2随机抽取3个特征点，列出三维方程组求解参数a、b和c：

(4)

步骤3为检验拟合的效果，设定评价指标S。S越小，说明模型更能准确地表达车道线。评价指标S表达式为

(5)

式中：n是采样点个数。重复步骤1—3，多次迭代取S最小时的参数为最优模型参数。

步骤4筛选所有接近最优模型的样本点作为局内点，局内点应满足：

(6)

式中：ε为设定的距离误差。利用最小二乘法对局内点进行求解，见式(7)。使所有点到拟合曲线偏差的平方和最小，最终可实现对车道线的精准检测。

(7)

对于车道宽度，可在检测出车道线的基础上建立测距模型进行求解。根据车道线位于地面，即Zw=0的先验条件，由式(1)推导出地面位置世界坐标和像素坐标的变换关系如式(8)所示。

(8)

相机安装在车辆横向的中心位置，在相机前方，即图像中间位置固连一条纵向和横向引导线，两者与车辆保持相对静止，见图3。其中，Ll、Lm和Lr为引导线与车道线之间的交点，W和Wr表示车道宽度和车辆相对道路右边界的偏移量。分别将Ll、Lr和Lm的像素坐标(u,v)代入式(8)即可得到其世界坐标(Xl,Yl)、(Xr,Yr)和(Xm,Ym)，据此可得到W和Wr表达式为：

图3 车道测距模型示意图

W=|Xl-Xr|

(9)

Wr=|Xm-Xr|

(10)

2.2 基于Yolo-3DBox的车辆目标检测

使用Yolo-3DBox神经网络方法实现目标的3D检测。首先采用YOLOv4算法对目标的位置和类别进行检测，该算法利用CSPDarknet53主干网络提取图像特征，结合SPP模块和FPN-PAN结构对提取的浅层特征进行加工、增强，最后利用滑动窗口的思想对目标框的坐标和种类同步回归，从而得到物体的2D检测框。在此基础上，结合Deep3DBOX[17]的思想，通过数据集的中位数和预测偏差直接回归物体的维度D，利用透视原理和相机内参，采用MultiBin Loss损失函数回归物体的方向角R(θ)，最后根据目标的维度、方向角和3D包围盒到2D检测框的几何投影约束，最小重投影误差即可得到3D包围盒的中心坐标T，由此得到目标的3D包围盒。YOLO-3DBox网络架构框图如图4所示。

图4 YOLO-3DBox网络架构框图

算法在KITTI数据集上的检测效果如图5所示。图5的上部分为2D检测结果和类别得分，下部分为3D检测结果和目标与相机的相对位置(x=31.11 m,y=4.06 m)。

图5 目标检测结果

3 基于人工势场的路径规划

为了定量描述人工势场，设定如下场景：汽车行驶在直道或弯道上，主车道前方存在1辆静止车辆。基于上述场景设计对应的人工势场，主要包含主车道引力场、道路边界场和障碍物斥力场。

3.1 主车道引力场

参考引力势场设计方法，将主车道中心线上的点作为引力点，使道路势能在横向上从主车道中心线向两侧逐渐增大，纵向上沿行驶方向逐渐减小，确保车辆在非避撞情况下沿主车道中心线行驶，由此构建直道工况下的主车道引力场函数为

(11)

式中：α为主车道引力场所占权重；Xp为纵向路径搜索长度；Ym和Yl分别为主车道中心线和道路左边界的横坐标。对于弯道工况，道路横坐标对应极径R，道路纵坐标对应极角θ，构建弯道主车道引力场函数为

(12)

式中：θp为极角搜索范围；Rm和Rl分别为主车道中心线和道路左边界的极径。

直道和弯道的主车道引力场三维分布如图6 所示。

图6 主车道引力场曲面

3.2 道路边界场

根据道路行驶规范，车辆一般情况下应保持在当前车道行驶，前方存在障碍物时，可在保证安全的前提下向左换道超车，但要避免与道路边沿发生碰撞。因此，设计的道路边界场应满足如下要求：

1) 非避撞区域内，设定两车道中心线处势场幅值最小，向两侧逐渐增大，在道路边沿达到最大值。

2) 避撞区域内，为保证顺利换道，设定道路中间区域势场幅值全部为0，其他与非避撞区域相同。

根据以上要求，在障碍物前后一定距离设定避撞区域，其大小参考动量守恒定律，根据车辆当前速度下的刹车距离来确定，表达式如(13)所示。另外，为防止势场突变，应在避撞和非避撞区域间设一平滑过渡区域，保证规划路径的连续性。

(13)

Xs=1.2Xc

(14)

式中：Xc、Xs分别为避撞区域和平滑过渡区域半径；m为被控车辆质量；v、vobs分别为被控车辆和障碍物车辆的速度；Fm为车辆单轮最大制动力。

由此，设定道路边界场在横向和纵向的幅值变化函数为

(15)

(16)

式中：Ym和Yp分别为主车道和超车道中心线的横坐标；Pm为设定的道路中界势能；Xobs为障碍物的纵坐标。

得到道路边界场的幅值变化函数为

A(X,Y)=A(X)×A(Y)

(17)

选择相对平缓的三角函数对变化函数归一化，最终得到道路边界场表达式为

(18)

式中，W为单车道宽度。

同理，可得到弯道边界场表达式，这里不再详细介绍。道路边界场的三维分布如图7所示。

图7 道路边界场的三维分布

3.3 障碍物斥力场

与道路边界相比，障碍物的危险系数较高，因此，参考传统人工势场中的指数函数，构建符合车辆外形的椭圆避障区域，得到障碍物斥力场的表达式为

Uo(X,Y)=|exp[-px(X-Xobs)2-py(Y-Yobs)2]-Pt|

(19)

式中：Pt为设定的势能阈值；px、py分别为斥力场在纵向和横向上的指数参数，其值影响斥力场的作用范围。

根据人工势场的设计原则，斥力势场应在避撞区域的临界点处恰好为0，越靠近障碍物时斥力势能越大。因此，设计纵向指数参数px为

(20)

障碍物的横向参数主要根据换道过程中的横向位移和车辆的转向特性。理想条件下，车辆在主动避撞过程中应从主车道的中心线移动到超车道的中心线，也就是横向位移为单条车道宽度。同时，为保障规划路径的平滑，满足车辆的运动学约束，设定车辆的横向位移在避撞过程中按正弦曲线变化。因此，结合px设定横向指数参数py为

(21)

将px、py代入式(19)即可得到障碍物斥力场的三维分布，如图8所示。

图8 障碍物斥力场的三维分布

3.4 面向避撞的人工势场

直接将3种势场叠加融合，即可得到如图9所示的面向避撞的人工势场的三维分布，表达式为

图9 人工势场的三维分布

U(X,Y)=Um(X,Y)+Ur(X,Y)+Uo(X,Y)

U(R,θ)=Um(R,θ)+Ur(R,θ)+Uo(R,θ)

(22)

利用梯度下降法得到人工势场的等高面图及其确定的避撞路径，如图10所示。其中，红色曲线即按梯度下降最快方向确定的避撞路径。

图10 道路等高面图及避撞路径示意图

4 路径规划结果分析

为检验本文算法的有效性，分别对3种工况的路径规划结果进行分析，并通过Matlab/Carsim对路径进行跟踪仿真，仿真参数设置如表1所示。

表1 仿真参数设置

4.1 静态障碍物的直线道路

该场景下，假设被控车辆以恒定速度(10、20、25 m/s)行驶在主车道中心线上，前方存在静止车辆。此时，避撞路径及相关分析结果见图11。

图11显示了不同速度下的避撞路径、航向角和质心间距。由图11(a)和(b)可看出，被控车辆在10、20、25 m/s速度下的纵向避撞距离分别为40、100、140 m，最大航向角分别为0.257、0.122、0.088 rad。经对比分析发现，被控车辆速度越快，规划路径的纵向避撞距离越长，最大航向角越小，变化越平缓。由此可以证明，新路径规划算法能够自适应车辆速度调整转向快慢，保证车辆在换道过程中的平稳性和舒适性。图11(c)显示，在3种速度下，车辆与障碍物之间的质心间距在两车辆平行时达到最小值，分别为3.60、3.74、3.86 m，近似等于车道宽度W，两车之间能保持足够的距离，符合安全性要求。

图11 静止障碍物的直道路径分析结果

4.2 动态障碍物的直线道路

该场景下，假设被控车辆以恒定速度20 m/s行驶在主车道中心线上，前方存在匀速运动(5、10、15 m/s)的车辆。根据车辆之间的相对速度规划避撞路径，分析结果见图12。

图12(a)、(b)为障碍车辆不同速度时被控车辆的避撞路径和航向角变化曲线。经对比可发现，车辆之间的相对速度越小，避撞路径的纵向距离越小，最大航向角越大，变换越迅速。图12(c)为避撞过程中两车之间的质心间距，分别为3.73、3.72、3.57 m。图12(d)中选取障碍物速度为10 m/s的避撞路径进行动态分析，其中实线和虚线矩形分别代表不同时间下的主车与障碍车辆所在位置。可以证明，在前方存在动态障碍物的情况下，采用新算法规划的路径在保证足够安全距离的同时能尽量缩短换道的纵向距离，减少占用超车道的时间。

图12 动态障碍物的直道路径分析结果

4.3 静态障碍物的弯曲道路

该场景下，被控车辆以恒定速度20 m/s行驶在固定曲率半径的弯曲道路上，主车道上存在静止车辆。根据城市道路设计规范，选取曲率半径300、400、650 m的弯道进行仿真分析，结果如图13所示。

图13 不同曲率道路的路径规划分析结果

图13(a)所示为不同曲率半径下的路径规划结果，可以看出生成的无避撞路径足够平滑。图13(b)所示为换道过程中的航向角变化，由于道路是弯曲的，导致航向角不断增加，但总体而言仍存在先增大、后减小的趋势，能够保证车辆实现主动避撞目标后回到主车道行驶。由图13(c)可知，被控车辆与障碍物车辆相互平行时距离最近，近似等于车道宽度。

4.4 路径跟踪仿真

为验证规划路径的跟踪性能，利用Simulink/CarSim联合仿真，对被控车辆速度为20 m/s、前方存在静止障碍物这一场景的路径进行跟踪仿真。

图14(a)展示了采用Carsim成熟驾驶员模型(DM)控制器跟踪路径结果和规划路径结果；图14(b)展示了规划路径的曲率变化，显示整体曲率较为连续，不存在阶跃式突变现象，最大曲率为0.013 m-1，能满足车辆的运动学约束。

图14 路径跟踪仿真结果

图14(c)和(d)显示了跟踪过程的误差，表明在行驶过程中规划路径与跟踪路径的最大横向偏差不超过0.094 m，航向角偏差最大不超过0.021 rad。仿真结果表明，新算法生成的无碰撞路径具有较好的跟踪性能。

5 实车试验

为进一步验证主动避撞系统的有效性，结合现有条件，选取一段空旷的结构化直线道路作为试验场地(包含左右双车道和车道线)。被控车辆前方停放静止车辆，试验场景与实车平台如图15所示。

图15 试验场景与设备实拍图

试验平台为具有阿克曼线控转向的无人驾驶平台，搭载的主要设备包括：① 司南M300接收机和GNSS天线，与基站配合获取高精度RTK定位信息，实现车辆自身的厘米级定位；② 车载相机和16线激光雷达，实现对前方道路以及周围障碍物的感知；③ USBCAN系列CAN接口卡，使PC机可通过USB连入CAN网络，进行CAN总线数据采集和处理；④ 轮速传感器，实时检测车辆的速度和车轮转角。

试验基于ROS架构实现各模块间的通信。感知模块根据相机采集的图像实现前方车道线参数和障碍物位置的检测；路径规划模块基于感知信息规划车辆主动避撞的局部路径；控制模块根据规划路径、车辆几何模型和GPS定位信息发出控制指令，并通过USBCAN卡将执行指令发送到线控底盘，控制无人车的速度和转向，实现路径跟踪。

图16所示为环境感知的检测结果。实线为车道线的检测结果，x=8.36 m和y=0.86 m为障碍车辆与被控车辆在纵向和横向上的相对距离。根据这些信息，建立当前场景的人工势场，并基于此进行路径规划，试验结果如图17所示。

图17(a)是将人工势场的等高线图等比例投影到无人机拍摄的俯视图上，红色曲线为得到的被控车辆针对前方障碍物的局部避撞路径。图17(b)中，绿色曲线为GPS采集到的路径点，即车辆实际行驶轨迹。图18为车辆当前位置与规划的最近路径点的横向偏差和航向角偏差。可以看出，车辆行驶过程中的横向偏差最大不超过0.105 m，航向角偏差最大不超过0.092 rad，均在合理范围内。试验结果表明，新算法可以实现车辆主动避撞，且规划的路径具有较好的跟踪性能。

图17 路径规划及跟踪结果

图18 试验跟踪误差

6 结论

1) 提出一种基于单目视觉的人工势场构建方法，利用逆透视变换消除相机成像的投影特性，根据车道线的平行关系和随机采样一致算法对车道线进行拟合，提高了检测准确率；提出一种地面测距模型测量车道线宽度。同时，利用YOLO-3DBOX算法检测前方车辆的位姿用于后续人工势场的构建。

2) 利用三角函数和指数函数构建适用于车辆主动避撞的人工势场，对多工况下的路径规划效果进行仿真分析和实车试验。结果表明，单目相机能够实现人工势场的构建，新路径规划算法能够实现直道和弯道的局部避撞路径规划，具有较好的跟踪性能。

3) 目前只采用基于单目相机的感知方案，下一步工作将考虑融合全景相机和激光雷达实现360°感知，同时继续对路径规划算法进行优化，使能够应对更复杂的多障碍物工况。