专利名称：自主式水下机器人组合导航系统及方法
技术领域：
本发明涉及一种导航技术，特别是一种自主式水下机器人组合导航系统及方法，属于海洋工程领域。
背景技术：
伴随着现代科学技术的发展，自主式水下机器人(Autonomous UnderwaterVehicle，AUV)无论在军用还是民用方面都有广泛的应用，例如战区侦查、探测扫除水雷、海上预警、水下搜索和测绘、近海石油工程作业等。就当前的发展而言，导航技术是实现AUV自主航行的关键，导航系统必须提供远距离及长时间范围内的精确定位、速度及姿态信息。但由于受其大小、重量、电源使用的限制及水介质的特殊性、隐蔽性等因素的影响，实现AUV的精确导航是一项艰难的任务。 就AUV而言，可供应用的主要有两类导航方式基于外部信号的非自主导航和基于传感器的自主导航。非自主导航方式，如罗兰、欧米加、GPS等，仅在接收机能接收到信号时能够完成导航，且由于电波在水中快速衰减的原因，这些基于无线电的导航方式，在AUV上的使用受到很大限制。基于传感器的自主导航方式靠AUV自身携带的装备如惯性测量装置(IMU)、声换能器、地形匹配或地磁传感等手段完成导航。目前，国际上水下导航已由单一导航方式向高精度、高可靠、综合化、智能化的组合导航系统发展；信息处理方法也由单一数据源的处理，向多导航传感器多数据源的信息融合发展，总的来说，组合导航代表了未来水下导航的发展方向，克服了传统导航的缺陷和不足，使水下导航领域呈现出崭新的面貌，具有无比广阔的发展前景。现有的水下机器人通常利用惯性导航系统(Inertial Navigation System, INS)采集的姿态及加速度信息或磁罗经(Magnetic Compass, MC)采集姿态信息，以及多普勒计程仪(Doppler Velocity Log, DVL)采集的速度信息进行组合导航。但是由于上述姿态和速度数据存在一定误差，且这些误差会随时间积累到不可接受的程度。而研究结果表明基于定时接收GPS修正的组合导航系统可提供比常规惯性导航技术高一到两个数量级的测量精度，并且能够把横滚角误差和俯仰角误差抑制到一定的精度，但是很难抑制组合导航系统航向角误差的积累。磁航向仪(MCP)可以输出地磁场矢量在载体坐标系的投影，且精度不发散，故利用磁航向仪的信息与捷联惯性导航系统(SINS)、GPS、多普勒计程仪进行组合，可以抑制航向角误差的发散，可显著提高AUV整体导航的精度。
发明内容
本发明的目的在于提供一种自主式水下机器人组合导航系统及方法，针对现有技术存在的缺陷，将磁航向仪的信息与捷联惯性导航系统(SINS)、GPS、多普勒计程仪进行组合，抑制航向角误差的发散，显著提高AUV导航系统整体的精度。本发明的目的通过以下技术方案予以实现一种自主式水下机器人组合导航系统，由捷联惯性导航系统I、全球定位导航系统2、多普勒测速仪3、磁航向仪4组成，所述捷联惯性导航系统I利用陀螺仪和加速度计计算出相对地球沿地理坐标系的速度信息、位置信息和姿态信息；所述全球定位导航系统2获取自主式水下机器人的初始绝对位置信息和速度信息；所述多普勒测速仪3计算出速度信息；所述磁航向仪4计算出航向信息；所述全球定位导航系统2对捷联惯性导航系统I定时重调，系统对捷联惯导系统I输出的速度信息、位置信息和姿态信息，多普勒测速仪3所计算的速度信息，磁航向仪4所计算的航向信息进行数据融合，得到组合导航信息。一种自主式水下机器人组合导航系统的导航数据融合方法，步骤如下I)对于水下组合导航系统，线性化后离线系统状态方程和观测方程如下Χ(^=φ (k/k-1)X(k-1)+ Γ (k-1)W(k-1)Z(k) =H(k)X(k) +V (k)式中X(k)为系统状态向量；Z(k)为量测向量；Φ (k/k-1)为一步转移阵； Γ (k-1)为系统噪声驱动阵；W(k)为系统激励噪声序列；H(k)为量测阵；V(k)为量测噪声序列，W(k)、V(k)是不相关的高斯白噪声序列，其均值、方差分别为Ε[ω (k)]=0, Ε[ω (k) T(j)]=Q (k) δ kJ ；E[v(k)]=0, E[v(k) vT(j)]=R(k) δ kJ; cov [ ω (k)v(j)]=02)模糊自适应卡尔曼滤波算法编排设计矩阵0(10=01(10>0，1 (10=矿(10>0，0(10为系统噪声方差阵，R(k)为量测噪声方差阵，其中时间更新(I)状态变量估计X (k/k-1) =Φ (k/k-1) X (k-1)(2)误差协方差估计P (k/k-1) =Φ (k/k-1) P (k-1) Φτ (k/k-1)+ Γ (k-1) Q (k-1) rT(k_l)测量更新(3)定义残差的理论方差阵为Pr=H(k) (Φ (k/k-1)P (k-1) ΦT(k/k-1) +Q)H(k)T+R (k-1),以及残差的实测方差阵
权利要求
1.一种自主式水下机器人组合导航系统，其特征在于，由捷联惯性导航系统(I)、全球定位导航系统(2)、多普勒测速仪(3)、磁航向仪(4)组成，所述捷联惯性导航系统(I)利用陀螺仪和加速度计计算出相对地球沿地理坐标系的速度信息、位置信息和姿态信息；所述全球定位导航系统(2)获取自主式水下机器人的初始绝对位置信息和速度信息；所述多普勒测速仪(3)计算出速度信息；所述磁航向仪(4)计算出航向信息；所述全球定位导航系统(2)对捷联惯性导航系统(I)定时重调，系统对捷联惯导系统(I)输出的速度信息、位置信息和姿态信息，多普勒测速仪(3)所计算的速度信息，磁航向仪(4)所计算的航向信息进行数据融合，得到组合导航信息。
2.如权利要求I所述的自主式水下机器人组合导航系统的导航数据融合方法，其特征在于，该方法步骤如下 1)对于水下组合导航系统，线性化后离线系统状态方程和观测方程如下Χ(10=Φ (k/k-l)X(k-l) + r (k-l)ff(k-l) Z(k) =H(k)X(k) +V (k) 式中X(k)为系统状态向量；Z(k)为量测向量；o(k/k-i)为一步转移阵；r(k-i)为系统噪声驱动阵；W(k)为系统激励噪声序列；H(k)为量测阵；V(k)为量测噪声序列，W(k)、V(k)是不相关的高斯白噪声序列，其均值、方差分别为Ε[ω (k)]=0, Ε[ω (k) T(j)]=Q(k)δ kJ ；E[v(k)]=0, E[v(k) vT(j)]=R(k) δ kJ; cov [ ω (k)v(j)]=0 2)模糊自适应卡尔曼滤波算法编排 设计矩阵Q (k) =Qt (k) >0，R(k) =Rt (k) >0，Q (k)为系统噪声方差阵，R(k)为量测噪声方差阵，其中时间更新 (1)状态变量估计X(k/k-l) = Φ (k/k-l)X(k-l) (2)误差协方差估计P(k/k-l)=0 (k/k-l)P (k-1) 0T(k/k_l) + r (k-l)Q(k-l) rT(k_l) 测量更新 (3)定义残差的理论方差阵为Pr=H(k) (Φ (k/k-l)P (k-1) 0T(k/k-l)+Q)H(k)T+R (k-1), 以及残差的实测方差阵
全文摘要
本发明公开了一种自主式水下机器人组合导航系统，由捷联惯性导航系统、全球定位导航系统、多普勒测速仪、磁航向仪组成，所述捷联惯性导航系统计算出相对地球沿地理坐标系的速度信息、位置信息和姿态信息；所述全球定位导航系统获取自主式水下机器人的初始绝对位置信息和速度信息；所述多普勒测速仪计算出速度信息；所述磁航向仪计算出航向信息；所述全球定位导航系统对捷联惯性导航系统定时重调，系统对捷联惯导系统输出的速度信息、位置信息和姿态信息，多普勒测速仪所计算的速度信息，磁航向仪所计算的航向信息用自适应模糊卡尔曼滤波进行数据融合，得到精确组合导航信息。
文档编号G01C21/20GK102829777SQ20121033202
公开日2012年12月19日 申请日期2012年9月10日 优先权日2012年9月10日
发明者曾庆军, 王冲, 王彪, 章飞, 刘利 申请人:江苏科技大学