CN108303095B - 适用于非高斯系统的鲁棒容积目标协同定位方法 - Google Patents

Abstract

适用于非高斯系统的鲁棒容积目标协同定位方法，属于非合作目标协同定位技术领域。本发明是为了解决现有协同定位方法认为传感器的测量噪声符合高斯白噪声，造成对目标估计精度差的问题。它基于当前统计模型建立状态方程；基于传感器测角与机动目标位置关系建立测量方程；然后基于三阶球面‑径向容积准则进行时间更新，获取机动目标状态和协方差的一步预测；再利用机动目标状态和协方差的一步预测生成容积点，根据系统测量方程计算量测预测值和互协方差；最后，将测量更新转换为线性衰退的求解问题，通过求取指标函数的最小值，利用迭代方法获得机动目标状态和协方差的后验估计值，完成机动目标的协同定位。本发明用于目标的协同定位。

Description

适用于非高斯系统的鲁棒容积目标协同定位方法

技术领域

本发明涉及适用于非高斯系统的鲁棒容积目标协同定位方法，属于非合作目标协同定位技术领域。

背景技术

目标协同定位是工程领域中一种常用的确定非合作目标位置和速度的方法。该方法以目标运动模型为基础，基于两个或多个传感器对目标进行测量，通过卡尔曼滤波等手段对来自目标运动模型的信息和带误差的测量信息进行融合，从而估计出目标的位置和速度。但传统的协同定位方法均认为传感器的测量噪声符合高斯白噪声，但在实际系统中，传感器的噪声呈现为以闪烁噪声为代表的非高斯噪声。在这种情况下，传统的协同定位方法出现估计精度下降甚至发散的问题。

发明内容

本发明目的是为了解决现有协同定位方法认为传感器的测量噪声符合高斯白噪声，造成对目标估计精度差的问题，提供了一种适用于非高斯系统的鲁棒容积目标协同定位方法。

本发明所述适用于非高斯系统的鲁棒容积目标协同定位方法，它包括：

基于当前统计模型建立状态方程；基于传感器测角与机动目标位置关系建立测量方程；然后基于三阶球面-径向容积准则进行时间更新，获取机动目标状态和协方差的一步预测；再利用机动目标状态和协方差的一步预测生成容积点，根据系统测量方程计算量测预测值和互协方差；最后，将测量更新转换为线性衰退的求解问题，通过求取指标函数的最小值，利用迭代方法获得机动目标状态和协方差的后验估计值，完成机动目标的协同定位。

本发明的优点：本发明针对目标定位过程中传感器量测受闪烁噪声等非高斯噪声污染而提出，用于提高非高斯系统的目标协同定位精度。通过数学仿真表明，本发明方法更好地处理了传感器所引入的非高斯噪声，具有更高的位置和速度估计精度，可以更为有效地处理非高斯系统下的目标协同定位问题。

附图说明

图1是本发明所述适用于非高斯系统的鲁棒容积目标协同定位方法的协同跟踪场景示意图；

图2是分别在l₁、l₂和Huber范数时，代价函数ρ的函数曲线；

图3是分别在l₁、l₂和Huber范数时，权重函数ψ的函数曲线；

图4是采用本发明方法和传统方法进行目标位置估计的误差对比图；

图5是采用本发明方法和传统方法进行目标速度估计的误差对比图。

具体实施方式

下面结合图1至图5，对本发明的具体实施方式进行说明：

本发明所述适用于非高斯系统的鲁棒容积目标协同定位方法，它包括：

基于当前统计模型建立状态方程；基于传感器测角与机动目标位置关系建立测量方程；然后基于三阶球面-径向容积准则进行时间更新，获取机动目标状态和协方差的一步预测；再利用机动目标状态和协方差的一步预测生成容积点，根据系统测量方程计算量测预测值和互协方差；最后，将测量更新转换为线性衰退的求解问题，通过求取指标函数的最小值，利用迭代方法获得机动目标状态和协方差的后验估计值，完成机动目标的协同定位。

当前统计模型认为，当机动目标以加速度机动时，下一时刻的加速度取值是有限的，即只能在“当前”的加速度领域内，并且机动加速度的“当前”统计概率服从修正的瑞利分布，均值是“当前”的加速度预测。该模型本质上是一种非零均值时间相关模型。

