引言

多机器人系统是由多个智能体组成的群体，它们可以相互协作完成某一共同任务^[1].多机器人系统可以有效突破单机器人系统在感知、决策及执行能力等方面的局限，在系统鲁棒性、任务复杂性、可扩展性、灵活性和适应性等方面具有显著优势^[2-4].多机器人系统的研究最早可以追述到1987年Toshio等人开发的多机器人系统CEBOT^[5]，这是世界上首个基于智能体的多机器人系统.

多机器人系统的行为研究常关注多机器人如何在系统空间中实现聚集、围捕以及不同形态的队形模式^[6].其中，聚集是多机器人系统一致性的基本问题^[7，8].多移动机器人聚集是指空间中任意分布的机器人根据给定的任务目标，在空间中选取合适的聚集区域，并规划从各自位置到聚集目标点的路径，使各机器人按规划路径移动，最终聚集到合适的聚集区域内.聚集任务在无人机编队、仓储物流机器人集群、模块化机器人自重构^[9]等领域中具有广泛应用.在确定特定的聚集点之后，需要进行路径规划，规划出各机器人到达聚集点的路径，以便后续多个机器人能够沿着路径安全、高效地到达聚集点完成任务.常见的路径规划的方法有基于图搜索^[10]、基于采样^[11]、基于智能优化算法的方法^[12].图搜索算法一般指在图形环境中（如二维栅格地图中）搜索路径的算法，其代表有A^*、Dijkstra等路径规划算法.基于采样的路径规划方法则是在空间内随机采样，然后按一定的规则连接从起点到终点之间的一些采样点构成路径，其代表有RRT^[13]、PRM^[14]算法等.基于智能优化方法主要是指启发于动物种群的习惯与行为，其代表有蚁群算法^[15]、粒子群算法^[16]等.部分多移动机器人路径规划方法还会考虑机器人之间的协同^[17]，本文研究内容为多机器人聚集问题，即规划聚集点和路径使分散的多个机器人移动至一个区域内，这一过程下各个机器人的最优路径之间一般不会出现交叉的情况，因此本文不考虑机器人之间的协同问题.

一般的路径规划算法在终点发生改变时，需要重新计算才能获得路径，并且聚集点选取和路径规划很难协同考虑.针对上述多移动机器人聚集路径规划方法的局限性，本文基于快速行进平方法^[18]，对多移动机器人聚集点的选择、路径规划与控制问题进行研究.自各移动机器人起点开始做快速行进平方法就可以获得整个地图相对于该起点的时间场，终点改变时不需要重新完整运行一遍算法流程，只需要从新的终点开始，在时间场内沿梯度下降最快的方向回溯即可得到路径.因此，针对多机器人聚集任务的目标多样性，聚集点选取的多变性，快速行进平方法可以快速响应.此外，本文对快速行进平方法的速度图进行改进，使用改进后的快速行进平方法可实现在同一框架下完成多移动机器人聚集任务的聚集点的选取以及路径规划，使得聚集点选取和路径规划两项子任务协同优化决策而不是相互独立.

本文提出了基于改进快速行进平方法的路径规划策略.具体地，本文考虑三种常见的任务需求，基于改进的快速行进平方法所获得信息确定最优的聚集点，并规划出各机器人自起点至聚集点的路径.通过改进快速行进网格地图，在保证障碍安全的前提下减少路径冗余，并将路径上的速度作为移动机器人在进行轨迹跟踪时的指导速度，以提升轨迹跟踪的精度与准确性.最后，为了验证改进后速度场所提供的路径上速度信息的有效性，我们建立了轮式机器人的动力学模型，给出了模型预测控制策略，并进行了仿真验证.

1 多移动机器人聚集路径规划

1.1 快速行进平方法

快速行进平方法（Fast marching square method，FM²）是在快速行进法的基础上发展而来.快速行进法（Fast marching method）是Osher和Sethian提出的水平集方法的一个特例，其是一种有效的数值计算方法，用于跟踪和模拟物理界面波的运动^[19].在快速行进法中，波前的运动由程函方程（Eikonal equation）给出：

