CN115129079B - 一种基于改进弹性带算法的三维直升机航迹重规划方法 - Google Patents
一种基于改进弹性带算法的三维直升机航迹重规划方法 Download PDFInfo
- Publication number
- CN115129079B CN115129079B CN202210692004.5A CN202210692004A CN115129079B CN 115129079 B CN115129079 B CN 115129079B CN 202210692004 A CN202210692004 A CN 202210692004A CN 115129079 B CN115129079 B CN 115129079B
- Authority
- CN
- China
- Prior art keywords
- point
- track
- current
- threat
- straight line
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 60
- 238000000034 method Methods 0.000 title claims abstract description 39
- 239000013598 vector Substances 0.000 claims description 21
- 230000011218 segmentation Effects 0.000 claims description 16
- 238000004364 calculation method Methods 0.000 claims description 11
- 230000001502 supplementing effect Effects 0.000 claims description 10
- 238000012545 processing Methods 0.000 claims description 7
- 238000011284 combination treatment Methods 0.000 claims description 4
- 238000001514 detection method Methods 0.000 claims description 3
- 238000000605 extraction Methods 0.000 claims description 3
- 239000011159 matrix material Substances 0.000 claims description 3
- 239000013589 supplement Substances 0.000 claims 2
- 230000015572 biosynthetic process Effects 0.000 claims 1
- 238000005755 formation reaction Methods 0.000 claims 1
- 238000010586 diagram Methods 0.000 description 4
- 238000011161 development Methods 0.000 description 2
- 230000018109 developmental process Effects 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 238000003672 processing method Methods 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
- G05D1/106—Change initiated in response to external conditions, e.g. avoidance of elevated terrain or of no-fly zones
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
- Y02A90/10—Information and communication technologies [ICT] supporting adaptation to climate change, e.g. for weather forecasting or climate simulation
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Radar Systems Or Details Thereof (AREA)
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
Abstract
本发明公开了一种基于改进弹性带算法的三维直升机航迹重规划方法：在地图中添加威胁区；对每个威胁区作为当前威胁区得到重规划航迹并更新；提取威胁区覆盖点；选取起点P0和终点P3并连接得到直线，直线与当前威胁区相交则继续，否则以直线作为航迹；执行改进弹性带算法得到位于当前直线上方、下方以P0为起点、以P3为终点且不与对应威胁区相交的航迹；计算并选出两条航迹的总代价较小的作为P0与P3之间的航迹；将P0与P3之间的航迹与P0之前、P3之后的全局航迹点连接，得到重规划航迹。本发明能够在初始航迹的基础上高效计算出避开威胁区的最优航迹，解决了飞行器飞行过程中遇到突发情况后航迹重归化效率低下且容易受威胁区形状复杂度影响的问题。
Description
技术领域
本发明属于无人机领域，涉及航迹重规划方法，具体是一种基于改进弹性带算法的三维直升机航迹重规划方法。
背景技术
随着现代科技的发展，特别是无人机技术的发展与应用，仅仅依靠人工操作驾驶飞行器已经不能满足人们的需求，无人机航迹规划技术应运而生。无人机航迹规划就是在综合考虑无人机到达时间、油耗、威胁以及飞行区域等因素的前提下，为无人机规划出最优或满意的飞行航迹，以保证圆满地完成飞行任务。全局航迹规划算法可以利用计算机技术对数字地图进行分析处理，寻找到适合无人机飞行的最优航迹，包含A*算法，Dijkstra算法等等。在飞行器按照全局航迹飞行过程中，周围的环境或飞行器自身情况往往会发生变化，例如飞行中发现了新的需要回避的威胁源，这就需要对全局航迹进行实时重规划。然而，全局航迹规划算法效率较低，计算时间长，无法满足在飞行器遇到突发威胁源后能够迅速规划出一条新航迹的要求，现有航迹实时重规划算法容易受到威胁区形状等因素影响导致无法得到避开威胁区域的最优重规划航迹。
发明内容
针对现有技术存在的不足，本发明的目的在于，提供一种基于改进弹性带算法的三维直升机航迹重规划方法，解决现有技术中障碍物形状单一化导致的航迹重规划路径的技术问题。
为了解决上述技术问题，本发明采用如下技术方案予以实现：
一种基于改进弹性带算法的三维直升机航迹重规划方法，具体包括以下步骤：
步骤一，提取全局路径规划得到的全局航迹点、用于路径规划的高程数字地图以及处理高程数字地图得到的灰度图，结合直升机雷达的实时威胁区探测结果，在高程数字地图中添加相对应数量的任意形状的威胁区并记录每个威胁区高度；
步骤二，判断添加的威胁区数量是否大于1，如果等于1，则进入步骤三；如果大于1，则对所有威胁区进行合并处理，如果合并处理得到的威胁区数量为1，则将合并得到的威胁区作为当前威胁区，进入步骤三；如果数量大于1，则将该n个威胁区分别视作单个威胁区，提取威胁区xj中所有点构成集合Aj，将重规划之前的原航迹设为H(0)，j＝1、2、…、n；对于每个威胁区xj作为当前威胁区，执行步骤三到十，得到重规划航迹H(j)，并将H(j)对上一个重规划航迹H(j-1)更新，直至得到最终的重规划航迹H(n)。
步骤三，对当前威胁区提取威胁区覆盖点构成集合A，所述威胁区覆盖点包括威胁区内的点以及威胁区边上的点；
步骤四，判断全局航迹点中是否存在与威胁区覆盖点重叠的点，如果有则执行步骤五，否则，重规划结束，不对原航迹进行修改；
步骤五，选取全局航迹点与威胁区覆盖点重叠的点中最先到达的航迹点P1与最后到达的航迹点P2；将全局航迹点中由P1的前一个航迹点开始的第三个航迹点设置为航迹重规划的起点P0，在全局航迹点中由P2的后一个点开始的第三个航迹点设置为航迹重规划的终点P3；
步骤六，将P0与P3连接得到直线L03，判断直线L03是否与当前威胁区相交，若相交则继续步骤七，若不相交则以直线L03作为P0与P3之间的航迹，执行步骤十；
步骤七，提取当前威胁区的边界点，构成集合B；将直线L03作为当前直线，以点P0为当前起点、点P3为当前终点；执行步骤八；
步骤八，执行改进弹性带算法，得到位于当前直线上方以P0为起点、以P3为终点且不与对应的威胁区相交的路径，该路径包含的航迹点构成集合Q1；同理，执行改进弹性带算法，得到位于直线L03下方以P0为起点、以P3为终点且不与对应的威胁区相交的路径，该路径包含的航迹点构成集合Q2；
步骤九，分别计算步骤八中得到集合Q1、Q2对应的两条航迹的距离代价、灰度代价，并结合距离权重与灰度权重计算得到该路径的总代价；然后对两条航迹总代价进行比较，选择总代价较小的航迹作为P0与P3之间的航迹；
步骤十，将步骤六或步骤九得到的P0与P3之间的航迹与点P0之前、点P3之后的其他全局航迹点连接，得到重规划航迹。
进一步的，所述步骤六中，判断直线L03是否与当前威胁区相交的方法为：利用直线插补算法在给定起点和终点之间补充中间航迹点，然后判断起点与终点之间的航迹点与对应威胁区覆盖的点是否有交集，如果有交集则说明直线L03与对应的威胁区相交。
进一步的，所述步骤六中，判断直线L03是否与当前威胁区相交的方法具体包括以下子步骤：
步骤6.1，分别计算与直线L03重合的以P0为起点、P3为终点的向量在x轴方向的分量/>与y轴方向的分量/>
步骤6.2，判断向量的模/>与向量/>的模/>的大小，如果/>执行步骤6.3，如果/>执行步骤6.4；
步骤6.3，将步骤一中的高程数字地图中两个距离最近的像素点之间的距离作为单位长度，对向量进行分段，保证分段后的向量/>中每一段在x轴的分量上的模的长度为单位长度；然后将分段后的每一个分段点的y坐标四舍五入得到一个对应的航迹点，从而得到以P0为起点、P3为终点且每个航迹点均与添加威胁区后的高程数字地图中像素点重合的航迹，执行步骤6.5；
步骤6.4，将步骤一中的高程数字地图中两个相距距离最近的像素点之间的距离为单位长度，对向量进行分段，保证分段后的向量/>中每一段在y轴的分量上的模的长度为单位长度；然后将分段后的每一个分段点的x坐标做四舍五入处理得到一个对应的航迹点，从而得到以P0为起点、P3为终点且每个航迹点均与添加威胁区后的高程数字地图中像素点重合的航迹，执行步骤6.5；
步骤6.5，判断得到的航迹中包含的航迹点与步骤三得到的威胁区覆盖点构成的集合A是否有交集，如果有交集则说明直线L03与集合A对应的威胁区相交。
进一步的，所述步骤七中，具体采用边界提取算法提取威胁区的边界点构成集合B，包括如下操作：
若包含当前威胁区的高程数字地图中的某一点不在威胁区内，同时该点的八邻域与威胁区相交，则说明该点是威胁区的边界点，按照这种方式对包含当前威胁区的高程数字地图中所有点进行判断，得到威胁区的边界点构成集合B。
进一步的，所述步骤八中，所述执行改进弹性带算法，得到位于当前直线上方以P0为起点、以P3为终点且不与对应的威胁区相交的路径的方法具体包含以下子步骤：
步骤8.1，连接当前起点P0(i)与当前终点P3(i)得到直线L(i)，对当前威胁区边界点集合B内的点进行分组，得到位于直线L(i)上方的边界点构成集合B1(i)，将集合B1(i)作为当前威胁区边界点集合；计算集合B1(i)中每一个点距离当前直线L(i)的距离，得到在集合B1(i)中距离当前直线L(i)最远的点K(i)，进入步骤8.2；
步骤8.2，判断当前起点P0(i)与点K(i)之间连线是否与对应威胁区相交，若相交则以当前起点P0(i)为当前起点、以点K(i)为当前终点，调用递归函数Function8，得到当前起点P0(i)与点K(i)之间的航迹后，进入步骤8.3；若不相交则以当前起点P0(i)与点K(i)之间的连线作为两点之间的航迹，进入步骤8.3；
调用递归函数模块Function8即为执行步骤八；在递归函数Function8中，i为调用函数Function8的次数；
步骤8.3，判断点K(i)与当前终点P3(i)之间连线是否与对应威胁区相交，若相交则以点K(i)为当前起点，以当前终点P3(i)为当前终点，调用函数Function8，得到点K(i)与当前终点P3(i)之间的航迹后，进入步骤8.4，若不相交则以点K(i)与当前终点P3(i)之间的连线作为两点之间的航迹，进入步骤8.4；
步骤8.4，将步骤8.2与步骤8.3得到的每一段航迹按顺序首尾相连，即得到当前起点P0(i)与当前终点P3(i)之间的与对应威胁区不相交的航迹。
执行函数Function8的递归调用结束后，即可得到点P0与点P3之间的位于直线L03上方且与对应威胁区不相交的航迹；
进一步的，所述步骤8.1中，所述对当前威胁区边界点集合B中的点进行分组，得到位于直线L(i)上方的边界点构成集合B1(i)中，分组的方法包括如下操作：
已知当前起点P0(i)与当前终点P3(i)的坐标值，求得经过当前起点P0(i)与当前终点P3(i)的直线方程：
Ax+By+C＝0
A＝y2-y1
B＝x1-x2
C＝y1·x2-x1·y2
其中：
x1,y1表示当前起点的x坐标与y坐标；
x2,y2表示当前终点的x坐标与y坐标；
将集合B中某一点的坐标值代入直线方程Ax+By+C＝0，若Ax+By+C<0则说明该点在当前直线上方，将其归入集合B1(i)。
进一步的，所述步骤8.1中，所述计算集合B1(i)中每一个点距离当前直线L(i)的距离的计算公式如下：
其中：
x1,y1表示当前直线L(i)的起点的x坐标与y坐标；
x2,y2表示当前直线L(i)的终点的x坐标与y坐标；
x,y表示集合B1(i)中某一点的x坐标与y坐标；
det(A)表示求矩阵A的行列式。
进一步的，所述步骤九具体包括以下子步骤：
步骤9.1，采用下述公式分别计算步骤八得到的两条航迹所经过的距离代价；
其中：
d表示当前航迹的距离代价；
xi,yi表示当前航迹上第i个航迹点的坐标；
n表示最大航迹点数量，该参数的值等于步骤八中得到的当前航迹(即路径)的航迹点数量；
步骤9.2，利用直线插补算法对两条航迹轨迹分别进行补充，得到补充后航迹，补充后的航迹构成集合Q1’和集合Q2’；直线插补算法参见前文6.1-6.4的相同的做法。
步骤9.3，结合由步骤一中提取得到的灰度图与步骤9.2得到的插补后的航迹轨迹分别计算两条航迹的灰度代价，计算方法如下：
将集合Q1’中包含的所有航迹点在灰度图中所对应的灰度值相加得到位于直线L03上方的补充后的航迹的灰度代价，同理，将集合Q2’中包含的所有航迹点在灰度图中所对应的灰度值相加得到位于直线L03下方补充后的航迹的灰度代价。
步骤9.4，采用下述公式分别计算两条航迹的总代价；
v＝d·k+h·p
其中：
d表示航迹的距离代价；
k表示距离权重系数；
h表示航迹的灰度代价；
p表示灰度权重系数；
距离权重系数和灰度权重系数之和为1；
步骤9.5，选择总代价较小的航迹作为P0与P3之间的航迹。
相较于现有技术，本发明具有如下技术效果：
1、本发明的步骤二中给出了对于多个威胁区的处理方法，解决了常规重规划算法只能避开单一威胁区的问题。
2、本发明的步骤八中给出的改进弹性带算法，通过寻找威胁区边界点中距离重规划当前起点与当前终点间连线最远的点，作为重规划航迹中间点的方法，适应性强，安全可靠，解决了常规重规划算法容易受到威胁区形状、大小等因素影响而无法得到重规划航迹的问题。
同时，步骤八的改进弹性带算法也是本发明核心算法，其空间复杂度和时间复杂度都较低，计算速度非常快，能够满足直升机在飞行过程中遇到突发威胁区后能够迅速进行路径重规划的需求，同时，使用该方法进行路径重规划，计算所耗费的时间受威胁区形状复杂程度影响较小。
3、本发明的步骤九中给出的结合距离权重与灰度权重选择最优航迹的方法，能够按照飞行要求选择飞行路径较短或者飞行过程更平稳的航迹，解决了常规重规划算法仅仅只能给出一条可行重规划航迹，不能对重规划航迹做具体要求的问题。
附图说明
图1是本发明的方法的步骤6的原理图，其中，(a)为步骤6.3的情况，(b)为步骤6.4的情况。
图2是关于函数Function8递归调用寻找威胁区上方航迹的一个实例的示意图。
图3是关于函数Function8递归调用寻找威胁区上方航迹的另一个实例的示意图。
图4是航迹重规划测试的示意图。
图5是检测到四个不同形状的威胁区时，飞机执行航迹重规矩后得到的航迹。
具体实施方式
如图1所示，本发明给出的基于改进弹性带算法的三维直升机航迹重规划方法，具体包括以下步骤：
步骤一，提取由A*算法进行全局路径规划得到的全局航迹点、用于路径规划的高程数字地图以及在A*算法中处理高程数字地图得到的灰度图(该处理高程数字地图得到灰度图的方法可以采用中国专利申请(申请号为202111615448.0，名称：一种基于地形梯度二值化的直升机航迹规划地图处理方法，其中灰度值代表该点最大坡度值)，结合来源于直升机雷达的实时威胁区探测结果，在高程数字地图中添加相对应数量的任意形状的威胁区并记录每个威胁区高度；
步骤二，判断添加的威胁区数量是否大于1，如果等于1，则进入步骤三；如果大于1，则对所有威胁区进行合并处理(即分别判断每两个威胁区之间是否有交集，如果有交集，则将有交集的两个威胁区的并集视作一个威胁区)，如果合并处理得到的威胁区数量为1，则将合并得到的威胁区作为当前威胁区，进入步骤三；如果数量大于1，则将该n个威胁区分别视作单个威胁区，提取威胁区xj(j＝1、2、…、n)中所有点构成集合Aj，将重规划之前的原航迹设为H(0)；对于每个威胁区xj作为当前威胁区，执行步骤三到十，得到重规划航迹H(j)，并将H(j)对上一个重规划航迹H(j-1)更新，直至得到最终的重规划航迹H(n)。
步骤三，对当前威胁区提取威胁区覆盖点(威胁区覆盖点包括威胁区内的点以及威胁区边上的点)，构成集合A；
步骤四，判断全局航迹点中是否存在与威胁区覆盖点重叠的点，如果有则执行步骤五，否则，重规划结束，不对原航迹进行修改；
步骤五，选取全局航迹点与威胁区覆盖点重叠的点中最先到达的航迹点P1与最后到达的航迹点P2；将全局航迹点中由P1的前一个航迹点开始的第三个航迹点设置为航迹重规划的起点P0，在全局航迹点中由P2的后一个点开始的第三个航迹点设置为航迹重规划的终点P3；
步骤六，将P0与P3连接得到直线L03，判断直线L03是否与当前威胁区相交，若相交则继续步骤七，若不相交则以直线L03作为P0与P3之间的航迹，执行步骤十；
步骤七，提取当前威胁区的边界点，构成集合B；将直线L03作为当前直线，以点P0为当前起点、点P3为当前终点；执行步骤八；
步骤八，执行改进弹性带算法，得到位于当前直线上方以P0为起点、以P3为终点且不与对应的威胁区相交的路径，该路径包含的航迹点构成集合Q1；同理，执行改进弹性带算法，得到位于直线L03下方以P0为起点、以P3为终点且不与对应的威胁区相交的路径，该路径包含的航迹点构成集合Q2；
步骤九，分别计算步骤八中得到集合Q1、Q2对应的两条航迹的距离代价、灰度代价，并结合距离权重与灰度权重计算得到该路径的总代价；然后对两条航迹总代价进行比较，选择总代价较小的航迹作为P0与P3之间的航迹；
步骤十，将步骤六或步骤九得到的P0与P3之间的航迹与点P0之前、点P3之后的其他全局航迹点连接，得到重规划航迹。
作为本发明的优选实现方式，步骤六中，判断直线L03是否与当前威胁区相交的方法为：利用直线插补算法在给定起点和终点之间补充中间航迹点，然后判断起点与终点之间的航迹点与对应威胁区覆盖的点是否有交集，如果有交集则说明直线L03与对应的威胁区相交，具体操作包括以下子步骤：
步骤6.1，分别计算与直线L03重合的以P0为起点、P3为终点的向量在x轴方向的分量/>与y轴方向的分量/>
步骤6.2，判断向量的模/>与向量/>的模/>的大小，如果执行步骤6.3，如果/>执行步骤6.4；
步骤6.3，将步骤一中的高程数字地图中两个距离最近的像素点(即在x轴方向或y轴方向相邻的两个像素点)之间的距离作为单位长度，对向量进行分段，保证分段后的向量/>中每一段在x轴的分量上的模的长度为单位长度，如图1(a)所示；然后将分段后的每一个分段点的y坐标四舍五入得到一个对应的航迹点，从而得到以P0为起点、P3为终点且每个航迹点均与添加威胁区后的高程数字地图中像素点重合的航迹，如图1(a)中虚线航迹线所示，执行步骤6.5；
步骤6.4，将步骤一中的高程数字地图中两个相距距离最近的像素点之间的距离为单位长度，对向量进行分段，保证分段后的向量/>中每一段在y轴的分量上的模的长度为单位长度，如图1(b)所示；然后将分段后的每一个分段点的x坐标做四舍五入处理得到一个对应的航迹点，从而得到以P0为起点、P3为终点且每个航迹点均与添加威胁区后的高程数字地图中像素点重合的航迹，如图1(b)中虚线航迹线所示，执行步骤6.5；
步骤6.5，判断得到的航迹中包含的航迹点与步骤三得到的威胁区覆盖点构成的集合A是否有交集，如果有交集则说明直线L03与集合A对应的威胁区相交。
优选的，步骤七中，具体采用边界提取算法提取威胁区的边界点(威胁区边界点是指高程数字地图上位于当前威胁区一周的像素点)构成集合B，包括如下操作：
若包含当前威胁区的高程数字地图中的某一点不在威胁区内，同时该点的八邻域与威胁区相交，则说明该点是威胁区的边界点，按照这种方式对包含当前威胁区的高程数字地图中所有点进行判断，得到威胁区的边界点构成集合B。
步骤八中，所述执行改进弹性带算法，得到位于当前直线上方以P0为起点、以P3为终点且不与对应的威胁区相交的路径的思路是：如果起点和终点之间的连线与威胁区相交，找到威胁区的边界点构成的集合B中距离起点和终点连线最远的点，然后依次连接这三个点(即起点、最远点、终点)作为航迹，如果该航迹与威胁区相交，则执行递归算法，直至找到一条与威胁区不相交的航迹作为最终航迹，具体包含以下子步骤：
由于计算位于当前直线上方以及下方航迹的算法相类似，此处以计算位于当前直线上方的航迹为例。
将步骤八看做一递归函数模块Function8，在Function8中的所有参数中“()”内的值为调用函数Function8的次数，如第i次调用函数Function8中，当前起点P0(i)，当前终点P3(i)，连接两点得到直线L(i)，位于直线L(i)上方的边界点构成集合B1(i)，计算得到在集合B1(i)中距离直线L(i)最远的点K(i)。
步骤8.1，连接当前起点P0(i)与当前终点P3(i)得到直线L(i)，对当前威胁区边界点集合B内的点进行分组，得到位于直线L(i)上方的边界点构成集合B1(i)，将集合B1(i)作为当前威胁区边界点集合；计算集合B1(i)中每一个点距离当前直线L(i)的距离，得到在集合B1(i)中距离当前直线L(i)最远的点K(i)，进入步骤8.2；
步骤8.2，判断当前起点P0(i)与点K(i)之间连线是否与对应威胁区相交，若相交则以当前起点P0(i)为当前起点、以点K(i)为当前终点，调用递归函数Function8，得到当前起点P0(i)与点K(i)之间的航迹后，进入步骤8.3；若不相交则以当前起点P0(i)与点K(i)之间的连线作为两点之间的航迹，进入步骤8.3；
步骤8.3，判断点K(i)与当前终点P3(i)之间连线是否与对应威胁区相交，若相交则以点K(i)为当前起点，以当前终点P3(i)为当前终点，调用函数Function8，得到点K(i)与当前终点P3(i)之间的航迹后，进入步骤8.4，若不相交则以点K(i)与当前终点P3(i)之间的连线作为两点之间的航迹，进入步骤8.4；
步骤8.4，将步骤8.2与步骤8.3得到的每一段航迹按顺序首尾相连，即得到当前起点P0(i)与当前终点P3(i)之间的与对应威胁区不相交的航迹。
执行函数Function8的递归调用结束后，即可得到点P0与点P3(注意：点P0等价于点P0(1)，点P3等价于点P3(1))之间的位于直线L03上方且与对应威胁区不相交的航迹。
同理，可得到点P0与点P3之间的位于直线L下方且与对应威胁区不相交的航迹。
为了便于读者理解，下文给出一个关于函数Function8递归调用寻找威胁区上方航迹的实例，如图2所示，其中实线是第一次调用Function8时的航迹，其中点P0(1)与点K(1)之间连线与威胁区相交，因此在第一次调用Function8的步骤6.2中把P0(1)作为起点，把K(1)作为终点第二次调用Function8，此次调用中得到的点P1(2)与点P3(2)之间的航迹如图2中虚线所示。另外一个更复杂的实例如图3所示。
优选的，步骤8.1中，对当前威胁区边界点集合B中的点进行分组，得到位于直线L(i)上方的边界点构成集合B1(i)，分组的方法包含如下操作：
已知当前起点P0(i)与当前终点P3(i)的坐标值，求得经过当前起点P0(i)与当前终点P3(i)的直线方程：
Ax+By+C＝0
A＝y2-y1
B＝x1-x2
C＝y1·x2-x1·y2
其中：
x1,y1表示当前起点的x坐标与y坐标；
x2,y2表示当前终点的x坐标与y坐标；
将集合B中某一点的坐标值代入直线方程Ax+By+C＝0，若Ax+By+C<0则说明该点在当前直线上方，将其归入集合B1(i)。(在求位于直线下方航迹过程中，若Ax+By+C>0则说明该点在当前直线下方，将其归入集合B2(i))
优选的，步骤8.1中计算集合B1(i)中每一个点距离当前直线距离的计算公式如下：
其中：
x1,y1表示当前直线L(i)的起点(即点P0(i))的x坐标与y坐标；
x2,y2表示当前直线L(i)的终点(即点P3(i))的x坐标与y坐标；
x,y表示集合B1(i)中某一点的x坐标与y坐标；
det(A)表示求矩阵A的行列式。
优选的，步骤九具体包括以下子步骤：
步骤9.1，采用下述公式分别计算步骤八得到的两条航迹所经过的距离代价；
其中：
d表示当前航迹的距离代价；
xi,yi表示当前航迹上第i个航迹点的坐标；
n表示最大航迹点数量，该参数的值等于步骤八中得到的当前航迹(即路径)的航迹点数量；
步骤9.2，利用直线插补算法对两条航迹轨迹分别进行补充，得到补充后航迹，补充后的航迹构成集合Q1’和集合Q2’；直线插补算法参见前文6.1-6.4的相同的做法。
步骤9.3，结合由步骤一中提取得到的灰度图与步骤9.2得到的插补后的航迹轨迹分别计算两条航迹的灰度代价，计算方法如下：
将集合Q1’中包含的所有航迹点在灰度图中所对应的灰度值(即该点最大坡度值)相加得到位于直线L03上方的补充后的航迹的灰度代价，同理，将集合Q2’中包含的所有航迹点在灰度图中所对应的灰度值(即该点最大坡度值)相加得到位于直线L03下方补充后的航迹的灰度代价。
步骤9.4，采用下述公式分别计算两条航迹的总代价；
v＝d·k+h·p
其中：
d表示航迹的距离代价；
k表示距离权重系数；
h表示航迹的灰度代价；
p表示灰度权重系数；
距离权重系数和灰度权重系数之和为1；距离权重系数与灰度权重系数按照飞行要求给出，即希望飞行路程最短则选择较大的距离权重系数，希望飞行过程平稳则选择较大的灰度权重系数。
步骤9.5，选择总代价较小的航迹作为P0与P3之间的航迹。
为了证明本发明的可行性和有效性，以下给出本发明的实测案例，需要说明的是本发明并不局限于以下具体实施例，凡在本申请技术方案基础上做的等同变换均落入本发明的保护范围。
实测案例
采用上述技术方案，在西藏地区进行航迹重规划测试，首先利用A*算法得到起点与终点之间的航迹，如图4所示，地图参数和仿真数据如表1所示。然后，假设在飞行过程中检测到四个不同形状的威胁区(三角形、矩形、圆形、凹多边形)飞机在执行航迹重规矩后得到的航迹如图5所示，四次执行航迹重规划算法所使用的时间如表2所示。
表1西藏地区地图信息及仿真数据
| 地图区域 | 西藏地区地图 |
| 地图起始纬度 | 27.7771 |
| 地图起始经度 | 91.6967 |
| 地图终止纬度 | 28.1670 |
| 地图终止经度 | 92.1496 |
| 一经度或者一纬度的栅格数 | 3600 |
| 航迹规划起点纬度 | 27.8155 |
| 航迹规划起点经度 | 91.9350 |
| 航迹规划终点纬度 | 28.1205 |
| 航迹规划终点经度 | 91.9784 |
| 航迹规划路径长度 | 39003.627m |
| 航迹重规划后路径长度 | 44680.67m |
表2重规划用时信息
| 威胁区形状 | 重规划用时 |
| 三角形 | 0.013548s |
| 矩形 | 0.017569s |
| 圆形 | 0.1128s |
| 凹多边形 | 0.019247s |
| 总和 | 0.163164s |
该实测案例说明，本发明提出的基于改进弹性带算法的三维直升机航迹重规划方法适用于对多个形状各异的威胁区进行路径重规划，且重规划算法所使用时间非常短，用时最长的圆形威胁区也仅仅耗时0.1s左右，其他形状威胁区耗时均在0.02s内(见表2)，能够满足直升机在飞行过程中遇到突发威胁区后能够迅速完成路径重规划的需求。第四个威胁区的形状采用常规重规划算法无法解决的凹多边形，由此说明，本发明提出的路径重规划算法不受威胁区形状限制。并且在对于复杂多边形的处理中耗时并未显著增加，说明本发明提出的路径重规划算法耗时受到威胁区形状复杂程度的影响较小，适用于对各种复杂形状威胁区进行路径重规划。
Claims (8)
1.一种基于改进弹性带算法的三维直升机航迹重规划方法，其特征在于，具体包括以下步骤：
步骤一，提取全局路径规划得到的全局航迹点、用于路径规划的高程数字地图以及处理高程数字地图得到的灰度图，结合直升机雷达的实时威胁区探测结果，在高程数字地图中添加相对应数量的任意形状的威胁区并记录每个威胁区高度；
步骤二，判断添加的威胁区数量是否大于1，如果等于1，则进入步骤三；如果大于1，则对所有威胁区进行合并处理，如果合并处理得到的威胁区数量为1，则将合并得到的威胁区作为当前威胁区，进入步骤三；如果数量大于1，则将该n个威胁区分别视作单个威胁区，提取威胁区xj中所有点构成集合Aj，将重规划之前的原航迹设为H(0)，j＝1、2、…、n；对于每个威胁区xj作为当前威胁区，执行步骤三到十，得到重规划航迹H(j)，并将H(j)对上一个重规划航迹H(j-1)更新，直至得到最终的重规划航迹H(n)；
步骤三，对当前威胁区提取威胁区覆盖点构成集合A，所述威胁区覆盖点包括威胁区内的点以及威胁区边上的点；
步骤四，判断全局航迹点中是否存在与威胁区覆盖点重叠的点，如果有则执行步骤五，否则，重规划结束，不对原航迹进行修改；
步骤五，选取全局航迹点与威胁区覆盖点重叠的点中最先到达的航迹点P1与最后到达的航迹点P2；将全局航迹点中由P1的前一个航迹点开始的第三个航迹点设置为航迹重规划的起点P0，在全局航迹点中由P2的后一个点开始的第三个航迹点设置为航迹重规划的终点P3；
步骤六，将P0与P3连接得到直线L03，判断直线L03是否与当前威胁区相交，若相交则继续步骤七，若不相交则以直线L03作为P0与P3之间的航迹，执行步骤十；
步骤七，提取当前威胁区的边界点，构成集合B；将直线L03作为当前直线，以点P0为当前起点、点P3为当前终点；执行步骤八；
步骤八，执行改进弹性带算法，得到位于当前直线上方以P0为起点、以P3为终点且不与对应的威胁区相交的路径，该路径包含的航迹点构成集合Q1；同理，执行改进弹性带算法，得到位于直线L03下方以P0为起点、以P3为终点且不与对应的威胁区相交的路径，该路径包含的航迹点构成集合Q2；
步骤九，分别计算步骤八中得到集合Q1、Q2对应的两条航迹的距离代价、灰度代价，并结合距离权重与灰度权重计算得到该路径的总代价；然后对两条航迹总代价进行比较，选择总代价较小的航迹作为P0与P3之间的航迹；
步骤十，将步骤六或步骤九得到的P0与P3之间的航迹与点P0之前、点P3之后的其他全局航迹点连接，得到重规划航迹。
2.如权利要求1所述的基于改进弹性带算法的三维直升机航迹重规划方法，其特征在于，所述步骤六中，判断直线L03是否与当前威胁区相交的方法为：利用直线插补算法在给定起点和终点之间补充中间航迹点，然后判断起点与终点之间的航迹点与对应威胁区覆盖的点是否有交集，如果有交集则说明直线L03与对应的威胁区相交。
3.如权利要求1所述的基于改进弹性带算法的三维直升机航迹重规划方法，其特征在于，所述步骤六中，判断直线L03是否与当前威胁区相交的方法具体包括以下子步骤：
步骤6.1，分别计算与直线L03重合的以P0为起点、P3为终点的向量在x轴方向的分量/>与y轴方向的分量/>
步骤6.2，判断向量的模/>与向量/>的模/>的大小，如果/>执行步骤6.3，如果/>执行步骤6.4；
步骤6.3，将步骤一中的高程数字地图中两个距离最近的像素点之间的距离作为单位长度，对向量进行分段，保证分段后的向量/>中每一段在x轴的分量上的模的长度为单位长度；然后将分段后的每一个分段点的y坐标四舍五入得到一个对应的航迹点，从而得到以P0为起点、P3为终点且每个航迹点均与添加威胁区后的高程数字地图中像素点重合的航迹，执行步骤6.5；
步骤6.4，将步骤一中的高程数字地图中两个相距距离最近的像素点之间的距离为单位长度，对向量进行分段，保证分段后的向量/>中每一段在y轴的分量上的模的长度为单位长度；然后将分段后的每一个分段点的x坐标做四舍五入处理得到一个对应的航迹点，从而得到以P0为起点、P3为终点且每个航迹点均与添加威胁区后的高程数字地图中像素点重合的航迹，执行步骤6.5；
步骤6.5，判断得到的航迹中包含的航迹点与步骤三得到的威胁区覆盖点构成的集合A是否有交集，如果有交集则说明直线L03与集合A对应的威胁区相交。
4.如权利要求1所述的基于改进弹性带算法的三维直升机航迹重规划方法，其特征在于，所述步骤七中，具体采用边界提取算法提取威胁区的边界点构成集合B，包括如下操作：
若包含当前威胁区的高程数字地图中的某一点不在威胁区内，同时该点的八邻域与威胁区相交，则说明该点是威胁区的边界点，按照这种方式对包含当前威胁区的高程数字地图中所有点进行判断，得到威胁区的边界点构成集合B。
5.如权利要求1所述的基于改进弹性带算法的三维直升机航迹重规划方法，其特征在于，所述步骤八中，所述执行改进弹性带算法，得到位于当前直线上方以P0为起点、以P3为终点且不与对应的威胁区相交的路径的方法具体包含以下子步骤：
步骤8.1，连接当前起点P0(i)与当前终点P3(i)得到直线L(i)，对当前威胁区边界点集合B内的点进行分组，得到位于直线L(i)上方的边界点构成集合B1(i)，将集合B1(i)作为当前威胁区边界点集合；计算集合B1(i)中每一个点距离当前直线L(i)的距离，得到在集合B1(i)中距离当前直线L(i)最远的点K(i)，进入步骤8.2；
步骤8.2，判断当前起点P0(i)与点K(i)之间连线是否与对应威胁区相交，若相交则以当前起点P0(i)为当前起点、以点K(i)为当前终点，调用递归函数Function8，得到当前起点P0(i)与点K(i)之间的航迹后，进入步骤8.3；若不相交则以当前起点P0(i)与点K(i)之间的连线作为两点之间的航迹，进入步骤8.3；
调用递归函数模块Function8即为执行步骤八；在递归函数Function8中，i为调用函数Function8的次数；
步骤8.3，判断点K(i)与当前终点P3(i)之间连线是否与对应威胁区相交，若相交则以点K(i)为当前起点，以当前终点P3(i)为当前终点，调用函数Function8，得到点K(i)与当前终点P3(i)之间的航迹后，进入步骤8.4，若不相交则以点K(i)与当前终点P3(i)之间的连线作为两点之间的航迹，进入步骤8.4；
步骤8.4，将步骤8.2与步骤8.3得到的每一段航迹按顺序首尾相连，即得到当前起点P0(i)与当前终点P3(i)之间的与对应威胁区不相交的航迹；
执行函数Function8的递归调用结束后，即可得到点P0与点P3之间的位于直线L03上方且与对应威胁区不相交的航迹。
6.如权利要求5所述的基于改进弹性带算法的三维直升机航迹重规划方法，其特征在于，所述步骤8.1中，所述对当前威胁区边界点集合B中的点进行分组，得到位于直线L(i)上方的边界点构成集合B1(i)中，分组的方法包括如下操作：
已知当前起点P0(i)与当前终点P3(i)的坐标值，求得经过当前起点P0(i)与当前终点P3(i)的直线方程：
Ax+By+C＝0
A＝y2-y1
B＝x1-x2
C＝y1·x2-x1·y2
其中：
x1,y1表示当前起点的x坐标与y坐标；
x2,y2表示当前终点的x坐标与y坐标；
将集合B中某一点的坐标值代入直线方程Ax+By+C＝0，若Ax+By+C<0则说明该点在当前直线上方，将其归入集合B1(i)。
7.如权利要求5所述的基于改进弹性带算法的三维直升机航迹重规划方法，其特征在于，所述步骤8.1中，所述计算集合B1(i)中每一个点距离当前直线L(i)的距离的计算公式如下：
其中：
x1,y1表示当前直线L(i)的起点的x坐标与y坐标；
x2,y2表示当前直线L(i)的终点的x坐标与y坐标；
x,y表示集合B1(i)中某一点的x坐标与y坐标；
det(A)表示求矩阵A的行列式。
8.如权利要求5所述的基于改进弹性带算法的三维直升机航迹重规划方法，其特征在于，所述步骤九具体包括以下子步骤：
步骤9.1，采用下述公式分别计算步骤八得到的两条航迹所经过的距离代价；
其中：
d表示当前航迹的距离代价；
xi,yi表示当前航迹上第i个航迹点的坐标；
n表示最大航迹点数量，该参数的值等于步骤八中得到的当前航迹(即路径)的航迹点数量；
步骤9.2，利用直线插补算法对两条航迹轨迹分别进行补充，得到补充后航迹，补充后的航迹构成集合Q1’和集合Q2’；直线插补算法参见前文6.1-6.4的相同的做法；
步骤9.3，结合由步骤一中提取得到的灰度图与步骤9.2得到的插补后的航迹轨迹分别计算两条航迹的灰度代价，计算方法如下：
将集合Q1’中包含的所有航迹点在灰度图中所对应的灰度值相加得到位于直线L03上方的补充后的航迹的灰度代价，同理，将集合Q2’中包含的所有航迹点在灰度图中所对应的灰度值相加得到位于直线L03下方补充后的航迹的灰度代价；
步骤9.4，采用下述公式分别计算两条航迹的总代价；
v＝d·k+h·p
其中：
d表示航迹的距离代价；
k表示距离权重系数；
h表示航迹的灰度代价；
p表示灰度权重系数；
距离权重系数和灰度权重系数之和为1；
步骤9.5，选择总代价较小的航迹作为P0与P3之间的航迹。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210692004.5A CN115129079B (zh) | 2022-06-17 | 2022-06-17 | 一种基于改进弹性带算法的三维直升机航迹重规划方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210692004.5A CN115129079B (zh) | 2022-06-17 | 2022-06-17 | 一种基于改进弹性带算法的三维直升机航迹重规划方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115129079A CN115129079A (zh) | 2022-09-30 |
| CN115129079B true CN115129079B (zh) | 2024-05-03 |
Family
ID=83377810
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210692004.5A Active CN115129079B (zh) | 2022-06-17 | 2022-06-17 | 一种基于改进弹性带算法的三维直升机航迹重规划方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115129079B (zh) |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107392388A (zh) * | 2017-07-31 | 2017-11-24 | 南昌航空大学 | 一种采用改进人工鱼群算法规划无人机三维航迹的方法 |
| WO2017219639A1 (zh) * | 2016-06-20 | 2017-12-28 | 广州视源电子科技股份有限公司 | 一种机械臂的运动轨迹规划方法、装置及机器人 |
-
2022
- 2022-06-17 CN CN202210692004.5A patent/CN115129079B/zh active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2017219639A1 (zh) * | 2016-06-20 | 2017-12-28 | 广州视源电子科技股份有限公司 | 一种机械臂的运动轨迹规划方法、装置及机器人 |
| CN107392388A (zh) * | 2017-07-31 | 2017-11-24 | 南昌航空大学 | 一种采用改进人工鱼群算法规划无人机三维航迹的方法 |
Non-Patent Citations (2)
| Title |
|---|
| 基于改进蚁群算法的无人驾驶飞行器三维航迹规划与重规划;唐必伟;朱战霞;方群;陈攀峰;;西北工业大学学报;20131215(第06期);全文 * |
| 无人机三维实时航迹规划;席剑锐;杨金孝;张博亮;陈志星;;计算机测量与控制;20160625(第06期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115129079A (zh) | 2022-09-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN102521884B (zh) | 一种基于LiDAR数据与正射影像的3维屋顶重建方法 | |
| CN110084195B (zh) | 基于卷积神经网络的遥感图像目标检测方法 | |
| CN108519094B (zh) | 局部路径规划方法及云处理端 | |
| US20210333108A1 (en) | Path Planning Method And Device And Mobile Device | |
| WO2017215622A1 (zh) | 物体分割方法及装置、计算设备 | |
| CN110264468A (zh) | 点云数据标注、分割模型确定、目标检测方法及相关设备 | |
| CN111292563A (zh) | 一种航班航迹预测方法 | |
| US20230042968A1 (en) | High-definition map creation method and device, and electronic device | |
| CN105206041A (zh) | 一种考虑时序dbscan的智能手机轨迹链簇识别方法 | |
| CN112329789B (zh) | 点云的提取方法、装置、计算机设备和存储介质 | |
| CN103838829A (zh) | 一种基于分层次边界拓扑搜索模型的栅格转矢量系统 | |
| CN109739227A (zh) | 一种行驶轨迹构建系统与方法 | |
| CN116543310B (zh) | 一种基于Voronoi图和核密度的道路线提取方法 | |
| CN113345094A (zh) | 基于三维点云的电力走廊安全距离分析方法及系统 | |
| CN115270693A (zh) | 基于动态网格的135度pcb区域布线方法 | |
| CN113971723B (zh) | 高精地图中三维地图的构建方法、装置、设备和存储介质 | |
| CN110489511B (zh) | 等高线接边高程错误修正方法、系统及电子设备和介质 | |
| CN109241369B (zh) | 基于网格延展法的降雨等值线构建方法 | |
| CN106156245A (zh) | 一种电子地图中的线要素合并方法及装置 | |
| Wu et al. | A non-rigid hierarchical discrete grid structure and its application to UAVs conflict detection and path planning | |
| CN115129079B (zh) | 一种基于改进弹性带算法的三维直升机航迹重规划方法 | |
| CN112083734A (zh) | 一种利用概率天气预报的集合飞行路径规划方法 | |
| CN116518979B (zh) | 一种无人机路径规划方法、系统、电子设备及介质 | |
| CN112562419A (zh) | 一种基于离线多目标跟踪的天气规避区划设方法 | |
| CN116434181A (zh) | 地面点检测方法、装置、电子设备和介质 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |