CN103335646A - 一种基于分散式增广信息滤波的多艇协同导航方法 - Google Patents

Abstract

本发明公开了一种基于分散式增广信息滤波的多艇协同导航方法，分析多艇协同导航的系统特征，建立了协同导航的状态空间模型，包括单个艇的状态方程和观测方程以及艇之间的观测方程。为了解决在滤波过程中的计算复杂性问题，本发明采用增广信息滤波求解协同导航问题，利用分布式递增Cholesky修正算法，在状态恢复中将信息矩阵进行分解，即Y＝LL^T，其中L为下三角矩阵，是稀疏矩阵。利用稀疏矩阵的特点，大大降低了计算量。

Description

一种基于分散式增广信息滤波的多艇协同导航方法

技术领域

本发明涉及一种基于分散式增广信息滤波的多艇协同导航方法，尤其涉及一种利用分布式递增Cholesky修正算法的分散式增广信息滤波协同导航算法。

背景技术

协同导航是指一种运动平台之间观测或者间接相对观测，通过一定的信息交换实现平台间导航资源共享，从而获得比各平台独立导航更优性能的导航方式。当前，这种导航技术已经被成功用于人工操作和自主的运载系统，逐步成为一个研究热点，可被应用到环境监测、行星探测、水下开发和寻找搜救任务等等。同样这种基于协同导航的技术可以被用于无人战机(UAV)和自主水下潜器(AUV)等等的定位。而多艇的协同导航系统，是利用观测信息结合无人艇动力学方程，利用最优估计方法估计无人艇的位置航向角等导航信息。也就是说多艇的协同导航系统可分为三部分，一是无人艇状态方程，二是量测量的选择，三是滤波(估计)方法的设计。

无人艇动力学方程包括基于直角坐标系的多艇状态方程和观测方程，根据协同导航的特点，选取单艇位置信息和艇间距离差为量测量，可将观测方程分为单艇量测方程和艇间量测方程。这些方程都是非线性，需要利用泰勒展开式，将非线性方程线性化。

基于多艇协同导航问题的线性状态空间模型，目前先进的最优估计方法是增广信息滤波方法，这种方法采用信息参数的形式进行状态添加和观测更新，滤波状态的信息矩阵是稀疏矩阵，滤波所涉及的计算复杂度较小，使状态添加和观测更新具有局部性，但是这种滤波方法在状态恢复是全局运算，涉及一个稠密矩阵的求逆计算，该矩阵维数等于全体艇状态向量的维数之和，计算十分复杂。

发明内容

本发明解决的技术问题是：鉴于增广信息滤波方法中状态恢复稠密矩阵的求逆计算过于复杂问题，提出了一种利用分布式递增Cholesky修正算法的分散式增广信息滤波协同导航算法，分析多艇协同导航的系统特征，建立了协同导航的状态空间模型，包括单个艇的状态方程和观测方程以及艇之间的观测方程。为了解决在滤波过程中的计算复杂性问题，本发明采用增广信息滤波求解协同导航问题，利用分布式递增Cholesky修正算法，在状态恢复中将信息矩阵进行分解，即Y＝LL^T，其中L为下三角矩阵，是稀疏矩阵。利用稀疏矩阵的特点，大大降低了计算量。

本发明的技术方案为：

一种基于分散式增广信息滤波的多艇协同导航方法，包括以下步骤：

步骤(1)建立基于直角坐标系的单艇运动学方程；

步骤(2)建立以单艇位置信息为量测量的单艇量测方程和以艇间距离为量测量的艇间量测方程；

步骤(3)根据步骤(1)和步骤(2)，建立多艇协同导航的状态空间模型，协同导航系统的状态向量由N条艇的局部状态向量组成，利用泰勒展开式，将方程线性化；

步骤(4)添加新时刻的状态向量，结合信息参数算法过程，得信息参数和信息矩阵；

步骤(5)用新的观测向量更新信息参数，得新的信息参数和信息矩阵；

