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

权利要求书

1.一种水下机器人传感器安装偏差估计方法，其特征在于，包括以下步骤：

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

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

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

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

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

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

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

2.根据权利要求1所述的一种水下机器人传感器安装偏差估计方法，其特征在于：所述多普勒测速仪模块利用声波多普勒效应，利用4阵元实现在典型海底环境下速度信息的测量。

3.根据权利要求1所述的一种水下机器人传感器安装偏差估计方法，其特征在于：姿态传感器模块利用惯性测量单元，包括光纤陀螺与加速度计，完成水下机器人姿态信息的测量。

4.根据权利要求1所述的一种水下机器人传感器安装偏差估计方法，其特征在于：卫星定位模块利用卫星定位系统完成水下机器人在水面航行状态下的位置测量。