其中x表示是位置，F（x）是波在x处的速度，T（x）表示波从起始位置传播到x处的时间.

在二维的网格上，假设方程T在网格点（i，j）处的值为T_i，j=T（x_i，j），F_i，j表示网格点（i，j）位置处的速度.在x和y方向上可以定义前向差分算子：

其中，Δx和Δy表示在x和y方向上的网格间距.后向差分算子可以定义为：

快速行进法使用迎风策略来估计T（x，y）的梯度$\nabla$T的模，故而有：

这种数值方法已经被Sethian^[19]证明收敛于正确的连续解.

对于机器人路径规划问题，波从路径规划起点出发，可以根据各区域通行能力在二维网格地图上对F进行赋值（F≥0） .例如，为计算O₁和O₂两点之间的路径，可以从O₁点开始扩展一个波直到O₂，其中自由空间区域速度恒定为1，障碍物区域速度为0（机器人无法通过障碍物）.由此确定的时间场T（x）内仅有一个全局极小值，即起点O₁.在波扩展到终点时，从终点沿梯度下降最大的方向回溯到起点，即可以得到唯一的时间最短的路径，且因波传播的速度为固定值，故而也是路程最短的路径.

由快速行进法得到的路径虽然是最短路径，但仍存在一些问题.一方面，由于只考虑波的扩展，导致路径过于贴近障碍物；另一方面，在避开障碍物时，路径的转角曲率过大，机器人无法沿这条路径行驶.Garrido ^[20]等人提出将原始的二值地图中的所有障碍物作为波源进行扩散，得到速度图（又称为快速行进网格地图，下文统称为速度图），如图1（a）所示，此时自由区域中的网格点离障碍物越远，其T值越大.在此基础上，再通过快速行进法计算路径，但此时的速度不再是恒定速度，而是基于上述速度图确定的速度，即F（x，y）=T（x，y），其中T（x，y）为第一次快速行进法确定的速度图上（x，y）处的值，F（x，y）为第二次执行快速行进法时（x，y）处的速度.这种方法可以使回溯的路径远离障碍物，且路径更加平滑.因为在确定路径的过程中使用了两次快速行进法，这种方法被称为快速行进平方法（FM²）.

1.2 基于改进速度图的快速行进平方法

快速行进平方法（FM²）解决了快速行进法确定的路径过于贴近障碍物的问题，但也存在一些其他局限.注意到，得到的速度图中离障碍物越远的网格点速度越大，因此，在基于梯度下降的路径规划中，起点和目标点间的路径往往会沿着这些速度最大的网格点.尽管这可以使路径能够尽量远离障碍物，但也导致了路径冗余.在实际应用中，我们常希望机器人可以在确保障碍物安全的前提下，尽量缩短行驶路径，以达到降低能耗、高效到达目标点的目的.为此，Garrido等人^[18]将离最近障碍物距离超过机器人半径的网格点不作处理并按恒定速度（最大速度）赋值，而在离障碍物距离小于机器人半径的网格点上仍按前述步骤得到速度图.这在一定程度上提升了路径性能，但仍存在一些问题，比如在实际中一般希望即使离障碍物的距离超过机器人的半径时机器人也能适当降低速度，因为在轨迹跟踪过程中机器人不可能完全沿着期望路径移动，降低速度可以有效降低发生危险的可能.

为此，本文提出了进一步改进速度图的策略：对离障碍物距离小于机器人半径的网格点赋予很小的速度（接近于0），而在超过机器人半径时使速度逐渐递增并在超过一定距离后保持最大速度.根据以上策略，我们用以下分段函数将速度图中的速度映射到机器人速度上：

将机器人的速度V作为波传播的速度F.其中d为自由区域中网格点离其最近障碍物的距离，r为机器人半径，V_max为机器人最大速度.其函数图像如图2所示.经过该分段函数作用后的速度图如图1（b）所示.为展示改进速度图对规划路径的影响，分别在两张速度图上，以相同的起点和终点应用本算法，其结果如图3所示.

可以看到，基于改进的速度图生成的路径更适应实际需求，既保证了与障碍物的安全距离，也减少了原始路径中的冗余.定量地，基于改进的速度图生成的路径相比于原路径在长度上下降了28.5%.

