基于无人机的运动目标UKF滤波方法与流程

本发明涉及无人机目标定位技术领域，尤其涉及一种基于无人机的运动目标ukf滤波方法。

背景技术：

对运动目标而言，想要实现高精度轨迹跟踪能力，必须建立准确的目标运动模型(系统状态方程)和系统量测模型。然而目标模型和量测模型的建立很大程度上取决于自身所选取的坐标系。目标模型和量测模型大多建立在大地直角坐标系中，其中目标模型有cv、ca、时间相关模型以及当前统计模型等。状态变量为目标在大地直角坐标系中的位置、速度和加速度，量测值以目标定位算法求出的目标在大地直角坐标系中的坐标值为量测量。这样带来的后果一方面由于是间接计算出的结果导致滤波器使用时的量测值已经夹杂了非常大的目标定位算法误差，需要误差的统计特性，不利于提高目标定位精度；另一方面在于增大了量测噪声的不确定性，因为误差值与目标相对无人机距离有关，并非一成不变。

为提高系统量测方程的准确性，需要建立以传感器直接测量值(r,α,λ)^t(目标斜距、方位角和俯仰角)为量测变量的非线性量测方程，系统量测噪声方差阵直接从传感器获取，将大大提高量测模型的准确性。目前，针对这一问题的主要解决方法为混合坐标系求解法，以为球坐标系下状态变量，建立线性量测方程以及直角坐标系与球坐标系的转换关系。但是该方法涉及强非线性函数的一、二阶求导，过程复杂繁琐，并不好用，因此有必要建立符合无人机特性的简单有效的系统量测方程。

技术实现要素：

本发明所要解决的技术问题是针对传统目标跟踪方案中定位模型和滤波器彼此分开所带来的噪声模型不准确导致系统精度不高的问题，提供一种基于无人机的运动目标ukf滤波方法。

本发明为解决上述技术问题采用以下技术方案：

一种基于无人机的运动目标ukf滤波方法，包含以下步骤：

步骤1)，在无人机上设置红外光电吊舱和雷达，其中，所述红外光电吊舱用于测量运动目标相对无人机的距离，所述雷达用于测量运动目标相对无人机的方位角和俯仰角；

步骤2)，利用无人机上的红外光电吊舱、雷达对运动目标进行测量，获得运动目标相对无人机的距离、方位角和俯仰角(r,α,λ)^t，其中，r为运动目标相对无人机的距离，α为运动目标相对无人机的方位角，λ为运动目标相对无人机的俯仰角；

步骤3)，根据运动学原理建立运动目标的匀加速数学模型，其系统状态方程如下：

xk＝φk,k-1xk-1+gk-1wk-1

其中，

xk为运动目标k时刻的状态量，xk-1为运动目标k-1时刻的状态量，

wk-1为k-1时刻状态方程零均值高斯白噪声，i3×3为单位矩阵，t为采样周期；

步骤4)，令运动目标的非线性量测方程为zk＝h(xk)+vk，其中，h(xk)为量测函数，vk是k时刻量测零均值高斯白噪声，方差为r，zk＝[r,α,λ]^t；

建立运动目标在大地直角坐标系与运动目标相对无人机传感器基座坐标系之间的关系：

采用齐次坐标转换方法，得到运动目标的大地直角坐标系坐标值(x,y,z)到基座坐标系坐标值(xb,yb,zb)的转换矩阵其中：

hs是无人机高度，rn是卯酉圈曲率半径，λs是无人机纬度，αs是无人机经度，是无人机的横滚角，θas是无人机的俯仰角，ψas是无人机的偏航角，是测量设备基座的横滚角，δθba是测量设备基座的俯仰角，δψba是测量设备基座的偏航角；

运动目标相对于传感器基座坐标系坐标值(xb,yb,zb)与传感器对目标量测值(r,α,λ)^t之间的关系如下：

则结合转换矩阵运算得到(r,α,λ)与(x,y,z)的非线性关系式h(xk)；

步骤5)，ukf滤波

步骤5.1)，根据k-1时刻的运动目标的状态量及协方差pk-1构造2n+1个对称的sigma采样点，2n+1为采样点个数；

步骤5.2)，结合运动目标匀加速数学模型的系统状态方程得到k时刻的运动目标状态方程和协方差的一步预测值；

步骤5.3)，计算运动目标的坐标值和量测值的一步预测值；

步骤5.4)，计算自协方差阵互协方差阵和卡尔曼滤波增益；

