CN102052924A - 一种小型水下机器人组合导航定位方法 - Google Patents

Abstract

本发明提供的是一种小型水下机器人组合导航定位方法。采用强跟踪无迹卡尔曼滤波器对各种导航设备的测量数据加以融合；取导航系下的位置矢量和航向角与载体系下速度矢量和加速度矢量作为滤波器状态矢量；通过强跟踪无迹卡尔曼滤波器实现水下机器人的自主导航与数据滤波；取GPS接收机输出的水平位置信息，通过强跟踪无迹卡尔曼滤波器实现水下机器人的自主校正与数据滤波；通过GPS接收机输出信号的有效位实现水上和水下两个不同量测方程的切换。本发明可以融合各种导航设备测量到的小型水下机器人的位置、深度和姿态信息，在存在海流或海浪的干扰的情况下，实现小型水下机器人的自主导航与自主校正。

Description

一种小型水下机器人组合导航定位方法

技术领域

本发明涉及的是一种小型水下机器人自主导航与自主校正方法。

背景技术

小型水下机器人执行水下任务过程中，需为其控制系统和制导系统提供准确、平滑的位置、深度、姿态以及载体系下的速度和加速度信息，并能自动上浮至水面进行自主校正。小型水下机器人导航定位系统往往存在由海流或海浪干扰导致的系统模型失配问题，并且导航定位系统一般仅能提供未经滤波的加速度、深度信息和导航系下的速度信息，这些信息需经滤波和坐标变换到艇体系后方可被机器人控制系统使用。而取多路导航信号的导航方法通常是简单导航信息的备份，而没有进行有效的信息融合。本发明采用强跟踪无迹卡尔曼滤波算法有效地解决了小型水下机器人导航定位系统的模型失配问题，对多路导航设备提供的导航信息进行融合，提高了定位精度，平滑了航向、高度信息，并能直接为控制系统提供平滑的艇体系下的速度与加速度信息。《控制与决策》(1990，第5期)“非线性系统的带次优渐消因子的扩展卡尔曼滤波”提出了强跟踪滤波器有效解决了EKF由于模型失配导致的鲁棒性差、滤波发散等问题，但要求非线性系统状态函数和量测函数必须是连续可微的，而且强跟踪滤波器需要计算非线性函数的雅可比矩阵，不但过程繁琐易出错，而且对非线性函数的线性化近似精度通常偏低，当系统具有强非线性时精度严重下降，甚至发散。本发明采用的强跟踪无迹卡尔曼滤波算法通过采用UT变换克服了上述问题。《航空电子技术》(2008，第39卷第4期)“强跟踪UKF滤波在SINS/GPS组合导航中的应用研究”，针对大机动条件下飞行器SINS/GPS组合导航系统的滤波发散问题，设计了强跟踪无迹卡尔曼滤波算法，通过渐消因子在线调整卡尔曼滤波增益从而提高无迹卡尔曼滤波算法的跟踪性能。《仪器仪表学报》(2008，第29卷第8期)“强跟踪UKF方法及其在故障辨识中的应用”也基于强跟踪无迹卡尔曼滤波算法设计了GNSS/SINS组合导航系统。本发明采用的强跟踪无迹卡尔曼滤波算法同上述算法的渐消因子的计算方法不同，且更为合理。

发明内容

本发明的目的在于提供一种既能有效解决小型水下机器人因海流或海浪干扰而导致的导航定位系统模型失配问题，又能平滑小型水下机器人控制系统所需的航向、深度以及在艇体系下的速度与加速度信息的集自主水下导航定位与自主水面位置校正功能的小型水下机器人导航定位方法。

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

本发明的小型水下机器人组合导航定位方法主要包括如下步骤：1)采用强跟踪无迹卡尔曼滤波器进行组合导航系统的数据融合；2)取导航系下的位置矢量和航向角与载体系下速度矢量和加速度矢量作为滤波器状态矢量，滤波结果被机器人控制与制导系统直接使用；3)取深度计输出的深度信息同航姿参考系统输出的航向角、姿态角和载体系下的加速度信息与载体系下多普勒测速声纳的速度信息，通过强跟踪无迹卡尔曼滤波器实现水下机器人的自主导航与数据滤波；4)取GPS接收机输出的水平位置信息，航姿参考系统输出的航向、姿态及载体系下加速度信息与载体系下多普勒测速声纳的速度信息，通过强跟踪无迹卡尔曼滤波器实现水下机器人的自主校正与数据滤波；5)通过GPS接收机输出信号的有效位实现水上和水下两个不同量测方程的切换。

1、所述采用强跟踪无迹卡尔曼滤波器进行组合导航系统数据融合的步骤如下：

系统噪声方差阵为Q，观测噪声方差阵为R；

(1)选择对称点采样策略，根据

P(k|k)计算k时刻Sigma点集{χ_i}，其中i＝1、...、L；

(2)计算经过非线性状态方程

传播后的Sigma点χ_i(k+1|k)，由χ_i(k+1|k)计算得状态向量X(k)一步预测估计

${\mathrm{\χ}}_i(k+1|k)=f\left({\mathrm{\χ}}_i\left(k\right|k)\right)+G{\stackrel{\‾}{X}}_{\mathrm{SUB}}\left(k\right)$

$\hat{X}(k+1|k)=\underset{i=0}{\overset{L}{\mathrm{\Σ}}}{\mathrm{\ω}}_i^m{\mathrm{\χ}}_i(k+1|k)$

式中，

i＝0，·，L，为求一阶统计特性时的权系数；

i＝0，·，L，为求二阶统计特性时的权系数；

(3)根据测量值Z(k+1)通过量测方程Z(k)＝HX(k)+V(k)计算新息