步骤(6)从更新后的信息参数和信息矩阵中提取只与新时刻相关的信息量，利用求逆公式，通过分布式递增Cholesky修正算法简化计算，求得新时刻N条艇状态向量估计值

其中

表示第i(i＝1，2…N)条艇在新时刻的状态向量估计值，由第i条艇的经度

纬度

和航向角

所述的协同导航方法，所述步骤(4)中的信息矩阵Y_a定义为，

k-1时刻  k时刻  k+1时刻

其中，Y_m，n表示为m时刻的状态向量估计值

和n时刻的状态向量估计值

的协信息参数。

所述的协同导航算法，所述步骤(6)中利用分布式递增Cholesky修正方法，将信息参数转化为矩参数，完成对状态向量的恢复，状态恢复的具体步骤是：

利用分布式递增Cholesky修正算法，对信息矩阵Y_a进行矩阵分解，即

Y_a＝LL^T

其中，L称为信息矩阵Y_a的Cholesky因子；

通过求解Lf＝y_a

得f＝L^-1y_a，其中y_a为系统的信息参数，f称为Cholesky的前向向量；

通过求解 $L^T\hat{X}=f$

得

其中

为所要求的状态向量。

本发明具有以下特点：

a.DAIF(Decentralized Augmentation Information Filter，分散式增广信息滤波)算法的估计精确性更高；

b.信息矩阵形式的DAIF滤波进行持续的迭代将呈现出更明显的带状稀疏性，因此涉及计算量大大减少并能节省存储容量；

c.DAIF算法通信单一时刻仅涉及两个艇，且使用的累积链式通信大大减少了复杂性

d.DAIF算法在状态恢复时可以从状态链中消去非测距状态和一些故障艇节点，保证算法的继续运行，因此分散式算法可以使系统具有很好的稳固性。

附图说明

图1分散式增广信息滤波算法框架图；

具体实施方式

以下结合具体实施例，对本发明进行详细说明。

本发明具体实施的流程如图1所示，先由惯性器件测得的位置信息和激光测距仪测得的艇间距离作为量测量，结合多艇协同导航的状态方程，首先在状态添加和观测更新中利用增广信息滤波，使信息参数只在局部变换，然后在状态恢复中利用递增Cholesky修正算法，设计其分布式算法，估计出各个艇的位置信息和航向角。

本发明以两个艇的协同导航为例，讲解具体的实施方式：

两个艇分别标识为A、B，每个艇都装备已知航推设备和一定的通信设备，A艇并给予激光测距仪，可测得任一时刻A、B艇之间的距离，且A、B两艇在任意时刻都可以进行信息交换。

步骤(1)建立基于直角坐标系的两条艇运动学方程，即状态方程

列写艇系统的运动方程

其中，

分别为i艇在k+1时刻的经度、纬度、航向角和速度；

分别为i艇在k时刻的经度、纬度、航向角和速度；T是系统采样时间；w_λ、

w_φ分别为经度、纬度和航向角的系统噪声。

步骤(2)建立以单艇位置信息为量测量的单艇量测方程和以艇间距离差为量测量的艇间量测方程；

列写艇系统的观测方程；

其中，v_λ，

v_r分别为系统观测噪声。

步骤(3)根据步骤(1)和(2)，建立多艇协同导航的状态空间模型，协同导航系统的状态向量由N条艇的状态向量组成，利用泰勒展开式，将方程线性化；

将艇A、B系统的运动方程转化成状态方程形式

即X_k+1＝F_kX_k+G_k(U_k)+W_k，k＝1，2，3，…     (3)

对于A艇来说，其状态向量

上角标A表示为状态方程所有的矢量都是针对A艇来说的；

为A艇在k时刻的控制输入量，在此选取单艇速度和航向角作为输入量，即

为A艇系统状态雅可比函数，其取值为

$F_k^A=\left[\begin{array}{ccc}1& 0& -v_k^{A}T\sin \left({\mathrm{\φ}}_k^A\right)\\ 0& 1& -v_k^{A}T\cos \left({\mathrm{\φ}}_k^A\right)\\ 0& 0& 1\end{array}\right]---\left(4\right)$

