一种基于数字高程地图的无人机避障航迹规划方法

1.本发明涉及无人机航路规划技术领域，具体涉及一种基于数字高程地图的无人机避障航迹规划方法。


背景技术：

2.随着无人机技术的发展，无人机的应用场景也日益广泛起来，例如应用于航拍、空投物资、城市灭火、电力巡检、交通救援以及低空突防等场景中。在上述应用场景中，为了能够实现无人机的自主飞行，往往需要对无人机的飞行路线进行实时重规划，从而保证人机能够在飞行过程中躲避障碍物，安全到达目标位置，进而保证后续任务的顺利执行。
3.而在目前的路径规划领域中，通常有两类方法，第一类是，采用一种静态路网中求解最短路径的方法，该类方法利用数字地图的格栅存储方式，根据各节点间的距离、威胁度等信息确定一个评价函数，在搜索全部节点的情况下，最终得到一条最优的飞行路线。但这类方法的实时性较差，对于实时性要求较高的飞行任务来说是不太友好的。第二类是，从无人机避障、优化和目标可达性出发，基于全局规划和局部规划相结合策略，设计路径搜索方法，其主要包括了环境空间的表示、初始弹性带的生成、弹性带避开障碍物的方法设计、释放弹性带上的点的方法设计等。这类方法的优点是，简单可行，能够快速的规划出一条飞行路径，但是，当障碍物的形状比较复杂时，便很难找到障碍物的顶点，难以处理障碍物的重叠问题，如果添加的参考节点位于障碍物的包围圈里时，便会规划不到路径。


技术实现要素：