$\mathrm{\γ}(k+1)=Z(k+1)-H\hat{X}(k+1|k);$

(4)在线计算渐消因子矩阵Λ(k+1)

$V_0(k+1)=E\left(\mathrm{\γ}(k+1){\mathrm{\γ}}^T(k+1)\right)$

$\≈\left\{\begin{array}{cc}\mathrm{\γ}\left(1\right){\mathrm{\γ}}^T\left(1\right),& k=0\\ \frac{\mathrm{\ρ}{V}_0\left(k\right)+\mathrm{\γ}(k+1){\mathrm{\γ}}^T(k+1)}{1+\mathrm{\ρ}},& k\≥0\end{array}\right.$

N(k+1)＝V₀(k+1)-R-HQH^T

$P_{\stackrel{\·}{X}\stackrel{\·}{X}}=\underset{i=0}{\overset{L}{\mathrm{\Σ}}}{\mathrm{\ω}}_i^c[{\mathrm{\χ}}_i(k+1|k)-\hat{X}(k+1|k)]\·{[{\mathrm{\χ}}_i(k+1|k)-\hat{X}(k+1|k)]}^T$

$M(k+1)=M_{\mathrm{ii}}(k+1)=P_{\stackrel{\·}{X}\stackrel{\·}{X}}{H}^{T}H$

$\mathrm{\η}(k+1)=\frac{\mathrm{trace}\left(N(k+1)\right)}{\underset{i=1}{\overset{d}{\mathrm{\Σ}}}{M}_{\mathrm{ii}}(k+1)}$

${\mathrm{\λ}}_i(k+1)=\left\{\begin{array}{cc}{\mathrm{\α}}_i\mathrm{\η}(k+1),& {\mathrm{\α}}_i\mathrm{\η}(k+1)>1\\ 1& {\mathrm{\α}}_i\mathrm{\η}(k+1)\≤1\end{array}\right.$

Λ(k+1)＝diag(λ₁(k+1)，λ₂(k+1)，·，λ_d(k+1))；

(5)计算误差协方差阵预报值P(k+1|k)

$P(k+1|k)=\mathrm{\Λ}(k+1)P_{\stackrel{\·}{X}\stackrel{\·}{X}}+Q;$

(6)测量更新

K(k+1)＝P(k+1|k)H^T[HP(k+1|k)H^T+R]^-1

$\hat{X}(k+1|k+1)=\hat{X}(k+1|k)+K(k+1)\mathrm{\γ}(k+1)$

P(k+1|k+1)＝[I-K(k+1)H]P(k+1|k)；

(7)从状态向量中提取与

对应的状态变量。

2、所述取导航系下的位置矢量和航向角与载体系下速度矢量和加速度矢量作为滤波器状态矢量，滤波结果被机器人控制与制导系统直接使用的步骤如下：

采用NED导航坐标系O-XYZ，以O作为原点，OZ轴指向地心，OX轴指北，OY轴指东；艇体坐标系o-xyz定义如下，ox轴沿机器人纵轴，oy轴平行于基平面与ox轴垂直指向艇体右舷，oz轴垂直于基平面并与ox轴、oy轴构成右手直角坐标系；选取导航系下位置矢量P和航向角ψ与艇体系下的速度矢量V、加速度矢量A作为小型水下机器人运动连续时间模型的状态矢量；其中位置矢量为P＝[p_N(k)p_E(k)p_D(k)]^T，速度矢量为V＝[v_xb(k)v_yb(k)v_zb(k)]^T，加速度矢量为A＝[a_xb(k)a_yb(k)a_zb(k)]^T；定义SAUV的横滚角为γ，纵摇角为θ，航向角为ψ，经欧拉角计算得到艇体系b到水平面坐标系h的姿态矩阵：

$C_b^h=\left[\begin{array}{ccc}\cos \mathrm{\θ}& \sin \mathrm{\θ}\sin \mathrm{\γ}& \sin \mathrm{\θ}\cos \mathrm{\γ}\\ 0& \cos \mathrm{\γ}& -\sin \mathrm{\γ}\\ -\sin \mathrm{\θ}& \cos \mathrm{\θ}\sin \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]$

无迹卡尔曼滤波器的状态空间模型为：

$\stackrel{\·}{X}\left(t\right)=F\left(X\left(t\right)\right)+G{\stackrel{\‾}{X}}_{\mathrm{SUB}}\left(t\right)+W\left(t\right)$

式中

X(t)＝[p_N(t)p_E(t)p_D(t)v_xb(t)v_yb(t)v_zb(t)a_xb(t)a_yb(t)a_zb(t)ψ(t)]^T；

$G=\left[\begin{array}{c}{0}_{6\×4}\\ A_{3\×4}\\ {\mathrm{\Ψ}}_{1\×4}\end{array}\right];$

$A_{3\×4}=\left[\begin{array}{cccc}{\mathrm{\α}}_{\mathrm{xb}}& 0& 0& 0\\ 0& {\mathrm{\α}}_{\mathrm{yb}}& 0& 0\\ 0& 0& {\mathrm{\α}}_{\mathrm{zb}}& 0\end{array}\right];$

Ψ_1×4＝[000α_ψ]

函数F(X(t))由如下所示的运动学函数集确定；

${\stackrel{\·}{p}}_N\left(t\right)=(\cos \mathrm{\ψ}\·C_{b11}^h+\sin \mathrm{\ψ}\·C_{b21}^h)v_{\mathrm{xb}}$

$+(\cos \mathrm{\ψ}\·C_{b12}^h+\sin \mathrm{\ψ}\·C_{b22}^h)v_{\mathrm{yb}}+(\cos \mathrm{\ψ}\·C_{b13}^h+\sin \mathrm{\ψ}\·C_{b23}^h)v_{\mathrm{zb}}$

${\stackrel{\·}{p}}_E\left(t\right)=(-\sin \mathrm{\ψ}\·C_{b11}^h+\cos \mathrm{\ψ}\·C_{b21}^h)v_{\mathrm{xb}}$

$+(-\sin \mathrm{\ψ}\·C_{b12}^h+\cos \mathrm{\ψ}\·C_{b22}^h)v_{\mathrm{yb}}+(-\sin \mathrm{\ψ}\·C_{b13}^h+\cos \mathrm{\ψ}\·C_{b23}^h)v_{\mathrm{zb}}$

${\stackrel{\·}{p}}_D\left(t\right)=C_{b31}^h{v}_{\mathrm{xb}}+C_{b32}^h{v}_{\mathrm{yb}}+C_{b33}^h{v}_{\mathrm{zb}}$

${\stackrel{\·}{v}}_{\mathrm{xb}}\left(t\right)=a_{\mathrm{xb}}\left(t\right)$

${\stackrel{\·}{v}}_{\mathrm{yb}}\left(t\right)=a_{\mathrm{yb}}\left(t\right)$

${\stackrel{\·}{v}}_{\mathrm{zb}}\left(t\right)=a_{\mathrm{zb}}\left(t\right)$

${\stackrel{\·}{a}}_{\mathrm{xb}}\left(t\right)=-{\mathrm{\α}}_{\mathrm{xb}}{a}_{\mathrm{xb}}$

${\stackrel{\·}{a}}_{\mathrm{yb}}\left(t\right)=-{\mathrm{\α}}_{\mathrm{yb}}{a}_{\mathrm{yb}}$

${\stackrel{\·}{a}}_{\mathrm{zb}}\left(t\right)=-{\mathrm{\α}}_{\mathrm{zb}}{a}_{\mathrm{zb}}$

$\stackrel{\·}{\mathrm{\ψ}}\left(t\right)=-{\mathrm{\α}}_{\mathrm{\ψ}}\mathrm{\ψ}$

α_xb、α_yb、α_zb为反相关时间常数；α_ψ为航向角反相关时间常数；

为机动加速度和航向角均值，W(t)是均值为零与观测噪声V(t)及状态X(t)相互独立的系统高斯白噪声，满足cov(W(k))＝Q。

3、自主导航阶段取深度计输出的深度信息同航姿参考系统输出的航向角、姿态角和载体系下的加速度信息与载体系下多普勒测速声纳的速度信息，通过强跟踪无迹卡尔曼滤波器实现水下机器人的自主导航与数据滤波的步骤如下：

在机器人水下工作阶段选取

Z(t)＝[p_D(t)v_xb(t)v_yb(t)v_zb(t)a_xb(t)a_yb(t)a_zb(t)ψ(t)]^T作为观测向量，观测模型为：

Z(t)＝HX(t)+V(t)

H＝[0_8×2I_8×8]；

V(t)是均值为零与系统噪声W(t)及状态X(t)相互独立的观测白噪声，满足cov(V(k))＝R。

4、自主校正阶段取GPS接收机输出的水平位置信息，航姿参考系统输出的航向、姿态及载体系下加速度信息与载体系下多普勒测速声纳的速度信息，通过强跟踪无迹卡尔曼滤波器实现水下机器人的自主校正与数据滤波的步骤如下：在机器人水面校正阶段选取

Z(t)＝[p_N(t)p_E(t)p_D(t)v_xb(t)v_yb(t)v_zb(t)a_xb(t)a_yb(t)a_zb(t)ψ(t)]^T作为观测向量，观测模型为

Z(t)＝HX(t)+V(t)

H＝I_10×10。

V(t)是均值为零与系统噪声W(t)及状态X(t)相互独立的观测白噪声，满足cov(V(k))＝R。

5、上述两个观测方程间的变换实现了小型水下机器人自主导航阶段与自主校正阶段的切换，阶段间的切换需要通过GPS接收机输出信号的有效位进行控制具体步骤如下：

(1)通过串口实时采集GPS信号，按照数据格式从缓存中读取GPS提供的位置信息；

(2)判断耐压GPS接收机的导航数据有效位；

(3)如果在连续10秒钟内GPS导航数据有效则将观测模型切换至水面校正模式即令H＝I_10×10，否则保持观测模型的水下导航模式H＝[0_8×2I_8×8]不变。

本发明提出的小型水下机器人组合导航定位方法，通过强跟踪无迹卡尔曼滤波器和独特的滤波模型设计有效地融合了各路导航传感器提供的导航信息，实现了各种导航信息的平滑滤波以及自主导航和自主校正的自动切换。

本发明具有如下优点：

(1)本发明采用强跟踪无迹卡尔曼滤波器实现组合导航系统的信息融合，有效地解决了小型水下机器人因海流或海浪干扰而导致的导航定位系统模型失配问题，导航数据连续稳定、平滑有界，提高了定位精度；

(2)本发明的导航系统模型设计实现了对小型水下机器人控制系统所需的航向、高度信息以及在艇体系下的速度与加速度信息的直接平滑滤波，避免了一般导航系统所提供的加速度和导航系下的速度信息需经滤波和坐标变换到艇体系后方可被机器人控制系统使用的问题，更适合于水下无人潜器的导航、制导与控制系统。；

(3)本发明采用GPS接收机信号有效位切换滤波模型的观测方程，实现了小型水下机器人导航系统自主导航阶段与自主校正阶段的自动切换。

附图说明

图1小型水下机器人导航定位系统组成示意图；

图2水下导航定位系统数据流结构图；

图3强跟踪无迹卡尔曼滤波器程序流程图；

图4工作模式切换流程图。

具体实施方式

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

小型水下机器人导航定位系统的水上、水下两部分组成如图1所示。水上部分包括：水面监控计算机(含上位机数据处理系统、小型水下机器人状态信息显示系统)，水声、无线电通讯设备的水上部分系统。导航定位系统水下部分包括：嵌入式导航计算机、耐压GPS接收机、航姿参考系统、多普勒测速声纳、深度计以及水声、无线电通讯设备的水下部分系统。其中，航姿参考系统由MEMS陀螺仪、MEMS加速度计以及磁强计各三个构成，以上传感器均沿水下机器人的艇体系安装，用于实时提供小型水下机器人的航向、姿态以及沿艇体系的三维加速度信息。航姿参考系统和嵌入式导航计算机安装在水下机器人本体耐压舱内，多普勒测速仪、深度计布置在水下机器人本体下部，耐压GPS接收机布置在水下机器人本体上部，无线电系统安装在水下机器人本体上部。小型水下机器人与水面监控计算机通过两种方式进行通讯联系：水下作业阶段通过水声通讯机联系，水面校正阶段通过无线电通讯联系。水声通讯机水下部分的开关根据深度计信息进行控制，当深度计显示深度小于接近于零的某个阈值时关闭水声通讯，同时基于应答机制的水下无线电通讯设备便于其水上部分建立链接。相似的，若深度计显示深度大于此阈值时，水声通讯机水下部分开启，水声通讯建立，同时由于法拉第笼效应，随着水下无线电信号的消失，无线电通讯断开。

如图2所示，耐压GPS接收机、航姿参考系统、多普勒测速仪通过8串口卡接入嵌入式导航计算机，以RS232协议方式接收数据或指令信息，深度计通过DI/O板接入嵌入式导航计算机，各路导航数据经过导航模式切换模块筛选后(本发明设计的切换程序)，通过强跟踪无迹卡尔曼滤波器(本发明设计的导航计算机程序)进行信息融合，为小型水下机器人提供连续平滑精度较高的导航数据。

本发明小型水下机器人导航定位方法：1)采用强跟踪无迹卡尔曼滤波器实现组合导航系统的数据融合；2)取导航系下的位置矢量和航向角与载体系下速度矢量和加速度矢量作为滤波器状态矢量，滤波结果可被机器人控制与制导系统直接使用；3)取深度计输出的深度信息同航姿参考系统输出的航向角、姿态角和载体系下的加速度信息与载体系下多普勒测速声纳的速度信息，通过强跟踪无迹卡尔曼滤波器实现水下机器人的自主导航与数据滤波；4)取GPS接收机输出的水平位置信息，航姿参考系统输出的航向、姿态及载体系下加速度信息与载体系下多普勒测速声纳的速度信息，通过强跟踪无迹卡尔曼滤波器实现水下机器人的自主校正与数据滤波；5)通过GPS接收机输出信号的有效位实现水上和水下两个不同量测方程的切换。具体为：

