CN105259902B - 水下机器人惯性导航方法及系统 - Google Patents

Abstract

本发明公开一种水下机器人惯性导航方法及系统，采用陀螺仪和加速度计这两种传感器来构成惯性传感器实现水下机器人的惯性导航，惯性传感器被旋转调制机构带动而进行包含两个相反方向的周期性翻转运动，利用两个相反方向翻转的相互抵消作用，直接对低精度、高漂移的传感器进行有效自补偿，采用矩阵奇异值分解算法补偿安装误差，同时采用HDR算法去除由于陀螺仪摇摆产生的偏差，显著降低陀螺仪的随机漂移误差，降低陀螺仪噪声，提高陀螺仪输出精度；利用卡尔曼滤波算法消除了整个系统中的随机误差，提高后续计算出的姿态矩阵的准确性。

Description

水下机器人惯性导航方法及系统

技术领域

本发明涉及多功能水下机器人导航技术，尤其涉及一种水下机器人惯性导航方法及系统。

背景技术

水下机器人是开发海洋的手段之一，多数水下机器人只有浮游能力或爬行能力，能够实现浮游和爬壁的两种模式的水下机器人较为少见。模态切换水下机器人(Model-Converted ROV，MC-ROV)，是一种新颖的模块化、多功能、带缆遥控水下机器人，可以在浮游和爬行模态之间自由切换，被广泛应用于水下观察、浮游勘察、海底勘探、水下结构检修、海底管线检测、爬行清污以及水下安装等作业，已经成为海洋工程水下结构安全检测及维护的关键装备。精确的导航能力是水下机器人进行有效作业和安全回收的关键，为水下结构物检测、清污提供必要的技术支撑。水下机器人导航系统必须提供远距离及长时间范围内的精确定位、速度和姿态信息。但是由于受到水下环境的复杂性、机器人自身体积、重量、能源以及隐蔽性等因素的影响，实现高精度的水下导航仍然是一项艰巨的任务。惯性导航系统是水下导航最主要的导航方式，惯性导航系统是完全依靠自身设备进行导航的一种无源系统，依据牛顿惯性原理，利用陀螺仪、加速度计这些惯性传感器敏感元件测量物体相对惯性空间的线运动和角运动参数，在给定的初始条件下，通过积分输出载体的姿态参数和导航参数，由于其与外界不发生任何联系，不受环境的干扰影响，从而能够在相对封闭空间内进行较高精度的导航，具有隐秘性好的优点。实际上，惯性导航也可以理解为一种推算导航方法。由于惯性导航是通过对加速度二次积分而得到的。而陀螺仪会随时间不断漂移，这些都会导致惯性导航会存在累积误差，在长时间导航过程中，惯性导航必须要经过校正处理。

目前，捷联式惯性导航技术已经成为惯性导航发展的主流方向。捷联式惯性导航系统 (Strap-down Inertial Navigation System，简写SINS)是将加速度计和陀螺仪直接安装在载体上，在计算机中实时计算姿态矩阵，即计算出载体坐标系与导航坐标系之间的关系，从而把载体坐标系的加速度计信息转换为导航坐标系下的信息，然后进行导航计算，具有可靠性高、功能强、重量轻、成本低、精度高以及使用灵活等优点。陀螺仪的漂移误差和加速度计的零值偏值是影响系统精度的最直接的和最重要的因素，因此如何改善惯性传感器的性能，提高惯性组件的测量精度，特别是陀螺仪的测量精度，一直是惯性导航领域研究的重点。中国专利申请号为201310544880.4的专利文献公开了一种用于标定AUV自主导航传感器参数的装置和方法，把GPS融入到该系统中作为补偿，实时为水下机器人提供速度信息、姿态信息和位置信息，该装置结构复杂，成本高昂。中国专利申请号为201410413791.0的专利文献公开了一种水下结构检测机器人实时导航系统及方法，把磁罗盘融入到该系统中作为补偿，与陀螺仪和加速度计的频域互补，以校正角速度矢量，算法复杂，成本较高。

