B样条理论在无人机航迹平滑中的应用

段彩萍 刘超 王琪

摘要：航迹规划是指根据飞行要求，设计出一条从起点到终点的最优航迹，航迹平滑是航迹规划的最后阶段，好的平滑效果可以保证所规划的航迹具有实际可飞性，因此，对航迹平滑算法的研究对整个无人机的航迹规划都具有很大的意义。文章针对无人机航迹规划问题所需满足的约束条件，建立相应的机动性能模型和威胁约束模型，平滑阶段主要包括雷达、导弹、高炮等威胁模型。首先将规划空间栅格化，其次通过标准蚁群算法获得无人机的初始航迹，状态转移策略仅根据信息素及启发因子按照蚁群算法概率公式进行选择，最后根据航迹平滑中参数分布不均匀的特点，选择非均匀三次B样条处理初始航迹。整个算法在Matlab平台上仿真实现，经过计算仿真，实验证明非均匀三次B样条曲线呈现出较好的平滑效果。

关键词：无人机：航迹平滑；B样条；蚁群算法

航迹规划一般有3个阶段，第一阶段是根据无人机机型和载荷的不同分配不同的任务；第二阶段是在不考虑各种约束条件的情况下，进行航迹规划，得到总代价最小的航迹；第三阶段需要将一些因素考虑进去，比如最小飞行高度、最大转弯半径等，使第二阶段的航迹具有实际可飞性，第三阶段中最重要的一步即航迹平滑。航迹平滑是使航迹满足可飞要求，并且与威胁保持着安全距离。国内外文献对航迹规划的研究有很多，着重对航迹平滑的研究文献相对而言不是特别丰富。

对无人机航迹平滑的研究比较多的是kalman滤波，最小二乘曲线拟合法，圆弧串联法，平滑算子法等。经典的kalman滤波法需要假定目标运动状态模型，对目标机动的适应性较差。最小二乘法能减少测量以及模型的先验信息，同时降低初始值的敏感度，如果强机动目标阶数比较低，此时用最小二乘法得到的拟合精度和效果都不是特别理想。文献[1]阐述了一种基于模糊推理机制的平滑算法来调整航点的位置：文献[2]阐述了一种基于时间开关的平滑算法，该方法仅对各航迹的转折点做了平滑处理，适用于局部平滑。文献[3]使用了RTS平滑算法，其平滑步骤有点滞后，拟合的实时性比较难以保证。

通过文献分析，B样条曲线法是目前比较实用的航迹平滑算法。B样条理论应用范围比较广泛，例如用B样条函数处理力学问题；在图像领域，B样条结合小波方法可以完成完整性的认证和压缩实验数据；在遥测领域，应用不等距B样条提高遥测数据精度。在空间轨迹优化上，尤其在机器人轨迹规划[4]中，B样条的优越性得到充分的发挥，其既能进行局部优化又可以进行全局调整。本文应用非均匀B样条曲线优化初始航迹，可以使得无人机飞行航迹平滑连续且满足飞行的约束条件。

1 算法理论

1.1初始航迹算法

无人机的初始航迹获取，采用了标准的蚁群算法。蚁群算法（Ant Colony Optimization，ACO）即蚂蚁算法，是一种用来寻找最优路径的概率型算法。

蚂蚁在觅食的时候会在路上留下一种叫信息素的化学物质，并且蚂蚁们能够感知信息素的强度，根据强度的大小指引自己的前进方向，它们总是朝着强度高的地方前进。因此，整个蚁群的觅食过程就表现出对信息素强度的一种正反馈现象。

由此可知，蚂蚁之间利用信息素通信，根据信息素的浓度选择路径，蚂蚁具有记忆行为，一只蚂蚁搜索过的路径在下次搜索时候，将不再被选择，并且以此建立禁忌表，一只蚂蚁很难到达食物源，但是整个蚁群的搜索就会容易到达目的地。

蚂蚁从起点（xo，Yo）出发，到达下个路口的时，蚂蚁根据一定的概率会选择下一个坐标点，其选择概率公式是：

τij是边（i，j）上的信息素；α是信息素重要程度的参数；nij是从坐标点i到坐标点/的启发式因子；声是启发因子的重要程度的参数。

蚁群算法本身具有很多特点，以下4点是最主要的部分：（1）并行算法，每只蚂蚁的搜索过程是独立的，蚂蚁之间仅通过信息素传递信息，所以蚁群算法可以看作一个分布式的多智能体系系统。（2）自组织性，即蚂蚁觅食是一个从无序到有序的过程。（3）较强的鲁棒性，蚁群算法的求解结果不依赖于初始路线的选择，在搜索过程中不需要进行人工干预，而且参数少，设置简单。（4）正反馈算法，正反馈特性是蚁群算法最重要的一个特征。蚁群算法的特点让初始航迹的获取变得简单[5]。

1.2航迹平滑-B样条

