四元数转Yaw再转航向时NW/SE方向结果反向问题排查

问题根源 #

你使用的公式对应的坐标系轴定义或旋转顺序，和ROS默认的右手坐标系（X向前、Y向左、Z向上）不匹配，导致NW、SE象限的航向计算出现反向。ROS中Yaw是绕Z轴的旋转，定义为从X轴正方向逆时针旋转的角度，而你引用的公式采用了不同的轴映射逻辑，才会出现部分方向结果异常。

修正后的代码 #

替换为符合ROS坐标系规范的Yaw计算代码：

double heading = atan2(2.0 * (q.w * q.y - q.x * q.z), 1.0 - 2.0 * (q.y * q.y + q.z * q.z)) * 180 / M_PI;
if (heading < 0) {
    heading += 360;
}

原理说明 #

ROS标准下，四元数转欧拉角（Yaw，绕Z轴）的正确推导基于右手坐标系规则：

• 四元数q(w, x, y, z)对应的旋转顺序为Roll(X轴)→Pitch(Y轴)→Yaw(Z轴)
• 修正后的公式由四元数到旋转矩阵的转换推导而来，确保绕Z轴的旋转方向与RVIZ中显示的姿态完全对齐
• 原公式的分子、分母项对应的轴映射逻辑错误，导致部分象限的角度计算取反

效果验证 #

修正后各方向的航向结果将与RVIZ显示一致：

• NE方向（X正、Y正）：航向为0~90度
• NW方向（X负、Y正）：航向为90~180度，不再反向
• SE方向（X正、Y负）：航向转换后为270~360度（原负角度加360修正）
• SW方向（X负、Y负）：航向为180~270度，保持正确

内容的提问来源于stack exchange，提问作者Nicooost