所述机动目标的当前统计模型为：

式中X(t)为机动目标位置，为加速度当前均值，a(t)为加速度当前值，α为机动频率，即为机动时间常数的倒数，ω(t)为系统噪声；

设定加速度当前均值在采样周期内为常数，则：

进而将机动目标的当前统计模型变形为：

公式(4)的模型本质上是非零均值时间相关模型，加速度的“当前”概率密度用修正的瑞利分布描述，均值为“当前”加速度预测值，随机加速度在时间轴上符合一阶时间相关过程。

将公式(4)离散化获得相应的离散形式为：

其中为机动目标状态，F(k)为状态转移矩阵，X₁(k-1)为k-1时刻机动目标状态，U(k)为加速度驱动矩阵，为k时刻加速度当前均值；w(k)为过程噪声，是均值为零方差为Q(k)的高斯白噪声；

其中

式中T为采样周期；

加速度当前值a(t)的方差为：

式中a_max为最大加速度正值，a_-max为最大加速度负值；

可以看出，a_max和a_-max直接影响“当前”统计模型中“当前”加速度的方差而的大小又直接影响过程噪声的方差Q(k)，从而影响滤波器的跟踪性能。当a_max和a_-max的绝对值取较小值时，跟踪系统的系统方差较小，跟踪精度高，但跟踪目标机动变化范围较窄，且响应速度较慢；当a_max和a_-max的绝对值取较大值时，跟踪系统的系统方差较大，跟踪系统对较大范围机动的目标能以较大的系统方差保持快速响应，但跟踪精度较低；当a_max和a_-max取一定值后，在目标机动加速度较大时，系统方差较小，跟踪精度较高；但在机动加速度较小时，即目标弱机动情况下，系统的方差很大，跟踪精度较低。

对于空间中的机动目标，定义其在惯性坐标系下的位置、速度和加速度的状态量x为：

x＝[x y z v_x v_y v_z a_x a_y a_z]^T (9)

式中x为机动目标在x轴方向的值，y为机动目标在y轴方向的值，z为机动目标在z轴方向的值，v_x为机动目标在x轴方向的速度，v_y为机动目标在y轴方向的速度，v_z为机动目标在z轴方向的速度，a_x为机动目标在x轴方向的加速度，a_y为机动目标在y轴方向的加速度，a_z为机动目标在z轴方向的加速度；

机动目标的状态方程为：

x_k+1为k+1时刻机动目标状态，Φ_k+1/k为状态转移矩阵，x_k为k时刻机动目标状态，U_k为k时刻加速度驱动矩阵，为k时刻当前加速度均值，Γ_k为噪声驱动矩阵，

状态转移矩阵

系统噪声驱动矩阵

式中I为3×3的单位矩阵。

测量方程：

由于被动传感器仅能测量目标的视线角信息(高低角和方位角)，因此常利用两个传感器进行协同探测。

采用两个传感器测量机动目标的视线高低角γ和方位角η，两个传感器的测量量z为：

z＝[γ₁ η₁ γ₂η₂]^T (11)

γ₁为一号传感器的视线高低角，η₁为一号传感器的方位角，γ₂为二号传感器的视线高低角，η₂为二号传感器的方位角；

一号传感器在惯性坐标系下的位置坐标x_s,1为：

x_s,1＝[x_s,1 ,y_s,1, z_s,1]^T (12)

式中x_s,1为一号传感器在x轴方向的坐标值，y_s,1为一号传感器在y轴方向的坐标值，z_s,1为一号传感器在z轴方向的坐标值；

二号传感器在惯性坐标系下的位置坐标x_s,2为：

x_s,2＝[x_s,2,y_s,2,z_s,2]^T (13)

式中x_s,2为一号传感器在x轴方向的坐标值，y_s,2为一号传感器在y轴方向的坐标值，z_s,2为一号传感器在z轴方向的坐标值；

则机动目标相对于一号传感器的位置在惯性坐标系下表示为：

机动目标相对于二号传感器的位置在惯性坐标系下表示为：

由于传感器输出的测量信息为其本体坐标系下的高低角和方位角，因此需要将机动目标相对于传感器的位置信息在传感器本体系下进行表示：

