CN102980579A - 一种自主水下航行器自主导航定位方法 - Google Patents

Abstract

本发明涉及的是一种自主水下航行器自主导航定位方法，特别是涉及一种基于强跟踪容积卡尔曼滤波的自主水下航行器自主导航定位方法。本发明包括如下步骤：选取自主水下航行器运动模型；选取自主水下航行器测量模型；通过自主水下航行器运动模型和测量模型构建动态系统；滤波器参数初始化；选取渐消因子；更新滤波器时间；更新滤波器量测数据；由当前时刻更新到下一时刻，重复执行步骤(5)至步骤(7)，直到到达终止时刻，结束计算，输出结果。通过引入渐消因子实时调整滤波增益矩阵，强迫输出残差序列保持相互正交，以此来减小老数据的权值，相对地增加新数据的权值，提高了滤波器的估计精度和稳定性。

Description

一种自主水下航行器自主导航定位方法

技术领域

本发明涉及的是一种自主水下航行器自主导航定位方法，特别是涉及一种基于强跟踪容积卡尔曼滤波（Strong Tracking Cubature KalmanFilter，STCKF）的自主水下航行器自主导航定位方法。

背景技术

自主水下航行器（Autonomous Underwater Vehicle，AUV）作为一种可以在水下移动的装置，具有自带动力、活动自主的特点，可以进行海洋环境监测、海底搜索、打捞等水下作业，已被广泛应用于海洋勘察与资源开发等领域。但水下导航问题即如何确定AUV在水下环境中的位置以便其采取正确的行动成功的完成任务，依然是AUV设计所面临的主要关键技术之一。

目前国内外试图寻求一种有效的手段用于AUV自主导航定位。当AUV处于实际运行环境中时，受外部应用环境不确定性因素的影响，AUV自主导航系统的实际数学模型与理论模型不能完全匹配，即AUV自主导航系统具有模型不确定性。研究表明，当理论模型与实际系统完全匹配时，卡尔曼滤波器输出残差序列是互不相关的高斯白噪声序列。

针对上述问题，周东华等人在文献《非线性系统带次优渐消因子的扩展卡尔曼滤波》（控制与决策,1990,5(5):1-6）及文献《一种带多重次优渐消因子的扩展卡尔曼滤波器》（自动化学报,1991,17(6):689-695）中基于扩展卡尔曼滤波（Extended KalmanFilter，EKF）提出了强跟踪滤波器（Strong TrackingFilter，STF），其通过在EKF状态预测协方差阵中引入渐消因子，在线实时调整增益矩阵，强迫输出残差序列保持相互正交。这样，STF在AUV自主导航系统模型不确定时仍然能保持对系统状态的跟踪能力，有效解决了EKF关于模型不确定的鲁棒性差、滤波发散等问题。但STF仍然存在对非线性状态后验分布的一阶线性化近似精度偏低，及需要计算非线性函数的雅克比矩阵等自身无法克服的理论局限性。

加拿大学者Arasaratnam在文献《Cubature KalmanFilters》（IEEE TransactionsonAutomatic Control,2009,54(6):1254-1269）中提出了容积卡尔曼滤波（Cubature KalmanFilter，CKF）方法。CKF根据容积准则，通过一组具有相同权重的点经过非线性系统方程转换后产生新的点来给出下一时刻系统状态的预测，避免了对非线性模型的线性化处理，其精度达三阶。文献《Nonlinear estimation for spacecraft attitude using decentralizedunscented information filter》（International Conference on Control Automation andSyatems,2010:1562-1566）及文献《GaussHermite quadraturefilter with applicationto spacecraft attitude estimation》（Journal of Guidance and Dynamics,2011,34(2):367-379）均基于CKF进行飞行器姿态估计；文献《A CKF based GNSS/INS train integratedpositioning method》（International Conference on Mechatronics and Automation,2010:1686-1689）采用CKF与GNSS/INS协同进行火车定位。

发明内容

本发明的目的在于提供一种精度更高、稳定性更强的自主水下航行器自主导航定位方法。

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

本发明包括如下步骤：

（1）建立自主水下航行器运动模型：

${\left[\begin{array}{c}x\\ y\\ z\\ \mathrm{\ψ}\\ u\\ v\\ w\\ r\end{array}\right]}_k={\left[\begin{array}{c}x+\mathrm{uT}\cos \left(\mathrm{\ψ}\right)-\mathrm{vT}\sin \left(\mathrm{\ψ}\right)\\ y+\mathrm{uT}\sin \left(\mathrm{\ψ}\right)+\mathrm{vT}\cos \left(\mathrm{\ψ}\right)\\ z+\mathrm{wT}\\ \mathrm{\ψ}+\mathrm{rT}\\ u\\ v\\ w\\ r\end{array}\right]}_{k-1}+w_{k-1}$

式中，[x,y,z,Ψ]表示AUV在L中的位置和艏向；[u,v,w,r]表示AUV在V中相应的线速度和角速度；k表示任意采样时刻；T为航位推算传感器的采样时间间隔；w_k-1为随机系统噪声；

（2）建立自主水下航行器测量模型：

z_k＝Hx_k|k-1+v_k

式中，z_k是观测向量， $x_k={\left[\begin{array}{cccccccc}x& y& z& \mathrm{\ψ}& u& v& w& r\end{array}\right]}_k^T,$ v_k是观测噪声，测量矩阵H为：

