CN106568442B - 一种具有鲁棒特性的协同导航滤波方法 - Google Patents

Abstract

本发明公开了一种具有鲁棒特性的协同导航滤波方法。该方法是利用投影统计对从艇接收到的来自主艇的量测信息进行异常判断，获取量测信息的权值后，建立基于改进Huber估计的协同导航鲁棒滤波模型，提高了从艇的定位精度。本发明的具有鲁棒特性的协同导航滤波方法通过结合投影统计异常点鉴别方法和改进Huber估计法提高在量测噪声异常情况下的协同导航从艇的定位精度，增强滤波算法的鲁棒性。该方法的特点是当量测噪声为近似高斯分布并且出现异常值时滤波仍可以保持较高的精度，具有鲁棒性。

Description

一种具有鲁棒特性的协同导航滤波方法

技术领域

本发明涉及导航技术领域，具体地说，涉及一种具有鲁棒特性的协同导航滤波方法。

背景技术

自主水下航行器(Autonomous Underwater Vehicles,AUV)在民用还是军事方面都有广泛的应用。随着对海洋的探索越来越深入，使得AUV的使命也变得越来越复杂化和多样化，如果仅仅依靠单个AUV去实现这些功能，不仅在成本上会提高很多，而且可靠性也会降低，而由多台水下AUV组成的协作系统具有空间分布、功能分布、冗余等特点。然而不论单AUV还是多AUV系统以及其执行何种任务，导航系统的优劣直接影响系统的性能好坏。协同导航定位技术作为一种新的导航定位方法具有只对有限数目的AUV增加设备费从而来提高编队的导航精度等优点。由于水下环境和基于声学定位的特殊性，协同导航滤波过程中的量测噪声容易出现异常，传统算法中是将量测噪声视为高斯分布，降低了从AUV的定位精度。

发明内容

本发明提出了一种新的协同导航方法，利用投影统计法对量测值进行预处理再结合改进的Huber估计形式提高协同导航滤波中对量测异常的鲁棒性，该方法适合于在量测噪声异常环境下时对协同编队进行定位，提高了从AUV的定位精度。

根据本发明的一个方面，提供一种具有鲁棒特性的协同导航滤波方法，包括以下步骤：

(1)在协同导航从艇定位中，利用投影统计对从艇接收到的来自主艇的量测信息进行异常判断，包括以下分步骤：

1)获得k个N维量测向量的中位向量，表示为：

其中，x_k表示k个N维量测向量，k＝1，…K,

x_nk代表x_k中第n个元素，

m代表k个向量中的中位向量，

2)计算每个量测向量关于中值偏差的单位向量，其中，偏差向量u_k：

u_k＝x_k-m,k＝1....K

由此得到量测向量关于中值偏差的单位向量v_k为：

3)确定每个量测向量x_k在各单位向量v_k上的投影：

第k个量测向量的投影向量表示为z_k＝[z_1k,z_2k,...z_Kk]^T，

其中，H表示向量x转置，

4)对每个z_k，通过中值和完全中值偏差来计算标准投影p_nk，

其中，C为调节参数，

MAD_k表示第k个值的完全中值偏差，

5)对所有k值重复上述2)、3)、4)计算过程，则第K个点的PS值为

得到PS值后，根据该量测点的量测值相对于量测点簇的偏离程度，赋予该量测点权值ω_i；

(2)获取量测信息的权值后，建立基于改进Huber估计的协同导航鲁棒滤波模型：

由协同导航的运动方程和量测方程得到非线性模型：

其中，下标k代表在时刻k时，x为状态向量，y为量测值，u为系统输入，v为系统噪声，w为量测噪声，v与k均服从零均值白噪声，系统噪声协方差阵为Q，量测噪声协方差阵为R，

量测方程近似写作：

因此协同导航非线性模型改写为：

其中状态预测为

表示实际值与预测值的差，矩阵H定义为：

定义以下式子：

则非线性模型可另写为：

z_k＝G_kx_k+ξ_k

定义残差表示估计值和量测值之间的差，ζ＝Gx-z，

定义函数

则对角矩阵

其中w_i为量测值的权值，由步骤5)计算得到，

为了与状态预测和量测残差相一致，将对角矩阵Ψ分为两部分：

将权值对角矩阵Ψ代入卡尔曼滤波中进行滤波更新，实现滤波鲁棒性。

2.根据权利要求1所述的具有鲁棒特性的协同导航滤波方法，其特征在于，将权值对角矩阵Ψ代入卡尔曼滤波中进行滤波更新的过程如下：

时间更新

