多无人机航迹规划的自适应B样条算法

彭皓月，秦小林，侯 屿，张力戈

1.中国科学院 成都计算机应用研究所，成都610041

2.中国科学院大学，北京100049

1 引言

由于无人机快速增长的市场和广阔的应用领域，吸引了越来越多的研究人员和研究机构。与单个无人机相比，无人机集群能够以更高的效率完成更复杂和更具挑战性的任务。多无人机的研究方向主要包括无人机集群的路径规划，无人机编队以及路径平滑等。

无人机路径规划是无人机集群问题最主要的研究方向。其研究内容主要集中在环境建模、无人机航迹代价的定义、目标函数最优化求解方法等方向。在环境建模方面，王文彬等[1]将无人机飞行环境用人工势场和Voronoi 图进行建模，并结合滚动时域控制策略与改进的粒子群算法应用于单个无人机的航迹规划问题中，生成无人机在复杂环境下安全飞行的路径。

由于启发式算法求解问题可行解的时间优势十分突出，因此很多文献将启发式算法应用到无人机路径规划问题的求解中。Chen 等[2]采用改进的中心力优化方法来解决旋翼垂直起降飞机的复杂路径优化问题。在路径规划过程中，应用粒子群优化算法（PSO）和遗传算法（GA）变异算子的思想来改进原始中心力优化方法。该方法产生的轨迹优于大部分传统的启发式算法。周子为等通过分析雁群长途迁徙编队队形的内在机理，设计了多无人机编队飞行的队形和变换方法[3]。

无人机编队控制的主要方法有三种，即领航-跟随法、虚拟结构法和基于行为法[4-6]。在领航-跟随法中，编队中的一架无人机被指定为领队，其余的无人机被视为追随者。其基本思想是跟随者跟踪领队的位置和方向。Wang[7]开发了从理论到实际飞行的编队飞行试验系统，使用主动干扰抑制控制（ADRC）方法和PID控制方法设计了三维无人机编队方法，通过数值模拟设计并验证了基于领航跟随法的三维编队控制算法。领航-跟随法存在的弱点是，领航者是编队的单一故障点，如果领航者出现故障，整个无人机群会出现编队异常的现象。在虚拟结构法中，整个编队被视为一个单一的结构。每个无人机接收相同的虚拟领队的轨迹信息。虚拟结构法的优势在于易于规定编队行为。邵壮等提出了一种基于虚拟结构编队策略的、可以在三维动态环境下实时避障以及机间避碰的分布式编队保持算法[8]。虚拟结构法的缺点是对队形没有明确的反馈信息。基于行为法在飞机编队飞行中的控制策略受鸟类、鱼类、昆虫的本能行为的启发，为每架飞机规定几种行为，并使每架飞机的控制动作成为每种行为控制的加权平均值。这些行为可能是障碍回避、碰撞回避、目标寻找和保持编队等。比如无人机避障是通过移动到目标、保持相对距离、保持相对角度和停止移动到目标目的地等行为组合完成。Dougherty[9]指出，基于行为法所需的信息量比较大，一般不能保证队形的稳定性。

路径平滑是得到无人机集群实际可飞路径的有效方法，能提高无人机飞行的安全性，也能使无人机飞行更加高效。很多学者在各个领域对路径平滑技术进行了深入的研究。Huang[10]将K 度平滑方法与改进的蚁群算法结合，使多无人机在可接受的时间误差内到达目的地，生成的光滑的无人机路径满足实际飞行需求。Askari[11]对机器人和无人机的连续曲率路径进行了研究，将上限曲率和上限曲率导数包含在路径中，使用五次Bezier曲线替换Dubins曲线，通过考虑路径的运动学约束来计算形状参数。Fang[12]提出了一种基于二阶平滑的巡航导弹路径跟踪方法，在对称多项式曲线方面产生具有连续曲率的可飞路径，满足了导弹转弯半径和精度跟踪的要求。Heiden[13]提出了一种梯度相关平滑算法，通过局部优化顶点的位置，同时满足系统的动力学约束，使给定轨迹变形。比现有的后平滑算法产生的轨迹更短并且拥有更高的平滑度。Wang[14]提出了一种轨迹平滑的实时动态Dubins-Helix方法，该方法由Dubins-Helix 轨迹生成和俯仰角平滑两部分组成，Dubins 路径规划方法生成三维轨迹在水平面上的投影，使得轨迹满足曲率半径约束。螺旋曲线是其满足俯仰角约束。该方法能够在不同的初始条件下生成有效的轨迹，能够实现实时计算。Ravankar[15]较为全面地总结了机器人导航中的各种路径平滑技术，将其分为了基于插值的路径平滑、使用特殊曲线的路径平滑、基于优化的路径平滑，并且详细地比较了每种方法的优点和缺点以及适用场景。

