低速无人车编队控制研究∗

李 通 刘元盛 钮文良 钟启学 郭笑笑

（北京联合大学 北京 100101）

1 引言

在过去的数十年来，大家的注意力渐渐地从单一的受不完整约束的移动机器人的无人驾驶［1～2］转移到多车辆的编队无人驾驶控制。这是因为多车辆的协作编队行驶可以大大提高公路的效率和安全性。

有很多的编队控制方法，基于行为法［3］，领航跟随法［4～9］，虚拟结构法［10］，基于路径跟踪的方法［11～12］。其中最直观、最受关注的是领航跟随法。这个方法有两种控制器：l-l控制器和l-ψ控制器。在跟随车辆的跟随控制中还有横向控制［13］、纵向控制［14］、横向纵向综合控制［15］三种。

在文献［4～5］中分别使用车载传感器信息和基于图像处理的方式进行领航跟随算法的实现，在以上两种方法中都是使用传感器信息来进行计算从而产生跟随速度的输入量。但是在使用图像的方式进行领航跟随编队实现的时候，会发现它的可靠性会比较差，受光线影响非常大，尽管有文献进行过改进，增加一个红外成像探头进行红外图像识别但是在热源干扰强烈的环境下仍会误检。在文献［9］中，在领航跟随算法l-ψ控制器中，跟随车辆与领航车辆所保持的距离和角度是固定不变的，且可以通过领航车辆的实时位姿进行解算，可以实时精确地计算出跟随车辆应该处于的状态信息。于是将这个状态信息转化成一个虚拟车辆，进而使实际跟随车辆去跟随这个虚拟车辆。但在该文献中只是完成了编队控制器的设计并没有进行避障算法的设计。并且在前人所做的大量的研究上很少有在实际车辆上进行实验的，大部分都是使用计算机仿真进行实验，也有人使用很小尺寸的玩具车进行实验。

在本文中将利用文献［9］中所设计的跟踪虚拟跟随车辆的控制器进行编队控制，同时采用简化的VFH 算法进行避障。本文所采用的简化的VFH 算法是在经典的VFH、VFH+、VFH*算法［16～18］的基础上进行改进的方法，易于实现。同时采用基于行为的方法进行全局路径规划。在行驶路线上激光雷达没有检测到障碍物时就进行队形保持行为；如果检测到障碍物并且判断出可以避开障碍物就进行避障行为。并在实际车辆进行编队控制和避障实验，使用无线路由器进行编队车辆间的信息交互，实验结果表明了本算法的有效性。

2 全局路径规划

本节将利用基于行为的方法进行全局路径规划。基于行为的编队算法是将编队控制分解为一个个小的行为，如队形保持、避障、驶向目标等。利用一种有效的行为协调机制进行行为选择。

有限状态机（如图1 所示）集合S中包含了队形保持、避障、驶向目标。在收集各个车辆的运行状态反馈x1、xn后，进入F 中进行状态解算并将结果发送给各车辆。进行决定是进行队形保持还是避障状态。

图1 有限状态机

用于车间通信的设备本文采用的是一个有效通信距离在100m 的无线路由器如图2 所示，使用UDP协议进行数据传输［19～20］。

图2 无线路由器

3 跟踪控制器设计

本文采用领航-跟随的编队方法进行编队控制。由领航车辆的位姿信息计算出跟随车所对应的虚拟车辆的位姿信息（如图3 所示），进而将编队控制器转换为对虚拟车辆的跟踪。本节主要目的是设计出跟随车辆的跟随控制量跟随车辆的线速度、跟随车辆的角速度。

各车辆均满足纯滚动无摩擦的非完整约束，各车辆的运动学动态模型均为其中：x(t)，y(t)，θ(t)ϵR，表示车辆质心的坐标和方向；v(t)，ω(t)ϵR表示线速度和角速度；d 为质心到轴心的距离。

图3 领航、跟随车在全局坐标系下位姿关系

图中的h 点表示传感器所安装的位置，因为考虑到实际车体较长，为了提高模型精度，必须考虑车体长度。h 点与质心的距离为l。图2 中表示的是一对领航跟随车的位姿关系。可得h点的位姿：

根据领航跟随所需保持的距离σd和角度φd，可以得到虚拟车辆Rv的位姿信息：

为了设计出实际的跟随车辆Rf中的h 点与所对应的虚拟车辆Rv上的质心坐标的跟踪误差e 方程组为零并且导数也为零，才能保证稳定。

对式（4）求导可得：

由于式（5）过于复杂，现在为了简化计算，将以跟随车辆Rf的质心为原点，以Rf的前进方向为z1轴的正方向建立坐标系。如图4所示。

图4 领航、跟随车在以跟随车坐标系下位姿关系

根据新的坐标系可以得到如下误差方程：

为了使得式（6）的导数值最小选择如下控制率：

其中k1、k2为正常量；vl、ωl为领航车辆的线速度和角速度。这个控制器的稳定性的证明是利用反步法和李雅普诺夫稳定性理论得到的，证明过程在文献［9］有详细的介绍，这里不再赘述。

4 编队避障控制器设计

4.1 编队避障控制

本节主要目标是设计出整车队形的避障策略，以此来保证在编队车辆正常行驶过程中不会与路径上的障碍物发生碰撞。

在编队行驶过程中，任意一台车发现了障碍物就变换成纵列直线队形，跟踪领航车进行的轨迹避开障碍物。

