CN108759839B - 一种基于态势空间的无人飞行器路径规划方法 - Google Patents

Abstract

本发明提供的是一种基于态势空间的无人飞行器路径规划方法。一：无人飞行器实时侦测周围的环境信息，提取探测到的运动对象的状态信息；二：根据状态信息，求解出每个运动对象的态势空间，分析无人飞行器的物理约束，给出在机载坐标系下的约束空间；三：根据运动目标的收益函数获取跟踪策略；四：根据态势空间获取避开运动威胁的冲突避免策略；五：融合步骤三和四的策略加速度，生成新的控制加速度，并根据速度上界修正，然后根据到达点生成方法得到新的规划位置。本发明可以在高空稀疏的空间中，为机动性能极强的UCAV在跟踪目标时提供一条有效的无冲突路径。规划的路径不但满足实际的物理要求，能有效的与跟踪目标保持安全的飞行距离。

Description

一种基于态势空间的无人飞行器路径规划方法

技术领域

本发明涉及的是一种三维路径规划方法。具体地说是一种无人飞行器的三维路径规划方法。

背景技术

目标跟踪和避免碰撞对于无人飞行器(UCAV)任务的成功至关重要。为了提高无人驾驶器械的自主水平，保证规划路线的可行性和安全性，相关的导航和控制策略的发展正在迅速增加。这些器械在许多应用中引起了很多关注。传统的二维(2D)路径规划主要用于在已知或静态环境下离线寻找全局最优路径。但是，无人飞机跟踪的目标和威胁在三维(3D)高空飞行路线中通常是灵活的，并且无人飞机的运动控制受到一些性能参数的限制，因此在线寻找局部最优或甚至可行的路线对UCAV来说是一个很大的挑战。近年来，为了解决三维路径规划问题，许多经典的几何方法得到了改进。一般来说，面对不同的工程背景，要实现一个完整的路径规划算法，路径可行性和参数适应性应该由无人机动力学，运动学等因素来判断。因为无人飞机经常在稀疏的高空空间执行任务，所以碰撞威胁大多由机动飞机或生物组成，而不是静止的障碍物。故自主路径规划还涉及到环境建模和评估技术。此外对于路径规划过程，无人飞机必须根据实时要求获取每个采样时刻每个移动威胁的运动状态的详细信息，并且必须考虑到目标位置，航迹跟踪滤波技术也很重要。

发明内容

本发明的目的在于提供一种可靠性高，在稀疏高海拔的飞行空域能够在完成跟踪目标的同时避开冲突性威胁的基于态势空间的无人飞行器路径规划方法。

本发明的目的是这样实现的：

步骤一：无人飞行器实时侦测周围的环境信息，用跟踪/滤波算法提取探测到的运动对象的状态信息，所述运动对象为运动目标和运动威胁；

步骤二：根据步骤一中获得的状态信息，求解出每个运动对象的态势空间，同时分析无人飞行器的物理约束，给出在机载坐标系下的约束空间；

步骤三：根据运动目标的收益函数来获取跟踪策略；

步骤四：根据态势空间来获取避开运动威胁的冲突避免策略；

步骤五：融合步骤三和四的策略加速度，生成新的控制加速度，并根据速度上界修正，然后根据到达点生成方法得到新的规划位置。

本发明还可以包括：

1、所述跟踪/滤波算法采用混合多模型集合容积卡尔曼算法模型，运动模型设为匀加速模型、匀速模型、以及水平转弯模型，状态和量测方程公式为：

其中φ和H是非线性系统的状态方程和观测方程，

表示在时间步骤k中有效的第j个模型或模式，

和

分别是零点平均值和协方差

和

的独立过程和观测高斯噪声，根据混合方程计算状态变量的输出估计值和预测值为：

在规划空间中，设本地地理笛卡尔坐标系为O_eX_eY_eZ_e，O_eX_eY_eZ_e中的任意点都表示为p＝(x，y，z)，从O_e到p的位移，即p＝[x，y，z]^T，飞行速度v＝[v_x,，v_y，v_z]^T，并将无人飞行器的飞行加速度a＝[a_x，a_y，a_z]^T作为运动状态，使用状态变量

