水下机器人惯性导航方法及系统的制作方法
【技术领域】
[0001] 本发明涉及多功能水下机器人导航技术，尤其涉及一种水下机器人惯性导航方法 及系统。
【背景技术】
[0002] 水下机器人是开发海洋的手段之一，多数水下机器人只有浮游能力或爬行能 力，能够实现浮游和爬壁的两种模式的水下机器人较为少见。模态切换水下机器人 (Model-ConvertedROV，MC-R0V)，是一种新颖的模块化、多功能、带缆遥控水下机器人，可以 在浮游和爬行模态之间自由切换，被广泛应用于水下观察、浮游勘察、海底勘探、水下结构 检修、海底管线检测、爬行清污以及水下安装等作业，已经成为海洋工程水下结构安全检测 及维护的关键装备。精确的导航能力是水下机器人进行有效作业和安全回收的关键，为水 下结构物检测、清污提供必要的技术支撑。水下机器人导航系统必须提供远距离及长时间 范围内的精确定位、速度和姿态信息。但是由于受到水下环境的复杂性、机器人自身体积、 重量、能源以及隐蔽性等因素的影响，实现高精度的水下导航仍然是一项艰巨的任务。惯性 导航系统是水下导航最主要的导航方式，惯性导航系统是完全依靠自身设备进行导航的一 种无源系统，依据牛顿惯性原理，利用陀螺仪、加速度计这些惯性传感器敏感元件测量物体 相对惯性空间的线运动和角运动参数，在给定的初始条件下，通过积分输出载体的姿态参 数和导航参数，由于其与外界不发生任何联系，不受环境的干扰影响，从而能够在相对封闭 空间内进行较高精度的导航，具有隐秘性好的优点。实际上，惯性导航也可以理解为一种推 算导航方法。由于惯性导航是通过对加速度二次积分而得到的。而陀螺仪会随时间不断漂 移，这些都会导致惯性导航会存在累积误差，在长时间导航过程中，惯性导航必须要经过校 正处理。
[0003] 目前，捷联式惯性导航技术已经成为惯性导航发展的主流方向。捷联式惯性导航 系统（Strap-down Inertial Navigation System，简写SINS)是将加速度计和陀螺仪直 接安装在载体上，在计算机中实时计算姿态矩阵，即计算出载体坐标系与导航坐标系之间 的关系，从而把载体坐标系的加速度计信息转换为导航坐标系下的信息，然后进行导航计 算，具有可靠性高、功能强、重量轻、成本低、精度高以及使用灵活等优点。陀螺仪的漂移误 差和加速度计的零值偏值是影响系统精度的最直接的和最重要的因素，因此如何改善惯 性传感器的性能，提高惯性组件的测量精度，特别是陀螺仪的测量精度，一直是惯性导航 领域研究的重点。中国专利申请号为201310544880. 4的专利文献公开了一种用于标定 AUV自主导航传感器参数的装置和方法，把GPS融入到该系统中作为补偿，实时为水下机 器人提供速度信息、姿态信息和位置信息，该装置结构复杂，成本高昂。中国专利申请号为 201410413791. 0的专利文献公开了一种水下结构检测机器人实时导航系统及方法，把磁罗 盘融入到该系统中作为补偿，与陀螺仪和加速度计的频域互补，以校正角速度矢量，算法复 杂，成本较高。
[0004] 姿态更新算法是捷联惯导系统算法的核心，有限转动的不可交换性是求取姿态方 程数值解算过程中的一个主要误差源。1971年，Bortz提出的等效转动矢量微分方程，有效 的解决了不可交换性误差的问题。如果利用当前以及前两次迭代周期内的陀螺仪输出角增 量并引入航姿算法，能够明显提高姿态算法的精度。在捷联惯性系统中，采用毕卡法求解四 元数微分方程时使用了陀螺仪的角增量输出。角增量虽然微小，但不可视作无穷小，而刚体 做有限转动时，刚体的空间角位置与旋转次序有关，即存在不可交换性误差。等效旋转矢量 法在利用角增量计算等效旋转矢量时，对这种不可交换误差做了适当补偿，在姿态更新周 期内包含的角增量子样越多，补偿就越精确。
[0005] 2009, Johann Borenstein提出了 HDR(启发式漂移消减法）算法。最先将其用在 了车载导航的陀螺仪上面。算法的思想是首先判定载体是否严格的沿直线运动，若是则说 明陀螺仪的输出为零，而实际情况下陀螺仪输出并不为零，则把该值视为随机漂移对其进 行补偿。
[0006] 卡尔曼滤波（Kalman filtering) -种利用线性系统状态方程，通过系统输入输出 观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的 影响，所以最优估计也可看作是滤波过程。
【发明内容】
[0007] 本发明的目的在于提供一种水下机器人导航方法及系统，无需借助磁罗盘等惯性 组件的补偿作用，就能够有效补偿陀螺仪的漂移误差，实现高精度导航，降低导航成本。
[0008] 为解决上述问题，本发明水下机器人惯性导航系统采用的技术方案是：包括由加 速度计和陀螺仪组成的惯性传感器，还具有误差补偿器和主控制器，误差补偿器由矩阵奇 异值分解模块和HDR模块组成，主控制器由姿态矩阵计算模块、卡尔曼滤波模块、速度位置 计算模块、姿态计算模块及控制模块组成；加速度计测量水下机器人的线加速度并输出，加 速度计的输出分别连接速度位置计算模块和卡尔曼滤波模块的输入；陀螺仪测量水下机器 人的角速度并输出，陀螺仪的输出分别连接误差补偿器和卡尔曼滤波模块的输入；矩阵奇 异值分解模块的输出连接姿态矩阵计算模块和姿态计算模块的输入，卡尔曼滤波模块是姿 态矩阵计算模块的姿态矩阵修正模块；姿态矩阵计算模块的输出分别连接卡尔曼滤波模块 和姿态计算模块的输入，速度位置计算模块和姿态计算模块的输出连接控制模块的输入， 控制模块的输出连接于水下机器人，控制水下机器人的速度、位置和姿态角；水下机器人上 装有旋转调制机构，旋转调制机构连接于陀螺仪，带动陀螺仪进行包含两个相反方向的周 期性运动，旋转调制机构1的旋转角度信息传输到姿态计算模块。
[0009] 本发明水下机器人惯性导航方法采用的技术方案是：包括以下步骤：
[0010] A :陀螺仪输出的角速度ωΗ传输到误差补偿器和卡尔曼滤波模块，误差补偿器中 的矩阵奇异值分解模块先计算旋转调制机构的合力H，再对合力H进行矩阵奇异值分解，最 后计算安装误差矩阵^^ ;
[0011] B、HDR模块对陀螺仪输出的角速度CO11进行零值偏移误差和漂移误差补偿，得到 补偿后的角速度W hti;
[0012] C、加速度计输出的线加速度值a i传输给速度位置计算模块，速度位置计算模块 根据加速度值a i计算出水下机器人的位置和速度；
[0013] D、将补偿后的角速度值c〇hd"传输到姿态矩阵计算模块，姿态矩阵计算模块计算 陀螺仪的旋转矢量Φ，通过旋转矢量Φ计算机器人的姿态四元数q :
[0014] E、卡尔曼滤波模块根据输入值采用卡尔曼滤波算法修正，姿态矩阵计算模块通过 卡尔曼滤波修正后的四元数向量Xk计算出水下机器人当前时刻的姿态矩阵;
[0015] F、姿态矩阵计算模块将求得的姿态矩阵Ct传输到姿态计算模块34,姿态计算模块 求取当前时刻真实姿态信息，将该姿态信息传递给控制模块控制水下机器人的运动。
[0016] 与现有技术相比，本发明的技术方案具有以下优势：
[0017] 1、本发明采用陀螺仪和加速度计这两种传感器来构成惯性传感器，以实现水下机 器人的惯性导航，不仅可以大大减小惯性传感器的体积，同时还具有成本低和安装便利等 优点。
[0018] 2、本发明中的惯性传感器被旋转调制机构带动而进行包含两个相反方向的周期 性翻转运动，利用两个相反方向翻转的相互抵消作用，直接对低精度、高漂移的传感器进行 有效自补偿。
[0019] 3、本发明根据惯性传感器与旋转调制机构的安装特点，提出矩阵奇异值分解法， 采用矩阵奇异值分解算法补偿安装误差，同时采用HDR算法设计闭环网络，增加双低通滤 波器去除由于陀螺仪摇摆产生的