机动目标相对于一号传感器在一号传感器本体系下的位置信息为：

式中为惯性坐标系到一号传感器本体系的坐标转换矩阵；

机动目标相对于二号传感器在二号传感器本体系下的位置信息为：

为惯性坐标系到二号传感器本体系的坐标转换矩阵；

式(16)写成展开形式为：

x_rs,1为一号传感器在x方向的位置分量，y_rs,1为一号传感器在y方向的位置分量，z_rs,1为一号传感器在z方向的位置分量，

式(17)写成展开形式为：

x_rs,2为二号传感器在x方向的位置分量，y_rs,2为为二号传感器在y方向的位置分量，z_rs,2为二号传感器在z方向的位置分量，

根据高低角和方位角的定义，一号传感器的视线高低角γ₁为：

一号传感器的方位角η₁为：

η₁＝arctan2(-z_rs,1,x_rs,1) (21)

二号传感器的视线高低角γ₂为：

二号传感器的方位角η₂为：

η₂＝arctan2(-z_rs,2,x_rs,2) (23)

考虑传感器测量噪声的影响，可建立传感器的测量方程如下：

式中h(x)为测量方程，v为测量噪声矢量，为一号传感器高低角测量噪声？为一号传感器方位角测量噪声，为二号传感器高低角测量噪声，为二号传感器方位角测量噪声。

鲁棒容积滤波算法：

所述三阶球面-径向容积规则包括：

计算如下形式的积分运算：

式中F(t)为非线性函数，Rⁿ为n维实数，n为系统维数，t为均值为方差为Σ的随机变量，

不考虑常值部分，将式(25)中的多维积分简化为：

式中I(f)表示f的积分f(x)为系统方程；

令x＝ry，其中y^Ty＝1，r∈[0,∞)，通过积分转化为如下的球面-径向坐标积分：

式中r为球面半径，σ(·)为Uⁿ上的元素，Uⁿ为n维单位球面；

将积分I(f)分解，变为一个球面积分S(r)和一个径向积分R：

由此可知，该积分坐标变换是将积分计算式(26)转换为多维空间中某个几何体的容积计算，被称为容积规则。

对于球面积分和径向积分，假定分别由如下数值积分方法近似：

式中{y_i,w_s,i}为计算球面积分的积分点集合，N_s为相应的积分点数目；{r_j,w_r,j}为计算径向积分的积分点集合，N_r为相应的积分点数目；S(r_j)表示球面积分；

将式(26)所示的积分I(f)近似表示为：

将三阶球面准则和三阶径向准则代入式(32)所示的积分公式中，得到三阶球面-径向容积准则：

式中，N＝N_rN_s为积分点数目，e_i∈Rⁿ表示第i个元素为1的单位向量，A_n为n维单位球的表面积；

在积分权重函数为高斯概率密度函数下的三阶球面-径向容积准则为：

式中P_x为系统噪声，w_i为容积点权重，ξ_i为为点集，

三阶球面-径向容积准则的积分点数目为N＝2n，其积分点及权重如下：

式中χ_i为容积点，

其中，[1]_i表示点集[1]的第i组元素，[1]为

广义极大似然估计：采用Huber提出的广义极大似然估计方法求解如下的残差函数J(x)的极小值：

式中，ρ为任意函数，m为测量量维数，ζ_i为关心的自变量，

极大似然估计为ρ(ζ_i)＝-ln[f(ζ_i)]时，作为广义极大似然估计的特例。

对式(37)求导，得到广义极大似然估计的解为

式中中间变量φ(ζ_i)为：

定义权重函数ψ(ζ_i)和矩阵Ψ分别为：

Ψ＝diag[ψ(ζ_i)] (41)

式(38)解的隐函数方程为

H^TΨ(Hx-z)＝0 (42)

式中H为测量矩阵；

在广义极大似然估计问题中，可以通过选择函数ρ的形式获取估计器的某些特定性质。广义极大似然估计的一个重要的特性就是鲁棒性，Huber选择ρ函数为：

式中，μ为可调参数，ρ函数为混合l₁、l₂范数最小函数；当γ→0时，ρ函数趋近于l₁范数最小，当γ→∞时，ρ函数趋近于l₂范数最小。图2和图3分别给出了l₁、l₂和Huber范数时代价函数ρ和权重函数ψ的函数曲线。