来描述采样时间t_k的运动物体的运动状态；设无人飞行器的运动状态是$

令机载坐标系为O_cX_cY_cZ_c，

则两坐标系之间的变换矩阵为：

φ_k是无人飞行器的速度矢量与X_e的投影之间的角度，θ_k是飞行俯仰角度，

在机载坐标系中，无人飞行器在各个方向的加速度的上界表示为：

其中n_f和n_q分别是无人飞行器的最大正常超载和最大径向超载，g是重力加速度。

2、所述态势空间是以目标位置为圆心，以态势半径为球的三维空间，其中约束空间满足：

a_c,k是无人飞行器的有目的加速度，表示无人飞行器的预期调整方向；A(a_vc,k)为无人飞行器在方向a_c,k上产生的最大幅度的加速度的函数；

态势半径的公式为：

其中：

式中||p_u,k+1|k-p_u,k||和

分别表示无人飞行器和运动对象的一步预测距离；n_k为视线角的单位矢量；v_uo,k是无人飞行器相对运动对象的速度v_uo,k＝v_u,k-v_o,k；C为坐标变换矩阵。

3、运动目标的收益函数表示为：

本发明主要提出了一种基于情境空间的三维路径规划方法，为高机动性的无人飞行器(UCAV)找到一个在动态环境下追踪目标可靠的无碰撞路径。涉及到的领域主要是航空电子对抗和自动控制原理，该领域为当今复杂信息环境中的重要和热点研究领域，自治路径规划算法在导航过程中起着重要的作用。

本发明主要针对在稀疏高海拔的飞行空域中，无人飞行器完成跟踪目标的同时且避开冲突性威胁提出了新的解决办法。

本发明的实现过程主要包括：无人飞行器对环境实时侦察，根据跟踪算法更新运动物体的状态信息，然后根据公式计算各自的态势空间。再利用目标跟踪的收益函数推导出跟踪策略，和对应的避障策略。最后将两种策略融合，并对不合理的加速度修正，由此生成下一时刻的路径到达点。本发明的实现主要包括信息采集模块、状态信息处理模块、目标跟踪模块、障碍避免模块以及规划生成模块。本发明为高机动性的无人飞行器(UCAV)找到一个在动态环境下追踪目标可靠的无碰撞路径提供了解决方案，利用混合卡尔曼滤波增强了量测数据可靠性、用坐标系变换规划加速度调整策略，用态势圆实现目标跟踪策略、用态势圆实现障碍避免策略。

本发明的主要贡献在于：

(1)本发明提出一种基于UCAV机动特性的路径规划新方法。

(2)本发明给出一种基于情景空间的移动目标与威胁建模方法及相应的飞行路径安全评估手段。

(3)本发明方法可以分析基于约束边界的加速度计划的候选策略。

本发明可以在高空稀疏的空间中，为机动性能极强的UCAV在跟踪目标时提供一条有效的无冲突路径。规划的路径不但满足实际的物理要求，而且能有效的与跟踪目标保持安全的飞行距离。本发明在工程领域也能保证较优秀的时间效率。

附图说明

图1是机载的坐标系约束空间。

图2是态势空间中一步态势球的示意图。

图3是本发明的具体流程图。

图4是本发明实现自动路径规划的效果图。

具体实施方式

下面举例对本发明做更详细的描述。

本发明提供了一种在利用无源探测技术对机动目标进行跟踪的过程中，通过合理确定滤波初值来对跟踪算法的性能进行提升的新方法。由于以往针对该问题的研究甚少，而在实际工程应用中滤波初值确定得是否合适却对滤波的收敛速度有较大影响，在量测误差较大的前提下甚至可能导致算法趋近发散。所以发明可以提高滤波算法的收敛速度，同时给出了一些测试方案和数据结果分析。

本发明重在视线无人飞行器在动态环境下的路径规划。即设计了一个简单的基于情景空间的三维路径规划算法。考虑到检测的估计不确定性，依赖于IMM-CKF的状态球已经被分配给每个移动物体作为威胁区域，以提高避免碰撞的可靠性。为了规划可飞行路径，在CA的运动学模型中发展机载参考系中的机动约束，以找到下一个到达位置。在任何时刻，UCAV根据修改后的加速度来调整其姿态，从而确保根据UCAV的运动状态和可操作性切实达到预定的路径。需要说明的是，在目标和威胁都存在的情况下，避撞策略具有绝对优先权。