1)采用强跟踪无迹卡尔曼滤波器实现组合导航系统的数据融合；

如图3所示，设系统噪声方差阵为Q，观测噪声方差阵为R，则跟踪无迹卡尔曼滤波器的具体实现过程如下：

(1)选择对称点采样策略，根据

P(k|k)计算k时刻Sigma点集{χ_i}，(i＝1，·，L)；

(2)计算经过非线性状态方程传播后的Sigma点χ_i(k+1|k)，由χ_i(k+1|k)计算可得状态向量X(k)一步预测估计

${\mathrm{\χ}}_i(k+1|k)=f\left({\mathrm{\χ}}_i\left(k\right|k)\right)+G{\stackrel{\‾}{X}}_{\mathrm{SUB}}\left(k\right)$

$\hat{X}(k+1|k)=\underset{i=0}{\overset{L}{\mathrm{\Σ}}}{\mathrm{\ω}}_i^m{\mathrm{\χ}}_i(k+1|k)$

式中，

(i＝0，·，L)，为求一阶统计特性时的权系数；(i＝0，·，L)，为求二阶统计特性时的权系数。

(3)根据测量值Z(k+1)通过量测方程Z(k)＝HX(k)+V(k)计算新息

$\mathrm{\γ}(k+1)=Z(k+1)-H\hat{X}(k+1|k)$