为A艇系统状态方程的噪声阵，其取值为

$G_k^A\left(U_k^A\right)=\left[\begin{array}{cc}T\cos \left({\mathrm{\φ}}_k^A\right)& -v_k^{A}T\sin \left({\mathrm{\φ}}_k^A\right)\\ T\sin \left({\mathrm{\φ}}_k^A\right)& v_k^{A}T\cos \left({\mathrm{\φ}}_k^A\right)\end{array}\right]---\left(5\right)$

表示为A艇系统噪声，为零均值白噪声，即

同理，对于B艇来说，其状态向量为

上角标B表示为状态方程所有的矢量都是针对B艇来说的；

为B艇在k时刻的控制输入量，在此选取单艇速度和航向角作为输入量，即

为艇B系统状态雅可比函数，其取值为

$F_k^B=\left[\begin{array}{ccc}1& 0& -v_k^{B}T\sin \left({\mathrm{\φ}}_k^B\right)\\ 0& 1& -v_k^{B}T\cos \left({\mathrm{\φ}}_k^B\right)\\ 0& 0& 1\end{array}\right]---\left(6\right)$

为B艇系统状态方程的噪声阵，其取值为

$G_k^B\left(U_k^B\right)=\left[\begin{array}{cc}T\cos \left({\mathrm{\φ}}_k^B\right)& -v_k^{B}T\sin \left({\mathrm{\φ}}_k^B\right)\\ T\sin \left({\mathrm{\φ}}_k^B\right)& v_k^{B}T\cos \left({\mathrm{\φ}}_k^B\right)\end{array}\right]---\left(7\right)$

表示为B艇系统噪声，为零均值白噪声，即

对于这个艇系统来说，其状态向量为

U_k为艇系统在k时刻的控制输入量，在此选取A、B两个艇速度和航向角作为输入量，即

W_k表示为艇系统噪声，为零均值白噪声，即W_k～N_s(W_k；0，Q_k)。

将艇A、B系统和艇系统的观测方程列写成矩阵形式，

Z_k＝H_kX_k+V_k         (8)

其中，V_k表示为观测噪声，为零均值白噪声，即V_k～N_s(V_k；0，R_k)。

对于A艇来说，其状态向量为

以A艇位置信息和A、B艇间距离差为量测量，即

其中表示为k时刻A、B艇的距离差；

为A艇量测函数的雅可比函数，其取值为：

对于B艇来说，其状态向量为

以B艇位置信息和A、B艇间距离差为量测量，即

其中

表示为k时刻A、B艇的距离差；

为艇B量测函数的雅可比函数，其取值为

对于这个艇系统来说，其状态向量为

H_k为两条艇系统的观测函数的雅可比函数为

步骤(4)、状态添加。添加新时刻的状态向量，结合信息参数算法过程，得信息参数和信息矩阵；

开始时，k时刻系统的信息矩阵估计值为

$Y={\hat{Y}}_{k,k}=\left[\begin{array}{cc}Y(1:\mathrm{3,1}:3)& Y(1:\mathrm{3,4}:6)\\ Y(4:\mathrm{6,1}:3)& Y(4:\mathrm{6,4}:6)\end{array}\right]---\left(12\right)$

其中Y(A:B，C:D)表示矩阵块是由Y矩阵A到B行、C到D列构成；

将新的状态加入系统中，结合信息参数算法过程，利用马尔科夫随机场方法，得信息矩阵Y_a，

$Y_a=\left[\begin{array}{cccc}Y(1:\mathrm{3,1}:3)+{\left(F_k^A\right)}^T{Q}_A^{-1}{F}_k^A& 0& -{\left(F_k^A\right)}^T{Q}_A^{-1}& 0\\ Y(4:\mathrm{6,1}:3)& Y(4:\mathrm{6,4}:6)+{\left(F_k^B\right)}^T{Q}_B^{-1}{F}_k^B& 0& -{\left(F_k^B\right)}^T{Q}_B^{-1}\\ -Q_A^{-1}{F}_k^A& 0& Q_A^{-1}& 0\\ 0& -Q_B^{-1}{F}_k^B& 0& Q_B^{-1}\end{array}\right]$

