改进互动速度障碍的多机器人协同避障

徐 鹏，康雪晶

（天津工业大学 电气工程与自动化学院，天津300387）

目前移动机器人协作是国内外研究的热点之一，多机器人协同避障控制，要求每个机器人各自到达指定目标点，同时能安全地避开静态障碍物，并且当机器人之间路径交叉会导致碰撞时，能够协调自身完成动态避碰。在过去的20年中，国内外很多学者对其进行了研究，文献[1]首次提出了基于碰撞锥概念的速度障碍法（VO）的理论模型，实现了基本的多机器人动态避碰，但VO 本身有自身的缺点；文献[2]提出了互动速度障碍（RVO），消除了机器人之间的振荡运动；文献[3]提出了有限时间速度障碍（FVO），解决了拥挤环境下机器人速度无法选择的问题；文献[4]提出了广义速度障碍（GVO），解决了类似汽车机器人的实时导航问题；文献[5]提出了混合互动速度障碍（HRVO），减少了机器人交互舞蹈的影响；文献[6]将速度障碍法用于追踪动态目标点；文献[7-10]将改进后的速度障碍法应用于无人飞行器（UAV）；文献[11]提出了一种互相定位算法，解决了死锁问题；文献[12]提出一种最短距离算法，保证了平滑的轨迹和无碰撞的路径；文献[13]解决了完整椭圆机器人的局部避碰问题；文献[14]将动态窗口法与椭圆速度障碍结合，运用在无人艇上，保证了无人水面艇选择的速度是安全可达的。为提高机器人在动态环境中计算无碰撞轨迹的效率，考虑到各机器人自身半径大小不同，本文采用了动态窗口法与比例选择互动速度障碍法结合，让机器人根据自身大小自由地选择线速度与角速度，减少了不必要的速度抖动，缩短了避碰时间。

1 速度障碍的定义

速度障碍（VO）为一种动态障碍物的处理方法。如图1所示，移动机器人A、B 的相对位置关系，移动机器 人A可以 表示为RA（t）=（XA（t），vA（t）），其中，XA（t）=（xA（t），yA（t）），vA（t）是移动机器人A 的速度向量。同理，移动机器人B 可表示为RB（t）=（XB（t），vB（t）），XB（t）=（xB（t），yB（t）），vB（t）为移动机器人B 的速度向量。t 为当前时间，Δt 定义为时间间隔，并假设所有机器人在一个时间间隔内速度保持不变。

图1 速度障碍法Fig.1 Velocity obstacle

1.1 碰撞锥

通过以移动机器人A 为质点，并将移动机器人B 的半径膨化为ra+rb构建碰撞锥，碰撞锥定义了将来在两个机器人之间引起碰撞的相对速度的集合RCA。如图所示，在数学上定义如下：

式中：vab=va-vb代表A 相对于B 的相对速度；λab是vab的方向；Bab是移动机器人B 所膨化的圆。

1.2 速度障碍

将工作空间转化为Ra的速度空间后，将RCA沿vb平移后可以得到将来导致碰撞的绝对速度集合，定义为

等价于：

式中：⊕代表闵可夫斯基矢量和运算。由此可知，当va的末端点在里面时，会导致碰撞，故只需选择此区域之外的速度即可

1.3 互动速度障碍

速度障碍法可以解决机器人与运动障碍物的避碰问题，但当两个机器人互相避碰时，由于两个机器人使用相同的算法，会产生决策振荡。这是因为两个机器人都在彼此速度障碍之外选择新的速度，导致它们会在新的与旧的速度值之间振荡。互动速度障碍使每个机器人互相承担一半的避碰任务，通过观察其他机器人的反应解决振动问题，此时并不只是在速度障碍区之外选择一个速度，而是选择一个新的速度为当前速度与任意一个位于速度障碍区之外的平均值，保证了两个机器人不会发生碰撞和振荡，互动速度障碍原理如图2所示。

图2 互动速度障碍法Fig.2 Reciprocal velocity obstacle

RVO 的定义如下：

2 动态窗口法

动态窗口法[15]是一种广泛使用的局部路径规划方法。其思想是在速度空间中采样多组速度，它正确地融合了机器人的动态特性，通过将搜索空间减少到一个动态窗口，该窗口由在下一个时间间隔内可到达的速度组成。在动态窗口内，模拟出多组运动轨迹，并计算这些轨迹在下一时刻是否与障碍物碰撞，最后通过最大化目标函数来选择最优轨迹。

2.1 动态窗运动模型

本文采用了文献[16]所提出的运动学模型：

式中：t 代表当前时刻；Δt 代表时间间隔；v，ω 分别代表速度和角速度；（xt+Δt，yt+Δt）代表从当前位置（xt，yt）移动后的坐标；θt+Δt代表从当前角度θt转动后的角度。

2.2 速度采样

机器人运动时在速度（v，ω）的二维空间中，存在无穷多组速度。但是根据机器人自身的限制和环境限制可以将采样速度控制在一定范围内：

（1）移动机器人自身最大速度与最小速度的限制

（2）电机力矩限制

考虑到电机力矩有限导致最大加速度限制，因此在下一个时间间隔内会产生一个动态窗口，机器人可以到达的所有区域都在动态窗口内。

