对于Unified
Motion-Based Calibration of Mobile Multi-Sensor Platforms with Time Delay
Estimation的标定方案分析总结
方案标定流程:
- 优化理论
需要优化的参数
XR是机器人平台内参,包括左右轮周长与轮式里程计编码器刻度之比kl与kr和两轮间距b。
Xs是各个带观测的传感器与轮式里程计之间的外参,dtN是各个传感器与轮式里程计的时延。
优化思想是首先利用其他传感器的数据来对机器人进行航迹推理得到机器人当前预估位姿(公式2),优化机器人预估位姿与实际位姿之间的差,来优化其他参数。
为此转化为一个最大近似然的问题来优化(公式3,4)。
迭代优化时需要deltax,其值应该最小化公式5
然后迭代的代价函数满足
H与b矩阵设置如下:
- 优化时具体设置
优化的残差应包含两个部分,一是其他传感器航迹推理轨迹与机器人实际位姿残差,二是其他传感器航迹推理轨迹间的残差。
其他传感器航迹推理轨迹与机器人实际位姿残差优化
得到其他传感器某时刻数据变化
利用轮式里程计内参和其他传感器数据航迹推理机器人位姿变化。
换算到机器人坐标系下并计算与机器人本体位姿残差。
对应雅克比:
其他传感器航迹推理轨迹间的残差
将第i与j个传感器计算公式9的数据做残差。
雅克比设置同上。
- 关于输入数据的筛选。
对传感器一段时间内输入数据以及优化参数计算公式16的值如果小于一定阈值就抛弃这段轨迹的传感器数据加入优化。
- 方案评估
优点:有开源代码,有时延标定,有轮式里程计内参标定,会考虑筛选参与标定的数据。
缺点:主要是来标定轮式里程计与带观测的传感器之间外参(相机与激光),需要初值优化,不能得到封闭解。