$=\left[\begin{array}{cccc}{Y}_a\_11& Y_a\_12& Y_a\_13& Y_a\_14\\ Y_a\_21& Y_a\_22& Y_a\_23& Y_a\_24\\ Y_a\_31& Y_a\_32& Y_a\_33& Y_a\_34\\ Y_a\_41& Y_a\_42& Y_a\_43& Y_{\mathrm{aa}}\_44\end{array}\right]---\left(13\right)$

对信息矩阵Y_a进行递增Cholesky分解Y_a＝LL^T，L为下三角矩阵，可知：

$L=\left[\begin{array}{cccccc}L\_11& L\_12& L\_13& L\_14& L\_15& L\_16\\ L\_21& L\_22& L\_23& L\_24& L\_25& L\_26\\ L\_31& L\_32& L\_33& L\_34& L\_35& L\_36\\ L\_41& L\_42& L\_43& L\_44& L\_45& L\_46\end{array}\right]---\left(14\right)$

L_13(L_13)^T＝Y_a_11-L_11(L_11)^T-L_12(L_12)^T

L_23(L_13)^T＝Y_a_21-L_22(L_12)^T

L_24(L_24)^T＝Y_a_22-L_23(L_23)^T-L_22(L_22)^T

L_33(L-13)^T＝Y_a_31

其中L_34(L_24)^T＝Y_a_32-L_33(L_23)^T         (15)

L_35(L_35)^T＝Y_a_33-L_33(L_33)^T-L_34(L_34)^T

L44(L-24)^T＝Y_a42

L_45(L_35)^T＝Y_a_43-L_44(L_34)^T

L_46(L_46)^T＝Y_a_44-L_44(L_44)^T-L_45(L_45)^T

步骤(5)、观测更新。用新的观测向量z_k更新信息参数Y_a，得新的信息参数Y_a(+)，而Y_a(+)中各个矩阵块的取值为；

$Y_a\_33(+)=Y_a\_33+{\left(H_k^A\right)}^T{R}_k^{-1}{H}_k^A$

$Y_a\_34(+)=Y_a\_34+{\left(H_k^A\right)}^T{R}_k^{-1}{H}_k^B$

$Y_a\_43(+)=Y_a\_43+{\left(H_k^B\right)}^T{R}_k^{-1}{H}_k^A---\left(16\right)$

$Y_a\_44(+)=Y_a\_44+{\left(H_k^B\right)}^T{R}_k^{-1}{H}_k^B$

Y_a_ij(+)＝Y_a_ij，i、

且ij≠33、34、43、44

步骤(6)状态恢复。由信息参数求解矩参数的问题称为状态恢复，利用求逆公式，通过分布式递增Cholesky修正算法简化计算，求得状态向量的估计值；

第二次利用分布式递增Cholesky修正算法，由于只需要求解新时刻的状态量，故只取观测更新后的新的信息矩阵一部分Y_a(+)进行计算，得信息矩阵估计值

即

${\hat{Y}}_{k+1,k+1}=\left[\begin{array}{cc}{Y}_a(+)\_33& Y_a(+)\_34\\ Y_a(+)\_43& Y_a(+)\_44\end{array}\right]=\hat{L}{\hat{L}}^T---\left(17\right)$

其中 $\hat{L}=\left[\begin{array}{cc}\hat{L}\_35& \hat{L}\_36\\ \hat{L}\_45& \hat{L}\_46\end{array}\right],$ 矩阵

为信息矩阵估计值

的Cholesky因子。

由于

为上三角矩阵，根据Cholesky公式，可知

$\hat{L}\_35{(\hat{L}\_35)}^T=Y_a(+)\_33-L\_33{(L\_33)}^T-L\_34{(L\_34)}^T$