利用加权迭代法求解非线性隐函数，式(42)改写为

H^TΨHx＝H^TΨz (44)

于是

x＝(HTΨH)-¹HTΨz (45)

由于Ψ依赖于残差ζ_i，并且ζ＝Hx-y，式(42)的迭代解形式为

x^j+1＝(H^TΨ^(j)H)^-1H^TΨ^(j)z (46)

式中，j为迭代次数，上式的收敛后的值作为状态估计；

迭代初值x⁽⁰⁾为：

x⁽⁰⁾＝(H^TH)^-1H^Tz。 (47)

Huber已证明，当选择式(43)作为ρ函数的形式时，M估计滤波对测量噪声为闪烁噪声的系统具有渐近最优鲁棒性。

鲁棒容积滤波算法的基本步骤：

参数初始化：

状态估计初值和误差协方差阵初值分别为

式中E[x₀]为初始状态x₀的均值，对k＝1,2,…运行如下步骤：

计算k-1时刻的容积点P_k-1k-1：

式中S_k-1通过P_k-1k-1的Cholesky的分解得到；

计算k时刻的一步预测值及其协方差

式中Q_k-1为k-1时刻系统噪声协方差；

计算用于测量更新的容积点：

其中，点集ξ_i与式(51)一致；

计算量测预测值与互协方差P_xz：

为关于一步预测容积点的测量方程；

构造线性回归问题：

状态真值和预测值的关系为：

δ_k为预测值偏差；

对测量方程进行线性化

υ_k为测量值偏差；

其中，

H_k＝[(P_kk-1)^-1P_xz]^T (62)

将量测更新转化为线性回归问题：

定义变量：

则线性回归模型可以转化为

y_k＝M_kx_k+ξ_k (68)

求解线性回归问题：

将基于Huber滤波理论的鲁棒容积滤波测量更新转换为求解如下指标函数的最小值J(x_k)：

式中，ρ为指标函数，其具体形式为式(43)；

ζ_i为ζ的第i个元素，且

指标函数J最小的解满足如下方程

式中

φ(ζ_i)＝ρ'(ζ_i) (72)

定义ψ(ζ_i)和Ψ(ζ_i)分别为

Ψ(ζ_i)＝diag[ψ(ζ_i)] (74)

于是，式(71)可改写为

计算状态估计值及协方差：

通过迭代的方法得到上式的解为

式中，j为迭代次数；

迭代的初值选择为：

式(76)的收敛解为鲁棒容积滤波的测量更新

估计协方差矩阵P_k为

下面通过数学仿真来说明本发明方法的效果：

考虑图1所示场景，两架安装有被动传感器的无人机对一地面目标进行协同探测，无人机之间利用数据链进行通信，传输传感器所获取的测量信息，地面目标则在公路上进行变加速直线运动。

在地面坐标下描述目标运动，地面系X轴指向东，Y轴指向上，Z轴指向南，目标朝东南方向沿15°上坡进行变加速直线运动，其轨迹满足如下方程

其中，c₁＝cos15°sin45°，c₂＝sin15°，c₃＝cos15°cos45°。

两无人机在目标后方2000米处拉开500米基线进行跟踪，其传感器测量帧频为10Hz，测量噪声为闪烁噪声，其概率密度函数p(x)为：

其中，ε＝0.2为闪烁概率，和为相互独立的高斯分布，σ₁＝0.3°，σ₂＝1.5°。

在上述仿真场景下，基于250次独立的蒙特卡洛数学仿真，获得图4和图5所示仿真结果。

图4和图5分别对比了本专利所公开的基于鲁棒容积滤波的协同定位方法和基于扩展卡尔曼滤波的传统协同定位方法的位置和速度估计精度。可以看出，在非高斯系统下，本专利所给出的协同定位方法更好地处理了传感器所引入的非高斯噪声，具有更高的位置和速度估计精度，可以更为有效地处理非高斯系统下的目标协同定位问题。

Claims (4)

1.一种适用于非高斯系统的鲁棒容积目标协同定位方法，其特征在于，它包括：