$H=\left[\begin{array}{cccccccc}0& 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& 0\end{array}\right];$

（3）通过自主水下航行器运动模型和测量模型构建动态系统：

$\left\{\begin{array}{c}{x}_k=f\left(x_{k-1}\right)+w_{k-1}\\ z_k=h\left(x_k\right)+v_k\end{array}\right.$

式中， $x_k={\left[\begin{array}{cccccccc}x& y& z& \mathrm{\ψ}& u& v& w& r\end{array}\right]}_k^T,$ z_k是观测向量；随机系统噪声w_k~N(0,Q_k)，随机观测噪声v_k~N(0,R_k)，系统初始状态为x₀，x₀与w_k，v_k统计独立；非线性函数f(x_k-1)和h(x_k)为关于状态的一阶连续偏导数：

$f\left(x_{k-1}\right){\left[\begin{array}{c}x+\mathrm{uT}\cos \left(\mathrm{\ψ}\right)-\mathrm{vT}\sin \left(\mathrm{\ψ}\right)\\ y+\mathrm{uT}\sin \left(\mathrm{\ψ}\right)+\mathrm{vT}\cos \left(\mathrm{\ψ}\right)\\ z+\mathrm{wT}\\ \mathrm{\ψ}+\mathrm{rT}\\ u\\ v\\ w\\ r\end{array}\right]}_{k-1},$ h(x_k)＝Hx_k|k-1；

（4）滤波器参数初始化：

自主水下航行器初始时刻的状态x₀＝[0]_8×1，初始时刻的状态误差协方差P₀，系统噪声Q，观测噪声R，用于求取渐消因子的C_0,k的初始值C_0,0及遗忘因子ρ；

（5）选取渐消因子：

${\mathrm{\λ}}_k=\frac{\mathrm{tr}(C_{0,k}-R_k)}{\mathrm{tr}(\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{z}_{i,k|k-1}{z}_{i,k|k-1}^T-{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T)}$

其中，λ_k为渐消因子，tr(·)为求矩阵的迹，m＝2n是容积点的个数，n为AUV状态向量的维数，z_i，k|k-1为通过观测方程传播后的容积点，

为k时刻的观测预测值，R_k为k时刻的观测噪声， $C_{0,k}=\left\{\begin{array}{cc}{\mathrm{\γ}}_k{\mathrm{\γ}}_k^T& k=1\\ \frac{\mathrm{\ρ}{C}_{0,k-1}+{\mathrm{\γ}}_k{\mathrm{\γ}}_k^T}{1+\mathrm{\ρ}}& k>1\end{array}\right.,$ γ_k是k时刻的残差，0＜ρ≤1为遗忘因子。

（6）估计k时刻自主水下航行器的状态：

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1})$

式中，

为k时刻的状态估计值，

为k时刻的状态预测值，K_k为卡尔曼滤波增益，z_k为观测向量，为k时刻的观测预测值；

（7）估计k时刻自主水下航行器的状态协方差：

$P_{k|k}=P_{k|k-1}-K_k{P}_{\mathrm{zz},k|k-1}{K}_k^T$

式中，P_k|k为k时刻的状态误差协方差估计值，P_k|k-1为k时刻的状态协方差预测值，K_k为卡尔曼滤波增益，P_zz，k|k-1为自相关协方差矩阵；

（8）由当前时刻更新到下一时刻，重复执行步骤（5）至步骤（7），直到到达终止时刻，结束计算，输出结果。

本发明的有益效果在于：该方法基于容积准则，通过选取2n个具有同等权值的容积点计算高斯权重分布，计算出容积点集后在每个滤波周期内进行时间更新和量测更新，通过引入渐消因子实时调整滤波增益矩阵，强迫输出残差序列保持相互正交，以此来减小老数据的权值，相对地增加新数据的权值，提高了滤波器的估计精度和稳定性。该算法避免了对非线性模型的线性化处理，不依赖于具体系统模型的非线性方程，相对独立，其精度可达非线性系统泰勒级数展开的三阶项或更高阶项。

附图说明

图1为基于强跟踪容积卡尔曼滤波的AUV自主导航定位方法的结构框图；

图2为全局坐标系与船体坐标系示意图；

图3为海试试验一的EKF、CKF、STCKF的估计轨迹与GPS轨迹对比曲线示意图；

图4为海试试验一的东向误差比较示意图；

图5为海试试验一的北向误差比较示意图；

图6为海试试验二的EKF、CKF、STCKF的估计轨迹与GPS轨迹对比曲线示意图；

图7为图6中矩形框包围部分的局部放大图示意图；

图8为海试试验二的东向误差比较示意图；

图9为海试试验二的北向误差比较示意图。

具体实施方式

下面结合附图对本发明作进一步的详细描述。

本发明提出的一种AUV自主导航定位方法结构框图如附图1所示，该方法的主要步骤如下：

1）选取AUV运动模型

如附图2所示，以AUV初始位置和初始艏向角建立全局坐标系L；V是AUV船体坐标系；E为北东坐标系，North方向为地磁北向。x、y为AUV在L中的位置；Ψ为AUV在L中的艏向角，显然

其中z_Ψ为采用运动传感器OCTANS测得的AUV艏向角。

本发明选取式(1)一个简单的四自由度、常速动力学模型x_k＝f(x_k-1)+w_k-1对AUV的运动过程进行建模：

${\left[\begin{array}{c}x\\ y\\ z\\ \mathrm{\ψ}\\ u\\ v\\ w\\ r\end{array}\right]}_k={\left[\begin{array}{c}x+\mathrm{uT}\cos \left(\mathrm{\ψ}\right)-\mathrm{vT}\sin \left(\mathrm{\ψ}\right)\\ y+\mathrm{uT}\sin \left(\mathrm{\ψ}\right)+\mathrm{vT}\cos \left(\mathrm{\ψ}\right)\\ z+\mathrm{wT}\\ \mathrm{\ψ}+\mathrm{rT}\\ u\\ v\\ w\\ r\end{array}\right]}_{k-1}+w_{k-1}---\left(1\right)$

式中，[x,y,z,Ψ]表示AUV在L中的位置和艏向；[u,v,w,r]表示AUV在V中相应的线速度和角速度；k表示任意采样时刻；T为航位推算传感器的采样时间间隔；w_k-1为随机系统噪声。

2）选取AUV测量模型

AUV配置了深度计、运动传感器OCTANS和多普勒计程仪DVL。深度计即压力传感器，通过测量水柱压力提供AUV的深度数据；AUV通过OCTANS实时测量其艏向角，即AUV艏艉向与地磁北之间的夹角；DVL可以测量海流速度、对底跟踪速度等，AUV在海试中使用DVL进行对底跟踪速度的测量。它们提供状态向量中深度、艏向和对底速度的直接测量值，因而观测模型为线性的。本发明选取式(2)的测量模型z_k＝h(x_k)+v_k对AUV的传感器测量进行建模：

z_k＝Hx_k+v_k(2)

式中，z_k是观测向量， $x_k={\left[\begin{array}{cccccccc}x& y& z& \mathrm{\ψ}& u& v& w& r\end{array}\right]}_k^T,$ v_k是观测噪声，测量矩阵H为：

$H=\left[\begin{array}{cccccccc}0& 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& 0\end{array}\right].$

3）通过AUV运动模型和测量模型构建动态系统

根据AUV运动模型和测量模型构建动态系统：

$\left\{\begin{array}{c}{x}_k=f\left(x_{k-1}\right)+w_{k-1}\\ z_k=h\left(x_k\right)+v_k\end{array}\right.---\left(3\right)$

式中， $x_k={\left[\begin{array}{cccccccc}x& y& z& \mathrm{\ψ}& u& v& w& r\end{array}\right]}_k^T,$ z_k是观测向量；随机系统噪声w_k~N(0,Q_k)，随机观测噪声v_k~N(0,R_k)，系统初始状态为x₀，x₀与w_k，v_k统计独立；非线性函数f(x_k-1)和h(x_k)为关于状态的一阶连续偏导数：

$f\left(x_{k-1}\right){\left[\begin{array}{c}x+\mathrm{uT}\cos \left(\mathrm{\ψ}\right)-\mathrm{vT}\sin \left(\mathrm{\ψ}\right)\\ y+\mathrm{uT}\sin \left(\mathrm{\ψ}\right)+\mathrm{vT}\cos \left(\mathrm{\ψ}\right)\\ z+\mathrm{wT}\\ \mathrm{\ψ}+\mathrm{rT}\\ u\\ v\\ w\\ r\end{array}\right]}_{k-1},$ h(x_k)＝Hx_k|k-1。

4）滤波器参数初始化

非线性滤波是根据当前时刻及此前带有噪声的观测量来估计未知的系统状态。设定AUV初始时刻的状态x₀＝[0]_8×1，并设置初始时刻的状态误差协方差P₀，系统噪声Q，观测噪声R，用于求取渐消因子的C_0,k的初始值C_0,0及遗忘因子ρ。

5）选取渐消因子

CKF基于容积准则，计算出一组偶数个具有同等权值的容积点，它们能完全捕获高斯分布变量的均值和方差，且经过非线性系统方程转换后，其后验均值和方差能够精确到非线性系统泰勒级数展开的三阶项或更高阶项，无需对非线性模型进行线性化，不依赖于具体系统模型的非线性方程，算法相对独立，适用于任何形式的非线性模型。但CKF仍存在对模型参数变化的鲁棒性差、收敛速度慢及对突变状态的跟踪能力低等缺陷。因此，本发明结合STF，提出STCKF滤波算法。

定义1：为了增强CKF的鲁棒性，抑制模型误差对非线性系统CKF滤波器的影响，具备以下性能的CKF为STCKF：

1)较强的关于实际系统参数变动的鲁棒性；

2)较低的关于系统噪声和测量噪声，以及初值统计特性的敏感性；