本文将采用领航-跟随者方法来进行无人机编队控制。在实现过程中，采用滚动时域控制和快速粒子群优化算法为领航者生成平滑且安全的参考航迹；然后根据跟随者与领航者之间的几何关系，得到跟随者的参考航迹，采用自适应的B 样条方法对跟随者的参考航迹进行平滑和避障处理。由于B 样条方法应用于航迹平滑时得到的曲线满足C2连续性，所以采用自适应B样条方法进行航迹平滑。

2 相关工作

2.1 滚动时域控制

滚动时域控制（Receding Horizon Control，RHC）[16]将控制问题描述为有限时域内的含约束的最优化问题，它具有能够在线处理有约束地控制输入与输出的优势。在一个采样时间内，通过在确定的时间区间内搜索来优化目标函数，最终得出最优的控制输入序列，之后RHC会保留上述序列的第一个值；然后，系统状态在得出控制输入后发生变化；接下来系统会根据当前及未来的状态估计值重复上述过程。图1是RHC的基本框架。

2.2 B样条曲线

B 样条曲线[15]是贝赛尔曲线的一般化形式，其公式化的定义如下：

图1 RHC的基本框架

其中，m 是节点xi的个数，且xi的取值范围是{x0,x1,… ,xm-1}并 且x0≤x1≤ …≤xm-1。Pi是 控 制 节点，此处共有m-n-1个控制节点。Bi,n表示n阶的B 样条基函数，可以根据De Boor-Cox的递推定义得到：

B 样条曲线可以分为：（1）均匀B 样条曲线，其特点是基函数呈周期性；（2）准均匀B 样条曲线，其特点是两端的控制节点具有重复度k；（3）分段Bezier 曲线，其特点是两端的控制节点有重复度k，内部的控制节点有重复度k-1；（4）非均匀B 样条曲线，它的特点是基函数定义域的区间长度不相同。

其中，准均匀B 样条曲线和分段Bezier 曲线会与控制折线的首尾控制点接触并与首、尾边接触、相切，如图2（a）中实线所示，这样产生的曲线称为闭曲线；而另外的两类曲线则不会与首尾控制点接触，如图2（b）中实线所示，这样产生的曲线称为开曲线。

图2 B样条曲线实例

B 样条曲线主要的优点有：（1）计算曲线的成本低；（2）易于保证路径的C2连续性；（3）通过改变节点可以控制样条的形状。类似的航迹平滑方法还有Dubins 曲线和贝塞尔曲线，但是前者不具有曲率连续性，而后者所选取的任何一个控制点发生改变都会影响全局的结果。相比于这两种方法，B 样条曲线容易实现路径的C2连续性，并且在改变部分控制点时只会对局部曲线产生影响。因此，相比于这两者，B 样条曲线在航迹平滑这一任务上具有明显的优势。

3 基于自适应B样条的动态航迹规划

当多无人机编队在执行任务时，需要在飞行过程中保持预先设定的队形。为了实现这个目标，常用的编队保持控制方法有：领航-跟随法、虚拟结构法、基于行为法、图论法、基于一致性方法以及多种方法的混合。由于领航-跟随法在实际中的应用很多，同时该方法具有理论上容易分析、实现较为容易等优点，所以本文采用了领航-跟随法来进行编队控制，图3 是领航-跟随法的示例。

图3 领航-跟随法示例