(4)在线计算渐消因子矩阵Λ(k+1)

$V_0(k+1)=E\left(\mathrm{\γ}(k+1){\mathrm{\γ}}^T(k+1)\right)$

$\≈\left\{\begin{array}{cc}\mathrm{\γ}\left(1\right){\mathrm{\γ}}^T\left(1\right),& k=0\\ \frac{\mathrm{\ρ}{V}_0\left(k\right)+\mathrm{\γ}(k+1){\mathrm{\γ}}^T(k+1)}{1+\mathrm{\ρ}},& k\≥0\end{array}\right.$

N(k+1)＝V₀(k+1)-R-HQH^T

$P_{\stackrel{\·}{X}\stackrel{\·}{X}}=\underset{i=0}{\overset{L}{\mathrm{\Σ}}}{\mathrm{\ω}}_i^c[{\mathrm{\χ}}_i(k+1|k)-\hat{X}(k+1|k)]\·{[{\mathrm{\χ}}_i(k+1|k)-\hat{X}(k+1|k)]}^T$

$M(k+1)=M_{\mathrm{ii}}(k+1)=P_{\stackrel{\·}{X}\stackrel{\·}{X}}{H}^{T}H$

$\mathrm{\η}(k+1)=\frac{\mathrm{trace}\left(N(k+1)\right)}{\underset{i=1}{\overset{d}{\mathrm{\Σ}}}{M}_{\mathrm{ii}}(k+1)}$

${\mathrm{\λ}}_i(k+1)=\left\{\begin{array}{cc}{\mathrm{\α}}_i\mathrm{\η}(k+1),& {\mathrm{\α}}_i\mathrm{\η}(k+1)>1\\ 1& {\mathrm{\α}}_i\mathrm{\η}(k+1)\≤1\end{array}\right.$

Λ(k+1)＝diag(λ₁(k+1)，λ₂(k+1)，·，λ_d(k+1))

(5)计算误差协方差阵预报值P(k+1|k)

$P(k+1|k)=\mathrm{\Λ}(k+1)P_{\stackrel{\·}{X}\stackrel{\·}{X}}+Q$

(6)测量更新

K(k+1)＝P(k+1|k)H^T[HP(k+1|k)H^T+R]^-1

$\hat{X}(k+1|k+1)=\hat{X}(k+1|k)+K(k+1)\mathrm{\γ}(k+1)$

P(k+1|k+1)＝[I-K(k+1)H]P(k+1|k)

(7)从状态向量中提取与

对应的状态变量

基于上述过程融合各路导航传感器的输出信息，就可以为小型水下机器人提供连续平滑精度较高的导航数据。

2)取导航系下的位置矢量和航向角与载体系下速度矢量和加速度矢量作为滤波器状态矢量，滤波结果可被机器人控制与制导系统直接使用；

滤波器状态方程中采用NED导航坐标系O-XYZ，以O作为原点，OZ轴指向地心，OX轴指北，OY轴指东。艇体坐标系o-xyz定义如下，ox轴沿机器人纵轴，oy轴平行于基平面与ox轴垂直指向艇体右舷，oz轴垂直于基平面并与ox轴、oy轴构成右手直角坐标系。选取导航系下位置矢量P和航向角ψ与艇体系下的速度矢量V、加速度矢量A作为小型水下机器人运动连续时间模型的状态矢量。其中位置矢量为P＝[p_N(k)p_E(k)p_D(k)]^T，速度矢量为V＝[v_xb(k)v_yb(k)v_zb(k)]^T，加速度矢量为A＝[a_xb(k)a_yb(k)a_zb(k)]^T。定义SAUV的横滚角为γ，纵摇角为θ，航向角为ψ，经欧拉角计算得到艇体系b到水平面坐标系h的姿态矩阵：

$C_b^h=\left[\begin{array}{ccc}\cos \mathrm{\θ}& \sin \mathrm{\θ}\sin \mathrm{\γ}& \sin \mathrm{\θ}\cos \mathrm{\γ}\\ 0& \cos \mathrm{\γ}& -\sin \mathrm{\γ}\\ -\sin \mathrm{\θ}& \cos \mathrm{\θ}\sin \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]$

无迹卡尔曼滤波器的状态空间模型为：

$\stackrel{\·}{X}\left(t\right)=F\left(X\left(t\right)\right)+G{\stackrel{\‾}{X}}_{\mathrm{SUB}}\left(t\right)+W\left(t\right)$

式中

X(t)＝[p_N(t)p_E(t)p_D(t)v_xb(t)v_yb(t)v_zb(t)a_xb(t)a_yb(t)a_zb(t)ψ(t)]^T；

$G=\left[\begin{array}{c}{0}_{6\×4}\\ A_{3\×4}\\ {\mathrm{\Ψ}}_{1\×4}\end{array}\right];$

$A_{3\×4}=\left[\begin{array}{cccc}{\mathrm{\α}}_{\mathrm{xb}}& 0& 0& 0\\ 0& {\mathrm{\α}}_{\mathrm{yb}}& 0& 0\\ 0& 0& {\mathrm{\α}}_{\mathrm{zb}}& 0\end{array}\right];$

Ψ_1×4＝[000α_ψ]

函数F(X(t))由如下所示的运动学函数集确定。

${\stackrel{\·}{p}}_N\left(t\right)=(\cos \mathrm{\ψ}\·C_{b11}^h+\sin \mathrm{\ψ}\·C_{b21}^h)v_{\mathrm{xb}}$

$+(\cos \mathrm{\ψ}\·C_{b12}^h+\sin \mathrm{\ψ}\·C_{b22}^h)v_{\mathrm{yb}}+(\cos \mathrm{\ψ}\·C_{b13}^h+\sin \mathrm{\ψ}\·C_{b23}^h)v_{\mathrm{zb}}$

${\stackrel{\·}{p}}_E\left(t\right)=(-\sin \mathrm{\ψ}\·C_{b11}^h+\cos \mathrm{\ψ}\·C_{b21}^h)v_{\mathrm{xb}}$

$+(-\sin \mathrm{\ψ}\·C_{b12}^h+\cos \mathrm{\ψ}\·C_{b22}^h)v_{\mathrm{yb}}+(-\sin \mathrm{\ψ}\·C_{b13}^h+\cos \mathrm{\ψ}\·C_{b23}^h)v_{\mathrm{zb}}$

${\stackrel{\·}{p}}_D\left(t\right)=C_{b31}^h{v}_{\mathrm{xb}}+C_{b32}^h{v}_{\mathrm{yb}}+C_{b33}^h{v}_{\mathrm{zb}}$

${\stackrel{\·}{v}}_{\mathrm{xb}}\left(t\right)=a_{\mathrm{xb}}\left(t\right)$

${\stackrel{\·}{v}}_{\mathrm{yb}}\left(t\right)=a_{\mathrm{yb}}\left(t\right)$

${\stackrel{\·}{v}}_{\mathrm{zb}}\left(t\right)=a_{\mathrm{zb}}\left(t\right)$

${\stackrel{\·}{a}}_{\mathrm{xb}}\left(t\right)=-{\mathrm{\α}}_{\mathrm{xb}}{a}_{\mathrm{xb}}$

${\stackrel{\·}{a}}_{\mathrm{yb}}\left(t\right)=-{\mathrm{\α}}_{\mathrm{yb}}{a}_{\mathrm{yb}}$

${\stackrel{\·}{a}}_{\mathrm{zb}}\left(t\right)=-{\mathrm{\α}}_{\mathrm{zb}}{a}_{\mathrm{zb}}$

$\stackrel{\·}{\mathrm{\ψ}}\left(t\right)=-{\mathrm{\α}}_{\mathrm{\ψ}}\mathrm{\ψ}$

α_xb、α_yb、α_zb为反相关时间常数；α_ψ为航向角反相关时间常数；

为机动加速度和航向角均值，W(t)是均值为零与观测噪声V(t)及状态X(t)相互独立的系统高斯白噪声，满足cov(W(k))＝Q；

通过上述状态方程经过滤波得到的结果可以被机器人控制与制导系统直接使用。

3)取深度计输出的深度信息同航姿参考系统输出的航向角、姿态角和载体系下的加速度信息与载体系下多普勒测速声纳的速度信息，通过强跟踪无迹卡尔曼滤波器实现水下机器人的自主导航与数据滤波；