姿态更新算法是捷联惯导系统算法的核心，有限转动的不可交换性是求取姿态方程数值解算过程中的一个主要误差源。1971年，Bortz提出的等效转动矢量微分方程，有效的解决了不可交换性误差的问题。如果利用当前以及前两次迭代周期内的陀螺仪输出角增量并引入航姿算法，能够明显提高姿态算法的精度。在捷联惯性系统中，采用毕卡法求解四元数微分方程时使用了陀螺仪的角增量输出。角增量虽然微小，但不可视作无穷小，而刚体做有限转动时，刚体的空间角位置与旋转次序有关，即存在不可交换性误差。等效旋转矢量法在利用角增量计算等效旋转矢量时，对这种不可交换误差做了适当补偿，在姿态更新周期内包含的角增量子样越多，补偿就越精确。

2009,Johann Borenstein提出了HDR(启发式漂移消减法)算法。最先将其用在了车载导航的陀螺仪上面。算法的思想是首先判定载体是否严格的沿直线运动，若是则说明陀螺仪的输出为零，而实际情况下陀螺仪输出并不为零，则把该值视为随机漂移对其进行补偿。

卡尔曼滤波(Kalman filtering)一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

发明内容

本发明的目的在于提供一种水下机器人导航方法及系统，无需借助磁罗盘等惯性组件的补偿作用，就能够有效补偿陀螺仪的漂移误差，实现高精度导航，降低导航成本。

为解决上述问题，本发明水下机器人惯性导航系统采用的技术方案是：包括由加速度计和陀螺仪组成的惯性传感器，还具有误差补偿器和主控制器，误差补偿器由矩阵奇异值分解模块和HDR模块组成，主控制器由姿态矩阵计算模块、卡尔曼滤波模块、速度位置计算模块、姿态计算模块及控制模块组成；加速度计测量水下机器人的线加速度并输出，加速度计的输出分别连接速度位置计算模块和卡尔曼滤波模块的输入；陀螺仪测量水下机器人的角速度并输出，陀螺仪的输出分别连接误差补偿器和卡尔曼滤波模块的输入；矩阵奇异值分解模块的输出连接姿态矩阵计算模块和姿态计算模块的输入，卡尔曼滤波模块是姿态矩阵计算模块的姿态矩阵修正模块；姿态矩阵计算模块的输出分别连接卡尔曼滤波模块和姿态计算模块的输入，速度位置计算模块和姿态计算模块的输出连接控制模块的输入，控制模块的输出连接于水下机器人，控制水下机器人的速度、位置和姿态角；水下机器人上装有旋转调制机构，旋转调制机构连接于陀螺仪，带动陀螺仪进行包含两个相反方向的周期性运动，旋转调制机构 1的旋转角度信息传输到姿态计算模块。

本发明水下机器人惯性导航方法采用的技术方案是：包括以下步骤：

A：陀螺仪输出的角速度ω_1i传输到误差补偿器和卡尔曼滤波模块，误差补偿器中的矩阵奇异值分解模块先计算旋转调制机构的合力H，再对合力H进行矩阵奇异值分解，最后计算安装误差矩阵

B、HDR模块对陀螺仪输出的角速度ω_1i进行零值偏移误差和漂移误差补偿，得到补偿后的角速度ω_hdri；

C、加速度计输出的线加速度值α_i传输给速度位置计算模块，速度位置计算模块根据加速度值α_i计算出水下机器人的位置和速度；

D、将补偿后的角速度值ω_hdri传输到姿态矩阵计算模块，姿态矩阵计算模块计算陀螺仪的旋转矢量Φ，通过旋转矢量Φ计算机器人的姿态四元数q：

E、卡尔曼滤波模块根据输入值采用卡尔曼滤波算法修正，姿态矩阵计算模块通过卡尔曼滤波修正后的四元数向量X_k计算出水下机器人当前时刻的姿态矩阵

F、姿态矩阵计算模块将求得的姿态矩阵传输到姿态计算模块34，姿态计算模块求取当前时刻真实姿态信息，将该姿态信息传递给控制模块控制水下机器人的运动。

与现有技术相比，本发明的技术方案具有以下优势：