$\hat{L}\_36=0_{3\×3}$

                   (18)

$\hat{L}\_45{(\hat{L}\_35)}^T=Y_a(+)\_43-L\_44{(L\_34)}^T$

$\hat{L}\_46{(\hat{L}\_46)}^T=Y_a(+)\_44-L\_44{(L\_44)}^T-L\_45{(L\_45)}^T$

此时新的信息参数估计值

为

${\hat{y}}_{k+1}=Q_k^{-1}{U}_k+i_k---\left(19\right)$

其中， $i_k={\left(H_k\right)}^T{R}_k^{-1}(Z_k-H_k{X}_k)---\left(20\right)$

利用 ${\hat{Y}}_{k+1,k+1}{\hat{X}}_{k+!}={\hat{y}}_{k+1},$ 递增Cholesky分解后得 $\hat{L}{\hat{L}}^T{\hat{X}}_{k+1}={\hat{y}}_{k+1}.$ 求解 $\hat{L}f={\hat{y}}_{k+1},$ 得f

即 $f={\hat{L}}^{-1}{\hat{y}}_{k+1}---\left(21\right)$

然后通过求解 ${\hat{L}}^{T}f={\hat{X}}_{k+1},$ 得

即 ${\hat{X}}_{k+1}={\left({\hat{L}}^T\right)}^{-1}f---\left(22\right)$

则式(22)中的为所求得的新时刻状态估计值。

利用步骤1建立基于直角坐标系的单艇运动学方程，利用步骤2建立量测方程，利用步骤3建立多艇协同导航的状态空间模型。步骤4、5和6都是属于滤波过程，步骤4是滤波的第一步状态添加，步骤5是滤波的第二步观测更新，而步骤6是滤波的第三步状态恢复，三步滤波之后就估计出A、B艇状态向量估计值

其中

表示第i条艇在k+1时刻的导航状态估计值，即i＝A或B。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (3)

1.一种基于分散式增广信息滤波的多艇协同导航方法，其特征在于，包括以下步骤：

步骤(1)建立基于直角坐标系的单艇运动学方程；

步骤(2)建立以单艇位置信息为量测量的单艇量测方程和以艇间距离为量测量的艇间量测方程；

步骤(3)根据步骤(1)和步骤(2)，建立多艇协同导航的状态空间模型，协同导航系统的状态向量由N条艇的局部状态向量组成，利用泰勒展开式，将方程线性化；

步骤(4)添加新时刻的状态向量，结合信息参数算法过程，得信息参数和信息矩阵；

步骤(5)用新的观测向量更新信息参数，得新的信息参数和信息矩阵；

步骤(6)从更新后的信息参数和信息矩阵中提取只与新时刻相关的信息量，利用求逆公式，通过分布式递增Cholesky修正算法简化计算，求得新时刻N条艇状态向量估计值

其中

表示第i(i＝1，2…N)条艇在新时刻的状态向量估计值，

由第i条艇的经度

纬度

和航向角

2.如权利要求1所述的协同导航方法，其特征在于：所述步骤(4)中的信息矩阵Y_a定义为，

k-1时刻  k时刻  k+1时刻

其中，Y_m，n表示为m时刻的状态向量估计值

和n时刻的状态向量估计值

的协信息参数。

3.如权利要求1所述的协同导航算法，其特征在于：所述步骤(6)中利用分布式递增Cholesky修正方法，将信息参数转化为矩参数，完成对状态向量的恢复，状态恢复的具体步骤是：

利用分布式递增Cholesky修正算法，对信息矩阵Y_a进行矩阵分解，即

Y_a＝LL^T

其中，L称为信息矩阵Y_a的Cholesky因子；

通过求解Lf＝y_a

得f＝L^-1y_a，其中y_a为系统的信息参数，f称为Cholesky的前向向量；

通过求解 $L^T\hat{X}=f$

得其中为所要求的状态向量。

Images