本发明的具体步骤如下：

步骤一：

无人飞行器对指定区域进行扫描，检测和接收运动物体的状态信息来行分析和识别。用IMM-CKF算法对获得的量测数据进行处理。观测数据为：

其中ρ_k是从运动物体到UCAV的位移的两阶范数，即ρ_k＝||p_o,k-p_u,k||。

步骤二：

根据步骤一中获得的状态信息分析，通过状态信息处理模块获得两个关键参数，即态势半径和约束空间。根据图2所示的约束空间制定有目的的规划加速度，即满足公式：

a_c,k是无人飞行器的有目的加速度，它表示无人飞行器的预期调整方向。A(a_vc,k)被定义为UCAV可以在方向a_c,k上产生的最大幅度的加速度的函数。本发明所有规划加速都基于这两个候选策略。

然后计算态势半径，其中态势半径的计算公式为：

其中：

式中||p_u,k+1|k-p_u,k||和

分别表示UCAV和运动目标的一步预测距离。n_k为图2中视线角的单位矢量。v_uo,k是UCAV相对运动目标的速度v_uo,k＝v_u,k-v_o,k。其中C为坐标变换矩阵。

态势空间即为以探测目标位置当前位置为中心，以态势半径为球的空间。避开该空间可以有效保障规划路径的安全性。

步骤三：

根据需要跟踪的目标获得跟踪策略，其中收益函数为：

在一个采样周期τ内，距离方向的期望调整为：

故规划加速度为

同理速度方向的规划加速度为

融合两种策略：

这里c_s+c_v＝1，且两者都是大于1的常数。考虑到无人飞行器远离目标时c_s起着关键作用，而距离缩短时c_v将成为主导因素。因此，为了使a_tc,k适应性增强，本发明提出了c_s的计算方法：

这里的λ是一个大于1的正常数。当ρ_k/r_s小于1时，跟踪目标会变为威胁因素，跟踪策略也会变为避障策略。

步骤四：

根据态势圆实现避障策略制定。如图4所示，情况球S_k是t_k时刻的运动物体相对于UCAV的一步态势空间，p_u,kq是从点p_u,k开始的与S_k的表面的相切的射线。当ρ_k/r_k>1时，令m_k表示射线方向的单位矢量，则m_k和n_k之间的角度可称为态势角度，即α。因此，判断运动物体为威胁的条件是β≤α，其中β是n_k和v_uo,k之间的夹角，β满足0<β≤π/2。给出避障策略的加速度为：

当有M个威胁同时存在时：

步骤五：

融合避障策略和跟踪策略，为确保避障的安全性，避障策略有绝对的优先权，即：

然后根据

实现生成下一时刻的到达点，即完成路径规划。这里的到达点生成方法为：

其中

附图3是本发明算法的完整流程图展示。

同时，为了验证本发明的有效性，本发明给出了相应的仿真测试结果。测试环境为MATLAB 2016a，windows 7系统CPU i3 3.7G的PC。假设参数设定如表1所示：

表1仿真参数设计

模型转移概率矩阵Ψ和先验模型概率μ₀被设置为：

假设在规划空间有三个飞行物，其状态信息见表2所示：

表2仿真参数设计

表中三个运动物体都是未知的飞机。在这种情况下X_k(1)是UCAV的移动跟踪目标，而X_k(2)和X_k(3)是飞行威胁。假设它们的初始状态为X₀(1)＝[2km，1km，8km，60m/s，90m/s，50m/s，0,0,0]^T，X₀(2)＝[6km，2.5km，9.5km，57m/s，-30m/s，150m/s，6.5m/s^2,0，-3m/s^2]^T和X₀(3)＝[1.7km，4.3km，13.5km，90m/s，0，50m/s，0,0,0]^T。为了计算算法的规划时间，我们对算法进行了50次蒙特卡洛仿真的独立运行。UCAV的初始状态为U₀＝[12km，2km，14km，30m/s，0，0，0，0，0]^T。将总计划时间设置为200τ。