基于当前统计模型建立状态方程；基于传感器测角与机动目标位置关系建立测量方程；然后基于三阶球面-相径容积准则进行时间更新，获取机动目标状态和协方差的一步预测；再利用机动目标状态和协方差的一步预测生成容积点，根据系统测量方程计算量测预测值和互协方差；最后，将测量更新转换为线性衰退的求解问题，通过求取指标函数的最小值，利用迭代方法获得机动目标状态和协方差的后验估计值，完成机动目标的协同定位；

采用两个传感器测量机动目标的视线高低角γ和方位角η，两个传感器的测量量Z为：

Z＝[γ₁ η₁ γ₂ η₂]^T (1)

γ₁为一号传感器的视线高低角，η₁为一号传感器的方位角，γ₂为二号传感器的视线高低角，η₂为二号传感器的方位角；

一号传感器在惯性坐标系下的位置坐标X_S,1为：

X_s，1＝[x_s，1，y_s，1，z_s，1]^T (2)

式中x_s,1为一号传感器在x轴方向的坐标值，y_s,1为一号传感器在y轴方向的坐标值，z_s,1为一号传感器在z轴方向的坐标值；

二号传感器在惯性坐标系下的位置坐标X_S,2为：

X_s，2＝[x_s，2，y_s，2，z_s，2]^T (3)

式中x_s,2为二号传感器在x轴方向的坐标值，y_s,2为二号传感器在y轴方向的坐标值，z_s,2为二号传感器在z轴方向的坐标值；

则机动目标相对于一号传感器的位置r₁ ⁱ在惯性坐标系下表示为：

r₁ ⁱ＝x-x_s,1 (4)

式中，x＝[x y z]^T,x为机动目标在x轴方向的位置坐标，y为机动目标在y轴方向的位置坐标，z为机动目标在z轴方向的位置坐标；

机动目标相对于二号传感器的位置在惯性坐标系下表示为：

由于传感器输出的测量信息为其本体坐标系下的视线高低角和方位角，因此需要将机动目标相对于传感器的位置信息在传感器本体系下进行表示：

机动目标相对于一号传感器在一号传感器本体系下的位置信息r₁ ^s为：

式中为惯性坐标系到一号传感器本体系的坐标转换矩阵；

机动目标相对于二号传感器在二号传感器本体系下的位置信息为：

为惯性坐标系到二号传感器本体系的坐标转换矩阵；

式(6)写成展开形式为：

x_rs,1为一号传感器在x方向的位置分量，y_rs,1为一号传感器在y方向的位置分量，z_rs,1为一号传感器在z方向的位置分量，

式(7)写成展开形式为：

x_rs,2为二号传感器在x方向的位置分量，y_rs,2为二号传感器在y方向的位置分量，z_rs,2为二号传感器在z方向的位置分量，

一号传感器的视线高低角γ₁为：

一号传感器的方位角η₁为：

η₁＝arctan2(-z_rs,1,x_rs,1) (11)

二号传感器的视线高低角γ₂为：

二号传感器的方位角η₂为：

η₂＝arctan2(-z_rs,2,x_rs,2) (13)

建立传感器的测量方程如下：

式中h(X)为测量方程，v为测量噪声矢量，为一号传感器视线高低角测量噪声，为一号传感器方位角测量噪声，为二号传感器视线高低角测量噪声，为二号传感器方位角测量噪声。

2.根据权利要求1所述的适用于非高斯系统的鲁棒容积目标协同定位方法，其特征在于，

所述机动目标的当前统计模型为：

式中X(t)为机动目标位置，为加速度当前均值，a(t)为加速度当前值，α为机动频率，ω(t)为系统噪声；

设定加速度当前均值在采样周期内为常数，则：

进而将机动目标的当前统计模型变形为：

将公式(18)离散化获得相应的离散形式为：

其中为机动目标状态，F(k)为状态转移矩阵，X₁(k-1)为k-1时刻机动目标状态，U(k)为加速度驱动矩阵，为k时刻加速度当前均值；w(k)为过程噪声，是均值为零方差为Q(k)的高斯白噪声；

其中

式中T为采样周期；

加速度当前值a(t)的方差为：

式中a_max为最大加速度正值，a_-max为最大加速度负值；

对于空间中的机动目标，定义其在惯性坐标系下的位置、速度和加速度的状态量X为：

