专利名称：一种小型水下机器人组合导航定位方法
技术领域：
本发明涉及的是一种小型水下机器人自主导航与自主校正方法。
背景技术：
小型水下机器人执行水下任务过程中，需为其控制系统和制导系统提供准确、平 滑的位置、深度、姿态以及载体系下的速度和加速度信息，并能自动上浮至水面进行自主校 正。小型水下机器人导航定位系统往往存在由海流或海浪干扰导致的系统模型失配问题， 并且导航定位系统一般仅能提供未经滤波的加速度、深度信息和导航系下的速度信息，这 些信息需经滤波和坐标变换到艇体系后方可被机器人控制系统使用。而取多路导航信号 的导航方法通常是简单导航信息的备份，而没有进行有效的信息融合。本发明采用强跟踪 无迹卡尔曼滤波算法有效地解决了小型水下机器人导航定位系统的模型失配问题，对多路 导航设备提供的导航信息进行融合，提高了定位精度，平滑了航向、高度信息，并能直接为 控制系统提供平滑的艇体系下的速度与加速度信息。《控制与决策》(1990，第5期)“非线 性系统的带次优渐消因子的扩展卡尔曼滤波”提出了强跟踪滤波器有效解决了 EKF由于模 型失配导致的鲁棒性差、滤波发散等问题，但要求非线性系统状态函数和量测函数必须是 连续可微的，而且强跟踪滤波器需要计算非线性函数的雅可比矩阵，不但过程繁琐易出错， 而且对非线性函数的线性化近似精度通常偏低，当系统具有强非线性时精度严重下降，甚 至发散。本发明采用的强跟踪无迹卡尔曼滤波算法通过采用UT变换克服了上述问题。《航 空电子技术》0008，第39卷第4期)“强跟踪UKF滤波在SINS/GPS组合导航中的应用研 究”，针对大机动条件下飞行器SINS/GPS组合导航系统的滤波发散问题，设计了强跟踪无迹 卡尔曼滤波算法，通过渐消因子在线调整卡尔曼滤波增益从而提高无迹卡尔曼滤波算法的 跟踪性能。《仪器仪表学报》0008，第四卷第8期)“强跟踪UKF方法及其在故障辨识中 的应用”也基于强跟踪无迹卡尔曼滤波算法设计了 GNSS/SINS组合导航系统。本发明采用 的强跟踪无迹卡尔曼滤波算法同上述算法的渐消因子的计算方法不同，且更为合理。
发明内容
本发明的目的在于提供一种既能有效解决小型水下机器人因海流或海浪干扰而 导致的导航定位系统模型失配问题，又能平滑小型水下机器人控制系统所需的航向、深度 以及在艇体系下的速度与加速度信息的集自主水下导航定位与自主水面位置校正功能的 小型水下机器人导航定位方法。本发明的目的是这样实现的本发明的小型水下机器人组合导航定位方法主要包括如下步骤1)采用强跟踪 无迹卡尔曼滤波器进行组合导航系统的数据融合；幻取导航系下的位置矢量和航向角与 载体系下速度矢量和加速度矢量作为滤波器状态矢量，滤波结果被机器人控制与制导系统 直接使用；3)取深度计输出的深度信息同航姿参考系统输出的航向角、姿态角和载体系下 的加速度信息与载体系下多普勒测速声纳的速度信息，通过强跟踪无迹卡尔曼滤波器实现水下机器人的自主导航与数据滤波；4)取GPS接收机输出的水平位置信息，航姿参考系统 输出的航向、姿态及载体系下加速度信息与载体系下多普勒测速声纳的速度信息，通过强 跟踪无迹卡尔曼滤波器实现水下机器人的自主校正与数据滤波力)通过GPS接收机输出信 号的有效位实现水上和水下两个不同量测方程的切换。1、所述采用强跟踪无迹卡尔曼滤波器进行组合导航系统数据融合的步骤如下系统噪声方差阵为Q，观测噪声方差阵为R ；(1)选择对称点采样策略，根据X(A)、P (k I k)计算k时刻Sigma点集{ χ J，其中i 1、...、L<;(2)计算经过非线性状态方程++播后的Sigma点 Xi(k+l|k)，由X i (k+11 k)计算得状态向量X (k) —步预测估计+
χ. (k +1 μ) = /(χ (k I k)) + GXsm (k)
权利要求
1.一种小型水下机器人组合导航定位方法，其特征是主要包括如下步骤1)采用强跟 踪无迹卡尔曼滤波器进行组合导航系统的数据融合；幻取导航系下的位置矢量和航向角 与载体系下速度矢量和加速度矢量作为滤波器状态矢量，滤波结果被机器人控制与制导系 统直接使用；；3)取深度计输出的深度信息同航姿参考系统输出的航向角、姿态角和载体系 下的加速度信息与载体系下多普勒测速声纳的速度信息，通过强跟踪无迹卡尔曼滤波器实 现水下机器人的自主导航与数据滤波；4)取GPS接收机输出的水平位置信息，航姿参考系 统输出的航向、姿态及载体系下加速度信息与载体系下多普勒测速声纳的速度信息，通过 强跟踪无迹卡尔曼滤波器实现水下机器人的自主校正与数据滤波力)通过GPS接收机输出 信号的有效位实现水上和水下两个不同量测方程的切换。
2.根据权利要求1所述的小型水下机器人组合导航定位方法，其特征是所述采用强跟 踪无迹卡尔曼滤波器进行组合导航系统数据融合的步骤如下系统噪声方差阵为Q，观测噪声方差阵为R ；(1)选择对称点采样策略，根据X@)、P(k |k)计算k时刻Sigma点集{ xj，其中i = 1、...、L<;(2)计算经过非线性状态方程+= F(X(k)) + GXSUB(k)传播后的Sigma点 Xi(k+l|k)，由χ i (k+11 k)计算得状态向量X (k) —步预测估计+
3.根据权利要求2所述的小型水下机器人组合导航定位方法，其特征是所述取导航系 下的位置矢量和航向角与载体系下速度矢量和加速度矢量作为滤波器状态矢量，滤波结果 被机器人控制与制导系统直接使用的步骤如下采用NED导航坐标系0-ΧΥΖ，以0作为原点，OZ轴指向地心，OX轴指北，OY轴指东；艇 体坐标系o-xyz定义如下，ox轴沿机器人纵轴，oy轴平行于基平面与ox轴垂直指向艇体 右舷，OZ轴垂直于基平面并与ox轴、oy轴构成右手直角坐标系；选取导航系下位置矢量 P和航向角Ψ与艇体系下的速度矢量V、加速度矢量A作为小型水下机器人运动连续时间 模型的状态矢量；其中位置矢量为P= [PN(k)pE(k)pD(k)]T，速度矢量为V= [vxb(k) vyb(k) vzb(k)]T，加速度矢量为A=[知㈨ ㈨知㈨]、定义SAUV的横滚角为Y，纵摇角为Θ， 航向角为Ψ，经欧拉角计算得到艇体系b到水平面坐标系h的姿态矩阵 cos0 sin θ sin γ sin θ cos γQa =0 cos 7 - sin γ -sin0 cos 0 sin 7 cos 0 cos 7无迹卡尔曼滤波器的状态空间模型为
4.根据权利要求3所述的小型水下机器人组合导航定位方法，其特征是取深度计输出 的深度信息同航姿参考系统输出的航向角、姿态角和载体系下的加速度信息与载体系下多 普勒测速声纳的速度信息，通过强跟踪无迹卡尔曼滤波器实现水下机器人的自主导航与数 据滤波的步骤如下在机器人水下工作阶段选取Z (t) = [pD (t) Vxb (t) Vyb (t) ^b (t) axb (t) ayb (t) b (t) Ψ (t) ]τ 作为观测向量，观测模型为Z(t) = HX (t) +V (t)H — [O8X2I8X8]；V(t)是均值为零与系统噪声w(t)及状态x(t)相互独立的观测白噪声，满足 cov (V (k)) = R。
5.根据权利要求4所述的小型水下机器人组合导航定位方法，其特征是取GPS接收机 输出的水平位置信息，航姿参考系统输出的航向、姿态及载体系下加速度信息与载体系下 多普勒测速声纳的速度信息，通过强跟踪无迹卡尔曼滤波器实现水下机器人的自主校正与 数据滤波的步骤如下在机器人水面校正阶段选取Z(t) = [pN (t) pE (t) pD (t) vxb (t) vyb(t)vzb (t) axb (t) ayb (t) azb (t) ￥(0]工作为观测向量，观测模型为Z(t) = HX (t) +V (t)H — IlOXlO ；V(t)是均值为零与系统噪声W(t)及状态X(t)相互独立的观测白噪声，满足 cov (V (k)) = R。
6.根据权利要求5所述的小型水下机器人组合导航定位方法，其特征是通过GPS接收 机输出信号的有效位进行控制具体步骤如下(1)通过串口实时采集GPS信号，按照数据格式从缓存中读取GPS提供的位置信息；(2)判断耐压GPS接收机的导航数据有效位；(3)如果在连续10秒钟内GPS导航数据有效则将观测模型切换至水面校正模式即令H =I1(IX1。，否则保持观测模型的水下导航模式H= [O8x2I8x8]不变。
全文摘要
本发明提供的是一种小型水下机器人组合导航定位方法。采用强跟踪无迹卡尔曼滤波器对各种导航设备的测量数据加以融合；取导航系下的位置矢量和航向角与载体系下速度矢量和加速度矢量作为滤波器状态矢量；通过强跟踪无迹卡尔曼滤波器实现水下机器人的自主导航与数据滤波；取GPS接收机输出的水平位置信息，通过强跟踪无迹卡尔曼滤波器实现水下机器人的自主校正与数据滤波；通过GPS接收机输出信号的有效位实现水上和水下两个不同量测方程的切换。本发明可以融合各种导航设备测量到的小型水下机器人的位置、深度和姿态信息，在存在海流或海浪的干扰的情况下，实现小型水下机器人的自主导航与自主校正。
文档编号G01C21/20GK102052924SQ20101055936
公开日2011年5月11日 申请日期2010年11月25日 优先权日2010年11月25日
发明者万磊, 孙玉山, 张国成, 张强, 王建国 申请人:哈尔滨工程大学