4.有鉴于此，本发明提供一种基于数字高程地图的无人机避障航迹规划方法，解决现有技术中的方法很难处理复杂障碍物、障碍物重叠以及添加的参考节点位于障碍物包围圈里等场景下的路径规划的技术问题。
5.为解决上述技术问题，本发明提供一种基于数字高程地图的无人机避障航迹规划方法，包括如下步骤：
6.s1、加载数字高程地图；
7.s2、将地图数据转换成图片；
8.s3、加载初始航迹点；
9.s4、提取骨架图中的关键节点和关键路径；
10.s5、利用a*算法生成最优航迹；
11.s6、判断步骤5生成的最优航迹上是否存在障碍物或威胁区，如果存在则执行步骤s7，否则执行步骤s13；
12.s7、连接所有航迹点，利用基于并查集思想的算法进行二值图像障碍物连通区域的判断，提取连通区域的边界；
13.s8、判断路径是否和障碍物相交，若相交，则执行步骤s9；否则，起点和终点即为路径点，执行步骤s13；
14.s9、记录下与障碍物的相交次序和交点，在相邻两个障碍物的中间加入参考节点；
15.s10、计算局部路径；
16.s11、判断两两路径点连线是否与障碍物相交，若相交，则返回步骤s9；否则，生成一段局部路径；
17.s12、判断各个局部路径是否完成路径规划，若所有局部路径计算完成，则执行步骤s13；否则，返回步骤s10；
18.s13、删除多余的航迹点，将所得到的路径点按次序依次输出；
19.s14、输出最优避障航迹。
20.进一步的，数字高程地图是以hgt格式存储的文件，一个文件包含了一个经度一个纬度范围内的高程数据，包含采样点1201*1201个，文件名称代表该数据单元左下角的位置信息，数据精度为90米。
21.进一步的，将地图数据转换成*.bmp格式的图片，bmp格式图片中的点的像素数值表示了图像的数据区域，其扫描方式是从左到右，从上到下的顺序，在此部分记录着每点像素对应的颜色号，其记录方式也随颜色模式而定，在2色图像中每个点占1位，在16色图像中每个点占4位，8位为1字节；整个数据区域的大小与图像的颜色相关，计算公式：图像数据信息大小＝(图像宽度*图像高度*记录像素的位数)/8，在数字地图存储的时候将地形坡度转换成不同颜色号记录在图像中。
22.进一步的，数字地图转换方法如下：
23.步骤a、计算各点的梯度，得到与原始数字地图大小一样的梯度图，梯度表示的是地表曲面东西方向上的高度差与南北方向上的高度差的变化情况，是数据高程模型的一阶导数，其计算方法如下：
[0024][0025]
式中：
[0026]
p——地形梯度；
[0027]
f(x)——x方向的高程变化率；
[0028]
f(y)——y方向的高程变化率；
[0029]
所述的f(x)，f(y)的计算方法如下：
[0030]
利用三阶反距离平方权差分计算f(x)、f(y)，所述算法公式如下：
[0031][0032][0033]
式中，(z1,
…
,z9)表示各顶点的高程值，l表示栅格间水平方向或垂直方向的距离；
[0034]
步骤b、采用阈值分割法对梯度图进行图像分割处理，分割处理后，各点值为0或1，得到对应的二值图，值为1的点表示山谷或较为平坦的地形，值为0的点表示陡峭的山脊地形，阈值分割法如下：
[0035]
[0036]
式中，(x,y)为二维数字图像的平面坐标，f(x,y)为坐标点(x,y)上的像素点的灰度级，t为分隔阈值；
[0037]
步骤c、去除值为1的噪声点，检测每个值为1的点，如果该点在一个连通区域中，并且连通区域值为1的点的个数小于常数a，则认为这个连通区域是山顶小块平地或山脊小块平坦地形，这个点所在的连通区域就是噪声，直接删除，并将其赋值为0；
[0038]
步骤d、采用逐次消去目标边界点的迭代细化算法将二值图进行骨架化处理。
[0039]
进一步的，所述分隔阈值t的确定方法如下：
[0040]
b1：初始化一个阈值th，将图像的f(x,y)分为a，b两类；
[0041]
b2：分别计算a，b两类像素集合的均值μa，μb；
[0042]
b3：计算a，b两类的类间方差；
[0043]
b4：将th从0-255循环，分别计算a，b的类间方差，当类间方差最大时，对应的th就是所求的最佳分隔阈值。
[0044]
进一步的，迭代细化算法如下：
[0045]
d1：循环所有值为1的点，对符合如下条件的像素点标记为删除：
[0046]
(1)2≤n(p1)≤6；
[0047]
(2)s(p1)＝1；
[0048]
(3)p2*p4*p6＝0；
[0049]
(4)p4*p6*p8＝0；
[0050]
所述n(p1)表示跟p1相邻的8个像素点中，值为1的像素点个数，s(p1)表示从p2～p9～p2像素中出现0～1变化的累计次数；
[0051]
d2：循环所有值为1的点，对符合如下条件的像素点标记为删除：
[0052]
(1)2≤n(p1)≤6；
[0053]
(2)s(p1)＝1；
[0054]
(3)p2*p4*p8＝0；
[0055]
(4)p2*p6*p8＝0；
[0056]
循环上述两个步骤，直至两步中都没有像素被标记为删除为止，输出的结果即为二值图细化后的骨架。
[0057]
进一步的，初始航迹点加载包含读取配置文件、手动在地图上点选和手动输入初始航迹信息；初始航迹信息包含航点号、经度、纬度和高度。
[0058]
进一步的，关键节点为初始航迹信息中的航迹点和航迹点连接起来的路径。
[0059]
进一步的，所述障碍物为任何立体物体，其包含经度、纬度、宽度和高度信息；所述威胁区为圆形范围，包含威胁区中心经度、威胁区中心纬度和威胁区半径信息。
[0060]
进一步的，提取连通区域边界的方法如下：
[0061]
步骤a：初始化变量i＝1；
[0062]
步骤b：寻找二值图中第一个位于背景中的像素位置，对应等价类记为背景等价类；
[0063]
步骤c：遍历第i+1个位置；
[0064]
步骤d：若位于背景中，则将其对应等价类与背景等价类归并，跳转至步骤i；否则，继续步骤e；
[0065]
步骤e：若位于前景中，则检测其左上、正上、右上和正左方四个位置，分别对应等价类的标号：(left-top,top,right-top,left)，在对应的前景等价类中，选择其中位于前景中的相应位置的对应等价类，组成集合clt，ct，crt，cl；
[0066]
步骤f：若为空，则跳至步骤i；否则继续步骤g；
[0067]
步骤g：将步骤e中的等价类归并为一个前景等价类，对应连通区域的边界信息从1开始进行标号；
[0068]
步骤h：将所有的前景等价类归并，利用与步骤g中相同的方法更新归并后的等价类对应的连通区域的边界信息；
[0069]
步骤i：i＝i+1，k为总的顶点数，若i《＝k，则跳转至步骤c，否则算法结束。
[0070]
进一步的，计算局部路径的方法包含如下步骤：
[0071]
步骤a、使用模板匹配的方式提取出分段后每段局部路径中包含的障碍物的凸点作为顶点集；
[0072]
步骤b、把顶点集中所有的点均作为城市点，使用弹性带公式将障碍物的顶点和它与初始路径的交点进行排序；
[0073]
步骤c、执行去点操作，删除远离城市点的点以及在障碍物内部的点；
[0074]
步骤d、从一个交点出发分别按照两个不同的方向计算和另一个交点的距离，取距离较短的一侧的城市点为路径点。
[0075]
本发明所公开的无人机避障航迹规划方法，航迹预规划是结合数字高程地图与初始航点信息，利用a*算法进行最优路径规划，输出的最优路径数据通过udp通信协议上传至飞控，以为无人机提供飞行路线。
[0076]
本发明所公开的无人机避障航迹规划方法，路径重规划是在路径预规划的输出结果上添加障碍物、威胁区，以此来对所述航迹预规划输出的最优航迹进行修正，进而得到能够躲避障碍物和威胁区的最优航迹。障碍物为任何立体物体，其包含了经度、纬度、宽度和高度信息。威胁区为圆形范围，包含了威胁区中心经度、威胁区中心纬度和威胁区半径信息。
[0077]
采用本发明方案，进行无人机避障航迹规划，可以在执行低空飞行任务时，充分的考虑地形起伏变化的因素，以实现贴地飞行的目的；与此同时，本方案先进行离线航迹预规划，将规划好的航迹上传给飞控，以为其提供飞行导引，很好的解决了实时性不高的问题；当预规划的航迹上出现障碍物或威胁区时，本方案提供的方法，能够在障碍物的形状比较复杂的情况下找到障碍物的顶点，处理障碍物的重叠问题，从而快速的规划出一条飞行路径。
[0078]
本发明的上述技术方案的有益效果如下：
[0079]
(i)本发明方法基于数字高程地图进行路径规划，充分的考虑地形的起伏变化因素，在规定飞行高度的情况下，可以实现无人机低空飞行。
[0080]
(ii)本发明的方法对数字高程地图进行的二值化转换、二值图去噪处理以及二值图骨架化处理，基于此提取关键节点和关键路径，很好的提高了现有路径规划技术的实时性。
[0081]
(iii)本发明方法解决了现有路径规划技术中只对单一场景进行规划的问题，能够在没有障碍物的情况下规划出最优航迹，同时也能在加入障碍物之后对当前航迹进行优
化修正，做到航迹实时重规划，进而可以达到躲避障碍物或威胁区的目的。
[0082]
(iv)本发明方法在进行局部路径计算时，只取较短距离一侧的路径，相比现有技术对所有局部路径进行搜索的方法，减小了计算量，使得路径规划的实时性得到进一步提升。
[0083]
(v)本发明方法在生成最优路径后，依据无人机飞行性能和各航迹点距离信息，将多余的航迹点进行了去除，保证输出的航迹点不存在重复或者间距较近情况，同时减小了航迹文件的内存，是的数据传输速率得到了提升。
[0084]
(vi)本发明方法简单，实施简便，可以极大的节约人力物力。
附图说明
[0085]
图1为本发明的原始数字高程地图；
[0086]
图2为本发明的转换格式后的数字高程地图；
[0087]
图3为本发明的3x3移动窗口；
[0088]
图4为本发明的利用地形梯度生成的数字地图；
[0089]
图5为本发明中迭代细化算法示意图；
[0090]
图6为本发明中生成的骨架示意图；
[0091]
图7为本发明的无人机避障航迹规划流程图；
[0092]
图8为本发明的航迹预规划后的效果图；
[0093]
图9为本发明的航迹重规划后的效果图。
[0094]
附图中各标号的含义：1-航线，2-航迹点，3-威胁区，zi,(i＝1,2,...,9)为以中心点为5的3x3的窗口为网格点的高程。
具体实施方式
[0095]
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例的附图1-9，对本发明实施例的技术方案进行清楚、完整地描述。显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例。基于所描述的本发明的实施例，本领域普通技术人员所获得的所有其他实施例，都属于本发明保护的范围。
[0096]
本技术文件中附图的标号(如1,2,3
…
)是基于不同图的位置关系而建立的，附图不同，则相应的含义有所不同，故不能以此理解为对保护范围的限制。
[0097]
本发明中所述无人机并未特指某一机型，故涉及无人飞行器避障航迹规划的等同变化也均落入本发明的保护范围。
[0098]
本发明中加载初始航迹不特指加载固定的航迹文件、手动在地图上点选和手动输入初始航迹信等方法。凡是需要加载初始航迹文件的操作也均属于本发明保护范围。
[0099]
在本发明中，在未作相反说明的情况下，术语“常数”等术语应做广义理解，该值可以是本发明建议的常数，也可以是大于建议的常数，也可以是小于建议的常数。对于本领域的技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
[0100]
如图1-9所示：本实施例提供一种基于数字高程地图的无人机避障航迹规划方法，包括如下步骤：
[0101]
s1、加载*.hgt数字高程地图；
[0102]
s2、将*.hgt地图数据转换成*.bmp格式的图片；
[0103]
s3、加载初始航迹点；
[0104]
s4、提取骨架图中的关键节点和关键路径；
[0105]
s5、利用a*算法生成最优航迹；
[0106]
s6、判断步骤5生成的最优航迹上是否存在障碍物或威胁区，如果存在则执行步骤s7，否则执行步骤s13；
[0107]
s7、连接所有航迹点，利用基于并查集思想的算法进行二值图像障碍物连通区域的判断，提取连通区域的边界；
[0108]
s8、判断路径是否和障碍物相交，若相交，则执行步骤s9；否则，起点和终点即为路径点，执行步骤s13；
[0109]
s9、记录下与障碍物的相交次序和交点，在相邻两个障碍物的中间加入参考节点；
[0110]
s10、计算局部路径；
[0111]
s11、判断两两路径点连线是否与障碍物相交，若相交，则返回步骤s9；否则，生成一段局部路径；
[0112]
s12、判断各个局部路径是否完成路径规划，若所有局部路径计算完成，则执行步骤s13；否则，返回步骤s10；
[0113]
s13、删除多余的航迹点，将所得到的路径点按次序依次输出；
[0114]
s14、输出最优避障航迹。
[0115]
数字高程地图是以hgt格式存储的文件，一个文件包含了一个经度一个纬度范围内的高程数据，包含采样点1201*1201个，文件名称代表该数据单元左下角的位置信息，数据精度为90米。
[0116]
bmp格式图片中的点的像素数值表示了图像的数据区域，其扫描方式是从左到右，从上到下的顺序，在此部分记录着每点像素对应的颜色号，其记录方式也随颜色模式而定，在2色图像中每个点占1位，在16色图像中每个点占4位，8位为1字节；整个数据区域的大小与图像的颜色相关，计算公式：图像数据信息大小＝(图像宽度*图像高度*记录像素的位数)/8，在数字地图存储的时候将地形坡度转换成不同颜色号记录在图像中。
[0117]
数字地图转换方法如下：
[0118]
步骤a、计算各点的梯度，得到与原始数字地图大小一样的梯度图，梯度表示的是地表曲面东西方向上的高度差与南北方向上的高度差的变化情况，是数据高程模型的一阶导数，其计算方法如下：
[0119][0120]
式中：
[0121]
p——地形梯度；
[0122]
f(x)——x方向的高程变化率；
[0123]
f(y)——y方向的高程变化率；
[0124]
f(x)，f(y)不一定非要是东西方向高程差或是南北方向，只要表示的这两个方向相互垂直就可，如想要计算出某点的地形坡度只需求解f(x)，f(y)；
[0125]
所述的f(x)，f(y)的计算方法如下：
[0126]
采用如图3所示的3x3的移动窗口，利用三阶反距离平方权差分计算f(x)、f(y)，所述算法公式如下：
[0127][0128][0129]
式中，(z1,
…
,z9)表示各顶点的高程值，l表示栅格间水平方向或垂直方向的距离；
[0130]
步骤b、采用阈值分割法对梯度图进行图像分割处理，分割处理后，各点值为0或1，得到对应的二值图，值为1的点表示山谷或较为平坦的地形，值为0的点表示陡峭的山脊地形，阈值分割法如下：
[0131][0132]
式中，(x,y)为二维数字图像的平面坐标，f(x,y)为坐标点(x,y)上的像素点的灰度级，t为分隔阈值。
[0133]
步骤c、去除值为1的噪声点，检测每个值为1的点，如果该点在一个连通区域中，并且连通区域值为1的点的个数小于常数a(a值的设定与连通区域的大小有关，通常为连通区域总点数的1/3)，则认为这个连通区域是山顶小块平地或山脊小块平坦地形，这个点所在的连通区域就是噪声，直接删除，并将其赋值为0；
[0134]
步骤d、采用逐次消去目标边界点的迭代细化算法将二值图进行骨架化处理。
[0135]
去噪原因：根据生成的数字地图可以看到在不可飞区域(黑色区域)中有许多小型的灰色可飞域，可能是因为使用地形坡度作为可飞域和不可飞域的判断条件时，在这些山脉之中存在着小块的平缓区域。如果无人机想要飞到这些可飞的区域，它必然要穿越地形坡度较大的山脉，显然这种飞行方式在实际飞行过程中是不可取的，为了无人机的飞行安全并防止这些小块可飞域对后续的航迹规划过程造成干扰，需要将这些噪声剔除掉。
[0136]
另外需要注意的是在可飞区域中存在黑色的小块区域，这可能是因为在平原地区中存在的一些突出的山脉造成的，无人机在低空飞行的过程中是不能直接穿越这种地形的，因此这种黑色的小块点是规划算法需要绕开的点，不应该被剔除掉。
[0137]
所述分隔阈值t的确定方法如下：
[0138]
b1：初始化一个阈值th，将图像的f(x,y)分为a，b两类；
[0139]
b2：分别计算a，b两类像素集合的均值μa，μb；
[0140]
b3：计算a，b两类的类间方差；
[0141]
b4：将th从0-255循环，分别计算a，b的类间方差，当类间方差最大时，对应的th就是所求的最佳分隔阈值。
[0142]
迭代细化算法如下：
[0143]
d1：循环所有值为1的点，如图5所示，对符合如下条件的像素点标记为删除：
[0144]
(5)2≤n(p1)≤6；
[0145]
(6)s(p1)＝1；
[0146]
(7)p2*p4*p6＝0；
[0147]
(8)p4*p6*p8＝0；
[0148]
所述n(p1)表示跟p1相邻的8个像素点中，值为1的像素点个数，s(p1)表示从p2～p9～p2像素中出现0～1变化的累计次数；
[0149]
d2：循环所有值为1的点，对符合如下条件的像素点标记为删除：
[0150]
(5)2≤n(p1)≤6；
[0151]
(6)s(p1)＝1；
[0152]
(7)p2*p4*p8＝0；
[0153]
(8)p2*p6*p8＝0；
[0154]
循环上述两个步骤，直至两步中都没有像素被标记为删除为止，输出的结果即为二值图细化后的骨架。
[0155]
初始航迹点加载包含读取*.json等格式的取配置文件、手动在地图上点选和手动输入初始航迹信息；初始航迹信息包含航点号、经度、纬度和高度。
[0156]
关键节点为初始航迹信息中的航迹点和航迹点连接起来的路径。
[0157]
所述障碍物为任何立体物体，其包含经度、纬度、宽度和高度信息；所述威胁区为圆形范围，包含威胁区中心经度、威胁区中心纬度和威胁区半径信息。
[0158]
提取连通区域边界的方法如下：
[0159]
步骤a：初始化变量i＝1；
[0160]
步骤b：寻找二值图中第一个位于背景中的像素位置，对应等价类记为背景等价类；
[0161]
步骤c：遍历第i+1个位置；
[0162]
步骤d：若位于背景中，则将其对应等价类与背景等价类归并，跳转至步骤i；否则，继续步骤e；
[0163]
步骤e：若位于前景中，则检测其左上、正上、右上和正左方四个位置，分别对应等价类的标号：(left-top,top,right-top,left)，在对应的前景等价类中，选择其中位于前景中的相应位置的对应等价类，组成集合clt，ct，crt，cl；
[0164]
步骤f：若为空，则跳至步骤i；否则继续步骤g；
[0165]
步骤g：将步骤e中的等价类归并为一个前景等价类，对应连通区域的边界信息从1开始进行标号；
[0166]
步骤h：将所有的前景等价类归并，利用与步骤g中相同的方法更新归并后的等价类对应的连通区域的边界信息；
[0167]
步骤i：i＝i+1，k为总的顶点数，若i《＝k，则跳转至步骤c，否则算法结束。
[0168]
计算局部路径的方法包含如下步骤：
[0169]
步骤a、使用模板匹配的方式提取出分段后每段局部路径中包含的障碍物的凸点作为顶点集；
[0170]
步骤b、把顶点集中所有的点均作为城市点，使用弹性带公式将障碍物的顶点和它与初始路径的交点进行排序；
[0171]
步骤c、执行去点操作，删除远离城市点的点以及在障碍物内部的点；
[0172]
步骤d、从一个交点出发分别按照两个不同的方向计算和另一个交点的距离，取距离较短的一侧的城市点为路径点。
[0173]
以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明所述原理的前提下，还可以作出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。