摘要
本发明提出了一种用于水下机器人定位的多普勒速度计残差构建方法。属于水下机器人定位领域。包括以下步骤：首先通过DVL(多普勒速度计)测量水下机器人的三维速度；其次通过DVL与IMU(惯性测量单元)之间的外参将DVL测量数据的参考坐标系转换到IMU坐标系；再次通过DVL和IMU传感器测量噪声参数以及DVL与IMU之间的外参，计算DVL因子协方差矩阵；然后通过系统状态估计量、DVL与IMU之间的外参和DVL及IMU测量数据计算DVL残差；最后通过系统状态估计量和DVL与IMU之间的外参计算DVL残差关于系统状态估计量的雅可比矩阵。本发明提出的一种新的适用于非线性优化的DVL残差构建方法精确地利用DVL测量数据，使基于该DVL残差构建方法的水下SLAM算法在定位精度和鲁棒性上较其他水下定位算法有大幅提升。