领航-跟随法的基本思想是将无人机集群系统中的无人机分为领航者和跟随者，其中领航者跟踪一条预先给定的轨迹，跟随者与领航者之间保持指定的相对距离并跟随领航者运动。因此，要实现领航-跟随法，就要解决以下两个问题：（1）为领航者指定参考航迹；（2）基于领航者给出跟随者的参考航迹。

3.1 结合滚动时域的无人机编队路径规划

针对第（1）个问题，本文使用了文献[1]中王文彬等提出的基于滚动时域控制和快速粒子群优化（Receding Horizon Control and Fast Particle Swarm Optimization，RHC-FPSO）算法。该算法将复杂情况下单无人机的航迹规划问题分为代价评估阶段、在线航迹规划阶段，并分别给出了解决办法，如图4 所示，第一阶段，根据当前环境生成基于Voronoi 图的代价图，同时结合人工势场计算航迹端点到目标点的代价估计。第二阶段，采用快速粒子群优化算法在滚动时域的框架下进行在线航迹规划。

图4 RHC-FPSO方法结构示意图

RHC-FPSO 方法具有如下优点：（1）在满足障碍物约束和无人机自身约束的条件下，该方法能使无人机顺利地穿过障碍物区域，到达目标区域，由于为无人机设定了速度和加速度等动力学约束，所以规划出的航迹可行且平滑；（2）该方法在耗时方面具有优势，它满足在线计算的要求；（3）对新检测到的障碍物，该方法可以做出快速反应，以达到躲避障碍物的目的；（4）当环境发生变化时，可不计算代价图，只需更新势场，就减少了计算量。因此，在分布有动静态障碍物的环境中，采用RHCFPSO方法可以为领航者无人机生成安全的参考航迹。

针对第（2）个问题，首先要考虑无人机编队的队形问题。对于不同数量的无人机有不同的队形，本文考虑三架无人机构成编队的情况，并将期望队形选择为常见的等边三角形。编队中的各个无人机的期望位置分别是等边三角形的三个顶点，并且领航者位于队伍的最前方。该队形的几何表示图如图5。

图5 多无人机三角形编队

在图5中，ld表示两无人机之间的期望距离，(xi,yi)是无人机i在二维坐标系下的位置，vi是无人机i的速度，θi是无人机i的偏航角。1表示领航者，跟随者2、3在队形中的期望位置分别是2ʹ、3ʹ，并且可以分别通过公式（4）、（5）来计算：

于是，根据领航者1 的参考航迹就可以得到跟随者2 和跟随者3 的参考航迹。但是通过上述步骤生成的航迹，会存在两个不足：（1）在生成跟随者航迹的过程中并未考虑到环境中的动、静态障碍物，所以生成的航迹可能和障碍物相交；（2）得到的参考航迹是由一组航迹点组成的，可能会造成航迹不平滑。针对上述不足，本文提出了基于自适应参数B 样条的分段样条插值方法来进行避障处理和航迹平滑。

3.2 路径的动态重规划与平滑

针对3.1 节提出的两点不足，本文从避障的角度出发，使用B 样条样条插值方法来实现跟随者航迹的避障平滑处理。

首先，对环境中的所有动静态障碍物进行膨胀化处理，即把实际障碍物的边界往外扩展一定距离，得到膨胀后的障碍物范围，由此得出新的障碍物威胁图。这里扩展的距离为无人机的最小安全半径r，如图6所示。

图6 障碍物膨胀化处理

然后，结合3.1节中得到的跟随者航迹点，进行障碍物的规避并生成平滑的航迹。由于环境中可能会出现动态障碍物，所以为了保证无人机能实时地检测出动态障碍物并及时做出避障的措施，本文采用分段处理的方式。当无人机处于当前位置时，只考虑接下来的少量航迹点，根据航迹点是否在障碍物外部，做出平滑或航迹重规划处理。由于无人机从当前阶段的航迹点移动至下一阶段的航迹点时，都会进行动静态障碍物的检测与避障处理，而每一阶段只包含少量航迹点，确保无人机在每个阶段的运行时间很短，因此分段处理的方式能够实现无人机的动态避障。