3)极强的关于突变状态的跟踪能力，并在滤波器达稳态时，仍保持对缓变状态以及突变状态的强跟踪能力；

4)适中的计算复杂性。

标准CKF成为STCKF的充分条件是在线选择k时刻的时变滤波增益K_k，使得：

$E\left[(x_k-{\hat{x}}_{k|k}){(x_k-{\hat{x}}_{k|k})}^T\right]=\min ---\left(4\right)$

$E\left[{\mathrm{\γ}}_{k+j}{\mathrm{\γ}}_k^T\right]=0---\left(5\right)$

式中，γ_k是k时刻的残差，k＝0,1,2…，j＝1,2…，x_k为k时刻的真实状态值，

为k时刻的状态估计值。条件(4)是CKF状态估计残差最小方差性能指标；条件(5)要求不同时刻的输出残差序列处处保持正交。

实际应用中，系统模型不确定性造成滤波器的状态估计值偏离系统真实状态时，会导致滤波输出残差序列不正交，建立在性能指标(4)和(5)基础上的STCKF通过引入渐消因子强行使输出残差序列保持正交，具有类似高斯白噪声的性质，最大程度地提取输出残差序列中一切有效信息，使STCKF在模型不确定时仍能保持对系统状态的强跟踪能力。

以下给出STCKF的一个重要性质：引理1：对于系统模型(3)，令