步骤5.5)，更新运动目标坐标及协方差。

作为本发明基于无人机的运动目标ukf滤波方法进一步的优化方案，所述步骤5.1)中，根据以下公式构造2n+1个对称的sigma采样点：

以上式中χ表示状态量通过非线性量测函数h(xk)+vk的传播，χ的下标表示采样点顺序及时刻；γ＝ε²(n+κ)-n，ε、κ均为预设的阈值，其中，ε决定采样点距离均值的远近程度；κ≥0保证方差阵的半正定性。

作为本发明基于无人机的运动目标ukf滤波方法进一步的优化方案，所述步骤5.2)中，根据以下公式得到k时刻的运动目标状态方程和协方差的一步预测值：

作为本发明基于无人机的运动目标ukf滤波方法进一步的优化方案，所述步骤5.3)中，由以下公式计算运动目标的坐标值和量测值的一步预测值：

其中，

作为本发明基于无人机的运动目标ukf滤波方法进一步的优化方案，所述步骤5.4)中，由以下公式计算自协方差阵互协方差阵和卡尔曼滤波增益：

式中，rk为k时刻量测噪声的方差。

β为与状态向量的先验分布相关的参数。

作为本发明基于无人机的运动目标ukf滤波方法进一步的优化方案，所述步骤5.5)中，由以下公式计算目标坐标及协方差更新：

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

本发明直接以目标斜距、方位角和俯仰角三个参数为量测变量，在很大程度上降低了统计量测噪声特性时的工作量，并提高了噪声模型的准确性。与混合坐标系相比，系统状态变量统一，不存在混合坐标系中状态变量的转换问题，结合无人机特性构建的目标非线性量测方程不涉及强非线性方程的一二阶求导，大大降低了模型的复杂度。最终仿真结果验证了该模型的快速收敛性能和高精度性能。

附图说明

图1是本发明的流程示意图；

图2是本发明中x轴位置误差图；

图3是本发明中y轴位置误差图；

图4是本发明中z轴位置误差图；

图5是本发明中x轴速度误差图；

图6是本发明中y轴速度误差图；

图7是本发明中z轴速度误差图；

图8是本发明中x轴加速度误差图；

图9是本发明中y轴加速度误差图；

图10是本发明中z轴加速度误差图。

具体实施方式

下面结合附图对本发明的技术方案做进一步的详细说明：

本发明主要为面向基于无人机的运动目标ukf滤波方法，该方案的设计流程图如附图1所示。该方法利用雷达、红外光电吊舱等传感器为量测手段获得目标相对于传感器的距离、方位角和俯仰角等量测信息，以该量测信息为输入量结合无人机机载导航设备获取的无人机自身信息建立非线性量测方程。与此同时，由目标运动模型获得目标预测值，结合非线性量测方程与目标运动模型进行无迹卡尔曼滤波获得当前时刻精度较高的目标状态量。

本发明包含以下步骤：

步骤1)，在无人机上设置红外光电吊舱和雷达，其中，所述红外光电吊舱用于测量运动目标相对无人机的距离，所述雷达用于测量运动目标相对无人机的方位角和俯仰角；

步骤2)，利用无人机上的红外光电吊舱、雷达对运动目标进行测量，获得运动目标相对无人机的距离、方位角和俯仰角(r,α,λ)^t，其中，r为运动目标相对无人机的距离，α为运动目标相对无人机的方位角，λ为运动目标相对无人机的俯仰角；

步骤3)，根据运动学原理建立运动目标的匀加速数学模型，系统状态方程如下：

xk＝φk,k-1xk-1+gk-1wk-1(1)

其中，

xk为运动目标k时刻的状态量，xk-1为运动目标k-1时刻的状态量，φk,k-1为k-1时刻到k时刻的系统状态转移矩阵，gk-1为k-1时刻系统噪声矩阵，wk-1是k-1时刻系统零均值高斯白噪声，方差为q，t为采样周期，i3×3为三阶单位矩阵。把目标加速度变换量当做系统噪声干扰，提高了系统对机动目标的跟踪能力。

步骤4)，令运动目标的非线性量测方程为

zk＝h(xk)+vk(2)

其中，h(xk)为量测函数，vk是k时刻量测零均值高斯白噪声，方差为r，zk＝[r,α,λ]^t为目标相对雷达的距离、方位角和俯仰角度信息。由于状态变量是基于大地直角坐标系的，然而量测变量是基于传感器基座坐标系，因此需要构建两者之间的联系关系式。根据下面公式计算以上两个坐标系之间的关系式。

