传感器：声纳+惯性+视觉
提出一种实时SLAM算法，融合视觉，惯性，距离信息。
重点: 怎样融合声呐信息.
系统框图:
以OKIVS为框架, 融合声纳信息.
硬件配置:
- IDS UI-3251LE cameras *2
15帧/s, 1600x1200 - IMAGENEX 831L Sonar
最大探测距离6m; 角分辨率: 0.9°; 完整扫描6m范围耗时6s - Microstrain 3DM-GX4-15 IMU
100Hz - Bluerobotics Bar30 pressure sensor
15Hz - Intel NUC
系统概览
通过最小化重投影误差，IMU误差和声纳距离误差的联合估计来估计机器人R的状态XR。
-
状态向量:
非线性优化成本函数:
本文系统中,声呐测量用来校正机器人位姿估计.
计算距离误差的原理是：如果声纳在某个距离处检测到任何障碍物，则视觉特征更有可能位于该障碍物的表面上。即距离近似相等。
因此，误差项是两个距离之间的差。
估计误差项以与IMU和立体重投影误差类似的方式添加到非线性优化框架中.