其中

为采用STCKF得到的状态估计值。当STCKF可以比较准确地估计出系统状态时，即O[|ε_k|²]＜＜O[|ε_k|]，下式成立：

$C_{j,k}=E\left[{\mathrm{\γ}}_{k+j}{\mathrm{\γ}}_k^T\right]$

$\≈H\left({\hat{x}}_{k+j|k+j-1}\right)\·F\left({\hat{x}}_{k+j-1|k+j-1}\right)$ (6)

$\·[I-K_{k+j-1}H\left({\hat{x}}_{k+j-1|k+j-2}\right)]\·F\left({\hat{x}}_{k+j-2|k+j-2}\right)\·...\·[I-K_{k+1}H\left({\hat{x}}_{k+1|k}\right)]\·F\left({\hat{x}}_{k|k}\right)$

$\·[P_{\mathrm{xz},k|k-1}-K_k{C}_{0,k}]$

式中，j＝1,2,…，k＝0,1,2…，γ是残差，H和F分别是f(x_k)和h(x_k)关于x_k的雅克比矩阵，K_k是增益矩阵，P_xz，k|k-1为互相关协方差阵，

证明：

系统初始状态x₀与w_k、v_k统计独立。因此有：

$E\left[{\mathrm{\γ}}_{k+j}{\mathrm{\γ}}_k^T\right]=E\left\{\right[z_{k+j}-h_{\hat{x}(k+j|k+j-1)}\left]{\mathrm{\γ}}_k^T\right\}\≈E\left\{\right[H_{\hat{x}(k+j|k+j-1)}(x_{k+j}-{\hat{x}}_{k+j|k+j-1})\left]{\mathrm{\γ}}_k^T\right\}---\left(7\right)$

$x_{k+j}-{\hat{x}}_{k+j|k+j-1}$

$=[f\left({\hat{x}}_{k+j-1|k+j-1}\right)+F\left({\hat{x}}_{k+j-1|k+j-1}\right)\·(x_{k+j-1}-{\hat{x}}_{k+j-1|k+j-1})+H.O.T+w_{k+j-1}]$ (8)

$-f\left({\hat{x}}_{k+j-1|k+j-1}\right)$

$\≈F\left({\hat{x}}_{k+j-1|k+j-1}\right)\·(x_{k+j-1}-{\hat{x}}_{k+j-1|k+j-1})+w_{k+j-1}$

式(8)中，H.O.T代表二阶以上的所有项之和，证明过程中忽略H.O.T。

将式(8)代入式(7)可得：

$E\left[{\mathrm{\γ}}_{k+j}{\mathrm{\γ}}_k^T\right]\≈E\left\{\right[H\left({\hat{x}}_{k+j|k+j-1}\right)\·F\left({\hat{x}}_{k+j-1|k+j-1}\right)\·(x_{k+j-1}-{\hat{x}}_{k+j-1|k+j-1})+v_{k+j-1}\left]{\mathrm{\γ}}_k^T\right\}---\left(9\right)$

$x_{k+j-1}-{\hat{x}}_{k+j-1|k+j-1}=x_{k+j-1}-[{\hat{x}}_{k+j-1|k+j-2}+K_{k+j-1}{\mathrm{\γ}}_{k+j-1}]$

$\≈x_{k+j-1}-{\hat{x}}_{k+j-1|k+j-2}-K_{k+j-1}\·[H\left({\hat{x}}_{k+j-1|k+j-2}\right)\·(x_{k+j-1}-{\hat{x}}_{k+j-1|k+j-2})]$

$=[I-K_{k+j-1}H\left({\hat{x}}_{k+j-1|k+j-2}\right)]\·(x_{k+j-1}-{\hat{x}}_{k+j-1|k+j-2})$ (10)

$\≈[I-K_{k+j-1}H\left({\hat{x}}_{k+j-1|k+j-2}\right)]\·F\left({\hat{x}}_{k+j-2|k+j-2}\right)\·(x_{k+j-2}-{\hat{x}}_{k+j-2|k+j-2})+w_{k+j-1}$

$=[I-K_{k+j-1}H\left({\hat{x}}_{k+j-1|k+j-2}\right)]\·F\left({\hat{x}}_{k+j-2|k+j-2}\right)\·...\·[I-K_{k+1}H\left({\hat{x}}_{k+1|k}\right)]\·F\left({\hat{x}}_{k|k}\right)$

$\·E\left[(x_k-{\hat{x}}_{k|k}){\mathrm{\γ}}_k^T\right]$