从图4中我们可以看到飞行威胁对无人机的不同跟踪阶段(k＝40，80，115，200)的影响。无论是在早期还是晚期，UCAV都能成功避免碰撞。而且，我们可以注意到，基于态势空间的规划路径在姿态变化上是缓和的。本发明可以很好地实现软着陆跟踪，对于飞行安全的承诺是可靠的。

Claims (1)

1.一种基于态势空间的无人飞行器路径规划方法，其特征是：

步骤一：无人飞行器实时侦测周围的环境信息，用IMM-CKF算法提取探测到的运动目标的状态信息；其中，运动模型设为匀加速模型CA、匀速模型和水平转弯模型；

在规划空间中，设本地地理笛卡尔坐标系为O_eX_eY_eZ_e，O_eX_eY_eZ_e中的任意点表示为P＝(x,y,z)，从O_e到P的位移为p＝[x,y,z]^T，飞行速度v＝[v_x,v_y,v_z]^T，飞行加速度a＝[a_x,a_y,a_z]^T，使用状态变量

来描述采样时间t_k的运动目标的运动状态；设无人飞行器的运动状态是

观测数据为：

其中ρ_k是从运动目标到无人飞行器的位移的两阶范数，即ρ_k＝||p_o,k-p_u,k||；

步骤二：根据步骤一中获得的状态信息，求解出每个运动目标的态势空间，同时分析无人飞行器的物理约束，给出在机载坐标系下的约束空间；

令机载坐标系为O_cX_cY_cZ_c，则本地地理笛卡尔坐标系与机载坐标系的变换矩阵为：

其中φ_k是无人飞行器的速度矢量v_u,k在X_eO_eY_e平面上的投影与X_e轴之间的角度，θ_k是飞行俯仰角度；

在机载坐标系中，无人飞行器在各个方向的加速度的上界表示为：

其中n_f和n_q分别是无人飞行器的最大法向过载和最大径向过载，g是重力加速度；

根据约束空间制定有目的的规划加速度，即满足公式：

a_c,k是无人飞行器的有目的加速度，表示无人飞行器的预期调整方向；A(a_c,k)为无人飞行器在方向a_c,k上产生的最大幅度的加速度的函数；

然后计算态势半径，计算公式为：

其中L_u为无人飞行器的等效物理半径，L_o为运动目标的等效物理半径，

式中||p_u,k+1|k-p_u,k||和

分别表示无人飞行器和运动目标的一步预测距离，n_k为视线角的单位矢量，v_uo,k是无人飞行器相对运动目标的速度，v_uo,k＝v_u,k-v_o,k，C为坐标系之间的变换矩阵；

态势空间是以目标位置为中心，以态势半径为球半径的三维空间；

步骤三：根据需要跟踪的运动目标获得跟踪策略；

其中收益函数为：

在一个采样周期τ内，距离方向的期望调整为：

故距离方向的规划加速度为

同理，速度方向的规划加速度为

融合得到跟踪策略：

其中，c_s+c_v＝1，且两者都是大于1的常数，c_s的计算方法为：

λ是一个大于1的正常数，当ρ_k/r_k小于1时，跟踪目标会变为威胁因素，跟踪策略变为避障策略；

步骤四：根据态势空间实现避障策略制定；

球s_k是t_k时刻的运动目标相对于无人飞行器的一步态势空间，p_u,kq是从点p_u,k开始的与s_k的表面相切的射线，当ρ_k/r_k＞1时，令m_k表示射线方向的单位矢量，则m_k和n_k之间的角度称为态势角度，即α，判断运动目标为威胁的条件是β≤α，其中β是n_k和v_uo,k之间的夹角，β满足0＜β≤π/2，避障策略的加速度为：

其中v_p⊥,k为v_uo,k在与n_k方向垂直的方向上的分量；

当有M个威胁同时存在时：

步骤五：融合避障策略和跟踪策略，避障策略有绝对的优先权，即：

然后在匀加速模型CA中，根据

实现生成下一时刻的到达点，即完成路径规划；到达点生成方法为：

其中

Images