1946年，由Schoenberg最早提出B样条理论：1972年，Deboor和Cox分别独立地给出关于B样条计算的标准算法。B样条有多种等价定义，但Deboor-Cox的递推定义得出的递推性质使得计算非常简便稳定，并且已经得到了普遍的认同。Deboor-Cox递推公式是B样条理论最重要的进展之一。以下是递推公式：

Ni，k（u）的第一个下标i表示序号，第二下标k表示B样条次数。从递推公式可得，欲确定第i个k次B样条Ni，k（u），需要用到ui，ui+1，…，ui+k+1共k+2个节点。方程中n+l个控制顶点di（i=0，l，…，n）需要用到n+l个k次B样条基函数Ni，k（u）（i=0，1，…，n）。考察B样条曲线定义在区间u∈[uiui+1]上的曲线，忽略基函数取零项，B样条曲线可表示为：

由式（3）可知，B樣条曲线的定义仍然采用控制顶点来定义，且与其他顶点无关，这样不仅克服了贝塞尔曲线不具有局部性质的特点，又保留了它的优良控制的特性。

B样条根据不同的分类标准可以分为多种类型。根据节点的分布情况可划分成4种类型，分别是：均匀B样条曲线、准均匀B样条曲线、一般非均匀B样条曲线、分段贝齐尔曲线。均匀、准均匀和非均匀3种类型早就存在，STEP标准把分段贝齐尔曲线作为一种特殊的类型[1]。还可按照首末端点是否分离和相重划分为开曲线和闭曲线。

B样条曲线具有诸多性质，正因为有这些性质，才使得B样条曲线得以广泛的应用[6]。

（1）递推性，递推性可由递推公式表明。

（2）规范性，∑，Ni，k（u）=1。

（3）局部支撑性质：其他从公式还可以看出它包含了非负性。

（4）比贝塞尔曲线具有更强的凸包性；B样条曲线的凸包是多个曲线段凸包的并集，它的凸包区域小于或者等于同一组控制顶点定义的贝塞尔曲线的凸包区域，B样条曲线恒位于其凸包之内。凸包性导致顺序k+1个顶点重合时，由这些顶点定义的k次B样条曲线退化到这一个重合点：顺序k+1个顶点共线时，由这些顶点定义的k次B样条曲线退化为一直线段[1]。

（5）几何不变性与仿射不变性。由于实际得到的航迹节点通常是非等间距的，因此采用非均匀的B样条曲线进行计算。非均匀B样条基的节点矢量定义是任意分布的节点矢量U=[uo，u1…，un+k+1]，只要在数学上成立（其中节点序列非递减，两端节点重复度≤k+1，内节点重复度≤k）都可取。

考虑到计算的复杂度，一般选择二次或三次B样条曲线，只要曲线曲率大于等于无人机最小转弯半径即可。曲线曲率的计算公式如下：

曲线曲率半径，？，只要通过B样条算法平滑后的航迹的曲率满足r≥rmin（无人机的最小转弯半径），就认为满足无人机的飞行约束条件。

2 算法模型

2.1获取初始航迹

首先要确定无人机的飞行空间，其次是需要将飞行空间栅格化，即离散处理。因此此研究的对象是在二维空间进行的航迹规划，可用』轴表示水平方向，y轴表示垂直方向，在』轴和y轴方向都进行25等分，交点（xi，yi）即为无人机的航迹坐标点，本次飞行考虑的威胁因素有雷达威胁，高炮威胁，大气威胁以及导弹威胁。本次仿真假设无人机起点坐标为（x0，y0），飞行的目的地坐标是（Xn，yn）。应用蚁群算法规划处一条从起飞点到终点的路线，对初始航迹要求是避开所有的危险因素，同时要尽可能地缩短起点到终点的距离。算法步骤如下[7-8]。

（1）初始化。设定飞行环境，对飞行区域离散化处理。

（2）求出蚁群算法的相关参数。主要是信息素和启发式因子。启发式因子考虑到雷达、导弹、高炮和大气的威胁之外，还考虑了油耗的威胁，权重比为： W=[0.4 0.2 0.2 0.1 0.1]。 第i个节点的启发因子是所有代价加权和的倒数。信息素增加强度系数Q=10，信息素挥发程度p=0.6。每一代的每一只蚂蚁的爬行路线的长度存储在细胞结构中。

（3）迭代循环。迭代开始的初始状态假定无人机已经在起点处，那么禁忌表应当剔除起点坐标。蚂蚁能看到的范围可以简化成一个方格，速度半径是其中的一个重要参数，假设速度半径为3，那么整个观察的范围即为3X3个方格，蚂蚁的移动距离也在这个范围之内。但是起点位置能选择的下个坐标点只有3个。采用转轮赌法选择下一步到底选择哪个坐标点。循环的停止条件是蚂蚁没有遇到食物或者走进了死循环。

（4）状态更新和路线记录。

（5）信息素更新。信息素更新主要是信息素浓度的增强和挥发。迭代次数N每累加一次，所有路径上的信息素就挥发一次，挥发系数设为p，公式如下： τij（N+1）=ρτij（N）+（1-ρ）△τij，ρ∈（0，1） （5）

