[发明专利]一种水下机器人传感器安装偏差估计方法

说明书

本发明公开了一种水下机器人传感器安装偏差估计方法，通过建立水面航行状态机器人航行位姿的因子图模型，基于卫星定位模块的真值反馈，利用非线性优化方法，实现水下机器人艏向测量的常值线性加性误差α和水下机器人前进速度、横移速度的非线性乘性误差cos(β)和sin(β)的估计，补偿航位推算模块与卫星定位模块间的位置偏差。实现传感器安装偏差的鲁棒估计，提高航位推算模块的计算精度，进而提高水下机器人的定位精度。减少机器人出水校准次数，提高作业效率，满足长时间水下航行的需求。

技术领域

本发明属于水下机器人定位领域，涉及一种水下机器人传感器安装偏差估计方法，特别是一种基于图优化的水下机器人传感器安装偏差估计方法。

背景技术

水下定位技术是水下机器人领域的重要研究方向，是水下机器人进行有效任务作业的关键，然而传感器安装误差容易为水下机器人带来不必要的姿态、速度测量误差，降低了航位推算方法的精度，导致水下机器人作业过程需要不断出水完成位置修正，降低了作业效率。

本发明的水下机器人传感器安装偏差估计方法，利用卫星定位系统提供的水下机器人位置信息，结合基于图优化的估计模块，在机器人存在传感器安装偏差的条件下，通过建立基于航位推算位置估计、卫星定位测量的因子图模型，利用非线性优化算法，实现安装偏差的非线性估计。

发明内容

针对上述现有技术，本发明要解决的技术问题是提供一种能够实现对于安装偏差的修正，减少出水校准次数的水下机器人传感器安装偏差估计方法。通过图优化的方法完成水下机器人传感器安装偏差的估计，实现对于姿态传感器和多普勒测速仪测量数据的修正，提高了多水下机器人定位精度，满足水下机器人长时间的航海作业需求。

为解决上述技术问题，本发明的一种水下机器人传感器安装偏差估计方法，包括以下步骤：

步骤1：水下机器人开始水面航行，水下机器人利用多普勒测速仪模块采集速度信息，并通过速度测量修正环节完成对于多普勒安装偏差的修正，获得水下机器人在大地坐标系下的速度值，并将速度值输入至水下机器人航位推算模块；

步骤2：水下机器人利用姿态传感器模块采集姿态信息，并通过姿态测量修正环节完成对于姿态传感器安装偏差的修正，获取水下机器人在大地坐标系下的姿态值，并将姿态信息输入至机器人位置估计模块；

步骤3：航位推算模块根据输入的速度信息和姿态信息，采用航位推算算法，完成对于机器人自身位置的估计,并根据输出的位置信息，迭代更新水下机器人位置因子图的变量节点；

步骤4：水下机器人通过卫星定位模块获取自身真实的位置信息，并将该信息作为因子图不可更新的变量节点，同时，将安装偏差作为变量节点；

步骤5：图优化估计模块利用变量节点间估计与观测值，建立因子图的因子节点，并通过非线性优化求解因子图的最大后验估计，得到水下机器人自身位置和安装偏差的估计；

步骤6：获取安装偏差信息后，将安装偏差信息分别反馈至多普勒测速仪模块和姿态传感器模块；

步骤7：根据安装偏差估计的变化情况，判断安装偏差估计是否完成：当安装偏差估计的变化量小于期望阈值时，即可判定估计过程成，结束任务，否则回到步骤1。

本发明还包括：

1.图优化估计模块利用航位推算模块的位置信息与卫星定位模块的位置信息，采用因子图优化方法，建立非线性优化目标，完成传感器安装偏差的最优估计。

2.多普勒测速仪模块利用声波多普勒效应，利用4阵元实现在典型海底环境下速度信息的测量。

3.姿态传感器模块利用惯性测量单元，包括光纤陀螺与加速度计，完成水下机器人姿态信息的测量。

4.卫星定位模块利用卫星定位系统完成水下机器人在水面航行状态下的位置测量。