从理论到代码：手把手教你用Eigen库搞定机器人手眼标定中的AX=XB问题
从理论到代码：手把手教你用Eigen库搞定机器人手眼标定中的AX=XB问题
在机器人视觉系统中，手眼标定是一个至关重要的环节。想象一下，当机械臂需要精准抓取物体时，它必须知道"眼睛"（通常是摄像头）和"手"（末端执行器）之间的精确关系。这就是手眼标定要解决的核心问题——确定相机坐标系与机械臂坐标系之间的变换关系。
AX=XB方程是手眼标定中的经典数学模型，其中X就是我们要求解的变换矩阵。理解这个方程背后的数学原理，并能够用代码实现它，对于机器人开发者来说是一项必备技能。本文将带你从基础理论出发，逐步推导方程，最后用Eigen库实现完整的求解过程。
1. 坐标系变换与AX=XB方程的由来
1.1 机器人学中的坐标系变换基础
在机器人学中，我们常用4×4的齐次变换矩阵来表示坐标系之间的变换关系。一个典型的变换矩阵可以表示为：
T = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix}其中R是3×3的旋转矩阵，t是3×1的平移向量。这种表示方法既包含了旋转信息，也包含了平移信息，非常适合描述刚体运动。
为什么需要齐次坐标？齐次坐标让我们能够用矩阵乘法统一表示旋转和平移操作，这在计算机视觉和机器人学中非常方便。
1.2 AX=XB方程的推导
假设我们有两个坐标系变换：
- A：机械臂末端执行器相对于基座的变换
- B：标定板相对于相机的变换
- X：相机相对于末端执行器的变换（即我们需要求解的）
通过观察不同姿态下的A和B，我们可以建立方程AX=XB。这个方程的含义是：无论机械臂如何运动，通过X转换后的坐标系关系应该保持一致。
推导过程：
- 在姿态1时：A₁X = XB₁
- 在姿态2时：A₂X = XB₂
- 通过变换可以得到：A₂⁻¹A₁X = XB₂⁻¹B₁
这个方程看似简单，但由于矩阵乘法的不可交换性，求解起来并不容易。
2. 数学求解方法剖析
2.1 方程拆解：旋转部分与平移部分
我们可以将AX=XB拆分为旋转和平移两部分：
旋转部分：
R_A R_X = R_X R_B平移部分：
R_A t_X + t_A = R_X t_B + t_X这种拆分让我们可以分别处理旋转和平移问题，简化求解过程。
2.2 常用求解方法比较
| 方法 | 原理 | 优点 | 缺点 |
|---|---|---|---|
| 闭式解法 | 使用矩阵分解直接求解 | 计算速度快 | 对噪声敏感 |
| 优化方法 | 最小化误差函数 | 鲁棒性强 | 计算量大 |
| SVD分解 | 奇异值分解求最小二乘解 | 数值稳定 | 需要多组数据 |
在实际应用中，我们通常会结合多种方法来获得更稳定的解。下面重点介绍基于SVD的求解方法。
3. 使用Eigen库实现求解
3.1 Eigen库简介与环境配置
Eigen是一个高性能的C++模板库，用于线性代数运算。它提供了丰富的矩阵操作接口，非常适合实现机器人学中的各种算法。
安装Eigen非常简单：
# Ubuntu系统 sudo apt-get install libeigen3-dev在CMake项目中引用：
find_package(Eigen3 REQUIRED) target_link_libraries(your_target Eigen3::Eigen)3.2 单组数据的求解实现
我们先看一个基础的实现，使用单组数据求解AX=XB：
#include <Eigen/Dense> #include <iostream> void solveAXXB_SinglePair(const Eigen::Matrix4d& A, const Eigen::Matrix4d& B, Eigen::Matrix4d& X) { // 分离旋转和平移部分 Eigen::Matrix3d RA = A.block<3,3>(0,0); Eigen::Vector3d tA = A.block<3,1>(0,3); Eigen::Matrix3d RB = B.block<3,3>(0,0); Eigen::Vector3d tB = B.block<3,1>(0,3); // 求解旋转部分 RX Eigen::Matrix3d I = Eigen::Matrix3d::Identity(); Eigen::MatrixXd K(9,9); K = Eigen::KroneckerProduct(RB.transpose(), RA) - Eigen::KroneckerProduct(I, I); Eigen::JacobiSVD<Eigen::MatrixXd> svd(K, Eigen::ComputeThinU | Eigen::ComputeThinV); Eigen::VectorXd solution = svd.matrixV().col(8); Eigen::Matrix3d RX = Eigen::Map<Eigen::Matrix3d>(solution.data()).transpose(); // 求解平移部分 tX Eigen::Matrix3d M = RA - Eigen::Matrix3d::Identity(); Eigen::Vector3d b = RX * tB - tA; Eigen::Vector3d tX = M.colPivHouseholderQr().solve(b); // 组合结果 X.setIdentity(); X.block<3,3>(0,0) = RX; X.block<3,1>(0,3) = tX; }注意：单组数据求解通常不够稳定，实际应用中建议使用多组数据。
3.3 多组数据的鲁棒求解
为了提高求解精度，我们需要收集多组(A,B)数据对。下面是改进后的多组数据求解实现：
#include <vector> #include <Eigen/Dense> void solveAXXB_MultiPairs(const std::vector<Eigen::Matrix4d>& A_list, const std::vector<Eigen::Matrix4d>& B_list, Eigen::Matrix4d& X) { assert(A_list.size() == B_list.size()); assert(A_list.size() >= 2); // 至少需要两组数据 // 构建旋转部分的求解系统 Eigen::MatrixXd M(3*A_list.size(), 3); Eigen::VectorXd b(3*A_list.size()); for(size_t i = 0; i < A_list.size(); ++i) { Eigen::Matrix3d RA = A_list[i].block<3,3>(0,0); Eigen::Vector3d tA = A_list[i].block<3,1>(0,3); Eigen::Matrix3d RB = B_list[i].block<3,3>(0,0); Eigen::Vector3d tB = B_list[i].block<3,1>(0,3); // 旋转部分 M.block<3,3>(3*i,0) = RA - Eigen::Matrix3d::Identity(); b.segment<3>(3*i) = RB.transpose() * tB - tA; } // 使用SVD求解最小二乘问题 Eigen::JacobiSVD<Eigen::MatrixXd> svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV); Eigen::Vector3d tX = svd.solve(b); // 构建平移部分的求解系统 Eigen::MatrixXd K(9*A_list.size(), 9); Eigen::VectorXd zeros = Eigen::VectorXd::Zero(9*A_list.size()); for(size_t i = 0; i < A_list.size(); ++i) { Eigen::Matrix3d RA = A_list[i].block<3,3>(0,0); Eigen::Matrix3d RB = B_list[i].block<3,3>(0,0); K.block<9,9>(9*i,0) = Eigen::KroneckerProduct(RB, RA) - Eigen::KroneckerProduct(Eigen::Matrix3d::Identity(), Eigen::Matrix3d::Identity()); } Eigen::JacobiSVD<Eigen::MatrixXd> svd2(K, Eigen::ComputeThinU | Eigen::ComputeThinV); Eigen::VectorXd solution = svd2.matrixV().col(8); Eigen::Matrix3d RX = Eigen::Map<Eigen::Matrix3d>(solution.data()).transpose(); // 组合结果 X.setIdentity(); X.block<3,3>(0,0) = RX; X.block<3,1>(0,3) = tX; }4. 实际应用中的注意事项
4.1 数据采集技巧
为了提高标定精度，数据采集时应注意：
- 确保机械臂运动覆盖足够大的工作空间
- 避免纯旋转或纯平移运动
- 每组姿态之间应有足够的差异
- 通常需要10-20组数据才能获得稳定解
4.2 结果验证与误差分析
求解完成后，我们需要验证结果的准确性。一个简单的方法是计算重投影误差：
double computeReprojectionError(const std::vector<Eigen::Matrix4d>& A_list, const std::vector<Eigen::Matrix4d>& B_list, const Eigen::Matrix4d& X) { double total_error = 0.0; for(size_t i = 0; i < A_list.size(); ++i) { Eigen::Matrix4d AX = A_list[i] * X; Eigen::Matrix4d XB = X * B_list[i]; total_error += (AX - XB).norm(); } return total_error / A_list.size(); }4.3 性能优化技巧
- 并行计算：Eigen支持OpenMP，可以加速矩阵运算
- 内存预分配：提前分配好大矩阵所需内存
- 矩阵分解重用：对于需要多次求解的问题，可以重用分解结果
// 示例：使用Eigen的并行计算 Eigen::setNbThreads(4); // 使用4个线程5. 进阶话题与扩展
5.1 处理噪声与异常值
实际数据中难免会有噪声和异常值，我们可以采用以下方法提高鲁棒性：
- RANSAC算法剔除异常值
- 使用Huber损失函数代替平方误差
- 增加正则化项防止过拟合
5.2 与其他标定方法的比较
除了AX=XB方法，手眼标定还有其他几种常见方法：
| 方法 | 适用场景 | 优缺点 |
|---|---|---|
| AX=XB | 眼在手外(eye-to-hand) | 数学简洁，实现简单 |
| AX=ZB | 眼在手上(eye-in-hand) | 需要考虑更多坐标系 |
| 双阶段法 | 高精度要求 | 更复杂但更精确 |
5.3 实时标定与在线更新
对于需要长时间运行的机器人系统，可以考虑实现标定参数的在线更新：
class OnlineHandEyeCalibrator { public: void addMeasurement(const Eigen::Matrix4d& A, const Eigen::Matrix4d& B) { // 增量更新求解 } Eigen::Matrix4d getCurrentX() const { return current_X_; } private: Eigen::Matrix4d current_X_; // 其他状态变量... };在实际机器人项目中，手眼标定的质量直接影响整个系统的精度。记得第一次实现这个算法时，我花了整整两天时间调试才发现问题出在数据采集环节——机械臂的运动范围太小，导致标定结果不稳定。这个经验告诉我，理论正确只是第一步，实际应用中细节决定成败。