在机器人水下工作阶段选取

Z(t)＝[p_D(t)v_xb(t)v_yb(t)v_zb(t)a_xb(t)a_yb(t)a_zb(t)ψ(t)]^T作为观测向量，观测模型为：

Z(t)＝HX(t)+V(t)

H＝[0_8×2I_8×8]；

V(t)是均值为零与系统噪声W(t)及状态X(t)相互独立的观测白噪声，满足cov(V(k))＝R。

采用上述观测方程通过强跟踪无迹卡尔曼滤波器便可以实现水下机器人的自主导航与数据滤波。

4)取GPS接收机输出的水平位置信息，航姿参考系统输出的航向、姿态及载体系下加速度信息与载体系下多普勒测速声纳的速度信息，通过强跟踪无迹卡尔曼滤波器实现水下机器人的自主校正与数据滤波；在机器人水面校正阶段选取

Z(t)＝[p_N(t)p_E(t)p_D(t)v_xb(t)v_yb(t)v_zb(t)a_xb(t)a_yb(t)a_zb(t)ψ(t)]^T作为观测向量，观测模型为

Z(t)＝HX(t)+V(t)

H＝I_10×10。

V(t)是均值为零与系统噪声W(t)及状态X(t)相互独立的观测白噪声，满足cov(V(k))＝R。