将式(10)代入式(9)可得到：

$E\left[{\mathrm{\γ}}_{k+j}{\mathrm{\γ}}_k^T\right]\≈H\left({\hat{x}}_{k+j|k+j-1}\right)\·F\left({\hat{x}}_{k+j-1|k+j-1}\right)$

$\·[I-K_{k+j-1}H\left({\hat{x}}_{k+j-1|k+j-2}\right)]\·F\left({\hat{x}}_{k+j-2|k+j-2}\right)$ (11)

$\·...\·[I-K_{k+1}H\left({\hat{x}}_{k+1|k}\right)]\·F\left({\hat{x}}_{k|k}\right)$

$\·E\left[(x_k-{\hat{x}}_{k|k}){\mathrm{\γ}}_k^T\right]$

残差 ${\mathrm{\γ}}_k=z_k-{\hat{z}}_{k|k-1},$ 因此有：

$E\left[(x_k-{\hat{x}}_{k|k}){\mathrm{\γ}}_k^T\right]=E\left\{\right[x_k-({\hat{x}}_{k|k-1}+K_k{\mathrm{\γ}}_k)\left]{\mathrm{\γ}}_k^T\right\}=E[(x_k-{\hat{x}}_{k|k-1}){\mathrm{\γ}}_k^T-K_k{\mathrm{\γ}}_k{\mathrm{\γ}}_k^T]$

$=E[(x_k-{\hat{x}}_{k|k-1}){(z_k-{\hat{z}}_{k|k-1})}^T-K_k{\mathrm{\γ}}_k{\mathrm{\γ}}_k^T]---\left(12\right)$

$=P_{\mathrm{xz},k|k-1}-K_k{C}_{0,k}$

将式(12)代入式(11)即可得到式(6)，引理1证明完毕。证明中忽略二阶以上所有项之和。其中，x₀为系统初始状态；w_k为系统噪声；v_k为观测噪声；z_k+j为k+j时刻的实际观测值；x(k+j)为k+j时刻的状态向量；

为k+j时刻的状态预测值；H和F分别是f(k,x_k,u_k)和h(k,x_k，u_k)关于x_k的雅克比矩阵；

为k时刻的状态估计值；γ_k为k时刻的残差，且

K_k是增益矩阵；P_xz，k|k-1为互相关协方差阵；

引理1反映了残差自协方差的一个重要性质。当滤波器的增益最优时，C_j,k＝0，表明残差是不相关的，而当模型参数和噪声方差存在误差时，C_j,k≠0。如果能选择渐消矩阵使得C_j,k的最后一项，对于所有的j＝1,2,3,…,都有式(13)成立，可以认为K_k仍将是最优的。CKF满足式(13)时成为STCKF。

P_xz，k|k-1-K_kC_0,k＝0(13)

根据CKF算法， $K_k=P_{\mathrm{xz},k|k-1}{P}_{\mathrm{zz},k|k-1}^{-1},$ 因此有：

$P_{\mathrm{xz},k|k-1}(I-P_{\mathrm{zz},k|k-1}^{-1}{C}_{0,k})=0---\left(14\right)$

式(14)成立的一个充分条件是：

$I-P_{\mathrm{zz},k|k-1}^{-1}{C}_{0,k}=0---\left(15\right)$

即：

P_zz，k|k-1-C_0,k＝0(16)

本发明基于强跟踪滤波器的理论框架，在CKF协方差阵中引入一个时变的渐消因子λ_k实时调整增益矩阵K_k，减弱老数据对当前滤波值的影响，相对地增加新数据的影响，构成具有强跟踪性能的CKF（STCKF）。从而，CKF中自相关协方差阵P_zz，k|k1及互相关协方差阵P_xz，k|k-1计算式转换为：

