上篇把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.block3, 3(0, 0) R; // 旋转部分 T.block3, 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来修正漂移问题。面试中的关键考点四元数和旋转矩阵的转换四元数转矩阵直接展开公式就行。矩阵转四元数有个经典公式先算trace再根据最大值分量计算其他分量。Eigen的Quaterniond(Matrix3d)构造函数帮你做了这件事。为什么SLAM里用四元数做优化四元数只有4个参数比旋转矩阵的9个参数少很多。而且四元数可以用单位约束模长为1来保证旋转的合法性优化起来更方便更高效。但要注意归一化——四元数在经过数值运算后可能不再是单位四元数需要手动调用normalize来修正。Eigen的Isometry3d和Affine3d有什么区别Isometry3d只包含旋转和平移保持距离不变Affine3d还允许缩放和剪切变换。机器人坐标变换用Isometry3d就完全够了。给正在准备面试的你四元数和变换矩阵是机器人面试中的高频考点。如果你做SLAM、导航或者机械臂方向这些数学工具是必须熟练掌握的。建议你做这几件事来加深理解用Eigen手写一个坐标变换工具类能完成各种坐标系之间的相互转换理解四元数的基本原理能讲清楚和欧拉角、旋转矩阵各自的优缺点对比和适用场景知道TF2的底层原理其实就是变换矩阵的链式乘法。到这里这个阶段的全部内容——包括C基础、设计模式、内存管理、多线程、网络编程、数学库——就全部聊完了。从下篇开始进入CMake编译系统——大型工程项目管理的基础设施和工具。如果这篇文章对你有帮助欢迎点赞、在看、转发三连。 你的支持是我持续更新的最大动力。「机器人软件开发面试·从入门到精通」连载系列上一篇第61篇 Eigen矩阵库入门——机器人数学运算的标配工具 下一篇预告第63篇 CMake编译系统详解——从入门到能写复杂项目如果有任何问题欢迎评论区留言交流讨论我会尽量一一回复所有的问题。