分段处理的具体实现思路如下：对于每个跟随者的参考航迹，先按照顺序选取N 个航迹点，判断这些航迹点是否与环境中的障碍物发生碰撞，如果没发生碰撞，就用B 样条样条插值方法进行平滑处理，然后再继续往后选取N 个航迹点，并进行判断；如果会发生碰撞，则进行航迹的重规划过程。

航迹重规划方法的思想是：首先统计这N 个航迹点与障碍物相交的个数N0，如果N0＜N，则选取离相交区域最近的障碍物顶点的外扩点作为新的航迹点，同时把第一个交点前的第二、三个点与最后一个交点后的第二、三个点作为新的航迹点，使用B 样条方法来进行平滑处理；如果N0≥N，则再往后选取N 个航迹点，直到与障碍物没有相交为止，针对这种情况，选取离相交区域最近的障碍物顶点的外扩点作为新的航迹点，同时把第一个交点前的第二、三个点与最后一个交点后的第二、三个点作为新的航迹点，使用B 样条方法进行平滑处理，如图7 所示。在选取外扩点时，还需考虑相交部分的长度，如果超过了矩形障碍物的某一边长，则适当增加控制点的数量，如图8所示。

图7 障碍物的顶点外扩点作为新的控制点

图8 新增控制点

在上述处理过程中，会存在一种情况：如果连续两个分段都与障碍物没有交点，那么在分别进行平滑处理之后，会在两段的连接处出现不光滑的情况，如图9 中的点3 处。为了避免这种情况，文中提出了一种自适应B样条方法，如图10所示。

图9 分段平滑之后出现航迹不光滑的情况

图10 自适应B样条方法以避免航迹不平滑

下面将对自适应选点的思想进行说明。假设图10中的点1、2、3、4、5、3′、3″、5′、5″的坐标分别是P1、P2、P3、P4、P5、P3′、P3″、P5′、P5″，其中点1、2、3、4、5 是跟随者航迹上连续的五个控制点，如果每三个控制点进行一次判断，那么这里首先要考虑的是点1、2、3，接下来要考虑的是点3、4、5。假设这两段航迹跟障碍物是没有发生碰撞的，则这两段将直接进行B 样条的平滑处理，但是处理之后会出现如图9 中的情况，即平滑后的航迹在点3处不连续。所以本文在考虑点1、2、3 时，将点3 用点3′来替换，即对点1、2、3′构成的航迹进行平滑并得到图10中左起的第一段曲线。这里点3′的选取方式为：

其中，k 是大于0 的常数。接下来处理点3、4、5，为了避免连接处出现不平滑的情况，用点3″来替换点3，用点5′来替换点5，于是对点3″、3、4、5′进行B 样条平滑处理并得到图10 中间的曲线。其中，点3″和点5′的选取方式分别是：

l 是大于0的常数，且l ＞k。由于B样条方法具有C2连续性，所以在点3′、3″处与线段3′3″是相切的，在点5′、5″处也与线段5′5″是相切的。所以，经过上面的处理就得到了平滑的航迹。

3.3 算法流程

步骤1 根据RHC-FPSO 算法规划出领航者无人机的参考航迹。

步骤2 根据公式（4）和公式（5）计算出跟随者无人机的参考航迹。

步骤3 对环境中的障碍物进行膨胀化处理，获取新的障碍物分布图。

步骤4 分别对两个跟随者无人机的参考航迹点进行分段避障和平滑处理。若某一段与障碍物没有交点，则转步骤5；若某一段与障碍物有交点，则转步骤6。直至遍历所有航迹点。

步骤5 使用基于自适应B样条方法进行平滑。

步骤6 根据交点的具体情况重新选择航迹点，使用自适应B样条方法进行平滑、避障处理。

4 实验设计与结果分析

文中的实验针对的是三架无人机组成的编队，队形是等边三角形，编队保持策略是领航跟随法。这三架无人机要完成的任务是从指定区域出发，穿过分布了多个障碍物的区域，到达指定的区域。

4.1 领航者航迹生成

