4. 数学基础（下）

好，咱们接着聊。上一章我们把旋转矩阵、四元数这些数学工具捋了一遍。这一章，我们要解决一个非常实际的问题——手眼标定。

说白了，就是搞清楚激光雷达和IMU之间到底是怎么固定的。你想想看，激光雷达扫出来的点云，IMU测出来的加速度和角速度，它们各自在自己的坐标系里工作。但机器人要融合这些数据，就必须知道这两个坐标系之间的相对位姿。这个求解过程，就是手眼标定。

4.1 手眼标定问题：AX = XB

我第一次接触手眼标定，是在做AGV（自动导引车）项目的时候。当时装了一个2D激光雷达和一个低成本的IMU，结果发现融合后的定位精度总是不对。折腾了两天，最后发现是外参标定出了问题。嗯，从那以后，我再也不敢小看这一步了。

手眼标定这个名字，其实来源于机器人学里的“手”和“眼”。在机器人臂上，相机是“眼”，机械臂末端是“手”。我们想知道相机和机械臂末端的相对位姿。在咱们的激光雷达+IMU系统里，激光雷达相当于“眼”，IMU相当于“手”（或者反过来，看你怎么定义）。

核心方程就是：AX = XB

这个方程怎么来的？我来给你拆解一下。

• A：代表激光雷达（或相机）在两个不同时刻之间的相对运动。比如，t1时刻到t2时刻，激光雷达自己动了多少？这个可以通过点云配准（比如ICP）算出来。
• B：代表IMU在两个相同时刻之间的相对运动。这个IMU自己就能算出来，通过积分角速度和加速度。
• X：就是我们要求的外参，即激光雷达坐标系到IMU坐标系的变换矩阵。

为什么是AX = XB？

想象一下，从t1到t2，激光雷达的运动是A，IMU的运动是B。如果我们把激光雷达的位姿通过外参X变换到IMU坐标系下，那么激光雷达的运动A，经过X变换后，应该等于IMU自身的运动B。用数学表达就是：

X * A = B * X

稍微整理一下，就是AX = XB。你看，是不是很直观？

4.2 如何求解AX=XB？

方程是列出来了，怎么解呢？

我个人的习惯是，先解旋转，再解平移。因为旋转和平移是解耦的。你可以把A、B、X都写成齐次变换矩阵的形式：

A = [R_A, t_A; 0, 1]
B = [R_B, t_B; 0, 1]
X = [R_X, t_X; 0, 1]

代入AX=XB，展开后得到两个方程：

1. 旋转部分： R_A * R_X = R_X * R_B
2. 平移部分： R_A * t_X + t_A = R_X * t_B + t_X

你看，旋转部分只和旋转矩阵有关，平移部分需要先知道旋转。所以我们的步骤是：

4.2.1 先解旋转

方程R_A * R_X = R_X * R_B，可以写成：

R_A = R_X * R_B * R_X^T

这其实是一个相似变换。R_A和R_B是相似的，它们有相同的旋转角。但旋转轴不同，R_X就是那个旋转轴之间的变换。

常用的解法是Tsai方法。它把旋转矩阵用旋转轴和旋转角表示，然后构建一个线性方程组。我记得当时手算这个推导，整整花了一个下午。但现在有现成的库，比如OpenCV里的calibrateHandEye()函数，直接调用就行。

4.2.2 再解平移

旋转解出来后，代入平移方程：

(R_A - I) * t_X = R_X * t_B - t_A

这是一个关于t_X的线性方程。同样，多组数据联立，用最小二乘求解。

4.3 最小二乘基础

说到最小二乘，你肯定不陌生。它的核心思想就是：找到一组参数，使得所有观测值的误差平方和最小。

用数学公式表达就是：

min Σ (y_i - f(x_i, θ))^2

其中，y_i是观测值，f(x_i, θ)是模型预测值，θ是我们要估计的参数。

在手眼标定里，我们的观测值就是A和B，模型就是AX=XB，参数就是X。我们想找到X，使得所有数据对的误差最小。

最小二乘分为两种：

• 线性最小二乘： 模型是线性的，有闭式解。比如上面解平移部分，就是线性最小二乘。
• 非线性最小二乘： 模型是非线性的，需要迭代求解。比如同时优化旋转和平移，或者优化更复杂的代价函数。

4.4 非线性优化基础

为什么需要非线性优化？因为很多问题没法一步到位。比如，我们想同时优化旋转和平移，而且旋转用四元数表示，那代价函数就是非线性的。这时候，就需要用迭代的方法来逼近最优解。

常用的非线性优化方法有：

• 高斯-牛顿法： 对代价函数进行一阶泰勒展开，然后求解增量。收敛快，但需要计算雅可比矩阵。
• 列文伯格-马夸尔特法（LM）： 在高斯-牛顿法的基础上加了一个阻尼项，更鲁棒。即使初始值不太好，也能收敛。
• 梯度下降法： 沿着负梯度方向走。收敛慢，但实现简单。

我个人在实际项目中，最常用的是LM算法。它就像一个“智能”的高斯-牛顿法，当迭代顺利时，它表现得像高斯-牛顿；当遇到问题时，它又像梯度下降一样稳健。

4.5 本章知识体系图

为了让你更直观地理解本章的知识结构，我画了一张图。它展示了从问题建模到求解的完整流程。

这张图清晰地展示了整个流程：从AX=XB的建模，到分解为旋转和平移，分别用线性最小二乘求解，最后用非线性优化精化。每一步都有对应的数学工具。

4.6 总结与个人体会

手眼标定，说白了就是解一个AX=XB的方程。但实际工程中，难点往往不在数学本身，而在数据质量。我见过太多人花大量时间调优化算法，却忽略了数据采集的质量。

给你几个实用建议：

• 数据要“动”起来： 标定时，让机器人做充分的旋转和平移运动。只平移不旋转，或者只旋转不平移，都会导致解算不稳定。
• 避免退化运动： 比如绕同一个轴反复旋转，或者只做平面运动。这些情况会导致某些自由度不可观。
• 先粗标再精标： 先用线性方法得到一个粗略结果，再用非线性优化精化。这个策略我屡试不爽。

嗯，数学基础部分到这里就告一段落了。下一章我们会进入实战，用代码实现整个标定流程。到时候，你会发现这些数学公式其实没那么可怕。

可可精品资料库