1、本发明采用陀螺仪和加速度计这两种传感器来构成惯性传感器，以实现水下机器人的惯性导航，不仅可以大大减小惯性传感器的体积，同时还具有成本低和安装便利等优点。

2、本发明中的惯性传感器被旋转调制机构带动而进行包含两个相反方向的周期性翻转运动，利用两个相反方向翻转的相互抵消作用，直接对低精度、高漂移的传感器进行有效自补偿。

3、本发明根据惯性传感器与旋转调制机构的安装特点，提出矩阵奇异值分解法，采用矩阵奇异值分解算法补偿安装误差，同时采用HDR算法设计闭环网络，增加双低通滤波器去除由于陀螺仪摇摆产生的偏差，显著降低陀螺仪的随机漂移误差，降低陀螺仪噪声，提高陀螺仪输出精度。

4、本发明利用卡尔曼滤波算法消除了整个系统中的随机误差，提高后续计算出的姿态矩阵的准确性。

附图说明

图1是本发明水下机器人惯性导航系统的结构框图；

图2是图1中旋转调制机构的结构及带动惯性传感器旋转的原理示意图；

图3是图1中HDR模块的数据处理流程图。

图中：1.水下机器人；2.误差补偿器；3.主控制器；11.惯性传感器；12.旋转调制机构； 21.矩阵奇异值分解模块；22.HDR模块；31.姿态矩阵计算模块；32.卡尔曼滤波模块；33.速度位置计算模块；34.姿态计算模块；35.控制模块；111.加速度计；112.陀螺仪；121.电机； 122.旋转编码器；123.旋转盘；124.电机。

具体实施方式

参见图1，本发明水下机器人惯性导航系统具有惯性传感器11、误差补偿器2和主控制器3。误差补偿器2由矩阵奇异值分解模块21和HDR模块22组成。主控制器3由姿态矩阵计算模块31、卡尔曼滤波模块32、速度位置计算模块33、姿态计算模块34以及控制模块35组成。

惯性传感器11由加速度计111和陀螺仪112组成，安装在水下机器人1上。加速度计111的输出分别连接主控制器3中的速度位置计算模块33和卡尔曼滤波模块32的输入，加速度计111测量水下机器人1的线加速度，并将所测量到的线加速度输出给速度位置计算模块33，用于计算水下机器人的位置和速度，同时将线加速度作为初始值输出给的卡尔曼滤波模块32。

陀螺仪112测量水下机器人1的角速度，陀螺仪112的输出分别连接误差补偿器2和卡尔曼滤波模块32的输入，输出的角速度值一方面传输到误差补偿器2，另一方面将角速度值作为初始值传输到主控制器3的卡尔曼滤波模块32。在水下机器人1上还安装旋转调制机构12，将陀螺仪112连接旋转调制机构12。旋转调制机构12将旋转角度信息传输到主控制器3的姿态计算模块34，同时旋转调制机构12带动陀螺仪112进行包含两个相反方向的周期性运动。

当陀螺仪112安装到旋转调制机构12上时，惯性传感器11的惯性坐标系应该与旋转调制机构12的坐标系，然而在安装的过程中两个坐标系并不能完全保持平行，会有一定角度的偏差，因此，将陀螺仪112输出的角速度值输入误差补偿器2中的矩阵奇异值分解模块21 和HDR模块22，通过HDR模块22和矩阵奇异值分解法来补偿陀螺仪112和旋转调制机构12 的这种安装误差。

陀螺仪112输出的角速度值通过矩阵奇异值分解模块21和HDR模块22进行补偿后传输到主控制器3的姿态矩阵计算模块31，经姿态矩阵计算模块31求得姿态矩阵。卡尔曼滤波模块32作为姿态矩阵计算模块31的姿态矩阵修正模块。矩阵奇异值分解模块21的输出分别连接姿态计算模块34和姿态矩阵计算模块31的输入，通过矩阵奇异值分解模块21求取的安装误差传输到姿态计算模块34。姿态矩阵计算模块31的输出分别连接卡尔曼滤波模块32和姿态计算模块34的输入，姿态矩阵计算模块31求得的姿态矩阵一方面传输到姿态计算模块 34，用于求水下机器人1的姿态角，即航向角、横滚角和俯仰角。另一方面传输给卡尔曼滤波模块32，进行下一次的数据融合。速度位置计算模块33和姿态计算模块34的输出连接控制模块35的输入，速度位置计算模块33计算出的速度位置传输给控制模块35，姿态计算模块34计算出的姿态角传输给控制模块35。控制模块35根据实际要求调整速度、位置和姿态角控制水下机器人1，从而实现导航。

