一种固定翼路径追踪方法

1.本发明属于无人机技术领域的一种固定翼无人机飞行控制方法，尤其是一种利用改进粒子群算法和b样条曲线算法结合的固定翼无人机路径追踪的方法。


背景技术：

2.随着无人机技术的发展，固定翼在作战中发挥的作用越来越多。固定翼执行探测、侦察、作战等任务时必要的方法就是任务执行过程中对于规划好的路径进行实时的追踪。
3.而追踪过程中的避障问题是固定翼路径追踪中的一个重要环节。在固定翼路径追踪过程中，固定翼一般会面临在路径规划过程中难以处理的小范围内的避障、二次规划路径、高效稳定的追踪等问题，所以对于需要避障情况下的路径追踪问题，目前尚无较为完整解决方法。


技术实现要素：

4.本发明所要解决的技术问题是，在已知范围内的需要进行避障区域内的，在风速扰动和动力学约束下，快速实现避障和二次规划固定翼追踪路径，并沿路径进行位姿速度的动态分配，实现固定翼对于路径的高效稳定跟踪。
5.为了克服上述问题，本发明提出了一种利用改进粒子群算法和b样条曲线算法结合的固定翼无人机路径追踪的方法。
6.为了实现上述技术目的，本发明的技术方案为：
7.步骤s1：已知固定翼的终点和期望飞行路径d，设定固定翼终点的期望速度和期望位姿，通过损失函数的形式设置固定翼路径待优化的路径规划函数；
8.期望飞行路径d由一连串位置坐标点集[x_d，y_d，z_d]组成的集合，路径规划函数由位姿损失函数、避障损失函数、轨迹追踪损失函数三个部分组成，用于描述固定翼路径追踪过程中位置、速度、姿态这些相关参数之间的变化关系，对其求最优解可以得到最优的路径。
[0009]
步骤s2：实时获取固定翼的当前时刻的位姿ps，位姿包括位置和姿态根据前一个时刻的实际期望飞行路径更新设定固定翼的飞行约束条件；
[0010]
步骤s3：设置改进粒子群航点预估模块，在改进粒子群航点预估模块中根据固定翼的当前位姿ps、障碍物位置信息及其威胁半径、已知的期望飞行路径d和固定翼的转弯半径区间[r
min
，r
max
]，r
min
表示固定翼最小转弯半径，r
max
表示固定翼的最大转弯半径，处理获得从固定翼的当前位姿ps在躲避开临近的障碍物情况下且路径最短位姿变化最少的到期望行进路径d方案下的航点集；
[0011]
上述具体是根据固定翼的当前位姿ps、障碍物位置信息及其威胁半径、已知的期望飞行路径d和固定翼的转弯半径区间[r
min
，r
max
]，r
min
表示固定翼最小转弯半径，r
max
表示固定翼的最大转弯半径设置对步骤s1中的路径规划函数的约束条件，通过改进粒子群算法进行求解。
[0012]
障碍物位置信息及其威胁半径表示为[x
obs
,y
obs
,z
obs
,r
obs
]，其中x
obs
,y
obs
,z
obs
分别表示障碍物位置的三维坐标，r
obs
表示障碍物的威胁半径。
[0013]
航点集中的每个航点表示[x
opt
,y
opt
,z
opt
]，其中x
opt
,y
opt
,z
opt
分别表示航点的三维坐标。
[0014]
本发明中，固定翼在障碍物信息已知的情况下进行避障，飞行高度所在的平面上不具有其他未知障碍物。
[0015]
步骤s4：设置b样条航线拟合模块，通过b样条航线拟合模块将航点集进行插值拟合得到b样条曲线，从而得到一条光滑的预计航线，提取预计航线中下一时刻的位置，并和当前时刻的位置之间建立实际期望飞行路径ds，据实际期望飞行路径ds在步骤s5中设定固定翼的期望姿态和速度；
[0016]
所述步骤s4中，具体是控制固定翼从当前时刻的位置向预计航线中下一时刻的位置飞行前进。
[0017]
所述步骤s4中，在预计航线中进行时刻均分确定获得下一时刻的位置。
[0018]
步骤s5：改进粒子群航点预估模块中，实时根据行经至下一时刻后的当前位姿分别和固定翼的期望飞行路径d之间的关系改变、和障碍物的位置信息及其威胁半径之间的关系改变，重新回到步骤s2，再次计算实际期望路径ds，由此不断迭代实时控制固定翼每个时刻的飞行。
[0019]
由此，将固定翼实际期望路径保持不断更新，其最优路径下的行进状态进行优化重新设置。
[0020]
所述步骤s2中，根据前一个时刻的实际期望飞行路径更新设定固定翼的飞行约束条件，具体为：
[0021][0022][0023]
其中，ld表示期望飞行路径d的总长度，l
ref
表示当前位置距离起点位置的直线距离，v
min
表示在固定翼能气动条件下保持操控性的最低速度，w
max
表示固定翼的方向舵和副翼混控下的最大偏航角速度。
[0024]
固定翼的转弯半径要在转弯半径区间[r
min
，r
max
]内。
[0025]
在所述步骤s3的改进粒子群航点预估模块中，根据输入的固定翼当前位姿ps、障碍物位置信息及其威胁半径[x
obs
，y
obs
，z
obs
，r
obs
]、期望的固定翼行进路径d、最小固定翼姿态变动、固定翼的转弯半径区间[r
min
，r
max
]的情况下，建立以下目标函数：
[0026]
位姿损失函数：
[0027][0028]
速度损失函数：
[0029]
[0030]
轨迹追踪损失函数：
[0031]
cost_tra＝((psi_d-psi_a)/π)2+((pit_d-pit_a)/π)2[0032]
且按照以下公式设置设置总损失函数：
[0033]
func_tra＝lam_1*cost_pos+lam_2*cost_speed+lam_3*cost_tra
[0034]
其中，cost
pos
表示位姿损失函数，cost
speed
表示公式速度损失函数，cost
tra
表示轨迹追踪损失函数，func
tra
表示总的损失函数，即步骤s1中的待优化的路径规划函数，π表示圆周率，x_a，y_a，z_a为当前时刻下的三个坐标位置信息，x_d，y_d，z_d为当前时刻下的期望飞行路径的坐标位置信息，u_a，v_a，w_a为当前时刻下的三个方向上的速度信息，u_d，v_d，w_d为期望飞行路径的三个方向上的速度信息，psi_a为当前时刻下的航向角度，pit_a为当前时刻下的俯仰角度，psi_d为期望飞行路径下的航向角度，pit_d为期望飞行路径下的俯仰角度，lam1、lam2、lam3为三个损失函数的加权系数。
[0035]
再利用改进粒子群算法对目标函数进行求解获得最优的航点集。这样利用改进粒子群算法对于下一时段内相同间隔的一系列固定翼航点进行快速预估，择取最优的航点集作为期望航点集。
[0036]
其流程如图2所示，先初始化粒子群参数如粒子群个数、最大迭代次数、混合概率、混合池大小比例、加速因子和惯性权重系数，在对于每个粒子进行适应度计算(适应度即为在所需计算的函数中，在该粒子的位置上函数的值)，预先根据迭代次数的增加而增加惯性系数w的值迭代意为按照本算法流程中的步骤s2至步骤s5循环一次的过程，再根据适应度值更新pbest和gbest。之后将每个粒子适应度的集合作为母本集，在母本集中进行选择，选取其中较为优质的两个、之后转化为单个数字组成的两母本之间进行交叉，结合出最终解，最后根据更新公式更新粒子的速度和位置完成一次循环，然后根据新的粒子进行下一次的循环，直至达到迭代次数上限将最优解输出。
[0037]
选择是指设置一个值的范围在[1，2]中的一个均匀取值的随机数，并将他与适应度的集合进行点乘点乘即每个适应度都乘以该随机数，再依次与原适应度的集合做大小对比，直至找到更大的值或遍历完全。
[0038]
若找到了更大值，则将其作为其中一母本；若遍历完全后无更大值，则将其本身作为其中一母本。而这找更大的适应度的过程即为上一段中选取优质粒子的过程。
[0039]
交叉是指粒子的一部分随机地两两相交，产生同样数目的子辈粒子的过程。而之后便可使子代粒子代替母本粒子，由此保持种群粒子数目不变，其中子代粒子的位置由两个母本粒子的位置的算术加权和计算得到：
[0040][0041][0042]
其中，是目标搜索空间维度的位置向量，在本算法由于考虑的方向为四种相关函数组合成的损失函数，故而该向量是四个维度的，而和k＝1，2分别指明是子代粒子还是母本粒子的位置；是该维度空间中的一个均匀分布的随机向量，其在每个维度内的分量都在[0,1]的范围内均匀地随机取值。
[0043]
所述的改进粒子群算法是在粒子群算法中将惯性系数的权重w设定为按照以下公式计算获得：
[0044]
w＝0.4+0.006*(100-i)
[0045]
其中，i表示迭代次数，设置的总迭代次数为100次。
[0046]
所述的固定翼的改进粒子群航点预估模块，在原有的粒子群算法的基础上添加动态调整惯性权重w模块，该模块特点为：在迭代过程中设置一个随迭代次数增加而降低惯性权重系数w的模块，这样使得函数在迭代前期可由于拥有较大惯性权值而能使粒子跳出局部最小值点，而在迭代后期又可使得粒子对于当前区域的最优解的搜索更为精确，且收敛速度也有提高。
[0047]
在所述步骤s4的b样条航线拟合模块中，将已行进的航点和由改进粒子群航点预估模块中实时输出的航点集及当前的航向角，作为b类样条曲线插值拟合的前提条件，再加上用最低速可操作性下的最小偏航角速度作为约束，选取三阶的b样条函数进行求解，快速处理获得到过渡平滑的预计航线。
[0048]
具体实施中，设置以下三阶的b样条函数：
[0049][0050]
其中，n
i，k
(u)表示第i个位置点在第k次方时的函数，i表示已知的第几个位置点，k表示次方数，u表示所求的当前位置点，ui表示已知的第i个位置点的位置。
[0051]
本发明根据固定翼无人机预设的飞行航迹、当前位姿、最小转弯半径和障碍物位置及威胁半径，实时计算出预估的下一时段内的b类样条曲线，并择取下一时刻的路径位置，作为最优实际路径；通过改进粒子群算法，将预估的航迹规划问题变成结合约束寻找代价最小粒子，以此找到最佳航点组成的航迹；将各个预估出的航点通过拟合，做出b类样条曲线作为实际路径，并据此设置固定翼无人机的目标位姿及飞行速度。
[0052]
本发明可高效的进行规划，设置b类样条曲线和实时更新预设路径，可以大大提高固定翼无人机的避障效果和路径追踪能力，并且减轻了固定翼速度分配和位姿变化的压力，更好的实现了固定翼路径追踪能力。
[0053]
本发明提供的技术方案带来的益处：
[0054]
本发明通过改进粒子群算法实现的对于避障后的新航点进行预估，并且在行经至下一时刻时再次进行预估，使得计算得到的航点可以有效避免局部最优，且大大提高了避障效果的稳定性，且经过改进后的粒子群算法，收敛速度更快准确性更高。
[0055]
本发明通过增设构建了b样条航线拟合模块，利用b样条凸包的性质妥善的避开障碍物，利用其计算时可选择需要的阶数避免了航点数目增加带来的计算复杂度大幅提高的问题，且其形成的曲线会更加平滑，固定翼所需调整的姿态更小，稳定性更高，进一步消除了风速干扰对无人机追踪效果的影响。
附图说明
[0056]
图1是本发明方法的流程图；
[0057]
图2是改进粒子群算法的流程图；
[0058]
图3是使用了本发明生成的效果图。
具体实施方式
[0059]
下面结合说明书附图，对本发明的具体工作过程进一步详细说明。
[0060]
如图1所示，本发明的实施例及其实施过程如下：
[0061]
步骤s1：设定无人机的期望速度ve和期望位姿pe,位姿包括固定翼的三维空间中的位置坐标和固定翼的偏航角、俯仰角和滚转角，通过设置对于有关于路径形成各项参数和约束条件的控制率，通过各部分分别加权以损失函数的形式来设置固定翼的待优化路径规划函数。组成的规划函数包括：由与期望位置的误差和与障碍物位置的关系组成位置损失函数、由与期望速度的误差和与期望角速度的误差组成的速度损失函数、与期望航线误差的损失函数、与转弯半径、航线角速度的约束组成的约束损失函数。
[0062]
步骤s2：实时获取固定翼当前位姿ps，并设定固定翼的期望飞行路径d：
[0063]
ps＝[x_a,y_a,z_a,χ_a]
[0064]
d＝[x_d,y_d,z_d]
[0065]
其中，x,y,z表示北东地导航坐标系下的三轴方向坐标，χ表示固定翼的偏航角、俯仰角、滚转角形成的集,而期望飞行路径d是描述改路径的一些列位置点集，期望飞行路径是预设的固定翼飞行任务，故而设置为已知。x_a，y_a，z_a，χ_a分别表示当前时刻下的三个坐标位置和航向角，xd,yd,zd分别表示期望飞行路径的三个坐标位置。
[0066]
步骤s3：设置改进粒子群航点预估模块，根据步骤s1和步骤s2所获得的各个固定翼的当前位姿ps和固定翼的期望飞行路径d、障碍物位置信息及其威胁半径[x
obs
,y
obs
,z
obs
,r
obs
]以及固定翼的转弯半径区间[r
min
，r
max
]，r
min
表示固定翼最小转弯半径r
max
表示固定翼的最大转弯半径，又由下式计算得到：
[0067][0068][0069]
其中，ld表示期望行进路径的总长度，l
ref
表示当前位置下行进路径的相对长度，v
min
表示在固定翼能气动条件下保持操控性的最低速度，w
max
表示固定翼的方向舵和副翼混控下的最大偏航角速度。
[0070]
通过改进粒子群航点预估模块计算出从固定翼的当前位姿ps在躲避开临近的障碍物的情况下且路径最短位姿变化最少的到期望固定翼行进路径d方案下的航点集[x
opt
,y
opt
,z
opt
]。其流程如图2所示，先初始化粒子群参数如粒子群个数、最大迭代次数、混合概率、混合池大小比例、加速因子和惯性权重系数，在对于每个粒子进行适应度计算(适应度即为在所需计算的函数中，在该粒子的位置上函数的值)，预先根据迭代次数的增加而增加惯性系数w的值(迭代意为按照本算法流程中的步骤s2至步骤s5循环一次的过程)，再根据适应度值更新pbest和gbest。之后将每个粒子适应度的集合作为母本集，在母本集中进行选择，选取其中较为优质的两个、之后转化为单个数字组成的两母本之间进行交叉，结合出最终解，最后根据更新公式更新粒子的速度和位置完成一次循环，然后根据新的粒子进行下一次的循环，直至达到迭代次数上限将最优解输出。
[0071]
选择是指设置一个值的范围在[1，2]中的一个均匀取值的随机数，并将他与适应度的集合进行点乘(点乘即每个适应度都乘以该随机数)，再依次与原适应度的集合做大小对比，直至找到更大的值或遍历完全。若找到了更大值，则将其作为其中一母本；若遍历完全后无更大值，则将其本身作为其中一母本。而这找更大的适应度的过程即为上一段中选取优质粒子的过程。
[0072]
交叉是指粒子的一部分随机地两两相交，产生同样数目的子辈粒子的过程。而之后便可使子代粒子代替母本粒子，由此保持种群粒子数目不变，其中子代粒子的位置由两个母本粒子的位置的算术加权和计算得到：
[0073][0074][0075]
其中是目标搜索空间维度的位置向量，在本算法由于考虑的方向为四种相关函数组合成的损失函数，故而该向量是四个维度的，而和k＝1，2分别指明是子代粒子还是母本粒子的位置；是该维度空间中的一个均匀分布的随机向量，其在每个维度内的分量都在[0,1]的范围内均匀地随机取值。
[0076]
本发明中，固定翼在障碍物相关信息已知的情况下进行避障，飞行高度所在的平面上不具有其他未知障碍物。根据固定翼自身属性预先已知获得固定翼的最小转弯半径r
min
，将当前位置与终点距离的一半作为固定翼的最大转弯半径r
max
，构建固定翼的转弯半径区间[r
min
，r
max
]。改进粒子群算法是由杂交pso算法的基础上加上对于惯性加权系数的改动做出的
[0077]
步骤s4：设置b样条航线拟合模块，将航点集进行三阶的插值拟合得到b样条曲线，从而得到一条光滑的航线，将所得预计航线中下一时刻的位置信息作为实际期望的行进路径ds，据此设定固定翼的期望姿态和速度。
[0078]
其中b样条建立为以下公式：
[0079][0080]
本发明中，由改进粒子群算法得出的航点为相等距离，满足均匀b样条曲线的设计。
[0081]
步骤s5：由上述步骤后，固定翼飞行至下一期望航点，在改进粒子群航点预估模块中，根据行经至下一时刻后的位姿和固定翼的期望飞行路径d之间的关系发生改变，和障碍物的位置信息及其威胁半径[x
obs
,y
obs
,z
obs
,r
obs
]之间的关系也发生改变，故而重新回到步骤s2，再次计算实际期望路径ds以及输出的期望航向角和速度。
[0082]
由此，将固定翼实际期望路径保持不断更新，其最优路径下的行进状态进行优化重新设置。
[0083]
本发明方法为了验证可行性，借助matlab进行仿真，同时加入了满足固定翼气动
条件的运动学动力学函数来描述固定翼接收到期望航向和速度后的反馈，来增加仿真的可信度。
[0084]
采用本发明后的结果如图3所示。图中的实线线条为固定翼已经过的路径，圆点线条为预估出的期望航线，间隔线条为提前设置的固定翼期望航线。如图3所示，固定翼可在避开障碍物的同时实现对于预设期望航线的追踪。
[0085]
结果显示，本发明可实现避障条件下路径追踪的目的。