1.3 多移动机器人聚集点选取及路径规划

在不同任务需求下，多机器人聚集点选取的目标和约束不尽相同.本文提出的多移动机器人聚集

点选取和路径规划方法，可以适用于不同任务需求.下面，针对三种常见的任务需求，我们分别确定聚集点并规划各机器人从起点至聚集点的路径.

（1）以总能耗最小为目标的聚集点选取

在许多任务中，常要求多机器人能够最高效地聚集于同一地点，以实现后续更高水平的协作或集体行动.“最高效”一般指多机器人系统的总能量消耗最小，而机器人系统的能耗和很多因素有关，包括但不限于负载、移动、感知等.随着时间的推移，机器人能量消耗会变大，常可以用线性关系来描述运动时间和能量消耗间的关系：

其中，E代表总耗能，k表示机器人的单位时间耗能，τ表示机器人运动时间.

基于上述改进的速度图的快速行进平方法（FM²），在获得机器人路径的同时，还可以得到机器人从起点出发到达地图上任意位置的时间.此外，分段函数（5）将波扩展的速度映射到机器人的速度上，而这些速度同时可以作为移动机器人在此处的参考速度纳入后续的控制.由此，可以得到第i个机器人从起点s_i出发在位置x处的时间场T_i（s_i，x）.将n个机器人的时间场叠加，可以得到多机器人系统的总时间场T_total：

其中Ω表示地图区域，T_total（x）表示n个机器人到达x=（x，y）处所消耗的时间总和.时间总和越小，意味着消耗的能量越少.因此，如果以能耗最小为目标，则聚集点（x_end，y_end）应定在总时间场的最小值处，即：

以四个移动机器人为例，通过时间场叠加的方式确定了总时间场（图4），从中可以进一步确定总时间最小的点，其即为满足能耗最小的聚集点.随后，在四个机器人各自的时间场内从聚集点开始，沿着梯度下降最快的方向回溯到其起始位置，从而确定每个机器人从起点到聚集点的路径（图4）.

（2）以空间最大为目标的聚集点选取

在某些特定的任务场景中，常要求机器人能聚集于最开阔的地点，如仓储机器人需要在仓库最开阔处聚集以协调分流货物.注意到，速度图由确切的函数V（x）给出，即公式（5），其表明地图中某点的速度与其距离障碍物的距离d正相关，且是关于位置的函数.因此，如果以空间最大为目标，则聚集点（x_end，y_end）应定在地图中速度最大处，即：

仍以四个移动机器人为例，仿真结果如图5所示，可以看到优化得到的聚集点位于整个地图中空间最大的位置.

（3）队形约束下以能耗最小为目标的聚集点选取在以上两种情况中，在确定多机器人聚集点位置时，机器人被当作为一个质点考虑.在实际中，当聚集点接近障碍物时，机器人的尺寸不可忽略，不能将机器人认为是一个质点.此时不仅需要考虑所有机器人到达聚集点所消耗的时间总和，即关注多机器人系统的总时间场T_total（x），同时也要考虑多机器人聚集后编队是否会与障碍物发生碰撞，即还要关注V（x）.

作为一个例子，假设n个机器人到达聚集点后排列在正n边形的顶点，正n多边形的中心是我们选取的位置（x_end，y_end）.为避免机器人与障碍物发生碰撞，需要保证（x_end，y_end）与障碍物的距离大于L：

将式（10）代入式（5），可以得到满足距离要求所对应的速度阈值V_L：

关于（x_end，y_end）的选取，必须要满足：

在满足（11）式的基础上，希望机器人运动时间总和尽可能小，即在足够容纳的基础上以最小的能耗聚集在一起.即正n边形中心点（x_end，y_end）应满足：

同样以四个机器人为例，机器人的尺寸为45×35（cm），地图尺寸为1051×706（cm）.得到的仿真结果如图6所示.从图6和图4的对比中我们也可以看出，能耗最小的目标点并不满足容纳排成正方形的机器人队列.实际情况应选择能耗稍大，但空间更大的点作为多移动机器人的聚集点.

2 移动机器人轨迹跟踪控制