此外，算法采用择优法，即总是保留上一代最佳的航迹作为这一代蚁群算法航迹中的一条航迹，这样有利于收敛性。

2.2平滑处理

B样条曲线定义的三要素分别是：控制顶点、B样条次数K，以及确定节点矢量，即己知这三点，就确定了一条k次B样条曲线。而非均匀B样条曲线的定义与其他B样条曲线有所不同，除了要己知控制节点di（i=0，1，…，n），还需确定它的节点矢量U= [u0，u1，…，Un+k+l]中具体的节点值。算法的实现框如图1所示。

对于插值曲线，相比均匀参数化，弦长参数化方法更加合理些。弦长参数化使得顺序的两个数据的参数值和它的距离成正比例关系。同时还需要参数规范化，规范到[0，1]区间，其目的是方便计算规范后的结果对插值没有影响。求控制节点采用逆推法。 确定节点矢量有两种方法，分别是里森菲尔德方法和哈德利一贾德法。由于里森菲尔德法需要考虑样条次数的奇偶性，而哈德利一贾德法采用统一的计算公式，构成k条边的k+1个顶点相应的B样条曲线段，且不考虑其他顶点对该曲线段没有影响，所以后者更加合理[9-10]。

定义域节点区间长度按下式计算：于是可得节点值：

当得出控制节点d，和节点矢量时，即可得出3次B样条曲线。

3 仿真结果

3.1初始航迹

本文在4G内存的PC机上，使用Matlab7.o进行蚁群算法获得无人机初始航迹的仿真。无人机的起点坐标是（10，20），终点航迹坐标是（55，34），航迹坐标均代表距离单位，在仿真中都无量纲。Matlab软件运行蚁群算法程序得出的初始航迹如图2所示。

图2中雷达的坐标点为（26，55）、（52，45）、（35，26）、（51，31），导弹的坐标点为（17，22）、（24，35），（30，62），（40，38），高炮和大气的威胁分别有两个和3个。从运行的结果可以看出，初始航迹有几处并不光滑，实际飞行中，尖角处的曲线曲率小于最小转弯半径，因此无人机不能进行尖角处的转弯，该初始航迹不符合无人机实际飞行需求，需要对以上结果进行平滑处理。

3.2平滑后的航迹

经过3次非均匀B样条处理前后的航迹对比如图3所示。 从仿真结果可看出，平滑后的航迹比较光滑，没有尖角，符合无人机飞行的条件，满足曲率半径大于等于无人机最小转弯半径的要求。

4 结语

本文针对的是无人机二维航迹平滑问题，实验结果证明，3次非均匀B样条曲线插补的航迹平滑方法使得航迹过渡自然，航路光滑没有尖角，航向没有突变性，航迹的转弯半径基本大于无人机所允许的最小的转弯半径。同时还保证了曲线的保凸性，平滑后的航迹经过所有的航点，且逼近原始的航迹曲线。本文所研究的方法获得了较好的航迹平滑效果，保证了所规劃出的无人机航迹的真实可飞性。 本文的航迹规划模型是二维平面的航迹规划，威胁不多且分布的范围不是很广泛，且是将威胁因素都抽象为一个点，因此不需要用到高次B样条曲线。但实际的飞行环境是很复杂的，因此后续的工作中将考虑扩大威胁范围，飞行环境更加复杂化，并将算法应用到三维空间。

[参考文献]

[1]席庆彪，苏鹏，刘慧霞基于A*算法的无人机航路规划算法[J].火力与指挥控制，2013 （11）：5-9

[2]李林峰，马蕾三次均匀B样条在工业机器人轨迹规划中的应用研究[J].科学技术与工程，2013 （13）：3621-3625，3646

[3]张雷，王道波，段海滨基于粒子群优化算法的无人战斗机路径规划方法[J].系统工程与电子技术，2008 （3）：506-510

[4]TAYLOR R H.Planning and execution of straight line manipulator trajectories[J].Ibm Journal of Research & Development， 1979 （4）：424-436.

[5]王绪芝，姚敏，赵敏，等基于蚁群算法的无人机航迹规划及其动态仿真[J].指挥控制与仿真，2012 （1）：29-32

[6]董本志，张小燕，于呜，等三次NURBS曲线的Obrechkoff参数化插值算法[J].机械科学与技术，2016 （11）：1721-1726.

[7]THORSTEN G， DOMINIK H.Path planning and execution in fast-changing envlronmentsVVith known and unknown obstacles[C].SanDiego： Conference on Intelligent Robots and Systems， 2007

[8]陈丹.基于遗传算法-B样条曲线优化在机器人轨迹规划中应用[D].武汉：武汉科技大学，2007.

[9]李猛.基于智能优化与RRT算法的无人机任务规划方法研究[D].南京：南京航空航天大学，2012.

[10]陈金广，高新波.基于分段RTS平滑的凸组合航迹融合算法[J].计算机科学，2010 （4）：175-178.