Eigen进阶：四元数、变换矩阵与机器人坐标运算

郑伟强dev

99人浏览 · 2026-07-17 00:08:21

郑伟强dev · 2026-07-17 00:08:21 发布

上篇把Eigen的基本用法聊了。今天进阶——四元数和变换矩阵。这两个是机器人坐标运算的核心工具，面试里几乎必考。

做机器人开发，你绕不开坐标变换。传感器装在机器人身上，传感器有自己的坐标系；机器人有本体坐标系；还有全局的世界坐标系。数据在这些坐标系之间转来转去，全靠变换矩阵和四元数。

先说个面试场景。

面试官问："给你一个旋转矩阵，怎么转成四元数？"

我当时愣了一下，说"Eigen有现成的API……"面试官笑了笑说"我知道有API，但原理你了解吗？"

好吧，今天就把原理和用法都讲清楚。

旋转矩阵

旋转矩阵是最直观的坐标变换方式。一个3x3的正交矩阵，描述一个坐标系相对于另一个坐标系的旋转。

#include <Eigen/Geometry>

// 绕Z轴旋转30度
double angle = M_PI / 6;  // 30度
Matrix3d Rz;
Rz << cos(angle), -sin(angle), 0,
      sin(angle),  cos(angle), 0,
      0,           0,          1;

// 用Eigen的AngleAxis更简洁
AngleAxisd rotation(angle, Vector3d::UnitZ());
Matrix3d R = rotation.toRotationMatrix();

旋转矩阵的优点是直观、好理解。缺点是占空间（9个数字描述3个自由度，有冗余），而且做多次旋转组合的时候要连续矩阵乘法，数值误差会累积。

四元数

四元数用4个数字描述三维旋转，比旋转矩阵更紧凑，也不会有万向锁的问题。

// 创建四元数：绕Z轴旋转30度
Quaterniond q(AngleAxisd(M_PI / 6, Vector3d::UnitZ()));

cout << "四元数: " << q.coeffs().transpose() << endl;
// 输出: x y z w

// 四元数做旋转
Vector3d v(1, 0, 0);
Vector3d v_rotated = q * v;  // 用四元数旋转向量

// 四元数转旋转矩阵
Matrix3d R = q.toRotationMatrix();

// 旋转矩阵转四元数
Quaterniond q2(R);

// 四元数乘法（组合旋转）
Quaterniond q3 = q2 * q;  // 先q再q2

四元数的四个分量是(x, y, z, w)，其中w是实部，(x,y,z)是虚部。Eigen里coeffs()返回的顺序是[x, y, z, w]。

面试经常问"为什么机器人用四元数而不用欧拉角？"因为欧拉角有万向锁（Gimbal Lock）问题——当某个轴旋转到90度的时候，另外两个轴会重合，丢失一个自由度。四元数没有这个问题，任何旋转都能表示。

另外，四元数的插值（SLERP）比欧拉角平滑，这在轨迹规划里很重要。

变换矩阵：旋转+平移

实际应用中，坐标变换不只是旋转，还有平移。用一个4x4的齐次变换矩阵，可以把旋转和平移统一表示：

// 齐次变换矩阵 T = [R | t]
//                  [0 | 1]
Matrix4d T = Matrix4d::Identity();
T.block<3, 3>(0, 0) = R;    // 旋转部分
T.block<3, 1>(0, 3) = t;    // 平移部分

// 坐标变换：从坐标系A到坐标系B
Vector4d pointA(x, y, z, 1);  // 齐次坐标
Vector4d pointB = T * pointA;

// 逆变换：从B回到A
Vector4d pointA2 = T.inverse() * pointB;

在ROS2里，TF2库做的就是这件事——维护一棵坐标变换树，每个节点之间的变换就是一个齐次变换矩阵。你用tf2_ros::Buffer查两个坐标系之间的变换，底层就是在做矩阵连乘。

机器人开发中的实际应用

一个典型的场景：激光雷达装在机器人顶部，和机器人本体有一个固定的安装偏移。雷达检测到的障碍物坐标需要转换到机器人本体坐标系：

// 雷达到本体的变换（固定的安装参数）
Isometry3d T_lidar_to_base = Isometry3d::Identity();
T_lidar_to_base.rotate(AngleAxisd(M_PI / 4, Vector3d::UnitZ()));
T_lidar_to_base.pretranslate(Vector3d(0.3, 0.0, 0.5));

// 雷达检测到一个障碍物
Vector3d obstacle_lidar(5.0, 2.0, 0.0);

// 转换到本体坐标系
Vector3d obstacle_base = T_lidar_to_base * obstacle_lidar;

Eigen的Isometry3d是专门用来表示刚体变换的——内部用一个3x3旋转矩阵加一个平移向量，比4x4矩阵更省空间，运算也更快。

再举一个实际例子：IMU的简单积分。IMU提供加速度和角速度，通过对时间积分可以得到速度和位置的变化：

class IMUIntegrator {
    Vector3d position_ = Vector3d::Zero();
    Vector3d velocity_ = Vector3d::Zero();
    Matrix3d rotation_ = Matrix3d::Identity();
    double dt_;
    
public:
    IMUIntegrator(double dt) : dt_(dt) {}
    
    void integrate(const Vector3d& accel, const Vector3d& gyro) {
        // 更新旋转：用小角度近似
        double angle = gyro.norm() * dt_;
        if (angle > 1e-6) {
            rotation_ *= AngleAxisd(angle, gyro.normalized()).toRotationMatrix();
        }
        
        // 世界坐标系下的加速度（减去重力）
        Vector3d gravity(0, 0, -9.81);
        Vector3d accel_world = rotation_ * accel + gravity;
        
        // 积分
        velocity_ += accel_world * dt_;
        position_ += velocity_ * dt_;
    }
    
    Vector3d getPosition() const { return position_; }
    Matrix3d getRotation() const { return rotation_; }
};

这个简单的积分器虽然会有累积漂移误差，但在面试里展示你对惯性导航的理解很有帮助。实际项目里会用更复杂的算法（比如Madgwick滤波器、EKF或UKF）来修正漂移问题。