采用上述观测方程需将GPS接收机输出的小型水下机器人在惯性坐标系中的经纬度坐标值转换为p_N(t)与p_E(t)，然后再通过强跟踪无迹卡尔曼滤波器便可以实现水下机器人的自主校正与数据滤波。具体过程如下：

$p_N\left(t\right)=\frac{\mathrm{\π}(L-L_0)}{{180}^{\·}}R$

$p_E\left(t\right)=\frac{\mathrm{\π}(L-L_0)}{{180}^{\·}}R\cos \left(\frac{\mathrm{\π}}{{180}^{\·}}{L}_0\right)$

式中，(λ₀，L₀)和(λ，L)分别是小型水下机器人起始位置的经纬度坐标与实时水面位置经纬度坐标，R为地球半径，p_N(t)与p_E(t)分别是小型水下机器人在导航系中的坐标值。

5)通过GPS接收机输出信号的有效位实现水上和水下两个不同量测方程的切换；

如图4所示，水下导航定位系统通过如下步骤实现自主导航与自主校正间的切换。

(1)通过串口实时采集GPS信号，按照数据格式从缓存中读取GPS提供的位置信息；

(2)判断耐压GPS接收机的导航数据有效位；

(3)如果在连续10秒钟内GPS导航数据有效则将观测模型切换至水面校正模式即令H＝I_10×10，否则保持观测模型的水下导航模式H＝[0_8×2I_8×8]不变。

小型水下机器人在惯性系中坐标经纬度值(λ，L)的计算过程：

首先由耐压GPS接收机接收到小型水下机器人起始时刻在水面的经纬度坐标(λ₀，L₀)，水下导航阶段利用强跟踪无迹卡尔曼滤波器融合给出的位置坐标值p_N(t)与p_E(t)通过公式：

$L=L_0+\frac{\mathrm{\π}{p}_N}{{180}^{\·}R}$

$\mathrm{\λ}={\mathrm{\λ}}_0+\frac{\mathrm{\π}{p}_E}{{180}^{\·}}/R\cos \left(\frac{\mathrm{\π}}{{180}^{\·}}{L}_0\right)$

计算出此时小型水下机器人的经纬度坐标(λ，L)。

本发明提出的强跟踪无迹卡尔曼滤波算法同样适用于包括载人潜器以及遥控水下机器人在内的其他水下机器人。

Claims (6)

1.一种小型水下机器人组合导航定位方法，其特征是主要包括如下步骤：1)采用强跟踪无迹卡尔曼滤波器进行组合导航系统的数据融合；2)取导航系下的位置矢量和航向角与载体系下速度矢量和加速度矢量作为滤波器状态矢量，滤波结果被机器人控制与制导系统直接使用；3)取深度计输出的深度信息同航姿参考系统输出的航向角、姿态角和载体系下的加速度信息与载体系下多普勒测速声纳的速度信息，通过强跟踪无迹卡尔曼滤波器实现水下机器人的自主导航与数据滤波；4)取GPS接收机输出的水平位置信息，航姿参考系统输出的航向、姿态及载体系下加速度信息与载体系下多普勒测速声纳的速度信息，通过强跟踪无迹卡尔曼滤波器实现水下机器人的自主校正与数据滤波；5)通过GPS接收机输出信号的有效位实现水上和水下两个不同量测方程的切换。