P_k/k-1＝Φ_k,k-1P_k-1Φ^T _k,k-1+Q_k

量测更新

其中Φ_k,k-1为状态过渡矩阵，Ψ_y与Ψ_x为重新构造观测模型后利用Huber估计求得的对角矩阵，

为协同导航从艇的位置估计。

3.根据权利要求1所述的具有鲁棒特性的协同导航滤波方法，其特征在于，在步骤(2)中，协同导航运动方程的建立方法是：

记两领航者AUV的位置坐标分别为

从AUV的位置坐标设为(x^S,y^S,z^S)，同时由于深度信息可自测，为简化模型投影至二维空间，则从AUV运动方程可表示为：

其中V(k)表示k时刻前向合成速度，

为k时刻航向角，ω(k)表示k时刻偏航角速度，T为采样周期，假设实际模型中传感器量测输入均受到高斯白噪声干扰，此时系统输入模型为：

其中V(k)，ω(k)分别为前向合成速度以及偏航角速度的真实量测信息，

分别为服从高斯分布且相互独立的前向速度噪声以及偏航角速度噪声，其方差阵分别为

噪声协方差阵表示为：

因此AUV运动方程简化表示为：

X_k+1＝f(x_k,u_k,w_k)＝x_k+Γ(u_k+w_k)

式中

表示从AUV在k时刻的状态；

Γ(u_k+w_k)为非线性项，wk为高斯白噪声。

4.根据权利要求1所述的具有鲁棒特性的协同导航滤波方法，其特征在于，在步骤(2)中，协同导航量测方程的建立方法是：

由于装备了深度计，所以各个AUV的深度信息可以认为是已知的，假设已经获取了从AUV与两个领航者AUV的距离，又称为斜距分别为r₁和r₂，则可以通过几何关系得到下式：

其中P₁和P₂分别为领航者1和领航者2与从AUV的二维水平距离，同时P₁与P₂又可表示为：

P₁和P₂做为量测值，量测方程可表示如下：

上式中v_k表示在时刻k时的量测噪声，假设量测噪声服从零均值相互独立的高斯白噪声，其方差阵表示为：

5.根据权利要求1所述的具有鲁棒特性的协同导航滤波方法，其特征在于，在步骤5)中，得到PS值后，根据该量测点的量测值相对于量测点簇的偏离程度赋予该量测点权值的方法是，通过公式

计算得到。

6.根据权利要求5所述的具有鲁棒特性的协同导航滤波方法，其特征在于，步骤5)中的

取值6.0025。

附图说明

通过结合下面附图对其实施例进行描述，本发明的上述特征和技术优点将会变得更加清楚和容易理解。

图1是表示本发明实施例的一种具有鲁棒特性的协同导航滤波方法的示意图。

具体实施方式

下面将参考附图来描述本发明所述的具有鲁棒特性的协同导航滤波方法的实施例。本领域的普通技术人员可以认识到，在不偏离本发明的精神和范围的情况下，可以用各种不同的方式或其组合对所描述的实施例进行修正。因此，附图和描述在本质上是说明性的，而不是用于限制权利要求的保护范围。此外，在本说明书中，附图未按比例画出，并且相同的附图标记表示相同的部分。

本发明的协同导航方法利用投影统计法对量测值进行预处理再结合改进的Huber估计形式，能够提高协同导航滤波中对量测异常的鲁棒性。下面结合附图1详细说明该协同导航方法。

步骤S101，协同导航的量测值输入，即测量主艇的位置信息，并发送给从艇。

步骤S102：在协同导航从艇定位中，对从艇接收到的来自主艇的量测信息与储存的前时刻的量测值进行投影统计比较，做异常判断，包括以下分步骤：

1)从艇包含有前K-1个时刻的N维量测向量，而来自主艇的K个时刻的N维量测向量，记作：x_k，k＝1，…K,其中x_nk代表x_k中第n个元素，m代表k个向量中的中位向量，表示为：

2)计算每个量测向量关于中值偏差的单位向量，偏差向量u_k：

u_k＝x_k-m,k＝1....K

关于偏差向量的单位向量v_k为：

3)确定每个量测向量x_k在各单位向量上v_k的投影：

其中，H表示向量x的转置

第k个量测向量的投影向量表示为z_k＝[z_1k,z_2k,...z_Kk]^T，

4)对每个z_k，通过中值和完全中值偏差来计算标准投影p_nk

其中，c为一调节参数，其正常取值为1.4826，

MAD_k表示第k个值的完全中值偏差。

