基于龙格-库塔算法的无人机航迹规划

摘要：本文主要介绍利用龙格-库塔算法来优化无人机航迹规划。

1.龙格-库塔搜索算法

龙格-库塔算法原理请参考：https://blog.csdn.net/u011835903/article/details/122254286

2.无人机飞行环境建模

? 环境模型的建立是考验无人机是否可以圆满完成人类所赋予各项任务的基
础和前提，其中第一步便是如何描述规划空间中的障碍物。首先我们将采取函数模拟法模拟地貌特征。其函数表达式为：
$$z(x,y)=sin(y+a)+bsin(x)+ccos(d\sqrt{y^2+x^2})+ecos(y)+fsin(f\sqrt{y^2+x^2})+gcos(y)\text{\hspace{1em}(1)}$$
其中，$(x,y)$ 为地形上某点投影在水平面上的点坐标，$z$ 则为对应点坐标的高度。式中$a,b,c,d,e,f,g$ 是常系数，想要得到不同的地貌特征可以通过改变其常系数的大小，以上建模是作为环境模型的基准地形信息。但为了得到障碍区域我们还需要在这个基准地形上叠加山峰模型，这样就可以模拟像山峰、丘陵等障碍地理信息。山峰模型的数学表达式为：
$$h(x,y)=\sum _i{h}_{i}exp[-\frac{(x-x_{oi}{)}^2}{a_i^2}-\frac{(y-y_{oi}{)}^2}{b_i^2}]+h_o\text{\hspace{1em}(2)}$$
式 (2)中，$h_o$ 和 $h_i$ 分别表示基准地形和第$i$座山峰的高度，$(xoi,yoi)$则表示第 i座山峰的中心坐标位置，a i 和 b i 分别是第 i 座山峰沿 x 轴和 y 轴方向的坡度。由式(1)和(2)，我们可以得到如下表达式：
$$Z(x,y)=max[z(x,y),h(x,y)]\text{\hspace{1em}(3)}$$
无人机在躲避障碍物的同时也会经常遇到具有威胁飞行安全的区域，我们称之为威胁区域。这些威胁区域可以是敌人的雷达和防空导弹系统的探测威胁区域也可以是一些其它的威胁，一旦无人机进入这些区域很有可能会被击落或者坠毁。为了简化模型，本文采用半径为 r 的圆柱形区域表示威胁区域，其半径的大小决定威胁区域的覆盖范围。每一个圆柱体的中心位置是对无人机构成最大威胁的地方并向外依次减弱。

3.无人机航迹规划建模

? 在环境建模的基础上，无人机航迹规划需要考虑到在执行复杂任务的过程中自身性能约束要求，合理的设计航迹评价函数才能使得龙格-库塔搜索算法得出的最后结果符合要求，并保证规划出的航迹是有效的。考虑到实际环境中，无人机需要不断适应变化的环境。所以在无人机路径规划过程中，最优路径会显得比较复杂，并包含许多不同的特征。基于实际的情况，本文采用较为复杂的航迹评价函数进行无人机路径规划。影响无人机性能的指标主要包括航迹长度、飞行高度、最小步长、转角代价、最大爬升角等。

