【文献阅读】Probabilistic Terrain Mapping for Mobile Robots With Uncertain Localization

“Probabilistic Terrain Mapping for Mobile Robots With Uncertain Localization”是ETH的四足机器人组的一个研究子项目，其动机是研究如何用机器人摄像机逐帧相对运动和3D点云观测对机器人的局部地面地形进行重构建模，用来做局部规划和四足里面的足迹落点控制。该文发在2018 RA-L上，有ROS代码。

1. 整体框架

该文在他们先前工作“ROBOT-CENTRIC ELEVATION MAPPING WITH UNCERTAINTY ESTIMATES”上改进得到，局部地形评估有一个经典工作是2007年的“Real-time localization and elevation mapping within urban search and rescue scenarios”，发表在著名应用期刊Journal of Field Robotics上，写的很好。
该文主要框架是
一共三个模块，一是把观测信息融合到栅格地图，二是把机器人移动融合到栅格地图，即是把位姿估计不确定性体现到栅格地图里。最后是地图融合模块，这个模块的存在是因为第一二步处理的栅格地图是自定义的概率栅格地图，每个点（x,y）包含各自期望和方差，这一步把期望，置信上下界确定出来，导出我们常见的二维栅格地图（类似costmap）。

2. 符号与坐标系定义

如上定义4个坐标系$\mathbf{I},\mathbf{B},\mathbf{S},\mathbf{M}$，惯性系$\mathbf{I}$固定在真实世界地形上，机器人base系$\mathbf{B}$和传感器系$\mathbf{S}$固定连接。$\mathbf{I}$和$\mathbf{B}$之间关系有姿态估计确定：平移$r_{IB}$和旋转${\Phi }_{IB}$，并由6维协方差阵表示不确定性：

为了后面好处理，这里的旋转拆分为两部分：

前者包含yaw变换，后面包含pitch和roll。
最后看地图坐标系$\mathbf{M}$。${\Phi }_{BM}$规定为：yaw方向随着$\mathbf{B}$转，pitch，roll方向和$\mathbf{I}$保持一致。而 r B M r_{BM} rBM</