在第1节中，利用分段函数（5）将速度图映射到实际机器人移动时的参考速度，在保证路径不与障碍物冲突的前提下减少冗余.同时路径在障碍物附近时，其参考速度会下降，有利于移动机器人更加安全的通过.另一方面，轮式移动机器人在轨迹跟踪控制的过程中，当车速较高尤其是航向角发生变化的时候，可能会出现超调现象，这种现象是由于高速转向引起的惯性力增加导致的.通过减速可以降低车辆惯性力的影响，有助于减小转向角速度，提供更多的时间和空间进行控制调整，从而改善轨迹跟踪的精度和稳定性.基于本文方法的特点，在车辆航向改变时，均是遇到需要避开障碍物的情况.以图3中改进后的路径为例，在航向需要改变时，移动机器人的参考速度会下降，其路径点上的参考速度变化如图7所示.从图中可以看出，当航向角发生变化之时，速度也随之下降，因此改进的速度图提供的参考速度更有利于机器人跟踪轨迹.为了验证这一想法，本文在仿真中用基于车辆动力学的模型预测控制跟踪轨迹.

2.1 动力学建模

模型预测控制（Model Predictive Control）依赖于模型的准确性，本文采用阿克曼小车作为具体的移动机器人对象.图8是阿克曼车型常用的自行车模型，下面对该模型进行具体的推导.

首先，为使用这个模型，作出以下的假设^[21]：

（1）假设车辆在绝对平坦的平面上移动.

（2）用单轨模型来描述车辆运动，忽略载荷左右转移.

（3）忽略空气动力学.

（4）车身质量均匀分布在轮子上.

根据牛顿第二定律，可以构建如下的车辆动力学模型：

其中符号的含义如表1所示.前、后轮在车身坐标系y轴方向上的分力为：

轮胎受到的侧向力定义为：

轮胎侧偏角为车轮实际速度方向与车轮轴线之间的夹角，可由图8几何关系得：

同样根据几何关系可以得：

根据轮胎模型和小角度假设^[21]，即cosβ≈1，sinβ≈β.可以进一步简化模型以简化计算.轮胎侧偏角计算为：

进而得到轮胎侧向力：

同样基于小角度假设，前后轮在车身坐标系y轴方向的分力为：

综合（14）、（20）和（21），可以得到简化后的车辆非线性动力学模型：

为了实现轨迹跟踪控制，还需要构建与路径的位置和方向相关的误差状态量$\mathit{e}={\left[e_d，{\dot{e}}_d，e_{\psi }，{\dot{e}}_{\psi }\right]}^{\mathrm{T}}$，得到关于误差的状态空间方程.定义e_d为车辆横向误差，即车辆中心位置距离参考路径点的距离，如图8中所示.e_ψ为车辆相对于路径的航向误差，即为小车实际航向角ψ与期望航向角ψ_des之间的差值.

如图8所示，由Frene′t坐标系与世界坐标系间的转换关系可得横向速度误差为^[22]：

我们假设在每一个控制步长内，纵向速度v_x为常量.则横向加速误差为：

航向误差定义为：

结合式（22）~式（26），可以得到误差状态方程为：

其中A，B，C分别为：

其中G₁，G₂，G₃，G₄分别为：

由于这里考虑路径点上的参考速度，故而需要有纵向速度.状态量应当加入e_x和${\dot{e}}_x$，分别为纵向位置误差和纵向速度率.纵向加速度误差定义为：

最后选取的状态量为$\tilde{\mathit{e}}={\left[e_y，{\dot{e}}_y，e_{\psi }，{\dot{e}}_{\psi }，e_x，{\dot{e}}_x\right]}^{\mathrm{T}}$.矩阵A，B，C也相应改变为A_a，B_a，C_a：

此外，控制量应为u=[θ Δa]^T，由此误差状态方程为：$\dot{\tilde{\mathit{e}}}={\mathit{A}}_a\mathit{e}+{\mathit{B}}_a\mathit{u}+{\mathit{C}}_a{\dot{\psi }}_{\mathrm{d}\mathrm{e}\mathrm{s}}$.

2.2 模型预测控制（MPC）控制器

