对于Automatic Multi-Sensor Extrinsic Calibration for Mobile Robots的标定方案分析总结

对于Automatic
Multi-Sensor Extrinsic Calibration for Mobile Robots的标定方案分析总结

方案标定流程：

1. 标定与地面的z，pitch，roll

各个传感器运动与地面只相差z方向偏差，roll，pitch角，即公式12，所以可以根据公式13把每个传感器数据点转化到地面坐标系上，即公式13左边的值。

转化后这些点与地面的距离应该为0，所以根据这个距离做优化问题即公式14和15：

之后优化求出这些与地面相差的外参z，pitch，roll后，根据公式13和公式16求出各个传感器3维运动投影到地面上的2维运动，即公式16前两维。

2. 标定地面投影中各个传感器之间的2d外参(x,y,yaw)

需要标定的外参，因为图片的位姿数据文章中直接来自orbslam，所以还需要标定尺度即xs，

应用的两个李群的增量变化公式

第i个传感器第k个时刻和第k+1个时刻之间的位移增量表示如公式5，右边是第i个传感器第k个时刻投影位姿和第k+1个时刻的投影位姿。(这时已经变化到地平面上)

(5)

两个传感器i，j在第k个时刻的外参变换满足公式6

结合5和6得到公式7

然后变化到公式8，得到两个传感器之间某个时刻的增量位置变换与外参关系，并以此建立优化方程求解

上述优化在3的算法细节1中可以只使用数个观测数据求解为此加入ransac算法，从传感器所有观测中选出需要的若干个最合理的数据进行优化求解，ransac设置的去除外点公式38，设置的值是第k个时刻第i传感器与第j传感器位移偏差的平移部分。即公式11的平移部分值。

通过以上优化，可以得到多个传感器与第一个参考传感器的外参，然后建立新的优化方程利用g2o统一优化所有传感器的2d外参。39右边第一部分是各个传感器在第k个时刻与首个传感器位移差值的平移部分，第二部分是各个传感器对在第k个时刻与另一个传感器位移差值的平移部分，然后把这些全部加起来作为优化方程。

3. 解算方法（拉格朗日数乘）

1.对标定地面投影中各个传感器之间的2d外参(x,y,yaw)的解算

将公式11变为

将求解量参数化表示：

重写代价函数如以下形式：

从而变为以下的求解并满足相应条件，此为拉格朗日数乘解的问题，

将23的后半部分写成以下形式

从而变为拉格朗日数乘形式如下25

26就是优化的求解等式。

所以要求

从而得到

，是的特征向量，因为尺度是正数，所以根据1的值来选择合适的值，然后根据公式29转化为所要求解的外参

2.对标定与地面的z，pitch，roll的解算

首先规定地平面参数满足一下条件

从而把与地平面的垂直距离转化为以下形式

同理把需要标定的数据用参数化表示

然后误差函数和代价函数被转化为以下形式

优化问题求解变化为以下求解

然后根据36后半段令

从而和第一步一样转化为

的拉格朗日数乘问题，采取同样方式求解出参数然后转换得到与地平面的外参。

4. 方案评估

优点：有开源代码，各个部分代码方便分开测试，把所有传感器数据转化为统一形式标定，对于外参求解是用若干数据求封闭解，所以可以用ransac的方式去除一些不合理的传感器数据或者其他合适的方式人为选择合适的数据参与求解。

缺点：所有传感器数据都是转化为位姿数据，所以对于只能输出2d平面位姿的传感器比如(轮式里程计)z轴高度没法标定，所有传感器外参要初步标定对齐之后然后转化对应输出位姿，而且这个位姿数据必须足够准确。imu如果要参与标定，应该要和相机一起跑vins得到位姿，要不然就采取其他方式单独标定imu与其他传感器外参。