记目标在大地直角坐标系中的坐标为(x,y,z)，目标在无人机传感器基座坐标系中的坐标为(xb,yb,zb)，表示采用齐次坐标系表示的大地直角坐标系到基座坐标系转换矩阵，则运动目标在大地直角坐标系与运动目标相对无人机传感器基座坐标系之间的关系为：

采用齐次坐标转换方法，得到运动目标的大地直角坐标系坐标值(x,y,z)到基座坐标系坐标值(xb,yb,zb)的转换矩阵为：

其中，为从大地直角坐标系(g系)到地理系(s系)的变换矩阵，为从地理系(s系)到载机系(a系)的变换矩阵，为从载机系(a系)到基座系(b系)的变换矩阵。

下面分别求取和

其中，αs,λs,hs分别为载机的大地经度、纬度和高度，e为地球参考椭球的第一偏心率，rn为卯酉圈曲率半径。

其中，φas,θas,ψas分别为载机的横滚角、俯仰角和偏航角。

其中，δθba,δψba分别为减振器的横滚角、俯仰角和偏航角。

运动目标相对于传感器基座坐标系坐标值(xb,yb,zb)与传感器对目标量测值(r,α,λ)^t之间的关系如下：

则结合转换矩阵运算，得到(r,α,λ)与(x,y,z)的非线性关系式h(xk)。

步骤5)，ukf滤波

步骤5.1)，计算采样点

根据k-1时刻的目标状态量及协方差pk-1构造2n+1个对称的sigma采样点如下：

以上式中χ表示状态量通过非线性量测函数h(xk)+vk的传播，χ的下标表示采样点顺序及时刻；γ＝ε²(n+κ)-n，ε、κ均为预设的阈值，其中，ε决定采样点距离均值的远近程度；κ≥0保证方差阵的半正定性。

步骤5.2)，建立时间更新方程

由系统状态方程(1)可得到k时刻的运动目标状态更新方程和协方差一步预测值分别为：

步骤5.3)，计算目标坐标值及量测量的一步预测值

由(9)式计算得到的采样点及量测方程(2)获得k-1时刻对于k时刻的一步预测采样点χk/k-1、目标坐标值预测值及量测量一步预测值如下式所示：

其中，

步骤5.4)，将式(12)、(13)代入下式中求得自协方差阵和互协方差阵为：

(14)式中，rk为k时刻量测噪声的方差。

β为与状态向量的先验分布相关的参数。

利用式(14)、(15)求得卡尔曼滤波增益

步骤5.5)，目标坐标及协方差更新

将(16)、(13)、(14)和(15)式求得的量代入以下(17)、(18)式得到状态量及协方差更新方程

将步骤3)建立的运动目标状态方程与步骤4)建立的系统量测方程代入到步骤5)中进行滤波处理，可得到k时刻较精确的目标状态量，该算法可循环计算，由此可根据传感器直接对目标的量测值得到各时刻目标的精确定位。

附图2为用本发明ukf滤波后和用线性卡尔曼滤波后x轴方向位置误差对比图。

附图3为分别用ukf滤波和用线性卡尔曼滤波后y轴方向位置误差对比图。

附图4为分别用ukf滤波和用线性卡尔曼滤波后z轴方向位置误差对比图。

附图5为分别用ukf滤波和用线性卡尔曼滤波后x轴方向速度误差对比图。

附图6为分别用ukf滤波和用线性卡尔曼滤波后y轴方向速度误差对比图。

附图7为分别用ukf滤波和用线性卡尔曼滤波后z轴方向速度误差对比图。

附图8为分别用ukf滤波和用线性卡尔曼滤波后x轴方向加速度误差对比图。

附图9为分别用ukf滤波和用线性卡尔曼滤波后y轴方向加速度误差对比图。

附图10为分别用ukf滤波和用线性卡尔曼滤波后z轴方向加速度误差对比图。

由图2～图10可知，初始时刻，改进后的ukf滤波方法和卡尔曼滤波方法具有相同的定位跟踪精度，但随着时间的推移，误差差异开始逐渐凸显，且目标距离无人机越远，差异越明显，尤其在速度和加速度方面表现尤为突出，改进后的算法精度更高。

本技术领域技术人员可以理解的是，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)具有与本发明所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样定义，不会用理想化或过于正式的含义来解释。

以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。