X＝[x y z v_x v_y v_z a_x a_y a_z]^T (23)

式中x为机动目标在x轴方向的位置坐标，y为机动目标在y轴方向的位置坐标，z为机动目标在z轴方向的位置坐标，v_x为机动目标在x轴方向的速度，v_y为机动目标在y轴方向的速度，v_z为机动目标在z轴方向的速度，a_x为机动目标在x轴方向的加速度，a_y为机动目标在y轴方向的加速度，a_z为机动目标在z轴方向的加速度；

机动目标的状态方程为：

X_k+1为k+1时刻机动目标状态，Φ_k+1/k为状态转移矩阵，X_k为k时刻机动目标状态，U_k为k时刻加速度驱动矩阵，为k时刻当前加速度均值，Γ_k为噪声驱动矩阵，

状态转移矩阵

系统噪声驱动矩阵

式中I为3×3的单位矩阵。

3.根据权利要求2所述的适用于非高斯系统的鲁棒容积目标协同定位方法，其特征在于，

采用广义极大似然估计求解如下的残差函数J(x)的极小值：

式中，ρ为任意函数，m为测量维数，ζ_i为关心的自变量，

对式(37)求导，得到广义极大似然估计的解为

式中中间变量φ(ζ_i)为：

定义权重函数ψ(ζ_i)和矩阵Ψ分别为：

Ψ＝diag[ψ(ζ_i)] (41)

式(38)解的隐函数方程为

H^TΨ(Hx-z)＝0 (42)

式中H为测量矩阵；

选择ρ函数为：

式中，μ为可调参数，ρ函数为混合l₁、l₂范数最小函数；

利用加权迭代法求解非线性隐函数，式(42)改写为

H^TΨHx＝H^TΨz (44)

于是

x＝(H^TΨH)^-1H^TΨz (45)

由于Ψ依赖于残差ζ_i，并且ζ＝Hx-y，式(42)的迭代解形式为

x^j+1＝(H^TΨ^(j)H)^-1H^TΨ^(j)z (46)

式中，j为迭代次数，上式的收敛后的值作为状态估计；

迭代初值x⁽⁰⁾为：

x⁽⁰⁾＝(H^TH)^-1H^Tz (47)。

4.根据权利要求3所述的适用于非高斯系统的鲁棒容积目标协同定位方法，其特征在于，

参数初始化：

状态估计初值和误差协方差阵初值分别为

式中E[x₀]为初始状态x₀的均值，表示的期望，对k＝1,2,…运行如下步骤：

计算k-1时刻的容积点

式中S_k-1通过P_k-1|k-1的Cholesky的分解得到，P_k-1|k-1为状态误差协方差阵，为k-1时刻的容积点，为k-1时刻的状态估计值；

计算k时刻的一步预测值及其协方差

式中Q_k-1为k-1时刻系统噪声协方差；

计算用于测量更新的容积点：

其中，点集ξ_i与式(51)一致；

计算量测预测值与互协方差P_xz：

为关于一步预测容积点的测量方程；

构造线性回归问题：

状态真值和预测值的关系为：

δ_k为预测值偏差；

对测量方程进行线性化

υ_k为测量值偏差，为k时刻的状态一步预测值的预测方程；

其中，

H_k＝[(P_k|k-1)^-1P_xz]^T (62)

将量测更新转化为线性回归问题：

定义变量：

其中，R_k为测量噪声方差阵；

则线性回归模型可以转化为

y_k＝M_kx_k+ξ_k (68)

求解线性回归问题：

将基于Huber滤波理论的鲁棒容积滤波测量更新转换为求解如下指标函数的最小值J(x_k)：

ζ_i为ζ的第i个元素，且

指标函数J最小的解满足如下方程

式中

φ(ζ_i)＝ρ'(ζ_i) (72)

定义ψ(ζ_i)和Ψ(ζ_i)分别为

Ψ(ζ_i)＝diag[ψ(ζ_i)] (74)

于是，式(71)可改写为

计算状态估计值及协方差：

通过迭代的方法得到上式的解为

式中，j为迭代次数；

迭代的初值选择为：

式(76)的收敛解为鲁棒容积滤波的测量更新

估计协方差矩阵P_k为