参见图2，旋转调制机构12包括电机124、电机121、旋转编码器122和旋转盘123，陀螺仪112安装在旋转盘123上。电机124的中心轴与y轴重合，电机121的中心轴与z轴和旋转盘123的中心轴重合。旋转调制机构12是基于y-z的两轴可逆旋转结构。分别在y、 z两个方向上安装旋转编码器122，用于补偿陀螺仪112输出的姿态角。在主控制器3的编码控制下对电机124和电机121进行旋转驱动，电机121的电源信号线穿过旋转盘123中心防止线路缠绕，电机124和电机121的运动将带动旋转盘123作可逆的正反两方向的周期性旋转，同一时刻只有一个电机工作，因此只进行正反两个方向的旋转，进而带动陀螺仪112进行可逆的正反两方向的周期性运动，消除陀螺仪112的误差。

参见图3，HDR模块22工作时，采用HDR算法，陀螺仪112输出角速度信号，但是由于陀螺仪112精度的限制，输出的角速度信号中含有一定的误差，该误差包含零值偏移误差ε₀和随机漂移误差ε_d。由于陀螺仪112分辨率精度的限制，对地球的自转角速度不敏感，理论上静止时陀螺仪112的输出为零，实际的输出有值，可以将这个值看作是零值偏移误差ε₀。随机漂移误差ε_d则表示陀螺仪112的随机漂移，陀螺仪112工作时间的增加，温度环境的变化等都会使得陀螺仪112产生微小的转动进而使得输出值与实际值之间产生偏差，这些偏差称之为随机漂移，这也是HDR估计和补偿的主要对象。图3中ω_true为真实输出值，为了测得准确的ω_true，必须从测量的数据中去除零值偏移误差ε₀和随机漂移误差ε_d。由图3可知，当载体旋转角速度为ω_true时，陀螺仪112实际上输出的角速度为ω_true+ε₀+ε_d。一般对零值偏移误差ε₀的补偿都比较简单，通常采用陀螺仪112工作稳定后一段静止数据的均值来补偿陀螺仪112在整个运行过程中的零值偏移误差，零值偏移误差补偿后的角速度为ω_true+ε_d，之后通过一个低通滤波器解决载体运动时的摇摆问题，经低通滤波器后的角速度变为ω″+ε_d。输入的设定角速度为0，即ω_set＝0。上一时刻的ω_i值通过一个z变换，一方面以负反馈的形式反馈到设定输入端，得到误差信号E,另一方面通过一个衰减信号控制器和积分控制器。误差信号E通过积分控制器的积分即可得到补偿因子I。将求得的补偿因子I和ω″+ε_d相加，则得到当前时刻的角速度ω_i。

参见图1-3，本发明水下机器人惯性导航系统工作时，具体按以下方法实施惯性导航功能：

步骤一：加速度计111测量并输出水下机器人在x、y、z三个方向的线加速度，记为α_i＝[α_ix,α_iy,α_iz]，陀螺仪112测量并输出水下机器人三个方向的角速度，记为ω_i＝[ω_ix,ω_iy,ω_iz]。

步骤二：旋转调制机构12带动陀螺仪112进行包含两个相反方向的周期性运动。旋转调制机构12采用双轴旋转技术，对陀螺仪112在y轴和z轴依次采用如下旋转方案：首先，采用前向和后向依次循环的旋转方式，避免标度因数误差和安装误差增大。其次，每次进行旋转，使每个方向和它相反方向持续时间达到一致。以水下机器人本体坐标系(该本体坐标系以水下机器人的质心为原点，x轴沿着水下机器人本体的横轴指右，y轴沿着水下机器人本体纵轴指前，z轴沿着纵轴并与x,y轴垂直，构成右手直角坐标系。)为基准进行，在一个周期内使陀螺仪112依次进行以下旋转：