$P_{\mathrm{zz},k|k-1}={\mathrm{\λ}}_k\·(\frac{1}{m}\underset{k=1}{\overset{m}{\mathrm{\Σ}}}{z}_{i,k|k-1}{z}_{i,k|k-1}^T-{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T)+R_k---\left(17\right)$

$P_{\mathrm{xz},k|k-1}={\mathrm{\λ}}_k\·(\frac{1}{m}\underset{k=1}{\overset{m}{\mathrm{\Σ}}}{X}_{i,k|k-1}{z}_{i,k|k-1}^T-{\hat{x}}_{k|k-1}{\hat{z}}_{k|k-1}^T)---\left(18\right)$

将式(17)代入式(16)，并对等式两侧求迹可得：

$\mathrm{tr}({\mathrm{\λ}}_k\·(\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{z}_{i,k|k-1}{z}_{i,k|k-1}^T-{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T))=\mathrm{tr}(C_{0,k}-R_k)---\left(19\right)$

式(35)可转化为：

${\mathrm{\λ}}_k=\frac{\mathrm{tr}(C_{0,k}-R_k)}{\mathrm{tr}(\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{z}_{i,k|k-1}{z}_{i,k|k-1}^T-{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T)}---\left(20\right)$

其中，tr(·)为求矩阵的迹，m＝2n（n为AUV状态向量的维数）是容积点的个数，z_i,k|k-1为通过观测方程传播后的容积点，

为k时刻的观测预测值，R_k为k时刻的观测噪声，C_0，k由式(21)确定：

$C_{0,k}=\left\{\begin{array}{cc}{\mathrm{\γ}}_k{\mathrm{\γ}}_k^T& k=1\\ \frac{\mathrm{\ρ}{C}_{0,k-1}+{\mathrm{\γ}}_k{\mathrm{\γ}}_k^T}{1+\mathrm{\ρ}}& k>1\end{array}\right.---\left(21\right)$

式中，γ_k是k时刻的残差，0＜ρ≤1为遗忘因子，能提高滤波器的快速跟踪能力，通常取ρ=0.95左右，其值越大，k时刻以前的信息所占的比例越小，就越突出当前残差向量的影响。该方法具有很强的关于突变状态的跟踪能力，且在滤波达到稳态时，仍保持对缓变状态及突变状态的跟踪能力。

6）滤波器时间更新

假设k-1时刻后验概率密度函数

已知，通过Cholesky分解误差协方差P_k-1|k-1：

$P_{k-1|k-1}=S_{k-1|k-1}{S}_{k-1|k-1}^T---\left(22\right)$

计算容积点(i＝1,2,…,m;m＝2n)：

$X_{i,k-1|k-1}=S_{k-1|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k-1|k-1}---\left(23\right)$

通过状态方程传播容积点：

$X_{i,k-1|k-1}^{*}=f\left(X_{i,k-1|k-1}\right)---\left(24\right)$

估计k时刻的状态预测值：

${\hat{x}}_{k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{X}_{i,k|k-1}^{*}---\left(25\right)$

估计k时刻的状态误差协方差预测值：

$P_{k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{X}_{i,k|k-1}^{*}{X}_{i,k|k-1}^{*T}-{\hat{x}}_{k|k-1}{\hat{x}}_{k|k-1}^T+Q_{k-1}---\left(26\right)$

其中，P_k-1|k-1为误差协方差，S_k-1k-1为P_k-1|k-1的Cholesky因子；X_i，k-1|k-1为k-1时刻的容积点，

[1]_i表示集合[1]的第i列，当状态向量的维数n＝2时，[1]={(1,0)^T,(-1,0)^T,(0,-1)^T,(0,1)^T}；

为通过状态方程传播的容积点，f为状态方程；

为k时刻的状态预测值，P_k|k-1为k时刻的状态误差协方差预测值，Q_k-1为k-1时刻的系统噪声。

7）滤波器量测更新

通过Cholesky分解P_k|k-1：

$P_{k-1|k-1}=S_{k-1|k-1}{S}_{k-1|k-1}^T---\left(27\right)$

计算容积点(i＝1,2,…,m;m＝2n)：

$X_{i,k|k-1}=S_{k|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k|k-1}---\left(28\right)$

通过观测方程传播容积点：

z_i，k|k-1＝h(X_i，k|k-1)(29)

估计k时刻的观测预测值：

${\hat{z}}_{k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{z}_{i,k|k-1}---\left(30\right)$

在自相关协方差阵和互相关协方差阵估计中引入渐消因子，以便对滤波器增益K_k进行实时调整，自相关协方差阵估计：

$P_{\mathrm{zz},k|k-1}={\mathrm{\λ}}_k\·(\frac{1}{m}\underset{k=1}{\overset{m}{\mathrm{\Σ}}}{z}_{i,k|k-1}{z}_{i,k|k-1}^T-{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T)+R_k---\left(31\right)$

互相关协方差阵估计：

$P_{\mathrm{xz},k|k-1}={\mathrm{\λ}}_k\·(\frac{1}{m}\underset{k=1}{\overset{m}{\mathrm{\Σ}}}{X}_{i,k|k-1}{z}_{i,k|k-1}^T-{\hat{x}}_{k|k-1}{\hat{z}}_{k|k-1}^T)---\left(32\right)$

估计卡尔曼滤波增益：

$K_k=P_{\mathrm{xz},k|k-1}{P}_{\mathrm{zz},k|k-1}^{-1}---\left(33\right)$

估计k时刻状态值：

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1})---\left(34\right)$

估计k时刻状态误差协方差值：

$P_{k|k}=P_{k|k-1}-K_k{P}_{\mathrm{zz},k|k-1}{K}_k^T---\left(35\right)$

其中，S_k|k-1为P_k|k-1的Cholesky因子，X_i，k|k-1为k时刻的容积点，z_i，k|k-1为通过观测方程传播的容积点，h为观测方程，

为k时刻的观测预测值，P_zz，k|k-1为自相关协方差阵，R_k为k时刻的观测噪声，P_xz，k|k-1为互相关协方差阵，K_k为卡尔曼滤波增益，

为k时刻状态估计值，P_k|k为k时刻状态误差协方差估计值。

8）时刻由当前时刻更新到下一时刻

时刻由当前时刻更新到下一时刻，然后判断是否到达终止时刻。若未到达终止时刻，则由当前时刻更新到下一时刻，重复执行步骤5）~7），直到到达终止时刻；若到达终止时刻，则结束计算，输出结果。

以下描述本发明的实施例。

1.海试试验一

(1)试验条件

采用自研AUV于2010年10月在大连小平岛完成海试，试验中AUV的航行路线为图2中A点至B点，采集的传感器数据包括深度计、运动传感器OCTANS、多普勒计程仪DVL等测量数据，所选取试验航段的时间为18分35秒。

(2)STCKF初始参数设置

AUV状态初始值：x₀＝[0]_8×1；

状态协方差初始值：P₀＝diag([11111111])；

系统噪声：Q＝diag([2e-41e-47e-43e-46e-44e-49e-43e-4])；

观测噪声：R＝diag([0.03²0.04²0.02²0.01²0.04²])；

C_0,0＝diag([9e-51e-58e-53e-49e-5])；

遗忘因子：ρ=0.95；

(3)试验结果及分析

附图3为采用EKF、CKF与本发明所提出的STCKF进行AUV自主导航定位运行轨迹与GPS轨迹的对比曲线。点线为EKF的航迹，点划虚线为CKF的航迹，实线为STCKF的航迹，虚线为GPS轨迹。

附图4、附图5分别对应三种滤波算法东向和北向的估计误差，点线是EKF的误差，点划虚线为CKF的误差，实线为STCKF的误差。横轴t代表实验迭代过程（共889步）。

对比附图4和附图5实验结果，有以下分析结果：

从估计算法稳定性角度分析，在东向上，三种算法中STCKF稳定性最好，误差值变化量在32m左右；CKF的稳定性稍差于STCKF；而EKF的稳定性最差，其误差值变化量高达40m。在北向上，STCKF稳定性最好，误差值变化量在61m以内；CKF的稳定性最差，误差值变化量超过68米；仅优于EKF；EKF的稳定性介于STCKF和CKF之间。

从估计精度角度分析，在东向上，三种算法的估计精度在后半段都有明显提高，当t＞350，STCKF的估计精度一直优于CKF和EKF；EKF的精度一直最低。在北向上，整个实验迭代过程中，STCKF的估计精度一直优于CKF和EKF；t＜650时，EKF的精度最低，当t＞650，CKF精度最低。总体上看，当t＞650时，STCKF的估计精度有明显提高，优势比较明显。

定义系统导航定位误差模型为：

${\mathrm{RMSE}}_{\mathrm{pos}}=\sqrt{\frac{1}{\mathrm{tf}}\underset{k=1}{\overset{\mathrm{tf}}{\mathrm{\Σ}}}({({\mathrm{tx}}_k-e{\hat{x}}_k)}^2+{({\mathrm{ty}}_k-e{\hat{y}}_k)}^2)}---\left(36\right)$

式中，k是任意时刻，tf是总的运行步数，(tx_k,ty_k)与

分别是AUV在k时刻的真实位置和滤波算法估计的位置。

通过式(36)计算可得，STCKF、CKF和EKF的导航定位误差分别为41.0718m、45.8963m和49.1986m。可见，STCKF导航定位误差最小，优于CKF和EKF。

2海试试验二

(1)试验条件

采用自研AUV于2010年3月在杭州千岛湖完成湖试，采集的传感器数据包括深度计、运动传感器OCTANS、多普勒计程仪DVL等测量数据，AUV近水面航行且采用GPS记录其航迹。湖试中AUV的运动轨迹大致为五边形，选取其中的4圈试验航段，历时48分钟36秒。

(2)STCKF初始参数设置

AUV状态初始值：x₀＝[0]_8×1；

状态协方差初始值：P₀＝diag([11111111])；

系统噪声：Q＝diag([8e-42e-38e-25e-17e-29e-38e-43e-4])；

观测噪声：R＝diag([0.01²0.01²0.01²0.01²0.01²])；

C_0,0＝diag([1e-53e-56e-58e-45e-5])；

遗忘因子：ρ=0.95；

(3)试验结果及分析

附图6为采用EKF、CKF与本发明所提出的STCKF进行AUV自主导航定位运行轨迹与GPS轨迹的对比曲线。附图7为附图6中矩形框所包围区域的局部放大图。点线为EKF的航迹，点划虚线为CKF的航迹，实线为STCKF的航迹，虚线为GPS轨迹。

附图8、附图9分别对应三种滤波算法在东向和北向的估计误差，点线是EKF的误差，点划虚线为CKF的误差，实线为STCKF的误差。横轴t代表滤波算法迭代过程（共5686步）。

对比附图8和附图9实验结果，有以下分析结果：

从估计算法稳定性角度分析，在东向上，三种算法中STCKF稳定性最好，误差值变化量在9m左右；CKF的稳定性稍差于STCKF，误差变化量在11m左右；而EKF的稳定性最差，其误差值变化量高达20m。在北向上，STCKF一直比较稳定，误差值变化量约为6m；CKF误差值变化量在10m以内；EKF的稳定性最差，误差值变化量约为30m。

从估计精度角度分析，在AUV运行一圈的过程中，在东向和北向上，三种算法的估计精度都是先降低后回升。在东向上，在整个实验迭代过程中，STCKF的估计精度一直优于CKF和EKF；CKF精度较低；而EKF的精度最低；随着AUV航行圈数的增多，EKF估计精度严重下降，STCKF和CKF估计精度也略微呈现出下降的趋势。在北向上，STCKF的估计精度一直优于CKF和EKF；随着AUV航行距离的增长，EKF估计精度严重下降，STCKF和CKF估计精度未随AUV运行圈数增多而降低。

通过式(36)计算可得，STCKF、CKF和EKF的定位误差分别为1.5878m、3.982m和18.6587m。可见，STCKF导航定位误差最小，优于CKF和EKF。

由前述两组海试数据集的验证结果可见，STCKF的导航定位估计精度和稳定性都优于CKF和EKF。基于STCKF的AUV自主导航算法，其导航定位精度的提高使得AUV无需周期性地上浮至水面进行GPS校正，这对于AUV执行长航时水下隐蔽监测与作业具有重要的实际应用意义。

Claims (1)

1.一种自主水下航行器自主导航定位方法，其特征在于，包括如下步骤：

（1）建立自主水下航行器运动模型：

${\left[\begin{array}{c}x\\ y\\ z\\ \mathrm{\ψ}\\ u\\ v\\ w\\ r\end{array}\right]}_k={\left[\begin{array}{c}x+\mathrm{uT}\cos \left(\mathrm{\ψ}\right)-\mathrm{vT}\sin \left(\mathrm{\ψ}\right)\\ y+\mathrm{uT}\sin \left(\mathrm{\ψ}\right)+\mathrm{vT}\cos \left(\mathrm{\ψ}\right)\\ z+\mathrm{wT}\\ \mathrm{\ψ}+\mathrm{rT}\\ u\\ v\\ w\\ r\end{array}\right]}_{k-1}+w_{k-1}$

式中，[x,y,z,Ψ]表示AUV在L中的位置和艏向；[u,v,w,r]表示AUV在V中相应的线速度和角速度；k表示任意采样时刻；T为航位推算传感器的采样时间间隔；w_k-1为随机系统噪声；

（2）建立自主水下航行器测量模型：

z_k＝H_xk|k-1+v_k

式中，z_k是观测向量， $x_k={\left[\begin{array}{cccccccc}x& y& z& \mathrm{\ψ}& u& v& w& r\end{array}\right]}_k^T,$ v_k是观测噪声，测量矩阵H为：

$H=\left[\begin{array}{cccccccc}0& 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& 0\end{array}\right];$

（3）通过自主水下航行器运动模型和测量模型构建动态系统：

$\left\{\begin{array}{c}{x}_k=f\left(x_{k-1}\right)+w_{k-1}\\ z_k=h\left(x_k\right)+v_k\end{array}\right.,$

式中， $x_k={\left[\begin{array}{cccccccc}x& y& z& \mathrm{\ψ}& u& v& w& r\end{array}\right]}_k^T,$ z_k是观测向量；随机系统噪声w_k~N(0,Q_k)，随机观测噪声v_k~N(0,R_k)，系统初始状态为x₀，x₀与w_k，v_k统计独立；非线性函数f(x_k-1)和h(x_k)为关于状态的一阶连续偏导数：

$f\left(x_{k-1}\right){\left[\begin{array}{c}x+\mathrm{uT}\cos \left(\mathrm{\ψ}\right)-\mathrm{vT}\sin \left(\mathrm{\ψ}\right)\\ y+\mathrm{uT}\sin \left(\mathrm{\ψ}\right)+\mathrm{vT}\cos \left(\mathrm{\ψ}\right)\\ z+\mathrm{wT}\\ \mathrm{\ψ}+\mathrm{rT}\\ u\\ v\\ w\\ r\end{array}\right]}_{k-1},$ h(x_k)＝Hx_k|k-1；

（4）滤波器参数初始化：

自主水下航行器初始时刻的状态x₀＝[0]_8×1，初始时刻的状态误差协方差P₀，系统噪声Q，观测噪声R，用于求取渐消因子的C_0,k的初始值C_0,0及遗忘因子ρ；

（5）选取渐消因子：

${\mathrm{\λ}}_k=\frac{\mathrm{tr}(C_{0,k}-R_k)}{\mathrm{tr}(\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{z}_{i,k|k-1}{z}_{i,k|k-1}^T-{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T)}$

其中，λ_k为渐消因子，tr(·)为求矩阵的迹，m＝2n是容积点的个数，n为AUV状态向量的维数，z_i，k|k-1为通过观测方程传播后的容积点，为k时刻的观测预测值，R_k为k时刻的观测噪声， $C_{0,k}=\left\{\begin{array}{cc}{\mathrm{\γ}}_k{\mathrm{\γ}}_k^T& k=1\\ \frac{\mathrm{\ρ}{C}_{0,k-1}+{\mathrm{\γ}}_k{\mathrm{\γ}}_k^T}{1+\mathrm{\ρ}}& k>1\end{array}\right.,$ γ_k是k时刻的残差，0＜ρ≤1为遗忘因子；

（6）估计k时刻自主水下航行器的状态：

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1})$

式中，

为k时刻的状态估计值，

为k时刻的状态预测值，K_k为卡尔曼滤波增益，z_k为观测向量，

为k时刻的观测预测值；

（7）估计k时刻自主水下航行器的状态协方差：

$P_{k|k}=P_{k|k-1}-K_k{P}_{\mathrm{zz},k|k-1}{K}_k^T$

式中，P_k|k为k时刻的状态误差协方差估计值，P_k|k-1为k时刻的状态协方差预测值，K_k为卡尔曼滤波增益，P_zz，k|k-1为自相关协方差矩阵；

（8）由当前时刻更新到下一时刻，重复执行步骤（5）至步骤（7），直到到达终止时刻，结束计算，输出结果。

Images