2.根据权利要求1所述的小型水下机器人组合导航定位方法，其特征是所述采用强跟踪无迹卡尔曼滤波器进行组合导航系统数据融合的步骤如下：

系统噪声方差阵为Q，观测噪声方差阵为R；

(1)选择对称点采样策略，根据

P(k|k)计算k时刻Sigma点集{χ_i}，其中i＝1、...、L；

(2)计算经过非线性状态方程

传播后的Sigma点χ_i(k+1|k)，由χ_i(k+1|k)计算得状态向量X(k)一步预测估计

${\mathrm{\χ}}_i(k+1|k)=f\left({\mathrm{\χ}}_i\left(k\right|k)\right)+G{\stackrel{\‾}{X}}_{\mathrm{SUB}}\left(k\right)$

$\hat{X}(k+1|k)=\underset{i=0}{\overset{L}{\mathrm{\Σ}}}{\mathrm{\ω}}_i^m{\mathrm{\χ}}_i(k+1|k)$

式中，

i＝0，·，L，为求一阶统计特性时的权系数；

i＝0，·，L，为求二阶统计特性时的权系数；

(3)根据测量值Z(k+1)通过量测方程Z(k)＝HX(k)+V(k)计算新息

$\mathrm{\γ}(k+1)=Z(k+1)-H\hat{X}(k+1|k);$

(4)在线计算渐消因子矩阵Λ(k+1)

$V_0(k+1)=E\left(\mathrm{\γ}(k+1){\mathrm{\γ}}^T(k+1)\right)$