绕y轴向前旋转180°，绕z轴向前旋转180°，绕y轴向后旋转180°，静止8s；

绕y轴向前旋转180°，绕z轴向后旋转180°，绕y轴向后旋转180°，静止8s；

绕y轴向后旋转180°，绕z轴向后旋转180°，绕y轴向前旋转180°，静止8s；

绕y轴向后旋转180°，绕z轴向前旋转180°,绕y轴向前旋转180°,静止8s。

步骤三：陀螺仪112周期性运动后，陀螺仪112输出的角速度等于ω_i＝[ω_ix,ω_iy,ω_iz]加上旋转调制机构12的角速度ω_xzi＝[ω_xzxi,ω_xzyi,ω_xzzi]，记为ω_1i＝[ω_1xi,ω_1yi,ω_1zi]。将此时的陀螺仪 112输出的角速度值ω_1i一方面传输到误差补偿器2，另一方面作为初始值传输到卡尔曼滤波模块32。此时加速度计111输出的值保持不变，仍然为α_i＝[α_ix,α_iy,α_iz]，将其传输给速度位置计算模块33和卡尔曼滤波模块32。

步骤四：由误差补偿器2中的矩阵奇异值分解模块21计算旋转调制机构12的合力H，合力H是一个m*n阶复矩阵，其中，b为惯性传感器11的坐标系Ox_by_bz_b(即传感器载体坐标系，原点与惯性传感器11的质心重合，z_b垂直于惯性传感器11向上， x_b与y_b互相垂直并与z_b构成右手直角坐标系)，r为旋转调制机构12坐标系(如图2所示，x 轴垂直于y、z轴并与y、z轴构成右手直角坐标系)，为旋转调制机构12绕b系y轴和z轴进行前向和后向旋转时惯性传感器11的重力值，T_m是与旋转方向一致的三阶方向余弦矩阵， f^r是惯性传感器11在r坐标系中的重力，满足关系式：式中，m＝1,2,3,4

步骤五：由矩阵奇异值分解模块21对合力H进行矩阵奇异值分解，通过公式H＝USV^T，对H进行分解得到U和V两个酉矩阵。

奇异值分解是线性代数中公知的一种重要的矩阵分解，合力H是一个m*n阶复矩阵,U 为m阶酉阵，V为n阶酉阵,U和V中分别是H的奇异向量,V^T是V的转置，U^T是U的转置。 S＝diag(σ1 ,σ2,……,σr),σi>0(i＝1,…,r),σi为H的奇异值，r的值为H的秩。

步骤六：由矩阵奇异值分解模块21经公式计算安装误差矩阵安装误差矩阵具体是b坐标系到r坐标系的变换矩阵。

步骤七：HDR模块22先补偿陀螺仪112的随机漂移误差，再通过均值计算，对陀螺仪112 补偿前输出的角速度ω_1i进行零值偏移误差ε₀补偿，得角速度ω_2i＝ω_1i-ε₀；然后通过启发式漂移消减算法对陀螺仪112输出的角速度ω_1i进行漂移误差补偿，补偿后的陀螺仪112的角速度为ω_hdri＝ω″+ε_d+I_i，其中，ε_d为随机漂移误差，ω_hdri为经过HDR算法补偿后的角速度值，ω″为角速度ω_2i经过低通滤波器的角速度值，I_i＝I_i-1-A_ii_csign(ω_i-1)，I_i-1为上一时刻的补偿因子，I_i为当前时刻的补偿因子，sign为取符号函数，i_c为常量，A_i为衰减函数，

ω_hdri-1为上一时刻的角速度值，θ_w为设定的阈值， p是设定的衰减因子。

步骤八：加速度计111输出的线加速度值α_i＝[α_ix,α_iy,α_iz]传输给速度位置计算模块33，速度位置计算模块33计算水下机器人的位置和速度。水下机器人在三个方向的坐标位置为：