? 搜索最佳路径通常与搜索最短路径是密不可分的。在无人机航迹规划过程中，航迹的长度对于大多数航迹规划任务来说也是非常重要的。众所周知，较短的路线可以节省更多的燃料和更多的时间并且发现未知威胁的几率会更低。我们一般把路径定义为无人机从起始点到终点所飞行路程的值，设一条完整的航线有$n$个节点，其中第$i$个航路点和第$i+1$个航路点之间的距离表示为 $l_i$ ，这两个航路点的坐标分别表示为$(x_i,y_i,z_i)$，$(x_{i+1},y_{i+1},z_{i+1})$并分别记作 $g(i)$和 $g(i+1)$。航迹需要满足如下条件：
$$\{\begin{array}{l}{l}_i=||g(i+1)-g(i)|{|}_2\\ L_{path}={\sum }_{i=1}^{n-1}{l}_i\end{array}\text{\hspace{1em}(4)}$$
在飞行的过程中会遇到障碍物或者进入威胁区域，如果无人机无法躲避障碍物或者飞入了威胁区域将面临被击落或坠毁的危险以至于无法到达终点，记为$L_{path}=\infty$，但是无穷函数在实际问题中很难表示，我们采用惩罚的方式进行处理。一般情况下，为了利用地形覆盖自身位置，无人机应尽可能降低高度这可以帮助自身避免一些未知雷达等威胁。但是太低的飞行高度同样会加大无人机同山体和地面的撞击几率，因此设定稳定的飞行高度是非常重要的。飞行高度不应该有太大的变化，稳定的飞行高度可以减少控制系统的负担，节省更多的燃料 。为了使无人机飞行更加安全，给出的飞行高度模型:
$$\{\begin{array}{l}{h}_{height}=\sqrt{\frac{1}{n}{\sum }_{i=0}^{n-1}(z(i)-\overline{z}{)}^2}\\ \overline{z}=\frac{1}{n}{\sum }_{i=0}^{n-1}z(i)\end{array}\text{\hspace{1em}(5)}$$
无人机的可操作性也受到其转角代价函数的限制。，在飞行过程中无人机的转角应不大于其预先设定的最大转角，转角的大小会影响其飞行的稳定性。本文的研究中，设定最大转角为 $\Phi$，当前转角为$\theta$并且$a_i$是第$i$段航路段向量。
$$\{\begin{array}{l}cos\theta =\frac{a_i^T{a}_{i+1}}{|a_i||a_{i+1}|}\\ J_{turn}={\sum }_{i=1}^n(cos(\Phi -cos\theta ))\end{array}\text{\hspace{1em}(6)}$$
其中，$|a|$代表矢量$a$的长度。

? 通过对以上三个方面建立了无人机航迹规划的代价函数，可以得出本文的航迹评价函数如下：
$$J_{cost}=w_1{L}_{path}+w_2{h}_{height}+w_3{J}_{turn}\text{\hspace{1em}(7)}$$
其中，$J_{cost}$是总的代价函数，参数$w_i$ ，$i=1,2,3$ 表示每个代价函数的权值，且满足如下条件：
$$\{\begin{array}{l}{w}_i\ge 0\\ {\sum }_{i=1}^3{w}_i=1\end{array}\text{\hspace{1em}(8)}$$
通过对总的代价函数进行有效地处理，我们可以得到由线段组成的航迹。不可否认的是得到的路径往往是仅在理论上可行，但为了实际可飞，有必要对航迹进行平滑处理。本文采用三次样条插值的方法对路径进行平滑。

4.实验结果

4.1地图创建

设置地图参数a, b, c, d, e, f , g=1。地图大小为：200*200。设置三个山峰，山峰信息如表1所示。威胁区域信息如表2所示

表1：山峰信息

信息	山峰中心坐标	山峰高度	山峰X方向坡度	山峰y方向坡度
山峰1	[60,60]	50	20	20
山峰2	[100,100]	60	30	30
山峰3	[150,150]	80	20	20

表2 威胁区域信息

信息	威胁区域中心坐标	威胁区域半径
威胁区域1	[150,50]	30
威胁区域2	[50,150]	20

创建的地图如下：

4.2 航迹规划

设置起点坐标为[0,0,20]，终点坐标为[200,200,20]。利用龙格-库塔算法对航迹评价函数式（7）进行优化。优化结果如下：

从结果来看，龙格-库塔算法规划出了一条比较好的路径，表明算法具有一定的优势。

5.参考文献

[1]薛建凯. 一种新型的群智能优化技术的研究与应用[D].东华大学,2020.DOI:10.27012/d.cnki.gdhuu.2020.000178.

6.Matlab代码