$\≈\left\{\begin{array}{cc}\mathrm{\γ}\left(1\right){\mathrm{\γ}}^T\left(1\right),& k=0\\ \frac{\mathrm{\ρ}{V}_0\left(k\right)+\mathrm{\γ}(k+1){\mathrm{\γ}}^T(k+1)}{1+\mathrm{\ρ}},& k\≥0\end{array}\right.$

N(k+1)＝V₀(k+1)-R-HQH^T

$P_{\stackrel{\·}{X}\stackrel{\·}{X}}=\underset{i=0}{\overset{L}{\mathrm{\Σ}}}{\mathrm{\ω}}_i^c[{\mathrm{\χ}}_i(k+1|k)-\hat{X}(k+1|k)]\·{[{\mathrm{\χ}}_i(k+1|k)-\hat{X}(k+1|k)]}^T$

$M(k+1)=M_{\mathrm{ii}}(k+1)=P_{\stackrel{\·}{X}\stackrel{\·}{X}}{H}^{T}H$

$\mathrm{\η}(k+1)=\frac{\mathrm{trace}\left(N(k+1)\right)}{\underset{i=1}{\overset{d}{\mathrm{\Σ}}}{M}_{\mathrm{ii}}(k+1)}$

${\mathrm{\λ}}_i(k+1)=\left\{\begin{array}{cc}{\mathrm{\α}}_i\mathrm{\η}(k+1),& {\mathrm{\α}}_i\mathrm{\η}(k+1)>1\\ 1& {\mathrm{\α}}_i\mathrm{\η}(k+1)\≤1\end{array}\right.$

Λ(k+1)＝diag(λ₁(k+1)，λ₂(k+1)，·，λ_d(k+1))；

(5)计算误差协方差阵预报值P(k+1|k)

$P(k+1|k)=\mathrm{\Λ}(k+1)P_{\stackrel{\·}{X}\stackrel{\·}{X}}+Q;$

(6)测量更新

K(k+1)＝P(k+1|k)H^T[HP(k+1|k)H^T+R]^-1

$\hat{X}(k+1|k+1)=\hat{X}(k+1|k)+K(k+1)\mathrm{\γ}(k+1)$

P(k+1|k+1)＝[I-K(k+1)H]P(k+1|k)；

(7)从状态向量

中提取与

对应的状态变量。

3.根据权利要求2所述的小型水下机器人组合导航定位方法，其特征是所述取导航系下的位置矢量和航向角与载体系下速度矢量和加速度矢量作为滤波器状态矢量，滤波结果被机器人控制与制导系统直接使用的步骤如下：

采用NED导航坐标系O-XYZ，以O作为原点，OZ轴指向地心，OX轴指北，OY轴指东；艇体坐标系o-xyz定义如下，ox轴沿机器人纵轴，oy轴平行于基平面与ox轴垂直指向艇体右舷，oz轴垂直于基平面并与ox轴、oy轴构成右手直角坐标系；选取导航系下位置矢量P和航向角ψ与艇体系下的速度矢量V、加速度矢量A作为小型水下机器人运动连续时间模型的状态矢量；其中位置矢量为P＝[p_N(k)p_E(k)p_D(k)]^T，速度矢量为V＝[v_xb(k)v_yb(k)v_zb(k)]^T，加速度矢量为A＝[a_xb(k)a_yb(k)a_zb(k)]^T；定义SAUV的横滚角为γ，纵摇角为θ，航向角为ψ，经欧拉角计算得到艇体系b到水平面坐标系h的姿态矩阵：

$C_b^h=\left[\begin{array}{ccc}\cos \mathrm{\θ}& \sin \mathrm{\θ}\sin \mathrm{\γ}& \sin \mathrm{\θ}\cos \mathrm{\γ}\\ 0& \cos \mathrm{\γ}& -\sin \mathrm{\γ}\\ -\sin \mathrm{\θ}& \cos \mathrm{\θ}\sin \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]$

无迹卡尔曼滤波器的状态空间模型为：

$\stackrel{\·}{X}\left(t\right)=F\left(X\left(t\right)\right)+G{\stackrel{\‾}{X}}_{\mathrm{SUB}}\left(t\right)+W\left(t\right)$

式中

X(t)＝[p_N(t)p_E(t)p_D(t)v_xb(t)v_yb(t)v_zb(t)a_xb(t)a_yb(t)a_zb(t)ψ(t)]^T；

$G=\left[\begin{array}{c}{0}_{6\×4}\\ A_{3\×4}\\ {\mathrm{\Ψ}}_{1\×4}\end{array}\right];$

$A_{3\×4}=\left[\begin{array}{cccc}{\mathrm{\α}}_{\mathrm{xb}}& 0& 0& 0\\ 0& {\mathrm{\α}}_{\mathrm{yb}}& 0& 0\\ 0& 0& {\mathrm{\α}}_{\mathrm{zb}}& 0\end{array}\right];$

Ψ_1×4＝[000α_ψ]

函数F(X(t))由如下所示的运动学函数集确定；

${\stackrel{\·}{p}}_N\left(t\right)=(\cos \mathrm{\ψ}\·C_{b11}^h+\sin \mathrm{\ψ}\·C_{b21}^h)v_{\mathrm{xb}}$

$+(\cos \mathrm{\ψ}\·C_{b12}^h+\sin \mathrm{\ψ}\·C_{b22}^h)v_{\mathrm{yb}}+(\cos \mathrm{\ψ}\·C_{b13}^h+\sin \mathrm{\ψ}\·C_{b23}^h)v_{\mathrm{zb}}$

${\stackrel{\·}{p}}_E\left(t\right)=(-\sin \mathrm{\ψ}\·C_{b11}^h+\cos \mathrm{\ψ}\·C_{b21}^h)v_{\mathrm{xb}}$

$+(-\sin \mathrm{\ψ}\·C_{b12}^h+\cos \mathrm{\ψ}\·C_{b22}^h)v_{\mathrm{yb}}+(-\sin \mathrm{\ψ}\·C_{b13}^h+\cos \mathrm{\ψ}\·C_{b23}^h)v_{\mathrm{zb}}$

${\stackrel{\·}{p}}_D\left(t\right)=C_{b31}^h{v}_{\mathrm{xb}}+C_{b32}^h{v}_{\mathrm{yb}}+C_{b33}^h{v}_{\mathrm{zb}}$

${\stackrel{\·}{v}}_{\mathrm{xb}}\left(t\right)=a_{\mathrm{xb}}\left(t\right)$

${\stackrel{\·}{v}}_{\mathrm{yb}}\left(t\right)=a_{\mathrm{yb}}\left(t\right)$

${\stackrel{\·}{v}}_{\mathrm{zb}}\left(t\right)=a_{\mathrm{zb}}\left(t\right)$

${\stackrel{\·}{a}}_{\mathrm{xb}}\left(t\right)=-{\mathrm{\α}}_{\mathrm{xb}}{a}_{\mathrm{xb}}$

${\stackrel{\·}{a}}_{\mathrm{yb}}\left(t\right)=-{\mathrm{\α}}_{\mathrm{yb}}{a}_{\mathrm{yb}}$

${\stackrel{\·}{a}}_{\mathrm{zb}}\left(t\right)=-{\mathrm{\α}}_{\mathrm{zb}}{a}_{\mathrm{zb}}$

$\stackrel{\·}{\mathrm{\ψ}}\left(t\right)=-{\mathrm{\α}}_{\mathrm{\ψ}}\mathrm{\ψ}$

α_xb、α_yb、α_zb为反相关时间常数；α_ψ为航向角反相关时间常数；

为机动加速度和航向角均值，W(t)是均值为零与观测噪声V(t)及状态X(t)相互独立的系统高斯白噪声，满足cov(W(k))＝Q。

4.根据权利要求3所述的小型水下机器人组合导航定位方法，其特征是取深度计输出的深度信息同航姿参考系统输出的航向角、姿态角和载体系下的加速度信息与载体系下多普勒测速声纳的速度信息，通过强跟踪无迹卡尔曼滤波器实现水下机器人的自主导航与数据滤波的步骤如下：

在机器人水下工作阶段选取

Z(t)＝[p_D(t)v_xb(t)v_yb(t)v_zb(t)a_xb(t)a_yb(t)a_zb(t)ψ(t)]^T作为观测向量，观测模型为：

Z(t)＝HX(t)+V(t)

H＝[0_8×2I_8×8]；

V(t)是均值为零与系统噪声W(t)及状态X(t)相互独立的观测白噪声，满足cov(V(k))＝R。

5.根据权利要求4所述的小型水下机器人组合导航定位方法，其特征是取GPS接收机输出的水平位置信息，航姿参考系统输出的航向、姿态及载体系下加速度信息与载体系下多普勒测速声纳的速度信息，通过强跟踪无迹卡尔曼滤波器实现水下机器人的自主校正与数据滤波的步骤如下：在机器人水面校正阶段选取

Z(t)＝[p_N(t)p_E(t)p_D(t)v_xb(t)v_yb(t)v_zb(t)a_xb(t)a_yb(t)a_zb(t)ψ(t)]^T作为观测向量，观测模型为

Z(t)＝HX(t)+V(t)

H＝I_10×10；

V(t)是均值为零与系统噪声W(t)及状态X(t)相互独立的观测白噪声，满足cov(V(k))＝R。

6.根据权利要求5所述的小型水下机器人组合导航定位方法，其特征是通过GPS接收机输出信号的有效位进行控制具体步骤如下：

(1)通过串口实时采集GPS信号，按照数据格式从缓存中读取GPS提供的位置信息；

(2)判断耐压GPS接收机的导航数据有效位；

(3)如果在连续10秒钟内GPS导航数据有效则将观测模型切换至水面校正模式即令H＝I_10×10，否则保持观测模型的水下导航模式H＝[0_8×2I_8×8]不变。

Images