S_xo代表水下机器人在x方向的初始位置，S_yo代表水下机器人在y方向的初始位置，S_zo代表水下机器人在z方向的初始位置。

步骤九，误差补偿器2补偿后的角速度值ω_hdri传输到姿态矩阵计算模块31，姿态矩阵计算模块31计算陀螺仪112的旋转矢量。旋转矢量Φ＝[φ_x φ_y φ_z]，φ_x代表陀螺仪112在x轴方向的角增量，φ_y代表陀螺仪112在y轴方向的角增量，φ_z代表陀螺仪112在z轴方向的角增量；当前时刻角速度ω_hdri＝[ω_hdrix,ω_hdriy,ω_hdriz]，上一时刻的角速度ω_hdri-1＝[ω_hdrix-1,ω_hdriy-1,ω_hdriz-1]，

步骤十：通过陀螺仪112的旋转矢量旋转矢量Φ＝[φ_x φ_y φ_z]计算机器人在t时刻的姿态四元数q＝[q₀,q₁,q₂,q₃]：

q＝[d₁ d₂·φ_x d₂·φ_y d₂·φ_z]

式中，

Φ^T是Φ的转置。

步骤十一：卡尔曼滤波模块32根据输入的量测值Z_k＝[α_ix,α_iy,α_iz,ω_ix,ω_iy,ω_iz]，采用卡尔曼滤波算法对姿态矩阵进行修正，计算出高精度四元数。

被估计矢量(四元数)初值：X_0/0＝[1 0 0 0]^T，

估计均方误差矩阵初值：P_0/0＝diag([0.001 0.001 0.001 0.001])；diag代表对角矩阵

滤波具体过程如下：

状态一步预测方程：X_k/k-1＝Φ_k,k-1X_k-1，

X_k-1为k-1时刻的系统估计状态量，即X_k-1＝[q₀,q₁,q₂,q₃]。Φ_k,k-1为X_k-1到X_k的一步转移矩阵。X_k/k-1是利用X_k-1计算得到的对X_k的一步预测。

状态估值方程：X_k＝X_k/k-1+K_k(Z_k-H_kX_k/k-1)，

X_k为k时刻的系统估计状态量，即X_k＝[q₀,q₁,q₂,q₃]。它是在一步预测X_k/k-1的基础上，根据量测值Z_k＝[α_ix,α_iy,α_iz,ω_ix,ω_iy,ω_iz]计算得到的。H_k为量测矩阵，表示量测值Z_k和估计值X_k之间的数学关系；V_k为量测值高斯白噪声序列，其均值E[v(k)]＝0，方差 E[v(k)v(k)^T]＝R_k，R_k即测量噪声方差；为了求得X_k的值，必须知道增益矩阵K_k即测量噪声方差；

滤波增益方程：

是量测矩阵H_k的转置；R_k即测量噪声方差；

一步预测均方误差方程：P_k/k-1＝Φ_k,k-1P_k-1Φ_k,k-1 ^T+Q_k-1；

Q_k-1为系统噪声的方差矩阵。Φ_k,k-1 ^T为一步转移矩阵Φ_k,k-1的

估计均方误差方程：P_k/k＝(I-K_kH_k)P_k/k-1；

通过上述五个公式，可以求得当前时刻的系统估计状态量X_k＝[q₀,q₁,q₂,q₃]，每完成一次数据融合，计算一次姿态矩阵。

步骤十二：姿态矩阵计算模块31通过卡尔曼滤波修正后的四元数向量X_k＝[q₀,q₁,q₂,q₃] 计算出水下机器人当前时刻的姿态矩阵

表示地理坐标系n到水下机器人本体坐标系b的转换关系；

步骤十三：姿态矩阵计算模块31将求得的姿态矩阵传输到姿态计算模块34，用于求取水下机器人的姿态信息，即航向角、横滚角和俯仰角。

原始姿态信息分别表示为ψ、θ和γ：

其中r₁₂、r₂₂、r₃₁、r₃₂、r₃₃为姿态矩阵中的矩阵元素。