5)对所有k值重复上述步骤2)、3)、4)的计算过程，则第K个点的PS值为

得到PS值后，根据该量测点的量测值相对于量测点簇的偏离程度，赋予该量测点 权值ω_i。优选地，通过公式

获得该量测点的权值，优选地，其中的可 以取值6.0025。

需要说明的是，PS为该量测点相对于量测点簇的偏离程度的量化值，通过

分配该量测点的权值，若该量测点相对于量测点簇的偏离程度大，则分配的权值小；若该量测点相对于量测点簇的偏离程度小，则分配的权值大。

步骤S103：获取量测信息的权值后，建立基于改进Huber估计下的权值对角阵，方法如下：

获得协同导航非线性模型：

其中，下标k代表在时刻k时，x为状态向量，y为量测值，u为系统输入，v为系统噪声，w为量测噪声，v与k均服从零均值白噪声，系统噪声协方差阵为Q，量测噪声协方差阵为R。

量测方程近似写作：

因此协同导航非线性模型改写为：

其中状态预测为

表示实际值与预测值的差。

矩阵H定义为

定义以下式子：

则非线性模型可另写为：

z_k＝G_kx_k+ξ_k

定义残差表示估计值和量测值之间的差，ζ＝Gx-z。

定义函数

则基于改进Huber估计下的权值对角矩阵为其中w_i为量测值的权值，在步骤S102中计算得到。

特别地，为了与状态预测和量测残差相一致，将对角矩阵Ψ分为两部分：

步骤S104，将权值对角矩阵Ψ代入卡尔曼滤波中进行滤波更新，实现滤波鲁棒性，得到改进Huber估计下卡尔曼滤波方程：

时间更新

量测更新

其中，Φ_k,k-1为状态过渡矩阵，Ψ_y与Ψ_x为重新构造观测模型后利用改进Huber估计求得的对角矩阵，

为协同导航从艇的位置估计。

步骤S105，将计算出的协同导航从艇位置数据发送给从艇，从艇根据该位置数据跟踪主艇移动。

在一个可选实施例中，协同导航运动方程的建立方法是：

记两领航者AUV的位置坐标分别为从AUV的位置坐标设为(x^S,y^S,z^S)，同时由于深度信息可自测，为简化模型投影至二维空间。则从AUV运动学方程可表示为：

其中V(k)表示k时刻前向合成速度，

为k时刻航向角，ω(k)表示k时刻偏航角速度，T为采样周期。假设实际模型中传感器量测输入均受到高斯白噪声干扰，此时系统输入模型为：

其中V(k)，ω(k)分别为前向合成速度以及偏航角速度的真实量测信息，分别为服从高斯分布且相互独立的前向速度噪声以及偏航角速度噪声，其方差阵分别为

噪声协方差阵表示为：

因此AUV运动方程简化表示为：

X_k+1＝f(x_k,u_k,w_k)＝x_k+Γ(u_k+w_k)

式中表示从AUV在k时刻的状态；

Γ(u_k+w_k)为非线性项，w_k为高斯白噪声

在一个可选实施例中，协同导航量测方程的建立方法是：

由于装备了深度计，所以各个AUV的深度信息可以认为是已知的。假设已经获取了从AUV与两个领航者AUV的距离，又称为斜距分别为r₁和r₂，则可以通过几何关系得到下式：

其中P₁和P₂分别为领航者1和领航者2与从AUV的二维水平距离。同时P₁与P₂又可表示为：

P₁和P₂做为量测值，量测方程可表示如下：

上式中v_k表示在时刻k时的量测噪声，假设量测噪声服从零均值相互独立的高斯白噪声，其方差阵表示为：

本发明的协同导航滤波方法通过结合投影统计异常点鉴别方法和Huber估计法，提高在量测噪声异常情况下的协同导航从艇的定位精度，增强滤波算法的鲁棒性。该方法的特点是当量测噪声为近似高斯分布并且出现异常值时滤波仍可以保持较高的精度，具有鲁棒性。

以上所述仅为本发明的优选实施例，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种具有鲁棒特性的协同导航滤波方法，其特征在于，包括以下步骤：

(1)在协同导航从艇定位中，利用投影统计对从艇接收到的来自主艇的量测信息进行异常判断，包括以下分步骤：

1)获得k个时刻的N维状态向量的中位向量，表示为：

x_k表示第k个时刻的N维状态向量，k＝1，…K，

x_nk代表x_k中第n个元素，

med代表取中位元素；

2)计算每个状态向量关于中值偏差的单位向量，其中，偏差向量u_k：

u_k＝x_k-m，k＝1....K

