1. 三维旋转的四种表示方法从数学到代码刚接触三维空间旋转时很多人会被各种术语搞晕。我刚开始做无人机姿态控制时就曾被四元数折磨得彻夜难眠。其实核心就四种表示法欧拉角像通俗小说般直观旋转矩阵如教科书般严谨四元数似黑魔法般高效旋转向量则像瑞士军刀般便携。以无人机飞控为例当传感器传来pitch俯仰、roll翻滚、yaw偏航数据时这是欧拉角进行姿态解算需要用到旋转矩阵而控制器内部为了避开万向锁问题往往会转用四元数计算最后电机驱动需要的轴角参数本质上就是旋转向量。这四种表示法各有所长欧拉角人类最容易理解直接对应物体朝向旋转矩阵完美适配线性代数运算四元数计算效率高且无奇异性旋转向量存储空间最小在ROS的TF2库中就同时支持这四种表示法的相互转换。我曾用以下代码验证过转换精度// 创建30度Z轴旋转的四种表示 Eigen::AngleAxisd vec(M_PI/6, Eigen::Vector3d::UnitZ()); Eigen::Matrix3d mat vec.toRotationMatrix(); Eigen::Quaterniond quat(vec); Eigen::Vector3d euler mat.eulerAngles(2,1,0); cout 旋转向量:\n vec.axis() , vec.angle() endl; cout 旋转矩阵:\n mat endl; cout 四元数:\n quat.coeffs().transpose() endl; cout 欧拉角(ZYX):\n euler.transpose() endl;2. 欧拉角直观中的陷阱欧拉角最大的优势就是符合人类直觉。调试机械臂时我常直接输入三个角度值来调整末端姿态。但新手容易忽略两个致命问题万向锁问题当pitch为±90°时roll和yaw会失去独立性。有次调试无人机在垂直爬升时突然失控就是因为没处理这个奇异点。数学上看当中间旋转轴与首/末轴重合时就丢失了一个自由度。顺序敏感性ZYX和XYZ旋转得到的结果完全不同。在Unity中默认是Y-X-Z顺序而ROS的TF2采用Z-Y-X。有次把Unity模型导入ROS时所有朝向都错了排查半天才发现这个差异。这里有个实用技巧在存储欧拉角时务必记录旋转顺序。我习惯用如下结构体struct EulerAngle { double yaw, pitch, roll; // Z-Y-X顺序 std::string order ZYX; };转换到旋转矩阵时Eigen库的eulerAngles()函数需要显式指定顺序Eigen::Matrix3d mat; mat Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());3. 旋转矩阵线性代数的完美诠释旋转矩阵的9个参数看似冗余却是计算机视觉的基石。在做视觉定位时每个特征点的投影计算都依赖这个3×3矩阵。它有两大核心特性正交性矩阵的逆等于其转置行列式为1保证是纯旋转无缩放验证旋转矩阵合法性时我常用这个检查函数bool isValidRotation(const Eigen::Matrix3d mat) { double err (mat * mat.transpose() - Eigen::Matrix3d::Identity()).norm(); return abs(mat.determinant() - 1.0) 1e-6 err 1e-6; }在点云配准中旋转矩阵能直接作用于点集Eigen::Matrix3d R; Eigen::Vector3d t; pcl::PointCloudpcl::PointXYZ transformed; for (const auto p : cloud.points()) { transformed.push_back(R * p t); }但要注意连续旋转时矩阵乘法不满足交换律。有次调试机械臂轨迹因为先roll后pitch和先pitch后roll的结果不同导致末端位置偏差了15cm。4. 四元数三维旋转的终极武器四元数由一个实部和三个虚部构成形式如qwxiyjzk。第一次看到Hamilton公式时我也一头雾水直到发现它其实就是轴角表示的升级版虚部对应旋转轴方向实部编码旋转角度在VR头盔的姿态跟踪中四元数因其计算效率被广泛使用。这是我常用的归一化处理Eigen::Quaterniond quat(w,x,y,z); quat.normalize(); // 必须归一化四元数最大的优势是平滑插值。在动画 blending 时用slerp比欧拉角的lerp效果更自然Eigen::Quaterniond q1, q2; Eigen::Quaterniond q q1.slerp(t, q2); // t∈[0,1]但要注意四元数的双覆盖特性q和-q表示同一个旋转。在比较姿态差异时我总会先统一符号if (quat1.dot(quat2) 0) quat2.coeffs() * -1;5. 旋转向量紧凑高效的表示旋转向量又称轴角表示只需3个参数旋转角度θ和单位旋转轴k。在存储大量位姿数据时这种表示能节省40%空间。其数学本质是李代数so(3)的元素。在BA优化时通常先用旋转向量表示增量Eigen::AngleAxisd delta_rot(k, theta); pose delta_rot * current_pose;罗德里格斯公式是旋转向量的核心Eigen::Vector3d k; double theta; Eigen::Matrix3d R cos(theta) * Eigen::Matrix3d::Identity() (1 - cos(theta)) * k * k.transpose() sin(theta) * skewSymmetric(k);其中skewSymmetric函数实现叉积矩阵Eigen::Matrix3d skewSymmetric(const Eigen::Vector3d v) { Eigen::Matrix3d m; m 0, -v.z(), v.y(), v.z(), 0, -v.x(), -v.y(), v.x(), 0; return m; }6. 实际应用中的转换策略在SLAM系统中通常采用这样的处理流程传感器输入欧拉角IMU转为四元数进行滤波避免万向锁需要时转为旋转矩阵点云变换优化时使用旋转向量参数少这是我在激光SLAM中用的转换链// 欧拉角(来自IMU) - 四元数 Eigen::Quaterniond q Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()); // 四元数 - 旋转矩阵(用于点云变换) Eigen::Matrix3d R q.toRotationMatrix(); // 旋转矩阵 - 旋转向量(用于位姿图优化) Eigen::AngleAxisd vec(R);在Unity中转换时要注意坐标系差异// Unity的左手系转ROS的右手系 Quaternion rosQuat new Quaternion(-unityQuat.x, -unityQuat.y, unityQuat.z, unityQuat.w);7. 避坑指南我踩过的那些坑精度损失连续转换会导致误差累积。有次发现转换后的矩阵行列式变成了0.99999解决方案是定期重新正交化Eigen::Matrix3d R; Eigen::JacobiSVDEigen::Matrix3d svd(R, Eigen::ComputeFullU | Eigen::ComputeFullV); R svd.matrixU() * svd.matrixV().transpose();方向混淆不同库的坐标系约定可能不同。在将OpenCV的旋转向量转成Eigen矩阵时我栽过跟头// OpenCV的Rodrigues输出是3x1Eigen需要3x3 cv::Mat rvec; Eigen::AngleAxisd vec(rvec.atdouble(0), Eigen::Vector3d(rvec.atdouble(1), rvec.atdouble(2), rvec.atdouble(3)));归一化遗忘四元数必须归一化这是血泪教训。现在我的代码里总会加上断言assert(abs(quat.norm() - 1.0) 1e-6);