步骤十四：求取当前时刻真实姿态信息[ψ_itrue,θ_itrue,γ_itrue]＝当前时刻原始姿态信息 [ψ_i,θ_i,γ_i]×安装误差矩阵-旋转调制机构的双轴旋转角ω_xzi

步骤十五：根据上述求得的位置，姿态等信息传递给控制模块35，并根据实际要求控制水下机器人的运动。

以下提供本发明的一个应用例：水下机器人惯性导航系统的初始化参数为：当地重力加速度g＝9.8m/s²，采样时间t＝0.02s。陀螺仪112的角速度为初值为零；加速度计111输出三个方向的加速度为a_x,a_y,a_z，初值为[0 0 g]。

系统噪声方差Q和测量噪声方差R为：

Q＝diag([0.0001 0.0001 0.0001 0.0001])

R＝diag([0.005 0.005 0.005 0.003 0.003 0.003])

估计均方误差矩阵初值：P_0/0＝diag([0.001 0.001 0.001 0.001])；

被估计矢量(四元数)初值：X_0/0＝[1 0 0 0]^T；

HDR模块22补偿随机漂移误差：ω_i＝ω″+ε_d+I_i，

补偿因子I_i＝I_i-1-A_isign(ω_i-1)i_c，其中sign为取符号函数，i_c为固定常量，衰减 函数取为θ_ω为设定的阈值，θ_ω＝3°，p＝1.5,i_c＝2。

等效旋转矢量法更新四元数：

其中：

卡尔曼滤波方程：

X_k＝Φ_k,k-1X_k-1+W_k-1

Z_k＝H_kX_k+V_k，

姿态矩阵：

其中，n系为导航坐标系为东北天坐标系，即表示水下机器人所在地理位置的坐标系，b系载体坐标系。

水下机器人姿态角；

ψ＝arctan(r₁₂/r₂₂)

θ＝arcsinr₃₂

γ＝arctan(-r₃₁/r₃₃)，

经过验证，姿态误差角小于0.5°，航向角误差小于2°，位置偏差5cm左右。

Claims (7)

1.一种水下机器人惯性导航系统，包括由加速度计(111)和陀螺仪(112)组成的惯性传感器(11)，其特征是：还具有误差补偿器(2)和主控制器(3)，误差补偿器(2)由矩阵奇异值分解模块(21)和HDR模块(22)组成，主控制器(3)由姿态矩阵计算模块(31)、卡尔曼滤波模块(32)、速度位置计算模块(33)、姿态计算模块(34)及控制模块(35)组成；加速度计(111)测量水下机器人的线加速度并输出，加速度计(111)的输出分别连接速度位置计算模块(33)和卡尔曼滤波模块(32)的输入；陀螺仪(112)测量水下机器人的角速度并输出，陀螺仪(112)的输出分别连接误差补偿器(2)和卡尔曼滤波模块(32)的输入；矩阵奇异值分解模块(21)的输出连接姿态矩阵计算模块(31)和姿态计算模块(34)的输入，卡尔曼滤波模块(32)是姿态矩阵计算模块(31)的姿态矩阵修正模块；姿态矩阵计算模块(31)的输出分别连接卡尔曼滤波模块(32)和姿态计算模块(34)的输入，速度位置计算模块(33)和姿态计算模块(34)的输出连接控制模块(35)的输入，控制模块(35)的输出连接于水下机器人，控制水下机器人的速度、位置和姿态角；水下机器人上装有旋转调制机构(12)，旋转调制机构(12)连接于陀螺仪(112)，带动陀螺仪(112)进行包含两个相反方向的周期性运动，旋转调制机构(12)的旋转角度信息传输到姿态计算模块(34)。

2.根据权利要求1所述水下机器人惯性导航系统，其特征是：旋转调制机构(12)是基于y-z的两轴可逆旋转结构，旋转调制机构(12)包括旋转盘(123)，陀螺仪(112)装在旋转盘(123)上，第一电机(124)的中心轴与y轴重合，第二电机(121)的中心轴与z轴和旋转盘(123)的中心轴重合，第一、第二电机(124、121)带动旋转盘(123)作可逆的正反两方向的周期性旋转，分别在y、z两个方向上装有旋转编码器(122)。

