3. 优化问题建模

好，前面我们聊了误差来源，也推导了基础方程。现在进入最核心的一步——怎么把物理问题变成数学问题。

说白了，手眼标定本质上是一个参数估计问题。我们手里有机器人末端位姿和相机观测到的标定板位姿，要找到那个让所有数据都「自洽」的变换矩阵。这天然就是一个优化问题。

3.1 为什么是「非线性最小二乘」？

你可能会问：之前不是有 AX=XB 的线性解法吗？为什么还要搞优化？

嗯，这里有个关键点。线性解法假设测量数据是完美的，或者噪声很小。但实际项目中，机器人位姿有误差，相机标定有误差，特征点提取也有误差。这些误差叠加在一起，线性解法往往给出一个「数学上正确、物理上离谱」的结果。

我记得有一次在产线上调试，用 Tsai 方法算出来的旋转矩阵，正交性检查居然差了 0.02。虽然看起来不大，但投影到 2 米外的抓取点，偏差直接到了 4 厘米。这就是线性解法对噪声敏感的典型表现。

非线性最小二乘的优势在于：

• 可以处理任意多的观测数据，数据越多，鲁棒性越强
• 能显式建模噪声模型，比如给不同测量赋予不同权重
• 直接优化最终误差，而不是中间步骤的误差

3.2 优化变量：我们要优化什么？

手眼标定的优化变量就是手眼变换矩阵。但这里有个坑——旋转矩阵有 9 个元素，但自由度只有 3。直接优化 9 个参数会引入冗余，导致矩阵不正交。

我个人习惯用李代数表示旋转。具体来说：

• 旋转部分：用 so(3) 的 3 个参数表示，即旋转向量
• 平移部分：直接用 3 个参数表示
• 总优化变量：6 维向量 ξ = [ω, t]^T

这样做的好处是：无约束优化，不需要担心矩阵正交性。每次迭代时，通过指数映射把 ξ 映射回 SE(3)。

3.3 误差函数：怎么定义「好坏」？

误差函数是优化的灵魂。我见过很多新手直接优化 AX=XB 的残差，结果收敛到局部最优。为什么？因为那个残差对旋转和平移的尺度不敏感。

我推荐使用重投影误差。思路是这样的：

1. 从标定板上取一个三维点 P_w（在标定板坐标系下）
2. 通过相机外参 T_cam_to_board，投影到相机坐标系
3. 再通过手眼矩阵 T_hand_to_eye，投影到机器人基坐标系
4. 最后通过机器人末端位姿 T_base_to_hand，投影到机器人基坐标系
5. 比较这个投影点和实际观测到的像素点位置

数学表达式如下：

// 重投影误差
e = p_obs - project(T_base_to_hand * T_hand_to_eye * T_cam_to_board * P_w)

// 其中：
// p_obs: 实际观测到的像素坐标
// project(): 相机内参投影函数
// T_base_to_hand: 机器人正运动学给出的末端位姿
// T_hand_to_eye: 待优化的手眼矩阵
// T_cam_to_board: 标定板在相机坐标系下的位姿
// P_w: 标定板上的三维点

这个误差的物理意义很直观：如果手眼矩阵正确，所有观测点都应该投影到正确位置。否则，误差就会累积。

3.4 优化目标函数

有了误差函数，目标函数就很自然了：

minimize Σ (e_i^T * W_i * e_i)

// 其中：
// e_i: 第 i 个观测点的重投影误差
// W_i: 信息矩阵（权重），表示该观测的置信度
// Σ: 对所有观测点求和

这里的信息矩阵 W_i 是个好东西。你可以根据测量质量动态调整权重：

观测类型	典型权重	说明
标定板中心点	1.0	特征提取精度高
标定板边缘点	0.5	可能存在畸变影响
机器人末端位姿	0.8	运动学模型有误差
相机外参	0.7	标定板位姿估计有噪声

3.5 优化流程总览

把整个流程画成图，会更清晰：

3.6 代码实现骨架

最后，给一个用 Ceres Solver 实现的骨架代码。你可以在实际项目中直接套用：

#include <ceres/ceres.h>
#include <sophus/se3.hpp>

// 定义残差块
struct ReprojectionError {
  ReprojectionError(const Eigen::Vector2d& observed_p,
                    const Eigen::Vector3d& point_3d,
                    const Eigen::Matrix4d& T_base_to_hand,
                    const Eigen::Matrix4d& T_cam_to_board)
      : observed_p_(observed_p), point_3d_(point_3d),
        T_base_to_hand_(T_base_to_hand), T_cam_to_board_(T_cam_to_board) {}

  template <typename T>
  bool operator()(const T* const params, T* residuals) const {
    // 将优化参数转为 SE(3)
    Sophus::SE3<T> T_hand_to_eye = Sophus::SE3<T>::exp(params);
    
    // 计算投影点
    Eigen::Matrix<T, 3, 1> p_cam = T_cam_to_board_.cast<T>() * point_3d_.cast<T>();
    Eigen::Matrix<T, 3, 1> p_hand = T_hand_to_eye * p_cam;
    Eigen::Matrix<T, 3, 1> p_base = T_base_to_hand_.cast<T>() * p_hand;
    
    // 投影到像素
    T u = p_base[0] / p_base[2];
    T v = p_base[1] / p_base[2];
    
    // 计算残差
    residuals[0] = u - T(observed_p_.x());
    residuals[1] = v - T(observed_p_.y());
    
    return true;
  }

 private:
  Eigen::Vector2d observed_p_;
  Eigen::Vector3d point_3d_;
  Eigen::Matrix4d T_base_to_hand_;
  Eigen::Matrix4d T_cam_to_board_;
};

// 主优化函数
void OptimizeHandEye(const std::vector<Observation>& data,
                     Sophus::SE3d& T_hand_to_eye) {
  ceres::Problem problem;
  
  // 优化参数：6维李代数
  double params[6] = {0, 0, 0, 0, 0, 0};
  
  for (const auto& obs : data) {
    ceres::CostFunction* cost_function =
        new ceres::AutoDiffCostFunction<ReprojectionError, 2, 6>(
            new ReprojectionError(obs.pixel, obs.point_3d,
                                  obs.T_base_to_hand, obs.T_cam_to_board));
    problem.AddResidualBlock(cost_function, nullptr, params);
  }
  
  // 配置求解器
  ceres::Solver::Options options;
  options.minimizer_progress_to_stdout = true;
  options.max_num_iterations = 50;
  
  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);
  
  // 更新结果
  T_hand_to_eye = Sophus::SE3d::exp(params);
}