matlab tsai手眼标定程序代码_标定系列一 | 机器人手眼标定的基础理论分析

旷视MegMaster机器人系列是旷视自主研发的一系列AI智能机器人硬件设备，基于旷视全球领先的人工智能算法及机器人技术，可实现搬运、分拣、托举、存储等功能，被广泛应用于物流仓储、工厂制造等场景。旷视SLAM组主要负责多传感器建图、定位、标定等工作，在开发过程中总结了一些理论和实践方面的经验教训，在此分享出来，希望能够与大家一起探讨，为机器人行业贡献自己的绵薄之力。

近期我们会首先推出机器人多传感器标定的系列内容，包括:

标定系列一 | 基础理论分析
标定系列二 | 实践之Camera-Odometry标定
标定系列三 | 实践之Camera-Lidar标定

本文作者是旷视研究院研究员卢彦斌，在文中为大家主要介绍了机器人手眼标定的一些理论基础，以及轮式机器人标定中的一些常见做法，希望能够给大家一些启发。

一、手眼标定的理论基础

1.1

手眼标定（hand eye calibration）是机器人领域中一个历史悠久的问题。这里的手常指的是机械臂，眼常指的是安装于机械臂上的相机或者固定于环境中的相机。如下图所示。

在实际应用中，我们通常需要将相机观察到的外界环境中物体的姿态从相机坐标系转换到机械臂的坐标系中，辅助机械臂规划一些后续动作（如抓取）。为了得到两坐标系之间的转换矩阵，我们就要对机器人进行手眼标定。手眼标定有两种常见的标定方法。第一种是通过特殊的标定物，第二种是通过机器人运动建立约束。本文主要讨论后一种方法。

假设k时刻，机械臂相对于世界坐标系的位姿为

，相机相对于世界坐标系的位姿为

，相机相对于机械臂的位姿 X，那么有

同样地，对k+1时刻有

那么

令

，上式即是：

其中，A和B表示相邻时刻机械臂和相机的局部运动。机械臂的局部运动可以通过其运动模型推算得到，相机的局部运动通常可通过外部的标定板辅助得到。由此，手眼标定问题转化为求解

的问题。这里，我们称k时刻与k+1时刻构成的

的方程为一个测量。

1.2 约束条件分析

从

方程出发，我们分别考虑旋转矩阵R和平移向量t需要的方程数量（即测量数量）。假设：

那么

可以转化为：

1.2.1 旋转矩阵所需的方程数量

本小节主要参考文献[1]。由上述等式条件可得：

首先回顾一下旋转矩阵的性质，

旋转矩阵有且只有一个实数特征值1；
不考虑单位矩阵，旋转矩阵的实数特征值对应的特征向量即是axis-angle表达式的旋转轴，旋转矩阵的迹与旋转角度相关。

axis-angle表达式示意图如下所示[6]。

旋转矩阵和axis-angle表达式的关系可以由Rodrigues公式表示。

其中

分别为旋转矩阵的旋转轴和旋转角度，

，定义如下

反对称矩阵有如下性质：

旋转矩阵的迹与旋转角度的关系为:

假设

分别为

的旋转向量，

分别为

的旋转角度，那么有：

即

所以，

是

的实数特征值对应的特征向量，

上式说明，

的旋转轴经过

变换为

的旋转轴。实际上，我们能从上式得到如下关系:

证明见附录A。

如果令

，上式等价于

上述方程有一个显然的特解

由于

秩为2，其一维零空间的基为

。因此

的解为

示意图如下图所示。

由

的定义可知，每个

对应于一个

。因此，从一个

方程无法求解得到

，有一个自由度求解不出来，且该自由度会同时影响

。

下面考虑两组组测量

。关于

的方程变为

有唯一解当且仅当左端方程满秩。文献[1]中证明，这等价于

。

因此如果我们有两组测量，且传感器A（等价于B）在两组测量中的（局部坐标系下）旋转矩阵的旋转轴不同，那么能够求解得到

