CN109254591A - 基于Anytime修复式稀疏A*与卡尔曼滤波的动态航迹规划方法 - Google Patents
基于Anytime修复式稀疏A*与卡尔曼滤波的动态航迹规划方法 Download PDFInfo
- Publication number
- CN109254591A CN109254591A CN201811078254.XA CN201811078254A CN109254591A CN 109254591 A CN109254591 A CN 109254591A CN 201811078254 A CN201811078254 A CN 201811078254A CN 109254591 A CN109254591 A CN 109254591A
- Authority
- CN
- China
- Prior art keywords
- node
- planning
- unmanned plane
- target
- algorithm
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 38
- 238000001914 filtration Methods 0.000 title claims abstract description 34
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 49
- 230000003044 adaptive effect Effects 0.000 claims abstract description 8
- 239000011159 matrix material Substances 0.000 claims description 16
- 230000008569 process Effects 0.000 claims description 10
- 230000008439 repair process Effects 0.000 claims description 9
- 230000000977 initiatory effect Effects 0.000 claims description 4
- 238000005259 measurement Methods 0.000 claims description 4
- 230000009897 systematic effect Effects 0.000 claims description 3
- 238000012546 transfer Methods 0.000 claims description 3
- 230000007423 decrease Effects 0.000 claims description 2
- 230000003252 repetitive effect Effects 0.000 claims description 2
- 230000008859 change Effects 0.000 abstract description 5
- 238000005096 rolling process Methods 0.000 abstract description 4
- 230000007613 environmental effect Effects 0.000 abstract description 3
- 238000005516 engineering process Methods 0.000 abstract description 2
- 238000004088 simulation Methods 0.000 description 7
- 230000008901 benefit Effects 0.000 description 3
- 238000012360 testing method Methods 0.000 description 3
- 241000209094 Oryza Species 0.000 description 2
- 235000007164 Oryza sativa Nutrition 0.000 description 2
- 230000033228 biological regulation Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 235000009566 rice Nutrition 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000002708 enhancing effect Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 238000004321 preservation Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000000007 visual effect 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
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)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Traffic Control Systems (AREA)
Abstract
本发明公开的基于Anytime修复式稀疏A*与卡尔曼滤波的动态航迹规划方法，属于无人机航迹规划技术领域。本发明实现方法为：首先需基于滚动规划策略，在目标观测周期T内，采用自适应扩展卡尔曼滤波算法对下一时刻任务环境中动态威胁及运动目标的位置进行估计，将估计结果作为无人机航迹规划的输入信息；然后，使用Anytime修复式稀疏A*算法进行无人机快速航迹规划，使得无人机能够提前感知环境变化，改变航路方向，并在有限时间内输出可行航迹，顺利规避动态威胁，准确抵达目标区域。本发明在无人机真实任务环境中存在动态威胁和运动目标的情况下，能够保证无人机规避全部威胁、缩短到达目标区域的时间。本发明具有求解效率高、鲁棒性强的特点。
Description
技术领域
本发明涉及一种基于修复式Anytime稀疏A*与卡尔曼滤波的动态航迹规划方法，属于无人机航迹规划技术领域。
背景技术
无人机真实作战环境中存着许多未知的、不确定的因素。例如，未知威胁突发的不确定性、动态威胁和目标运动参数的不确定性等，各不确定性因素之间相互作用、相互制约，使得无人机真实任务环境态势随时间的不断变化而呈现高度的动态性。因此，需要无人机对任务环境中动态因素的运动参数进行估计，通过预先估计，使无人机提前感知动态环境的变化，及时调整飞行航迹，规避任务环境中动态威胁，缩短到达目标区域的时间，提高无人机完成既定任务的效率。基于滤波技术的轨迹估计方法计算量较小，同时滤波技术得到的估计值能够使后续航迹规划具有超前性，具有较强的实用性，在工程上引用较为广泛，卡尔曼滤波算法及其相应衍生算法是其中的经典算法。
考虑无人机真实任务环境的动态性，无人机航迹规划系统需针对实时变化的环境及时调整无人机飞行航迹。然而，传统的航迹规划算法难以满足无人机在动态环境中的时效性和可靠性的要求，采用 Anytime算法架构与稀疏A*算法相结合的Anytime修复式稀疏A* (AR-SAS)算法能够有效解决上述具有时限约束的航迹规划问题。
因此，针对无人机任务环境中存在的动态威胁和运动目标等动态因素，开展无人机动态航迹规划方法的研究是十分必要且具有重要意义的。针对战场动态因素位置的不确定性，需要无人机提前对动态环境中的运动目标或威胁等进行估计，然后，无人机通过动态航迹规划适应复杂多变的任务需求，针对环境的变化及时调整无人机飞行航迹，以提升无人机系统整体的任务执行效能。
发明内容
针对无人机真实任务环境中存在的运动规律未知的动态威胁和运动目标，为保证无人机规避全部威胁、缩短到达目标区域的时间，增强无人机执行任务能力，本发明要解决的技术问题是：提供基于 Anytime修复式稀疏A*与卡尔曼滤波的动态航迹规划方法，在无人机真实任务环境中存在动态威胁和运动目标的情况下，能够保证无人机规避全部威胁、缩短到达目标区域的时间。本发明具有求解效率高、鲁棒性强的特点。
本发明的目的是通过下述技术方案实现的。
本发明公开的基于Anytime修复式稀疏A*与卡尔曼滤波的动态航迹规划方法，首先需基于滚动规划策略，在目标观测周期T内，采用自适应扩展卡尔曼滤波算法(AdaptiveExtended Kalman Filter, AEKF)对下一时刻任务环境中动态威胁及运动目标的位置进行估计，将估计结果作为无人机航迹规划的输入信息；然后，使用Anytime修复式稀疏A*算法(Anytime Repairing Sparse A*Algorithm,AR-SAS) 进行无人机快速航迹规划，使得无人机能够提前感知环境变化，改变航路方向，并在有限时间内输出可行航迹，顺利规避动态威胁，准确抵达目标区域。
本发明公开的基于Anytime修复式稀疏A*与卡尔曼滤波的动态航迹规划方法，包括如下步骤：
步骤一、输入初始任务态势信息以及无人机飞行性能参数。
步骤二、构建航迹规划求解模型。
步骤二具体实现步骤如下：
步骤2.1、无人机航迹表示。
无人机在三维空间中的位置表示为(x,y,z)，其中x、y分别表示东向和北向位置坐标，z表示无人机海拔高度。将无人机航迹记为一组节点序列{Sstart,S1,S2,…,Sn-1,G}，其中Sstart为无人机起始位置，G 为目标位置，S1,S2,…,Sn-1为待规划航迹点。航迹节点的以数组形式表述，包含自身序号、位置信息、代价信息，父节点序号、位置信息、代价信息。航迹节点具体表示如式(1)所示，
s＝{x,y,z,xf,yf,zf,g(s),h(s),key(s),ID,IDf} (1)
其中x,y,z表示该节点的位置坐标，xf,yf,zf表示父节点的位置坐标， g(s)表示由规划起始点到当前到达节点的实际代价，h(s)表示从当前到达节点到目标点的估计代价。key(s)表示节点总代价，即前两者代价之和，ID为该节点序号，IDf为父节点序号。
步骤2.2、构建无人机航迹规划模型过程中需要考虑的无人机飞行性能约束、禁飞区约束、地形约束，其中无人机飞行性能约束包括最大爬升/俯冲角约束、最大转弯角约束和最小航迹段长度约束。
步骤三、利用时间相关模型建立动态威胁和运动目标估计模型。
步骤三具体实现步骤如下：
步骤3.1、输入机动频率α，机动频率通常的经验取值范围为：大气扰动α1＝1，转弯机动α2＝1/20，逃避机动α2＝1/60，其确切数值根据实际情况或通过实时测量确定。
步骤3.2、离散时间下的时间相关模型表示为：
Xk+1＝ΦkXk+Γkwk (2)
其中，Xk+1为k+1时刻系统状态量，wk为系统噪声，系统状态转移矩阵为
系统噪声矩阵为
步骤四、设置Anytime修复式稀疏A*算法参数，包括权重系数ε，权重缩减系数Δε。Anytime修复式稀疏A*算法简称AR-SAS算法
步骤五、判断规划时间是否超出规划限定时间或启发项权重ε是否小于1，若超出规划限定时间或启发项权重ε小于1，输出当前航迹，否则转步骤六。
步骤六、执行AR-SAS算法，得到初始可行飞行航迹。
步骤六具体实现步骤如下：
步骤6.1、将目标节点真实代价设为无穷大，AR-SAS算法节点链表初始化，将起始节点放入OPEN表中，同时，置空CLOSE表和 INCONS表。OPEN表中存放未扩展节点，CLOSE表中存放已扩展节点，INCONS表中存放具有非一致性的节点。节点的非一致性是指当扩展到某节点s’∈CLOSE时，有
g(s)+c(s,s')＜g(s') (5)
式中，s为s’的父节点，c(s,s’)为s到s’的代价值。当AR-SAS算法搜索重复扩展至CLOSE表中某一点，并使该节点的g值下降，则更新该节点的代价值，并将该节点放入INCONS表中。INCONS表中的节点不会被立即扩展，而是会在下一次迭代搜索过程与OPEN表合并，重新进行排序扩展；
步骤6.2、起始点取无人机当前位置，目标点取估计的下一时刻目标位置。根据输入的起始点、目标点和初始启发项权重系数使用稀疏A*算法进行初始航迹规划；
步骤6.3、根据CLOSE表回溯得到并保存当前航迹；
步骤6.4、线性减小启发项权重系数ε，将INCONS表中节点移入 OPEN表中并根据新的启发项权重系数更新OPEN表中节点代价值，置空CLOSE表；
步骤6.5、根据新的节点链表进行新一次航迹规划；
步骤6.6、得到当前航迹并保存，判断规划时间是否超出规划限定时间或启发项权重ε是否小于1，若超出规划限定时间或启发项权重ε小于1，输出当前航迹，否则重复步骤6.1-6.5。
步骤6.7、输出当前航迹，即得到初始可行飞行航迹。
步骤七、当飞行时间达到一个目标观测周期T时，执行自适应扩展卡尔曼滤波，输出下一时刻威胁及目标的估计位置信息简称，其中，自适应扩展卡尔曼滤波算法简称AEKF算法。
步骤七具体实现步骤如下：
步骤7.1、初始化卡尔曼滤波参数，包括测量过程噪声Wk，观测向量Z，观测噪声协方差矩阵Rk，观测采样间隔T，以及Singer模型中机动频率，输入系统初始状态X；
步骤7.2、将非线性系统线性化，并估计下一时刻系统状态量及状态估计均方误差Pk；
步骤7.3、输入当前观测量，将观测量线性化，校正系统状态量估计值及状态估计均方误差Pk的值；
步骤7.4、输出下一时刻系统状态量的最优估计值，修正观测噪声协方差矩阵Rk，以提高后续估计精度；
步骤7.5、根据步骤7.4所述的系统状态量的最优估计值输出下一时刻威胁及目标的估计位置信息。
步骤八、以无人机当前位置为起点，估计位置为目标，考虑威胁当前位置，调用AR-SAS算法进行动态航迹规划。
步骤九、保存当前航迹，判断是否满足动态航迹规划终止条件S<l，即无人机与目标距离小于无人机能够执行任务的最大距离。若满足航迹规划终止条件，终止航迹规划，保存并输出当前最优航迹，即基于 Anytime修复式稀疏A*与卡尔曼滤波算法实现动态航迹规划，准确抵达目标区域；若不满足，重复步骤七-八。
有益效果：
1、本发明公开的基于Anytime修复式稀疏A*与卡尔曼滤波的动态航迹规划方法，在规划过程中考虑禁飞区约束，并且利用卡尔曼滤波算法预先估计位置信息，实现无人机执行任务过程中规避全部动态威胁，缩短到达目标区域的时间。
2、本发明公开的基于Anytime修复式稀疏A*与卡尔曼滤波的动态航迹规划方法，采用较大的启发项权重ε，并在规划过程中不断减小启发项权重值，实现动态航迹规划问题的快速求解，进而提高无人机执行任务效率。
3、本发明公开的基于Anytime修复式稀疏A*与卡尔曼滤波的动态航迹规划方法，在动态航迹规划过程中采用的Anytime修复式稀疏 A*算法具有鲁棒性强的优点，因此，本发明也具有鲁棒性强的特点。
附图说明
图1为本发明基于Anytime修复式稀疏A*与卡尔曼滤波的动态航迹规划方法流程图；
图2为具体实施方式案例一的动态航迹规划过程图，其中，图2(a) 为规划时刻10s时航迹规划结果图，图2(b)为规划时刻40s时航迹规划结果图，图2(c)为规划时刻80s时航迹规划结果图，图2(d)为规划时刻140s时航迹规划结果图；
图3为具体实施方式案例一的目标位置估计误差图,其中，图3(a) 为X方向上误差，其中，图3(b)为Y方向上的误差；
图4为具体实施方式案例二的目标运动示意图；
图5为具体实施方式案例二的动态航迹规划过程图，图5(a)为规划时刻10s时航迹规划结果图，图5(b)为规划时刻40s时航迹规划结果图，图5(c)为规划时刻80s时航迹规划结果图，图5(d)为规划时刻 120s时航迹规划结果图；
图6为具体实施方式案例二的动态威胁及运动目标位置估计误差图，其中，图6(a)为X方向上误差，其中，图6(b)为Y方向上的误差。
具体实施方式
为了更好的说明本发明的目的与优点，下面通过无人机航迹规划实例，结合附图与表格对本文发明做进一步说明，对本发明的性能进行分析。
案例一
为了验证所提出方法的有效性，使用本发明提出算法求解动态环境下无人机航迹规划问题。仿真硬件为Inter Core i5-2310CPU 3.10 GHz 4G内存，仿真环境为MATLAB。
本实施例公开的基于Anytime修复式稀疏A*与卡尔曼滤波的动态航迹规划方法，具体实现步骤如下：
步骤一、输入初始任务态势信息以及无人机飞行性能参数。
在本例中，设置飞行区域为6km×6km×1km的立方体区域，飞行区域地形数据为某山脉高程地图。规划起始点为[300m,5100m,0m]。无人机最小转弯半径1000m，最小飞行高度为50m，最大转弯角为 45°，速度为50m/s。动态环境中运动目标起始位置为 [3500m,500m,300m]，初速度为[2.5m/s,0m/s]，运动方式如表1所示。规划环境内存在的静态威胁信息如表2所示。
表1目标运动方式
表2威胁信息表
步骤二、构建航迹规划求解模型。
步骤2.1、无人机航迹表示：无人机在三维空间中的位置表示为 (x,y,z)，其中x、y分别表示东向和北向位置坐标，z表示无人机海拔高度。将无人机航迹记为一组节点序列{Sstart,S1,S2,…,Sn-1,G}，其中 Sstart为无人机起始位置，G为目标位置，S1,S2,…,Sn-1为待规划航迹点。航迹节点的以数组形式表述，其中包含自身序号、位置信息、代价信息，父节点序号、位置信息、代价信息等。航迹节点具体表示如式(1)所示。
步骤2.2、构建无人机航迹规划模型过程中需要考虑无人机飞行性能约束、禁飞区约束、地形约束等。
步骤三、利用时间相关模型建立运动目标估计模型，其状态向量为X＝[x,vx,ax,y,vy,ay]。
步骤三具体实现步骤如下：
步骤3.1、输入机动频率α＝1；
步骤3.2、离散时间下的时间相关模型表示为：
Xk+1＝ΦkXk+Γkwk (6)
其中，系统状态转移矩阵为
系统噪声矩阵为
步骤四、设置AR-SAS算法参数，初始启发项权重为ε＝1.5，迭代线性缩减值△ε＝0.1，扩展步长为500m，算法限定规划时间为0.1s；
步骤五、判断规划时间是否超出规划限定时间或启发项权重是否小于1，若是输出当前航迹，否则转步骤6。
步骤六、执行AR-SAS算法。
步骤6.1、初始化OPEN表和CLOSE表，将起始节点放入OPEN 表，并将CLOSE表置空，设置g(sgoal)＝∞。
步骤6.2、起始点取无人机当前位置，目标点取估计下一时刻的目标位置。根据输入的起始点、目标点和初始启发项权重系数使用稀疏A*算法进行初始航迹规划；
步骤6.3、根据CLOSE表回溯得到并保存当前航迹；
步骤6.4、线性减小启发项权重系数ε，将INCONS表中节点移入OPEN表中并根据新的启发项权重系数更新OPEN表中节点代价值，置空CLOSE表；
步骤6.5、根据新的节点链表进行新一次航迹规划；
步骤6.6、得到当前航迹并保存，判断规划时间是否到限制时间及启发项权重系数ε是否小于1，若是算法退出，输出保存的最优航迹，否则重复步骤6.1-6.5。
步骤6.7、输出当前航迹。
步骤七、当飞行时间达到一个目标观测周期T时，执行AEKF算法。
步骤7.1、初始化卡尔曼滤波参数，过程噪声为Wk＝[0 0w]T，观测向量为Z＝[x,y]，观测噪声协方差矩阵为Rk＝diag(10,10)2。观测矩阵为H＝[1 0 0 0 0 0；0 0 0 1 0 0]。观测采样周期T＝1s，时间相关模型中机动频率α取为1，其状态转移矩阵为
其中，O为3×3的零矩阵，且
系统协方差矩阵
其中
协方差矩阵P0＝I6×6。
步骤7.2、将非线性系统线性化，并估计下一时刻系统状态量及估计状态估计均方误差Pk；
步骤7.3、输入当前观测量，将观测量线性化，校正状态估计值及状态估计均方误差的Pk值；
步骤7.4、输出下一时刻系统状态量的最优估计值，修正观测噪声协方差矩阵Rk，以提高后续估计精度；
步骤7.5、根据步骤7.4所述的系统状态量的最优估计值输出下一时刻威胁及目标的估计位置信息。
步骤八、以无人机当前位置为起点，估计位置为目标及威胁当前位置，调用AR-SAS进行动态航迹规划。
步骤九、保存当前航迹，判断是否满足动态航迹规划终止条件S<l，即无人机与目标距离小于无人机能够执行任务的最大距离。若满足航迹规划终止条件，终止航迹规划，保存并输出当前最优航迹，若不满足，重复步骤七-八。
本实施例所述的面向动态环境的滚动即时航迹规划方法，在无人机航迹规划过程中，能够准确估计运动目标位置，不断进行航迹重规划，在规定时限内输出可行航迹，引导无人机准确抵达目标区域。仿真结果如图2所示,其中，白色大圆表示威胁区，点线表示规划航迹，米形表示当前规划时刻无人机位置，五角星表示估计的目标位置，正方形表示目标真实位置。仿真试验中无人机飞行时间为161s，算法始终能够在限定规划时间内找到当前最优航迹并规避全部威胁。由图3 可知，自适应扩展卡尔曼滤波算法能够较好地对运动目标轨迹进行估计。X方向上位置估计误差在10m以内，Y方向上位置估计误差在 20m以内。一般的固定翼无人机侦察设备地面视场宽度在100m-500m 之间，飞行速度在50m/s左右，因此，对于无人机的任务规划系统而言，该估计误差满足任务需求。
案例二
为了进一步验证本发明提出的算法能有效处理含运动威胁的动态场景，在三维任务环境中设置动态威胁和运动目标进行无人机航迹规划仿真试验。仿真硬件为Inter Corei5-2310CPU 3.10GHz 4G内存，仿真环境为MATLAB。
本实施例公开的基于Anytime修复式稀疏A*与卡尔曼滤波的动态航迹规划方法，具体实现步骤如下：
步骤一、输入初始任务态势信息以及无人机飞行性能参数。
在本例中，设置飞行区域在6km×6km×6km的范围内，规划起始点为[300m,5100m,300m]，目标做机动运动，初始位置为 [3500m,500m,300m]，初始速度为[5m/s,0m/s,0m/s]，图4为目标运动示意图。无人机飞行速度为50m/s，最小转弯半径1000m，最小飞行高度为50m，最大转弯角为45°。任务环境中威胁匀速运动，威胁运动信息如表3所示。
表3威胁运动信息
步骤二、构建无人机航迹规划问题模型。其具体方式与案例一步骤二相同。
步骤三、利用时间相关模型建立动态威胁和运动目标估计模型。
步骤四、设置AR-SAS算法参数，其具体设置参数与案例一步骤四相同。
步骤五、判断规划时间是否超出规划限定时间或启发项权重ε是否小于1，若是输出当前航迹，否则转步骤六。
步骤六、执行AR-SAS算法，得到初始可行飞行航迹。
步骤七、当飞行时间到达目标观测周期T时。执行AEKF算法。
步骤八、以无人机当前位置为起点，估计位置为目标及威胁当前位置，调用AR-SAS进行动态航迹规划。
步骤九、保存当前航迹，判断是否满足动态航迹规划终止条件S<l，即无人机与目标距离小于无人机能够执行任务的最大距离。若满足航迹规划终止条件，终止航迹规划，保存并输出当前最优航迹，若不满足，重复步骤七-八。
整个仿真试验中无人机飞行时间为163s，当无人机到达目标点 1km范围内不再进行规划。图5为动态航迹规划过程，其中，白色大圆表示估计的威胁区位置，灰色大圆表示威胁区初始位置，点线表示规划航迹，米形表示当前规划时刻无人机位置，五角星表示估计的目标位置，正方形表示目标真实位置。该算法能够保证在规定时限内输出可行航迹，并且保证无人机规避全部威胁，准确抵达运动目标附近。图6为动态位置及运动目标位置估计误差，运动目标X方向上估计位置误差在10m以内，Y方向上位置估计误差在20m以内，对于一般飞行器航迹规划系统而言，该误差可以接受。动态威胁在X方向与 Y方向上的误差均在20m以内，在无人机航迹规划过程中，无人机与威胁之间安全距离为50m，因此无人机能够成功规避全部威胁。仿真试验结果验证本发明提出的面向动态环境的滚动即时航迹规划算法能够高效求解动态环境下无人机快速航迹规划问题。因此本发明有较强的工程使用性，并基本实现预期的发明目的。
以上的具体描述，是对发明的目的、技术方案和有益效果的进一步详细说明，所应理解的是，以上所示仅为本发明的具体实例，仅用于解释本发明，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包括在本发明的保护范围之内。
Claims (5)
1.基于Anytime修复式稀疏A*与卡尔曼滤波的动态航迹规划方法，其特征在于：包括如下步骤，
步骤一、输入初始任务态势信息以及无人机飞行性能参数；
步骤二、构建航迹规划求解模型；
步骤三、利用时间相关模型建立动态威胁和运动目标估计模型；
步骤四、设置Anytime修复式稀疏A*算法参数，包括权重系数ε，权重缩减系数Δε；Anytime修复式稀疏A*算法简称AR-SAS算法
步骤五、判断规划时间是否超出规划限定时间或启发项权重ε是否小于1，若超出规划限定时间或启发项权重ε小于1，输出当前航迹，否则转步骤六；
步骤六、执行AR-SAS算法，得到初始可行飞行航迹；
步骤七、当飞行时间达到一个目标观测周期T时，执行自适应扩展卡尔曼滤波，输出下一时刻威胁及目标的估计位置信息简称，其中，自适应扩展卡尔曼滤波算法简称AEKF算法；
步骤八、以无人机当前位置为起点，估计位置为目标，考虑威胁当前位置，调用AR-SAS算法进行动态航迹规划；
步骤九、保存当前航迹，判断是否满足动态航迹规划终止条件S<l，即无人机与目标距离小于无人机能够执行任务的最大距离；若满足航迹规划终止条件，终止航迹规划，保存并输出当前最优航迹，即基于Anytime修复式稀疏A*与卡尔曼滤波算法实现动态航迹规划，准确抵达目标区域；若不满足，重复步骤七-八。
2.如权利要求1所述的基于Anytime修复式稀疏A*与卡尔曼滤波的动态航迹规划方法，其特征在于：步骤二具体实现步骤如下，
步骤2.1、无人机航迹表示；
无人机在三维空间中的位置表示为(x,y,z)，其中x、y分别表示东向和北向位置坐标，z表示无人机海拔高度；将无人机航迹记为一组节点序列{Sstart,S1,S2,…,Sn-1,G}，其中Sstart为无人机起始位置，G为目标位置，S1,S2,…,Sn-1为待规划航迹点；航迹节点的以数组形式表述，包含自身序号、位置信息、代价信息，父节点序号、位置信息、代价信息；航迹节点具体表示如式(1)所示，
s＝{x,y,z,xf,yf,zf,g(s),h(s),key(s),ID,IDf} (1)
其中x,y,z表示该节点的位置坐标，xf,yf,zf表示父节点的位置坐标，g(s)表示由规划起始点到当前到达节点的实际代价，h(s)表示从当前到达节点到目标点的估计代价；key(s)表示节点总代价，即前两者代价之和，ID为该节点序号，IDf为父节点序号；
步骤2.2、构建无人机航迹规划模型过程中需要考虑的无人机飞行性能约束、禁飞区约束、地形约束，其中无人机飞行性能约束包括最大爬升/俯冲角约束、最大转弯角约束和最小航迹段长度约束。
3.如权利要求2所述的基于Anytime修复式稀疏A*与卡尔曼滤波的动态航迹规划方法，其特征在于：步骤三具体实现步骤如下，
步骤3.1、输入机动频率α，其确切数值根据实际情况或通过实时测量确定；
步骤3.2、离散时间下的时间相关模型表示为：
Xk+1＝ΦkXk+Γkwk (2)
其中，Xk+1为k+1时刻系统状态量，wk为系统噪声，系统状态转移矩阵为
系统噪声矩阵为
4.如权利要求3所述的基于Anytime修复式稀疏A*与卡尔曼滤波的动态航迹规划方法，其特征在于：步骤六具体实现步骤如下，
步骤6.1、将目标节点真实代价设为无穷大，AR-SAS算法节点链表初始化，将起始节点放入OPEN表中，同时，置空CLOSE表和INCONS表；OPEN表中存放未扩展节点，CLOSE表中存放已扩展节点，INCONS表中存放具有非一致性的节点；节点的非一致性是指当扩展到某节点s’∈CLOSE时，有
g(s)+c(s,s')＜g(s') (5)
式中，s为s’的父节点，c(s,s’)为s到s’的代价值；当AR-SAS算法搜索重复扩展至CLOSE表中某一点，并使该节点的g值下降，则更新该节点的代价值，并将该节点放入INCONS表中；INCONS表中的节点不会被立即扩展，而是会在下一次迭代搜索过程与OPEN表合并，重新进行排序扩展；
步骤6.2、起始点取无人机当前位置，目标点取估计的下一时刻目标位置；根据输入的起始点、目标点和初始启发项权重系数使用稀疏A*算法进行初始航迹规划；
步骤6.3、根据CLOSE表回溯得到并保存当前航迹；
步骤6.4、线性减小启发项权重系数ε，将INCONS表中节点移入OPEN表中并根据新的启发项权重系数更新OPEN表中节点代价值，置空CLOSE表；
步骤6.5、根据新的节点链表进行新一次航迹规划；
步骤6.6、得到当前航迹并保存，判断规划时间是否超出规划限定时间或启发项权重ε是否小于1，若超出规划限定时间或启发项权重ε小于1，输出当前航迹，否则重复步骤6.1-6.5；
步骤6.7、输出当前航迹，即得到初始可行飞行航迹。
5.如权利要求4所述的基于Anytime修复式稀疏A*与卡尔曼滤波的动态航迹规划方法，其特征在于：步骤七具体实现步骤如下，
步骤7.1、初始化卡尔曼滤波参数，包括测量过程噪声Wk，观测向量Z，观测噪声协方差矩阵Rk，观测采样间隔T，以及Singer模型中机动频率，输入系统初始状态X；
步骤7.2、将非线性系统线性化，并估计下一时刻系统状态量及状态估计均方误差Pk；
步骤7.3、输入当前观测量，将观测量线性化，校正系统状态量估计值及状态估计均方误差Pk的值；
步骤7.4、输出下一时刻系统状态量的最优估计值，修正观测噪声协方差矩阵Rk，以提高后续估计精度；
步骤7.5、根据步骤7.4所述的系统状态量的最优估计值输出下一时刻威胁及目标的估计位置信息。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811078254.XA CN109254591B (zh) | 2018-09-17 | 2018-09-17 | 基于Anytime修复式稀疏A*与卡尔曼滤波的动态航迹规划方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811078254.XA CN109254591B (zh) | 2018-09-17 | 2018-09-17 | 基于Anytime修复式稀疏A*与卡尔曼滤波的动态航迹规划方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109254591A true CN109254591A (zh) | 2019-01-22 |
| CN109254591B CN109254591B (zh) | 2021-02-12 |
Family
ID=65047908
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811078254.XA Expired - Fee Related CN109254591B (zh) | 2018-09-17 | 2018-09-17 | 基于Anytime修复式稀疏A*与卡尔曼滤波的动态航迹规划方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109254591B (zh) |
Cited By (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109857134A (zh) * | 2019-03-27 | 2019-06-07 | 浙江理工大学 | 基于A*/minimum_snap算法的无人机轨迹控制系统和方法 |
| CN109933087A (zh) * | 2019-03-18 | 2019-06-25 | 西安爱生技术集团公司 | 无人机与地面机动目标虚拟编队战位保持控制方法 |
| CN110031007A (zh) * | 2019-03-22 | 2019-07-19 | 深圳先进技术研究院 | 一种航迹规划方法、装置及计算机可读存储介质 |
| CN110220513A (zh) * | 2019-04-30 | 2019-09-10 | 中国科学院深圳先进技术研究院 | 一种目标定位的方法、系统、无人机及存储介质 |
| CN110597264A (zh) * | 2019-09-25 | 2019-12-20 | 中国人民解放军陆军工程大学 | 无人机反制系统 |
| CN110908395A (zh) * | 2019-11-26 | 2020-03-24 | 西北工业大学 | 改进的无人机航迹实时规划方法 |
| CN111811511A (zh) * | 2020-06-23 | 2020-10-23 | 北京理工大学 | 一种基于降维解耦机制的无人机集群实时航迹生成方法 |
| CN111865395A (zh) * | 2020-06-12 | 2020-10-30 | 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) | 一种面向无人机编队通信的轨迹生成与跟踪方法及系统 |
| CN112148008A (zh) * | 2020-09-18 | 2020-12-29 | 中国航空无线电电子研究所 | 一种基于深度强化学习的实时无人机路径预测方法 |
| CN113139232A (zh) * | 2021-01-15 | 2021-07-20 | 中国人民解放军91550部队 | 一种基于不完全测量的飞行器事后定位方法及系统 |
| CN117113216A (zh) * | 2023-07-25 | 2023-11-24 | 北京理工大学重庆创新中心 | 基于动态贝叶斯网络的异构多无人机打击决策方法及装置 |
| CN117408084A (zh) * | 2023-12-12 | 2024-01-16 | 江苏君立华域信息安全技术股份有限公司 | 一种用于无人机航迹预测的增强卡尔曼滤波方法及系统 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102981505A (zh) * | 2011-08-01 | 2013-03-20 | 空中客车运营简化股份公司 | 用于确定飞机的飞行参数的方法和系统 |
| CN103926933A (zh) * | 2014-03-29 | 2014-07-16 | 北京航空航天大学 | 一种无人飞行器室内同时定位与环境建模方法 |
| KR101507422B1 (ko) * | 2013-10-11 | 2015-03-31 | 부경대학교 산학협력단 | 하이브리드 무인잠수정 |
| CN107607120A (zh) * | 2017-09-06 | 2018-01-19 | 北京理工大学 | 基于改进修复式Anytime稀疏A*算法的无人机动态航迹规划方法 |
| CN108536171A (zh) * | 2018-03-21 | 2018-09-14 | 电子科技大学 | 一种多约束下多无人机协同跟踪的路径规划方法 |
-
2018
- 2018-09-17 CN CN201811078254.XA patent/CN109254591B/zh not_active Expired - Fee Related
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102981505A (zh) * | 2011-08-01 | 2013-03-20 | 空中客车运营简化股份公司 | 用于确定飞机的飞行参数的方法和系统 |
| KR101507422B1 (ko) * | 2013-10-11 | 2015-03-31 | 부경대학교 산학협력단 | 하이브리드 무인잠수정 |
| CN103926933A (zh) * | 2014-03-29 | 2014-07-16 | 北京航空航天大学 | 一种无人飞行器室内同时定位与环境建模方法 |
| CN107607120A (zh) * | 2017-09-06 | 2018-01-19 | 北京理工大学 | 基于改进修复式Anytime稀疏A*算法的无人机动态航迹规划方法 |
| CN108536171A (zh) * | 2018-03-21 | 2018-09-14 | 电子科技大学 | 一种多约束下多无人机协同跟踪的路径规划方法 |
Non-Patent Citations (1)
| Title |
|---|
| 王军政等: "机动目标跟踪中机动频率的自适应调整", 《北京理工大学学报》 * |
Cited By (18)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109933087B (zh) * | 2019-03-18 | 2021-12-10 | 西安爱生技术集团公司 | 无人机与地面机动目标虚拟编队战位保持控制方法 |
| CN109933087A (zh) * | 2019-03-18 | 2019-06-25 | 西安爱生技术集团公司 | 无人机与地面机动目标虚拟编队战位保持控制方法 |
| CN110031007A (zh) * | 2019-03-22 | 2019-07-19 | 深圳先进技术研究院 | 一种航迹规划方法、装置及计算机可读存储介质 |
| CN109857134A (zh) * | 2019-03-27 | 2019-06-07 | 浙江理工大学 | 基于A*/minimum_snap算法的无人机轨迹控制系统和方法 |
| CN110220513A (zh) * | 2019-04-30 | 2019-09-10 | 中国科学院深圳先进技术研究院 | 一种目标定位的方法、系统、无人机及存储介质 |
| CN110220513B (zh) * | 2019-04-30 | 2022-10-04 | 中国科学院深圳先进技术研究院 | 一种目标定位的方法、系统、无人机及存储介质 |
| CN110597264A (zh) * | 2019-09-25 | 2019-12-20 | 中国人民解放军陆军工程大学 | 无人机反制系统 |
| CN110908395A (zh) * | 2019-11-26 | 2020-03-24 | 西北工业大学 | 改进的无人机航迹实时规划方法 |
| CN111865395A (zh) * | 2020-06-12 | 2020-10-30 | 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) | 一种面向无人机编队通信的轨迹生成与跟踪方法及系统 |
| CN111865395B (zh) * | 2020-06-12 | 2022-07-05 | 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) | 一种面向无人机编队通信的轨迹生成与跟踪方法及系统 |
| CN111811511A (zh) * | 2020-06-23 | 2020-10-23 | 北京理工大学 | 一种基于降维解耦机制的无人机集群实时航迹生成方法 |
| CN111811511B (zh) * | 2020-06-23 | 2024-04-19 | 北京理工大学 | 一种基于降维解耦机制的无人机集群实时航迹生成方法 |
| CN112148008A (zh) * | 2020-09-18 | 2020-12-29 | 中国航空无线电电子研究所 | 一种基于深度强化学习的实时无人机路径预测方法 |
| CN113139232A (zh) * | 2021-01-15 | 2021-07-20 | 中国人民解放军91550部队 | 一种基于不完全测量的飞行器事后定位方法及系统 |
| CN113139232B (zh) * | 2021-01-15 | 2023-12-26 | 中国人民解放军91550部队 | 一种基于不完全测量的飞行器事后定位方法及系统 |
| CN117113216A (zh) * | 2023-07-25 | 2023-11-24 | 北京理工大学重庆创新中心 | 基于动态贝叶斯网络的异构多无人机打击决策方法及装置 |
| CN117408084A (zh) * | 2023-12-12 | 2024-01-16 | 江苏君立华域信息安全技术股份有限公司 | 一种用于无人机航迹预测的增强卡尔曼滤波方法及系统 |
| CN117408084B (zh) * | 2023-12-12 | 2024-04-02 | 江苏君立华域信息安全技术股份有限公司 | 一种用于无人机航迹预测的增强卡尔曼滤波方法及系统 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109254591B (zh) | 2021-02-12 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109254591A (zh) | 基于Anytime修复式稀疏A*与卡尔曼滤波的动态航迹规划方法 | |
| CN111024092B (zh) | 一种多约束条件下智能飞行器航迹快速规划方法 | |
| CN107103164B (zh) | 无人机执行多任务的分配方法及装置 | |
| CN105426970B (zh) | 一种基于离散动态贝叶斯网络的气象威胁评估方法 | |
| CN107607120B (zh) | 基于改进修复式Anytime稀疏A*算法的无人机动态航迹规划方法 | |
| CN110262548B (zh) | 一种考虑抵达时间约束的无人机航迹规划方法 | |
| Sun et al. | AGV path planning based on improved Dijkstra algorithm | |
| CN109724612A (zh) | 一种基于拓扑地图的agv路径规划方法及设备 | |
| CN107169608A (zh) | 多无人机执行多任务的分配方法及装置 | |
| CN106767820B (zh) | 一种室内移动定位与制图方法 | |
| CN103676944A (zh) | 基于Dubins路径和稀疏A*搜索的无人机航迹规划方法 | |
| CN109597864A (zh) | 椭球边界卡尔曼滤波的即时定位与地图构建方法及系统 | |
| CN103267528A (zh) | 禁飞区限制下的多无人机协同区域搜索方法 | |
| CN112881979B (zh) | 一种基于ekf滤波的初始状态自适应融合定位方法 | |
| CN104834317A (zh) | 一种智能识别威胁类型的无人机飞行路径规划方法 | |
| CN106197432B (zh) | 一种基于FastSLAM算法的无人机着陆方法 | |
| Geng et al. | UAV surveillance mission planning with gimbaled sensors | |
| CN109141427A (zh) | 在非视距环境下基于距离和角度概率模型的ekf定位方法 | |
| CN112527010B (zh) | 基于人工势场与粒子优化的室内变电站无人机多机协同巡检方法 | |
| CN112712193A (zh) | 基于改进Q-Learning的多无人机局部航路规划方法及装置 | |
| CN110530373A (zh) | 一种机器人路径规划方法、控制器及系统 | |
| CN115562357B (zh) | 一种面向无人机集群的智能路径规划方法 | |
| CN111895995A (zh) | 基于pso的飞行器编队多维地磁匹配导航方法及系统 | |
| Huang et al. | A novel route planning method of fixed-wing unmanned aerial vehicle based on improved QPSO | |
| CN111123953A (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 | ||
| CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20210212 Termination date: 20210917 |
|
| CF01 | Termination of patent right due to non-payment of annual fee |