手眼标定AX=XB

09 Dec 2021

VINS系统中使用了相机来获取图像信息，使用IMU来“获得”精确的尺度，那么当搭载 VINS 系统的设备（无人车、无人机等）运动的时候，相机中连续的每一帧图像之间通过匹配的特征点利用九点法可以计算出基础矩阵，进而通过svd分解可以得到旋转矩阵序列：

\[R_{c_{1}}^{c_{0}}, R_{c_{2}}^{c_{1}}, R_{c_{3}}^{c_{2}} \ldots . R_{c_{k+1}}^{c_{k}} \ldots \ldots\]

相应的IMU通过和图像对齐之后的预积分可以得到旋转矩阵序列：

\[R_{b_{1}}^{b_{0}}, R_{b_{2},}^{b_{1}} R_{b_{3} \cdots \cdots}^{b_{2}} \ldots R_{b_{k+1} \ldots \ldots}^{b_{k}}\]

相机与IMU之间的外参标定，就是计算相机与IMU之间的相对旋转将上边的两个旋转序列对应起来，由于搭载相机和IMU的设备（无人车或者无人机）是刚体，所以计算出来的旋转是个常数。当计算出相对旋转后，就可以把相机和IMU之间的运动完美对应起来。下图是港科大沈老师讲座中提到的相机和IMU之间旋转标定的图片。图中绿色的方块和绿色虚线表示IMU的运动旋转轨迹，蓝色图标和蓝色虚线表示相机的运动旋转轨迹。通过下面两张图片，可以很清楚的理解相机与IMU之间旋转的标定。

当计算出相机相对于IMU的旋转后，对相机的运动轨迹按照其相对于IMU的旋转进行旋转调整后，相机和IMU的运动轨迹就可以完美匹配在一起，如下图所示：

而实际上，设相机与IMU之间的旋转为 $R_c^b$，那么，以下公式成立：

\[R_{b_{k+1}}^{b_{k}} \cdot R_{c}^{b}=R_{c}^{b} \cdot R_{c_{k+1}}^{c_{k}}\]

求解外参其实就是手眼标定的问题，这个公式也是手眼标定当中著名的 $AX=XB$ 方程。

手眼标定法的核心公式只有一个，$AX=XB$，这里的 $X$ 就是指手（机械臂末端）与眼（摄像头）之间的坐标转换关系。下面结合机械臂的两种使用场景，讲一下这个公式的由来。

用Base表示机械臂的底座（可以认为是世界坐标系），用End表示机械臂的末端，用Camera表示摄像头，用Object表示标定板。

所谓Eye-In-Hand，是指摄像头被安装在机械臂上。此时要求取的是，End到Camera之间的坐标转换关系，也就是$X=T_{E \rightarrow C}$。这种情况下，有两个变量是不变的：

1. 摄像头与机械臂末端之间的坐标转换关系不变，也就是说， $T_{E \rightarrow C}$ 始终不变；
2. 标定板与机械臂底座之间的坐标转换关系不变，也就是说， $T_{B \rightarrow O}$ 也是不变的。

把 $T_{B \rightarrow O}$ 按照前后两次运动展开，有

\[\begin{aligned} T_{B \rightarrow E_{1}} T_{E_{1} \rightarrow C_{1}} T_{C_{1} \rightarrow O} &=T_{B \rightarrow E_{2}} T_{E_{2} \rightarrow C_{2}} T_{C_{2} \rightarrow O} \\ \left(T_{B \rightarrow E_{2}}\right)^{-1} T_{B \rightarrow E_{1}} T_{E_{1} \rightarrow C_{1}} T_{C_{1} \rightarrow O} &=T_{E_{2} \rightarrow C_{2}} T_{C_{2} \rightarrow O} \\ \left(T_{B \rightarrow E_{2}}\right)^{-1} T_{B \rightarrow E_{1}} T_{E_{1} \rightarrow C_{1}} &=T_{E_{2} \rightarrow C_{2}} T_{C_{2} \rightarrow O}\left(T_{C_{1} \rightarrow O}\right)^{-1} \\ T_{E_{2} \rightarrow E_{1}} T_{E_{1} \rightarrow C_{1}} &=T_{E_{2} \rightarrow C_{2}} T_{C_{2} \rightarrow C_{1}} \end{aligned}\]

记

\[\begin{array}{l} A=T_{E_{2} \rightarrow E_{1}}=\left(T_{B \rightarrow E_{2}}\right)^{-1} T_{B \rightarrow E_{1}} \\ B=T_{C_{2} \rightarrow C_{1}}=T_{C_{2} \rightarrow O}\left(T_{C_{1} \rightarrow O}\right)^{-1} \\ X=T_{E \rightarrow C}=T_{E_{1} \rightarrow C_{1}}=T_{E_{2} \rightarrow C_{2}} \end{array}\]

就得到了手眼标定方程。也就是，如果能够计算移动前后，机械臂末端的坐标变换关系 $A$ 以及相机的坐标变换关系 $B$ ，即可求出机械臂末端到相机之间的坐标变换关系 $X$ 。

沿用 VINS 论文中的公式，使用四元数表达旋转，手眼标定方程如下：

\[q_{b_{k+1}}^{b_{k}} \otimes q_{c}^{b}=q_{c}^{b} \otimes q_{c_{k+1}}^{c_{k}}\]

将其写成左乘和右乘的形式为：

\[\left[Q_{1}\left(q_{b k+1}^{b_{k}}\right)-Q_{2}\left(q_{c k+1}^{c_{k}}\right)\right] q_{c}^{b}=Q_{k+1}^{k} q_{c}^{b}=0\]

其中

\[\begin{array}{l} Q_{1}(q)=\left[\begin{array}{cc} q_{w} I_{3}+\left\lfloor q_{x y z} \right\rfloor_\times & q_{x y z} \\ -q_{x y z} & q_{w} \end{array}\right] \\ Q_{2}(q)=\left[\begin{array}{cc} q_{w} I_{3}-\left\lfloor q_{x y z} \right\rfloor_\times & q_{x y z} \\ -q_{x y z} & q_{w} \end{array}\right] \end{array}\]

Q1 和 Q2 是四元数左乘和右乘的矩阵形式，注意这个地方因为四元数实部在前在后的表达不一样，而左右乘的形式也不一样，VINS 中使用的是 Eigen 实部在后的表达方式。

对于 n 对测量值，则有：

\[\left[\begin{array}{c} w_{1}^{0} Q_{1}^{0} \\ w_{2}^{1} Q_{2}^{1} \\ \vdots \\ w_{N}^{N-1} Q_{N}^{N-1} \end{array}\right] q_{c}^{b}=Q_{N} q_{c}^{b}=0\]

其中 $w_{N}^{N-1}$ 为外点剔除权重，其与相对旋转求取得残差有关，$N$ 为最新的视觉帧的index，其由最终的终止条件决定。残差可以写为

\[r_{k+1}^{k}=\operatorname{acos}\left(\left(\operatorname{tr}\left(\hat{R}_{c}^{b^{-1}} R_{b_{k+1}}^{b_{k}^{-1}} \hat{R}_{c}^{b} R_{c_{k+1}}^{c_{k}}\right)-1\right) / 2\right)\]