4.2 单辆车避障的局部路径规划

上一节通过李雅普诺夫函数设计出了基于领航跟随的队形保持控制器。该控制器是使各个实际车辆分别跟踪与之对应的相对应于领航车辆一定距离和角度的虚拟车辆来保持队形，从而本身就具有一定的队形内避碰的功能。

为了能够更加稳定可靠地通过障碍物或者在无法通过的情况下停车，又在编队队形内车辆间距离过近并且有碰撞趋势的情况下，利用本节方法可以实现紧急停车。本文参考比较成熟的避障算法VFH、VFH+、VFH*，采用一种简化的避障方法。

由于在规划好了全局路径之后，障碍物的分布比较简单且大致可以预测。

i车在行驶路线τi上发现了障碍物A（如图5所示）此时目标方向决策如下。

图5 路径上出现障碍物A

1）活动窗口创建

本车所用激光雷达精度很高，误差远比声呐传感器低得多，所以在接收到雷达反射数据后可以很轻易地判断出是否为障碍物。若发现障碍物就在栅格图中对应位置的栅格元填上1，表示不可通行。因为是多线激光雷达，所以在不同层次的雷达光束上可以探测到障碍物后面的更高的障碍物，亦既在栅格图中y轴方向的栅格位置。如图6所示。

扇区具有任意的角分辨率α，为了使扇区数(n=360°/α)为整数也为了能使得扇区精度更高，此处的α取值为1。并且此处只考虑车的前向180°部分，所以此时实际扇区数(n=180°/α)。对于每个扇区k(k=0、1、2、3…)都可以很方便地计算出所对应角度ρ=kα。同理由于k是个整数值可得k=INT(βij/α)。其中βij为以车辆激光雷达为原点的坐标系中，在栅格中位置坐标为(xi，yj)的栅格元相对于坐标原点的角度值。

图6 栅格化后的障碍物分布

2）障碍物与车辆描述

本文通过沿障碍物边缘栅格区域膨胀一块区域为rr+s=ds⋅rr（如图7 所示），其中ds安全系数，rr为车身的宽度。

图7 障碍物与膨胀区域

此时车辆就可以看成一个质点，用一个小栅格表示，图中黑色数字1 部分就是雷达实际所探测到的障碍物，浅色数字1就是所膨胀部分。

3）行驶方向决策

在障碍物膨胀后的栅格图中进行行驶方向决策，首先要进行可行驶区域规划。在栅格图中将各个无障碍区域标记出后，构建掩膜直方图（图8 所示），并且根据车辆的运动特性，将当前不可到达的区域在直方图中置为1。

图8 掩膜直方图

图9 可行驶区域决策

从掩膜直方图中可以看出有三个可行驶区域，此时本文选择距离目标点最近的那个可行驶区域的角平分线方向。

对于障碍物比较稀少连续的可行驶扇区超过了smax，本文采用smax=15。则在此时的决策方向为从距离目标点最近的一个扇区开始到smax之间的角平分线方向。

5 实验结果与分析

5.1 实验平台

本实验所采用的实验平台为北京联合大学的低速无人智能车项目组的旋风巴士如图10 所示。本车辆上装备了导航设备、激光雷达、摄像机、工控机，在领航车还加装了用于编队车辆间的信息传递的无线路由器。本实验所采用的无线路由器的有效通信距离在100m 内，所以在编队队形内能保证通信的稳定性。

图10 “旋风巴士”无人驾驶智能车实验平台

5.2 实验结果与分析

在本次验证实验中编队队形采用水平编队。车辆间距σd取2.5m，保持角度φd取180°，实验地点北京联合大学校区内。

本次实验中的线路上有一段直线行驶的路线，在线路中加入一个标准高度的锥桶作为障碍物，以此来判断编队车辆能否安全的绕开障碍物。

在实验结果中能看出编队队形在避障上的动作安全有效。在障碍物前根据雷达数据以及简化的VFH算法计算出有效的决策角度。图11中进行方向决策，在成功地避开了障碍物之后，整个编队能够跟随领航车辆回归到预定路线。与图11 对应的角度决策直方图如图12 中的虚线为预定的轨迹，中间的灰色圆圈所框出来的是障碍物，左边第一条灰色实线为在障碍物膨胀后的第一个可行驶扇区，最右边的是最大连续扇区，所以决策方向为这两个扇区的角平分线。

图11 可行驶方向决策图

图12 障碍物分布图

实际队形行驶轨迹如图13 所示，图中黑色圆圈内为障碍物，黄色曲线为领航车辆其余两条白色曲线为跟随车辆。在障碍物前后方向可以看出来编队队形在直线行驶的时候队形保持的有效性。

图13 编队车辆的实际行驶轨迹

6 结语

本文研究了低速无人驾驶智能车的编队与避障，在实车试验中证明了算法的有效性。车辆之间能够有效地保持设定的距离和角度，并且在遇到障碍物时可以及时有效的避开。

在完成编队与避障的同时，本文也有很多需要改进提升的地方。在本文中只考虑了简单的少量的车辆的编队控制，对于大量的、高速的车辆的编队控制还有很多要改进的地方，对于庞大的编队群体来说一个稳定可靠的通信环境也是至关重要的，本文只是采用了一个无线路由器进行车辆间的通信，因此编队规模和车辆间的距离都要受限于路由器的通信范围，将来需要在这方面进行改进。