slam 中的手眼标定最后可以归结为求解如下问题
已知 ,需要求解 ,满足
其中即为旋转矩阵,网上有多种求解方法,这里只描述其中几种,首先介绍下一些基本的变换
1、基本变换
hat运算符
把三维实向量 映射到 反对称阵
性质1:,直接推导即可证明
性质2:,直接推导即可证明
性质3:,直接推导即可证明
性质4:,证明如下
vee运算符
把 反对称阵 映射到三维实向量
指数映射
把 反对称阵 映射到旋转矩阵
性质1:假设 为向量模长,即
这也正是罗德里格斯公式,可以从旋转几何出发进行推导
性质2:,证明如下
对数映射
把旋转矩阵 映射到 反对称阵
其中
2、Navy手眼标定算法
论文 "Robot sensor calibration: solving AX=XB on the Euclidean group"
考虑到 均为旋转矩阵,因此存在三维实向量 使得
利用前面的性质可以得到
因此求解 等价于求解 因此原问题将会变为如下问题
如果定义距离 为欧式距离,则该问题将会变成经典问题
具体推导在另一篇文章《点云精配准》求解旋转矩阵 中有详细描述,这里直接给出求解步骤:
- 找到一系列旋转矩阵
- 计算对应的三维向量
- 对协方差矩阵进行SVD分解,其中协方差矩阵为
- 求解得到旋转矩阵
带有初值的近似解
如果有一个不错的初值,问题变成需要求解得到一个 使得
注意到 ,因此 可以用欧拉角表示,同时注意到 是一个不错的初值,因此有 ,进而 可以近似表示成
带入前式可以得到
因此原问题变成
将个方程可以写成矩阵形式
此问题为经典 问题,其解为
其中,为伪逆
误差分析
对于优化方程,其中为需要求解的变量,为观测得到的变量,如果通过求导来求得最优解,那么有,该方程通过计算可以得到,假设均在真值附近,将函数在真值处进行泰勒展开可以得到
如果观测量相互之间独立,则有
根据隐函数定理
由此可以得到最优解 的协方差矩阵为
当前
采用右扰动,推导得到一阶导
继续推导得到二阶导
采用左扰动,推导得到一阶导
继续推导得到二阶导
因此有