[发明专利]水下机器人惯性导航方法及系统

权利要求书

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]。