由此得到状态向量关于中值偏差的单位向量v_k为：

3)确定每个状态向量x_k在各单位向量v_k上的投影：

第k个时刻的状态向量的投影向量表示为z_k＝[z_1k，z_2k，...z_Kk]^T，

其中，H表示向量x转置，

4)对每个z_k，通过中值和完全中值偏差来计算标准投影p_nk，

其中，c为调节参数，

MAD_k表示第k个时刻的值的完全中值偏差，

5)对所有k值重复上述2)、3)、4)计算过程，则第K个点的PS值为

得到PS值后，根据该量测点的量测值相对于量测点簇的偏离程度，赋予该量测点的量测值的权值ω_i；

(2)获取量测信息的权值后，建立基于改进Huber估计的协同导航鲁棒滤波模型：

由协同导航的运动方程和量测方程得到非线性模型：

其中，下标k代表在时刻k时，x为状态向量，y为量测值，u为系统输入，v为系统噪声，w为量测噪声，v与w均服从零均值白噪声，量测噪声协方差阵为R，

量测方程近似写作：

因此协同导航非线性模型改写为：

其中

是协同导航从艇的位置预测值，

表示实际值与预测值的差，

矩阵H_k定义为：

其中，

是量测噪声的平均值；

定义以下式子：

其中，R_k是k时刻的量测噪声协方差阵；

P_k/k-1为一步预测误差均方差；

则非线性模型可另写为：

Z_k＝G_kx_k+ξ_k

定义残差表示估计值和量测值之间的差，ζ＝Gx-Z，

定义函数

则对角矩阵

其中ω_i为量测值的权值，由步骤5)计算得到，

将对角矩阵Ψ分为两部分：

Ψ_y与Ψ_x为重新构造观测模型后利用Huber估计求得的对角矩阵，将权值对角矩阵Ψ代入卡尔曼滤波中进行滤波更新，实现滤波鲁棒性。

2.根据权利要求1所述的具有鲁棒特性的协同导航滤波方法，其特征在于，将权值对角矩阵Ψ代入卡尔曼滤波中进行滤波更新的过程如下：

时间更新

P_k/k-1＝Φ_k，k-1P_k-1Φ^T _k，k-1+Q_k

量测更新

其中Φ_k，k-1为状态过渡矩阵；

Q_k是k时刻的系统噪声协方差阵；

Ψ_y与Ψ_x为重新构造观测模型后利用Huber估计求得的对角矩阵。

3.根据权利要求1所述的具有鲁棒特性的协同导航滤波方法，其特征在于，在步骤(2)中，协同导航运动方程的建立方法是：

记两领航者AUV的位置坐标分别为

从AUV的位置坐标设为(x^s，y^s，z^s)，同时由于深度信息可自测，为简化模型投影至二维空间，则从AUV运动方程可表示为：

其中V(k)表示k时刻前向合成速度，

为k时刻航向角，ω(k)表示k时刻偏航角速度，T为采样周期，假设实际模型中传感器量测输入均受到高斯白噪声干扰，此时系统输入模型为：

其中V_m(k)，ω_m(k)分别为前向合成速度以及偏航角速度的真实量测信息，

分别为服从高斯分布且相互独立的前向速度噪声以及偏航角速度噪声，其方差阵分别为

噪声协方差阵表示为：

因此AUV运动方程简化表示为：

X_k+1＝f(x_k，u_k，w_k)＝X_k+Γ(u_k+w_k)

式中

表示从AUV在k时刻的状态；

Γ(u_k+w_k)为非线性项，w_k为k时刻的零均值的高斯白噪声。

4.根据权利要求3所述的具有鲁棒特性的协同导航滤波方法，其特征在于，在步骤(2)中，协同导航量测方程的建立方法是：

由于装备了深度计，所以各个AUV的深度信息认为是已知的，假设已经获取了从AUV与两个领航者AUV的距离，又称为斜距分别为r₁和r₂，则通过几何关系得到下式：

其中P₁和P₂分别为领航者1和领航者2与从AUV的二维水平距离，同时P₁与P₂又可表示为：

P₁和P₂做为量测值，量测方程可表示如下：

上式中v_k表示在时刻k时的系统噪声，假设系统噪声服从零均值相互独立的高斯白噪声，其方差阵表示为：

5.根据权利要求1所述的具有鲁棒特性的协同导航滤波方法，其特征在于，在步骤5)中，得到PS值后，根据该量测点的量测值相对于量测点簇的偏离程度赋予该量测点权值的方法是，通过公式

计算得到。

Images