在采用领航跟随策略的无人机编队中，首先要为领航者指定参考航迹，本文使用了RHC-FPSO 方法生成了领航者无人机的参考航迹，其结果如图11 和图12所示。

图11 RHC-FPSO生成领航者航迹

图12 领航者参考航迹

图11 、图12 展示了领航者无人机在分布了多个障碍物的环境下的航迹，可以看出该航迹与所有障碍物都未发生碰撞，并能从起点到达指定的终点。

4.2 跟随者航迹生成

在生成领航者无人机的参考航迹之后，需要进一步考虑跟随着无人机的参考航迹，本文根据公式（4）、（5）的几何关系生成两跟随者的航迹，结果如图13所示。

图13 跟随者的参考航迹

从图13 可以看出这两条航迹存在很多不平滑的区域，并且图中的参考航迹与障碍物区域相交。与此同时，考虑到在生成跟随者参考航迹时并未考虑航迹的光滑性与安全性，所以必须要进一步考虑对航迹的平滑及避障处理。

4.3 跟随者航迹平滑及避障

在对跟随者的参考航迹进行平滑及避障处理之前，首先对可见范围内的障碍物进行膨胀化处理以得到绝对安全的区域。如果只考虑航迹的分段平滑问题，则可以通过自适应选点的B 样条平滑策略来处理，其结果如图14所示。

图14 跟随者的平滑航迹

从图14 中可以看出，此时的航迹与膨胀化之后的障碍物仍然相交，下面同时考虑参考航迹的平滑和避障问题。在平滑航迹的过程中，对碰撞的部分进行航迹的重规划过程，其结果如图15所示。

图15 展示的是平滑且避障之后的航迹，图中的两个小窗口显示了避障的重规划情况。在小窗口里，点状部分是参考航迹与膨胀后的障碍物发生碰撞的部位，在重规划过程之后得到了围绕在障碍物外围的光滑曲线。至此，就得到了两跟随者无人机的光滑且安全的航迹。

图15 航迹重规划示意图

4.4 平滑效果对比

预先生成的无人机航迹越平滑，那么无人机在实际飞行过程中的波动就会相对较少，所以参考航迹的平滑性是评价航迹优劣的一个重要标准。文中与圆弧插补技术进行了比较。圆弧插补技术[17]的思想是用圆弧GH去替代转折角ABC，以此来实现路径的平滑，其原理如图16所示。

图16 圆弧插补算法原理

在相同的环境设置下，用圆弧插补算法进行航迹平滑的结果如图17 所示。而使用本文提出的自适应B 样条方法的结果则如图18所示。

图17 圆弧插补算法生成跟随者航迹

从以上实验结果可以看出，图17 中标注部分的航迹存在不光滑的转折点，而总体而言用圆弧插补得到的结果存在不光滑处；图18 中标出了对应于图17 标注过的区域，相比较而言，图18 中展示的航迹更光滑，而总体而言用自适应B 样条方法来进行平滑得到的航迹不存在不光滑处。综上可以看出，相比于圆弧插补技术，自适应B样条方法能使航迹更光滑。

图18 自适应B样条生成跟随者航迹

5 结束语

本文对无人机编队控制问题进行了研究，采用了领航-跟随法的策略进行了编队控制。在生成领航者无人机的航迹时，使用了RHC-FPSO 算法；然后基于三架无人机组成的编队，根据三者的几何关系，结合领航者无人机的参考航迹，生成了两跟随者无人机的参考航迹。在进行航迹平滑时，针对传统B 样条曲线应用于分段的航迹点时会造成的不平滑现象，提出了自适应的B 样条曲线方法，并在此基础上考虑到航迹会与障碍物发生碰撞，提出了基于上述方法的航迹重规划算法。通过使用自适应的B 样条曲线方法来进行航迹平滑，其结果相较于使用圆弧插补算法得到的结果更好；使用航迹重规划算法可以使无人机安全避开环境中的障碍物。随着无人机编队的应用越来越多，将面对的环境会更多元化，因此在未来的工作中要更多的考虑到各种不同的动静态障碍物；同时，用于快速寻找光滑且安全的航迹规划算法仍然是研究的重点。