[发明专利]一种小型水下机器人组合导航定位方法

权利要求书

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)一步预测估计

χi(k+1|k)=f(χi(k|k))+GX‾SUB(k)]]>

X^(k+1|k)=Σi=0Lωimχi(k+1|k)]]>

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

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

γ(k+1)=Z(k+1)-HX^(k+1|k);]]>

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

V0(k+1)=E(γ(k+1)γT(k+1))]]>

≈γ(1)γT(1),k=0ρV0(k)+γ(k+1)γT(k+1)1+ρ,k≥0]]>

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

PX·X·=Σi=0Lωic[χi(k+1|k)-X^(k+1|k)]·[χi(k+1|k)-X^(k+1|k)]T]]>

M(k+1)=Mii(k+1)=PX·X·HTH]]>

η(k+1)=trace(N(k+1))Σi=1dMii(k+1)]]>

λi(k+1)=αiη(k+1),αiη(k+1)>11αiη(k+1)≤1]]>

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

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

P(k+1|k)=Λ(k+1)PX·X·+Q;]]>

(6)测量更新

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

X^(k+1|k+1)=X^(k+1|k)+K(k+1)γ(k+1)]]>

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

(7)从状态向量中提取与对应的状态变量。