式中：vc，ωc分别代表移动机机器人的当前线速度和角速度；v˙a，v˙d，ω˙a，ω˙d分别代表最大加速度和最大减速度。

（3）安全制动距离限制

从安全方面考虑，为了保证机器人不会因撞到障碍物而停止，因此在最大减速度条件下，机器人速度有一个范围：

式中：dist（v，ω）为速度（v，ω）对应轨迹上离障碍物最近的距离。

3 算法融合

动态窗口法作为一种良好的局部路径规划算法，可以很好地避开静态障碍及突发的静态障碍。但在多机器人环境中，该算法无法避开动态障碍，但如果只使用速度障碍法动态避障，选出的避碰区域受机器人本身约束，有些速度是不可达的。为解决上述问题，本文将互动速度障碍法与动态窗口法融合，这样既保证其能静态避障又能完成动态避障。

由移动机器人本身的运动学性能，可以通过式（8）～式（10）计算下一时刻移动机器人在给定时间窗口内所达到的速度。然后将得到的速度信息离散化构成一个矢量速度集称为RV。如图3所示，通过互动速度障碍法算出动态避碰区域RVO，RVA 区域包含RVO 将要导致动态碰撞区域，用RV 集合剔除RVO 集合中会导致动态碰撞的速度即可得到安全可达的速度，即SRV：

图3 RVO-DWA 算法Fig.3 RVO-DWA algorithm

RVO-DWA 算法主要是通过移动机器人的激光雷达传感器采集数据，利用ROS 系统的分布式架构的特点，各个机器人之间能够共享位置信息和速度信息，根据已知的数据计算出RVO 区域，并判断DWA 采样的数据是否在RVO 区域内，如果DWA采样速度在RVO 区域内，则将其从RV 中剔除，否则继续生成轨迹。在获得多组采样轨迹后，通过建立最优评价函数选择下一时刻的最优速度，评价函数必须保证既能朝目标点前进又可以避开静态障碍。评价函数如下：

式中：heading（v，ω）是方位角评价；dist（v，ω）是距离评价，表示离障碍物最近的距离；velocity（v，ω）评价当前速度大小；α，β，γ 为加权系数。

通过对RVO-DWA 算法进行大量仿真发现，当我们假定两个移动机器人半径不同时，会出现移动机器人走的路径过长，导致避障效率过低。经分析发现，这是因为RVO 算法是两个移动机器人分别承担一半的避碰任务，两个移动机器人行动能力是不同的。从而导致大移动机器人耗费代价较大。

针对上述问题，本文在分配避碰速度上进行改进，如图4所示，将原有的分配方式改为按照各自半径占有的比例的反比来分配，假设两个移动机器人半径分别为ra，rb。则有，移动机器人A 分配rb/（ra+rb），移动机器人B 分配ra/（ra+rb）。这样就可以使两个移动机器人自由的分配避碰速度，移动机器人Ra对Rb的PRVO 计算公式为

本文将改进后的算法称为比例相互速度障碍法PRVO-DWA 算法。

图4 PRVO-DWA 算法Fig.4 PRVO-DWA algorithm

4 仿真实验

为了验证PRVO-DWA 算法可以使机器人合理分配避障任务，从而快速完成避障。首先在Ubuntu16.04 下ROS 机器人操作系统中的Gazebo 仿真平台搭建物理仿真环境，然后启动Gazebo 物理仿真环境、代价地图、移动机器人A 和移动机器人B 的move_base 节点、Rviz 可视化界面。最后，在Rviz 上通过设置目标点使移动机器人运动。

由图5，图6所示可知，移动机器人采用RVODWA 算法进行避障时，可以很明显发现，移动机器人的运动轨迹出现了扭曲，两移动机器人在避障时速度都有明显的减慢，会导致避障效率降低；而采用PRVO-DWA 的移动机器人运动轨迹相对较为平滑，并且移动机器人B 在避障时在最大速度限制内选择适当加速通过，由此缩短了避障时间，提高了避碰效率。

图5 RVO-DWA 和PRVO-DWA 直角行驶运动轨迹Fig.5 RVO-DWA and PRVO-DWA right-angle driving trajectory

图6 RVO-DWA 和PRVO-DWA 直角行驶速度曲线Fig.6 RVO-DWA and PRVO-DWA right-angle driving velocity curve

当移动机器人进行对向行驶时，由图7，图8所示，可以发现，采用RVO-DWA 算法所花费的距离代价较大，并且移动速度波动较大；而采用PRVODWA 算法，移动机器人运动轨迹明显缩短，速度也较为稳定。

图7 RVO-DWA 和PRVO-DWA 对向行驶运动轨迹Fig.7 RVO-DWA and PRVO-DWA opposite driving trajectory

图8 RVO-DWA 和PRVO-DWA 对向速度曲线Fig.8 RVO-DWA and PRVO-DWA opposite driving velocity curve

5 结语

本文首先将RVO 算法与DWA 算法进行融合，对移动机器人机器人施加了运动学约束限制，在此基础上对RVO-DWA 算法进行改进，提出了PRVODWA 算法，使移动机器人根据自身半径大小合理调整速度进行避障。最后将所提出了的算法在Gazebo 物理仿真平台进行了实验验证，仿真结果验证了算法的可行性。在今后的工作中，将算法移植到Turtlebot3（Waffel Pi）机器人上进一步验证。