（1）控制器设计

基于控制的需要，首先对连续的状态方程进行离散化.离散化后的状态方程如下：

其中$\tilde{\mathit{A}}=\left(\mathit{I}+T{\mathit{A}}_a/2\right)/\left(\mathit{I}-T{\mathit{A}}_a/2\right)，\tilde{\mathit{B}}=T{\mathit{B}}_a，\tilde{\mathit{C}}=T{\mathit{C}}_a{\psi }_{\mathrm{d}\mathrm{e}\mathrm{s}}$，T为采样时间.

若在k时刻，预测时域为N_p，则预测时域内的状态量和控制量为：

结合式（36），可以得到如下的关系

其中$\tilde{\mathbf{\Theta }}$、$\tilde{\mathit{K}}$、$\tilde{\mathit{Z}}$分别为：

每次优化结果会参考未来N_p时间内系统的表现，但每次只取优化得到的控制序列中的第一项作为下次的控制输入，这个过程称为滚动优化.

（2）滚动优化

在预测时域内，控制量U未知，小车的状态量偏差E也是未知的.因此需要设计合理的优化目标函数，从而获得满足约束条件下的最优控制序列.目标函数定义如下^[23]：

其中$\tilde{\mathit{Q}}$，$\tilde{\mathit{R}}$为权重矩阵.并且，因为E已经是状态量的误差值，故而E_ref（k）（参考误差状态量）为零矩阵.结合（38）式，令$\mathit{P}=（\tilde{\mathbf{\Theta }}\tilde{\mathit{e}}（k）+\tilde{\mathit{Z}}）$可以得到如下的关系式：

因为${\mathit{P}}^{\mathrm{T}}\tilde{\mathit{Q}}\mathit{P}$不会影响J_min，故这项可以舍去.并且加上约束条件为：

其中$\mathit{H}={\tilde{\mathit{K}}}^{\mathrm{T}}\tilde{\mathit{Q}}\tilde{\mathit{K}}+\tilde{\mathit{R}}，\mathit{G}=\tilde{\mathit{K}}\tilde{\mathit{Q}}\mathit{P}$.根据优化结果给出最佳的前轮转角θ和加速度补偿△a.

（3）仿真验证

为了验证速度场的设计是否合理，分别以匀速和改进速度场提供的速度作为参考速度，用基于动力学模型的模型预测控制（MPC）控制器去跟踪度图3（b）中的路径.假设小车对的最大速度为40 cm/s，比较分析跟踪匀速（40 cm/s）和改进速度场的速度作为参考速度的仿真结果，其中使用的车辆参数如表2所示.

仿真结果如图9所示，可以看到，在航向角发生变化的时候，如果仍以最大速度进行跟踪，会发生较大的轨迹偏离现象，而将改进的速度场速度作为参考速度，跟踪效果明显得到改善.图10是匀速和变速跟踪的横向误差箱体图.匀速跟踪的情况下，横向误差最大为1.13cm，而在改进速度场速度的指导下的轨迹跟踪最大横向误差为0.93cm.由此可以得出结论本文所设计的改进速度场是有效的，可以作为轨迹跟踪的过程中机器人位于某个位置的速度参考，并且有效提升轨迹跟踪的精度.

3 结论

本文基于改进快速行进平方法，提出了一种适用于多机器人聚集的路径规划方法，实现了多机器人系统根据不同任务需求，在已知地图上寻找聚集点并规划各起点到聚集点的路径.文中具体以能耗最小、空间最大和队形约束下的能耗最小三种常见任务需求，分析设计不同的目标函数，并通过仿真实验验证本文方法的有效性.为改进传统快速行进平方法的速度场，本文引入了分段函数设计速度映射，在避免障碍物碰撞的前提下减少不必要的冗余路径，并且可以将快速行进网格地图中的速度参考值有效地映射到真实机器人移动时的参考速度上.为验证路径上的参考速度信息的可行性，本文采用基于车辆动力学的模型预测控制轨迹跟踪方法进行仿真验证，结果显示使用本文速度场的参考速度信息进行轨迹跟踪比匀速轨迹跟踪可以实现更好的效果.

参考文献