[发明专利]基于Anytime修复式稀疏A*与卡尔曼滤波的动态航迹规划方法

权利要求书

1.基于Anytime修复式稀疏A*与卡尔曼滤波的动态航迹规划方法，其特征在于：包括如下步骤，

步骤一、输入初始任务态势信息以及无人机飞行性能参数；

步骤二、构建航迹规划求解模型；

步骤2.1、无人机航迹表示；

无人机在三维空间中的位置表示为(x,y,z)，其中x、y分别表示东向和北向位置坐标，z表示无人机海拔高度；将无人机航迹记为一组节点序列{S_start,S₁,S₂,…,S_n-1,G}，其中S_start为无人机起始位置，G为目标位置，S₁,S₂,…,S_n-1为待规划航迹点；航迹节点的以数组形式表述，包含自身序号、位置信息、代价信息，父节点序号、位置信息、代价信息；航迹节点具体表示如式(1)所示，

s＝{x,y,z,x_f,y_f,z_f,g(s),h(s),key(s),ID,ID_f} (1)

其中x,y,z表示该节点的位置坐标，x_f,y_f,z_f表示父节点的位置坐标，g(s)表示由规划起始点到当前到达节点的实际代价，h(s)表示从当前到达节点到目标点的估计代价；key(s)表示节点总代价，即前两者代价之和，ID为该节点序号，ID_f为父节点序号；

步骤2.2、构建无人机航迹规划模型过程中需要考虑的无人机飞行性能约束、禁飞区约束、地形约束，其中无人机飞行性能约束包括最大爬升/俯冲角约束、最大转弯角约束和最小航迹段长度约束；

步骤三、利用时间相关模型建立动态威胁和运动目标估计模型；

步骤3.1、输入机动频率α，其确切数值根据实际情况或通过实时测量确定；

步骤3.2、离散时间下的时间相关模型表示为：

X_k+1＝Φ_kX_k+Γ_kw_k (2)

其中，X_k+1为k+1时刻系统状态量，w_k为系统噪声，系统状态转移矩阵为

系统噪声矩阵为

步骤四、设置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、初始化卡尔曼滤波参数，包括测量过程噪声W_k，观测向量Z，观测噪声协方差矩阵R_k，观测采样间隔T，以及Singer模型中机动频率，输入系统初始状态X；

步骤7.2、将非线性系统线性化，并估计下一时刻系统状态量及状态估计均方误差P_k；

步骤7.3、输入当前观测量，将观测量线性化，校正系统状态量估计值及状态估计均方误差P_k的值；

步骤7.4、输出下一时刻系统状态量的最优估计值，修正观测噪声协方差矩阵R_k，以提高后续估计精度；

步骤7.5、根据步骤7.4所述的系统状态量的最优估计值输出下一时刻威胁及目标的估计位置信息；

步骤八、以无人机当前位置为起点，估计位置为目标，考虑威胁当前位置，调用AR-SAS算法进行动态航迹规划；

步骤九、保存当前航迹，判断是否满足动态航迹规划终止条件Sl，即无人机与目标距离小于无人机能够执行任务的最大距离；若满足航迹规划终止条件，终止航迹规划，保存并输出当前最优航迹，即基于Anytime修复式稀疏A*与卡尔曼滤波算法实现动态航迹规划，准确抵达目标区域；若不满足，重复步骤七-八。