。从而，根据

从Rodrigues公式可以求得

。

1.2.2 平移向量的约束数量

在已知旋转矩阵的情况下，平移向量的约束推导情况较为简单。从一个方程

中，我们可以得到

同样，因为

有非零解

不满秩，一个测量无法求解出

。当我们有两组测量时，文献[1]中证明，

满秩等价于

。因此，如果我们有两组测量，且传感器A（等价于B）在两组测量中的旋转矩阵的旋转轴不同，那么能够通过求解方程组求解得到

。

二、基于平面运动的外参标定方法

机器人上不同传感器之间的外参可以用手眼标定的方式来标定，这一般需要机器人在三维空间运动。对于一大类机器人而言，其通常被设计成在平面上运动。这种情况下，传感器实际上只能绕一个轴（即平面法向量）旋转，其在任何局部坐标系下的旋转运动的旋转轴均保持不变。此时，我们能够求解出哪些参数呢？

假设世界坐标系的XY平面即为机器人的运动平面，Z轴为平面法向量。直观上来说，两个传感器的安装高度差对于局部旋转和平面上的平移没有影响，因此两个传感器各自坐标系的原点在世界坐标系下的z轴高度差是无法观测到的。实际上，我们从

的方程能够看出这一点。首先，由第一部分我们知道，此时旋转轴

在所有的局部坐标系下是一样的。其次，我们可以证明

满足所有(A, B)的方程。

而

，

即是世界坐标系z轴在两个传感器局部坐标系下的表达式。

针对平面运动下的手眼标定问题，我们列举一些典型做法作为参考。

2.1 传感器的旋转轴与平面法向量重合

以带轮速计的机器人为例，通常可以假设轮速计的旋转轴和平面的法向量重合。我们能够证明，除了平移向量的z分量，其余5个参数都是可观的。

以参考文献[2]为例。作者的具体做法如下。

第一步，通过将

分解为欧拉角的ZYZ表示简化旋转矩阵参数的求解。假设

那么旋转矩阵对应的方程变为

即

已知

的情况下，我们可以同时求出

（注：如果轮速计内参未知，那么上述公式可用于提供轮速计的一个约束）。于是

变为只有最后一个Z旋转未知的旋转矩阵，可代入平移向量的方程中求解。文献[3]是类似的思路，同样都是利用A传感器的旋转轴是世界坐标系的z轴的性质，先解出旋转矩阵的两个参数，再利用平移向量的约束解出其余参数且有一个参数不可观。具体做法会由系列后续文章详细说明。

2.2 引入其它约束

我们可以引入外界约束以求解所有参数，例如，2D激光和相机的外参标定通常需要6自由度参数。文献[4]利用V型标定板和棋盘格建立点到面的约束。文献[5]则利用互相垂直的三面体构建直线到平面和点到平面的约束。这部分与本文重点关系较小，这里就不分析了。

参考文献

Tsai, Roger Y., and Reimar K. Lenz. "A new technique for fully autonomous and efficient 3D robotics hand/eye calibration." IEEE Transactions on robotics and automation 5.3 (1989): 345-358.

2. Antonelli, Gianluca, et al. "Simultaneous calibration of odometry and camera for a differential drive mobile robot." IEEE International Conference on Robotics and Automation(2010).

3. Guo, Chao X., Faraz M. Mirzaei, and Stergios I. Roumeliotis. "An analytical least-squares solution to the odometer-camera extrinsic calibration problem." 2012 IEEE International Conference on Robotics and Automation. IEEE, 2012.

4. Dong, Wenbo, and Volkan Isler. "A novel method for the extrinsic calibration of a 2D laser rangefinder and a camera." IEEE Sensors Journal 18.10 (2018): 4200-4211.

5. Gomez-Ojeda, Ruben, et al. "Extrinsic calibration of a 2D laser-rangefinder and a camera based on scene corners." 2015 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2015.

6. http://www.opengl-tutorial.org/intermediate-tutorials/tutorial-17-quaternions/