1.一种顾及复杂环境约束的无人机三维航迹快速规划方法,其特征在于,包括以下步骤:S1、构建无人机三维航迹规划的环境模型;
S2、将复杂环境约束信息融入环境模型;
S3、根据融入复杂环境约束信息的环境模型,采用多层扩展A*算法获取无人机的参考航迹;
S4、对参考航迹进行航迹简化与航迹平滑,得到无人机可飞最优航迹;
所述步骤S1具体为:
将无人机在改变方向时的最小惯性距离作为格网单元的长度,将路径规划区域划分为一个m×n的格网,在每个格网单元中存储其对应的行列号、该格网单元中无人机的最大飞行高度值以及可飞行的高度范围信息,得到无人机三维航迹规划的环境模型,表示为:其中Ωm表示环境模型空间,(Xi,Yj)表示格网单元的行列号,m,n分别为环境模型中格网单元的行数和列数, 表示格网单元(Xi,Yj)的无人机可飞行高度范围,Z1,Z2,Z3,Z4均为高度值,且Z1
所述步骤S2中的复杂环境约束信息包括地形环境约束和大气环境约束;
所述地形环境约束的融入方法具体为:
A1、通过仿射变换将真实三维地形数据每个栅格像元的行列号转换为环境模型中格网单元的行列号,转换公式为:其中(Xm,Ym)表示环境模型中格网单元的行列号,(Xp,Yp)表示真实三维地形数据栅格像元的行列号,(X0,Y0)表示真实三维地形数据在路径规划区域左上角的经度和纬度,Wc表示真实三维地形数据的像元宽度,Hc表示真实三维地形数据的像元高度,R1,R2为旋转参数;
A2、把真实三维地形数据每个栅格像元的地形高程信息添加进对应的格网单元,完成地形环境约束的融入;
所述大气环境约束的融入方法具体为:
B1、将大气环境约束区域竖直投影成一个多边形,并获取多边形每个顶点的经纬度坐标;
B2、根据经纬度坐标计算每两个相邻顶点的欧式距离,计算公式为:其中(X1,Y1)和(X2,Y2)分别表示多边形两个相邻顶点的经纬度坐标,D12表示这两个相邻顶点的欧氏距离;
B3、根据两个相邻顶点的欧式距离计算这两个相邻顶点连接成的直线的分段次数,计算公式为:S12=ceil(D12/Wc)
其中S12表示顶点(X1,Y1)和(X2,Y2)连接成的直线的分段次数,ceil(·)为向上取整函数,Wc表示环境模型中每个格网对应真实三维地形数据的像元宽度;
B4、初始化计数值j=0;
B5、判断j>S12是否成立,若是则进入步骤B8,否则进入步骤B6;
B6、根据分段次数计算分段后各个点的坐标,计算公式为:其中(X12j,Y12j)为分段后第j个点的坐标,j=0,1,2,...,S12;
B7、令计数值j加1,并返回步骤B5;
B8、获取分段后各个点的坐标在环境模型中对应格网单元的行列号,得到多边形对应的格网单元;
B9、判断相邻两个格网单元是否在同一行或同一列上,若是则进入步骤B11,否则进入步骤B10;
B10、在未处于同一行或同一列的两相邻格网单元之间补充一个格网单元,进入步骤B11;
B11、将多边形对应的格网单元和补充的格网单元作为大气环境约束区域,完成大气环境约束的融入。
2.根据权利要求1所述的无人机三维航迹快速规划方法,其特征在于,所述步骤S3包括以下分步骤:S3-1、获取融入复杂环境约束信息的环境模型以及起始点与目标点的位置信息与高程信息;
S3-2、创建openlist线性表与closelist线性表,并初始化将其置为空;
S3-3、计算起始点的代价值,并将起始点添加至openlist线性表中;
S3-4、判断openlist线性表是否为空,若是则进入步骤S3-5,否则进入步骤S3-6;
S3-5、航迹搜索失败,进入步骤S3-21;
S3-6、获取openlist线性表中代价值最小的点作为当前点;
S3-7、判断当前点是否与目标点位于同一格网单元与高程,若是则进入步骤S3-8,否则进入步骤S3-9;
S3-8、航迹搜索成功,获取最优航迹节点,进入步骤S3-21;
S3-9、将当前点移除出openlist线性表,并添加进closelist线性表中;
S3-10、对当前点进行分层扩展,得到多个扩展点;
S3-11、判断是否遍历完所有扩展点,若是则返回步骤S3-4,否则选取下一个扩展点并进入步骤S3-12;
S3-12、判断closelist线性表中是否存在该扩展点,若是则进入步骤S3-13,否则进入步骤S3-14;
S3-13、舍去该扩展点,返回步骤S3-11;
S3-14、计算该扩展点的代价值;
S3-15、判断openlist线性表中是否存在该扩展点,若是则进入步骤S3-16,否则进入步骤S3-19;
S3-16、判断该扩展点的代价值是否小于openlist线性表中所存在与该扩展点位置相同的点的代价值,若是则进入步骤S3-17,否则进入步骤S3-18;
S3-17、在openlist线性表中用该扩展点替换扩展前的当前点,进入步骤S3-20;
S3-18、舍去该扩展点,返回步骤S3-11;
S3-19、将该扩展点添加进openlist线性表,进入步骤S3-20;
S3-20、将扩展前的当前点设置为该扩展点的父节点,返回步骤S3-11;
S3-21、按顺序连接最优航迹节点,得到无人机的参考航迹。
3.根据权利要求2所述的无人机三维航迹快速规划方法,其特征在于,所述代价值的计算公式为:f(n)=g(n)+ε·h(n)
其中f(n)表示当前节点n位置的代价值,g(n)表示从起始点到当前节点n位置的实际代价值,h(n)表示从当前节点n位置到目标点的估计代价值,ε为启发项权重系数且ε>1;
实际代价值g(n)的计算公式为:
g(n)=ω1Lg+ω2Zg+ω3T1+ω4T2估计代价值h(n)的计算公式为:
h(n)=ω1Lh+ω2Zh+ω3T1+ω4T2其中ω1,ω2,ω3,ω4均为权重值,Lg表示从起始点到当前节点n位置的航迹段长度,Lh表示从当前节点n位置到目标点的航迹段长度,Zg表示从起始点到当前节点n位置的航迹平均高度,Zh表示从当前节点n位置到目标点的航迹平均高度,T1,T2均为影响因素代价。
4.根据权利要求2所述的无人机三维航迹快速规划方法,其特征在于,所述步骤S3-10中对当前点进行分层扩展的具体方法为:根据水平扩展距离与无人机最大俯仰角的大小计算本次扩展的竖直高度可达范围,计算公式为:其中Hnext表示竖直高度可达范围,Hcur为当前飞行高度,LC为环境模型的格网单元宽度,α为无人机最大俯仰角,direct为方向值,其取值为从正北方向开始,顺时针分成八个方向;
在竖直高度可达范围内以固定高度间隔为基准进行分层扩展,得到多个扩展点。
5.根据权利要求1所述的无人机三维航迹快速规划方法,其特征在于,所述步骤S4中航迹简化的具体方法为:C1、设从起始点到目标点,总共有n个具有顺序关系的三维坐标点;
C2、依次计算相邻两点的方向向量,得到n-1个方向向量d1,d2,...,dn-1;
C3、初始化计数值i=1;
C4、判断i>n-2是否成立,若是则进入步骤C5,否则进入步骤C6;
C5、按顺序连接所有不简化点,结束航迹简化;
C6、设置变量p=i,q=i+1,并初始化容忍计数值num=0;
C7、计算方向向量dp和dq的夹角余弦值,计算公式为:其中 为方向向量dp和dq的夹角;
C8、判断该余弦值是否大于容忍值v,若是则进入步骤C9,否则进入步骤C10;
C9、令容忍计数值num加1,进入步骤C11;
C10、重置容忍计数值num=0,进入步骤C13;
C11、判断容忍计数值num是否大于容忍数阈值t,若是则进入步骤C12,否则进入步骤C13;
C12、将第q+1-num个点设置为不简化点,并令i=q-num,返回步骤C4;
C13、令变量q加1,返回步骤C7。
6.根据权利要求1所述的无人机三维航迹快速规划方法,其特征在于,所述步骤S4中航迹平滑的具体方法为:采用三次Cardinal样条曲线对航迹简化后的参考航迹进行平滑,得到无人机可飞最优航迹。