3.一种如权利要求1所述水下机器人惯性导航系统的导航方法，其特征是包括以下步骤：

A：陀螺仪(112)输出的角速度ω_1i传输到误差补偿器(2)和卡尔曼滤波模块(32)，误差补偿器(2)中的矩阵奇异值分解模块(21)先计算旋转调制机构(12)的合力H，再对合力H进行矩阵奇异值分解，最后计算安装误差矩阵

B、HDR模块(22)对陀螺仪(112)输出的角速度ω_1i进行零值偏移误差和漂移误差补偿，得到补偿后的角速度ω_hdri；

C、加速度计(111)输出的线加速度值α_i传输给速度位置计算模块(33)，速度位置计算模块(33)根据加速度值α_i计算出水下机器人的位置和速度；

D、将补偿后的角速度值ω_hdri传输到姿态矩阵计算模块(31)，姿态矩阵计算模块(31)计算出陀螺仪(112)的旋转矢量Φ，通过旋转矢量Φ计算机器人的姿态四元数q；

E、卡尔曼滤波模块(32)根据输入值采用卡尔曼滤波算法修正，姿态矩阵计算模块(31)通过卡尔曼滤波修正后的四元数向量X_k计算出水下机器人当前时刻的姿态矩阵

F、姿态矩阵计算模块(31)将求得的姿态矩阵传输到姿态计算模块(34)，姿态计算模块(34)求取当前时刻真实姿态信息，将该姿态信息传递给控制模块(35)控制水下机器人的运动。

4.根据权利要求3所述的导航方法，其特征是：步骤A中，陀螺仪(112)输出水下机器人在x、y、z三个方向的角速度ω_1i＝[ω_1xi,ω_1yi,ω_1zi]，角速度ω_1i等于陀螺仪(112)周期性运动后输出的角速度ω_i＝[ω_ix,ω_iy,ω_iz]加上旋转调制机构(12)的角速度ω_xzi＝[ω_xzxi,ω_xzyi,ω_xzzi]。

5.根据权利要求4所述的导航方法，其特征是：步骤A中，旋转调制机构(12)的合力是一个m*n阶复矩阵，b为惯性传感器(11)坐标系，r为旋转调制机构(12)坐标系，为旋转调制机构(12)绕b系y轴和z轴进行前向和后向旋转时惯性传感器(11)的重力值，T_m是与旋转方向一致的三阶方向余弦矩阵，f^r是惯性传感器(11)在r坐标系中的重力；由公式H＝USV^T对H进行矩阵奇异值分解得到U和V两个酉矩阵，U为m阶酉阵，V为n阶酉阵,U和V中分别是H的奇异向量,V^T是V的转置，U^T是U的转置；S＝diag(σ1,σ2,……,σr ),σi>0,i＝1,……,r,σi为H的奇异值，r的值为H的秩；

由公式计算安装误差矩阵

6.根据权利要求5所述的导航方法，其特征是：步骤B中，补偿后的陀螺仪(112)的角速度ω_hdri＝ω″+ε_d+I_i，ε_d为随机漂移误差，ω″为角速度ω_2i经过低通滤波器的角速度值，ω_2i＝ω_1i-ε₀，ω_1i是陀螺仪(112)补偿前输出的角速度，ε₀是零值偏移误差，I_i＝I_i-1-A_ii_csign(ω_i-1)，I_i-1为上一时刻的补偿因子，I_i为当前时刻的补偿因子，sign为取符号函数，i_c为常量，A_i为衰减函数。

7.根据权利要求6所述的导航方法，其特征是：步骤D中，旋转矢量Φ＝[φ_x φ_y φ_z]，φ_x、φ_y、φ_z分别是陀螺仪(112)在x、y、z轴方向的角增量，ω_hdri＝[ω_hdrix,ω_hdriy,ω_hdriz]是当前时刻角速度，ω_hdri-1＝[ω_hdrix-1,ω_hdriy-1,ω_hdriz-1]是上一时刻角速度，姿态四元数q＝[d₁ d₂·φ_x d₂·φ_y d₂·φ_z]，其中：

Φ^T是Φ的转置。