机器人 / 系统课时
四元数如何表示机器人三维朝向
四元数如何表示机器人三维朝向
ROS 2 使用四元数记录机器人的三维朝向,但四个分量并不是四个旋转角度。理解欧拉角的旋转顺序、四元数的归一化规则以及航向角的提取方法,就能正确读取里程计姿态,并避免坐标变换中的方向错误。本课使用 Python 完成一组可验证的计算。
学习目标
- 区分 roll、pitch、yaw 的旋转轴,理解固定轴 RPY 的旋转顺序。
- 识别 ROS 2 四元数的
x、y、z、w分量并完成归一化。 - 使用 Python 将欧拉角转换为四元数,再从四元数读取航向角。
欧拉角的旋转轴与顺序
机器人朝向描述的是机体坐标系相对于参考坐标系的旋转。按照 ROS REP 103 的常用机体坐标约定,X 轴指向机器人前方,Y 轴指向左侧,Z 轴指向上方,三个轴构成右手坐标系。
欧拉角中的三个名称分别对应不同旋转:
- roll(滚转):绕 X 轴旋转,例如机器人向左侧或右侧倾斜。
- pitch(俯仰):绕 Y 轴旋转,例如车头抬起或低下。
- yaw(偏航):绕 Z 轴旋转,例如机器人在水平地面上转向。
角度正方向遵循右手定则。对于水平移动机器人,从上方观察,绕 Z 轴的正向旋转表现为逆时针转动。
ROS 2 的 tf2 常用固定轴 RPY 约定:先绕固定 X 轴施加 roll,再绕固定 Y 轴施加 pitch,最后绕固定 Z 轴施加 yaw。对列向量而言,对应的合成旋转矩阵为:
矩阵作用于向量时从右向左执行。三维旋转通常不可交换,因此把 yaw 和 pitch 的执行顺序颠倒,即使角度数值相同,最终朝向也可能不同。

四元数为什么有四个分量
欧拉角容易理解,但当 pitch 接近正负 90° 时,欧拉角表示会遇到万向节锁问题:部分旋转自由度在参数表达上耦合,导致某些角度无法唯一确定。
四元数使用四个实数表示三维旋转。在 ROS 2 中,geometry_msgs/msg/Quaternion 的字段顺序是 (x, y, z, w),其中前三项组成向量部分,w 是标量部分。它们不是三个欧拉角加一个附加参数。
表示旋转时通常要求使用单位四元数:
这个约束意味着四个分量并非四个独立旋转自由度,三维朝向仍然只有三个旋转自由度。
两个容易核对的例子是:
- 不发生旋转:
(0, 0, 0, 1),也称单位旋转四元数。 - 仅绕 Z 轴正向旋转 90°:约为
(0, 0, 0.707107, 0.707107)。
注意,q 与 -q 表示完全相同的空间旋转。例如 (0, 0, 0, 1) 和 (0, 0, 0, -1) 的朝向相同,因此不能仅凭四个分量是否逐项相等来判断两个姿态是否不同。
用 Python 转换欧拉角并读取航向
下面采用 ROS 2 常见的固定轴 RPY 约定,设置 roll 为 20°、pitch 为 30°、yaw 为 60°。先把角度转换为弧度,再计算四元数、归一化并提取 yaw。
示例仅使用 Python 3 标准库,可在 Ubuntu 24.04 的 ROS 2 Jazzy 环境中运行,也不依赖额外的 ROS Python 包。保存为 quaternion_demo.py 后执行 python3 quaternion_demo.py。
from math import atan2, cos, degrees, radians, sin, sqrt
def rpy_to_quaternion(roll, pitch, yaw):
# 固定轴 RPY,对应 Rz(yaw) * Ry(pitch) * Rx(roll)
cr, sr = cos(roll / 2), sin(roll / 2)
cp, sp = cos(pitch / 2), sin(pitch / 2)
cy, sy = cos(yaw / 2), sin(yaw / 2)
x = sr * cp * cy - cr * sp * sy
y = cr * sp * cy + sr * cp * sy
z = cr * cp * sy - sr * sp * cy
w = cr * cp * cy + sr * sp * sy
return x, y, z, w
def normalize(q):
# 使用实际计算值归一化,拒绝零长度四元数
norm = sqrt(sum(v * v for v in q))
if norm < 1e-12:
raise ValueError("zero-length quaternion")
return tuple(v / norm for v in q)
def yaw_from_quaternion(q):
# 按固定轴 ZYX 欧拉角约定读取偏航
x, y, z, w = normalize(q)
return atan2(
2 * (w * z + x * y),
1 - 2 * (y * y + z * z),
)
roll, pitch, yaw = map(radians, (20, 30, 60))
q = normalize(rpy_to_quaternion(roll, pitch, yaw))
print("q=(x,y,z,w):", " ".join(f"{v:.6f}" for v in q))
print("norm:", f"{sqrt(sum(v * v for v in q)):.6f}")
print("yaw(deg):", f"{degrees(yaw_from_quaternion(q)):.1f}")预期输出:
q=(x,y,z,w): 0.017816 0.304604 0.436703 0.846279
norm: 1.000000
yaw(deg): 60.0这里有三个可以直接检查的结果。
第一,四元数输出顺序是 x、y、z、w,不要将 0.846279 误认为 X 分量。第二,四元数长度约为 1,显示值经过六位小数舍入,实际计算仍使用完整精度。第三,提取出的 yaw 为 60°,与输入一致。
航向提取使用 atan2,输出通常落在 -180° 到 180° 的范围内。它计算的是固定轴 ZYX 约定下的偏航角,并不是简单读取四元数的 z 分量。
在 ROS 2 姿态消息中使用四元数
ROS 2 Jazzy 中,常见消息会直接使用 geometry_msgs/msg/Quaternion 表示朝向。
| 消息中的字段路径 | 表示内容 |
|---|---|
geometry_msgs/msg/Pose.orientation |
位姿中的旋转部分 |
geometry_msgs/msg/Transform.rotation |
两坐标系之间变换的旋转部分 |
nav_msgs/msg/Odometry.pose.pose.orientation |
里程计位姿中的朝向 |
例如将上面计算出的结果写入 Pose.orientation,需要按字段对应关系填写:
orientation.x = 0.017816orientation.y = 0.304604orientation.z = 0.436703orientation.w = 0.846279
这里列出的是便于阅读的舍入值。实际发布消息时应使用计算得到的完整精度数值。
机器人朝向始终相对于某个参考坐标系才有意义。假设里程计消息的 header.frame_id 为 odom,child_frame_id 为 base_link,那么其位姿表达的是 base_link 相对于 odom 的位姿。将同一组四元数放进另一个参考坐标系,不能认为其航向含义仍然相同。
对于主要在水平面运动的差速机器人,yaw 通常是控制器最关心的朝向量。它可以用于计算目标航向误差,但不能仅凭四元数的 z 和 w 计算一般三维姿态的 yaw;只有在 roll、pitch 均为零等受限条件下,才能使用相应的简化公式。
排查归一化与角度读取错误
如果机器人在 RViz 中出现异常旋转,或者控制器计算出的角度不合理,可以先检查以下问题。
- 分量顺序错误:ROS 消息采用
(x, y, z, w),其他数学库可能使用(w, x, y, z)。跨库转换时必须按字段重新映射。 - 没有归一化:计算
$x^2+y^2+z^2+w^2$,结果应接近 1。若长度明显偏离 1,应检查数据来源,并在非零前提下归一化。 - 角度单位混淆:Python 三角函数及 ROS 2 的相关旋转接口通常使用弧度。90° 应转换为约 1.570796 弧度,而不是直接传入数值 90。
- 旋转顺序不一致:不同库可能使用固定轴或旋转后的局部轴约定,转换前需核对轴顺序和乘法约定。
- 航向跨越边界:179° 与 -179° 的最短夹角是 2°,不能将直接相减得到的 358° 当作实际转向误差。
可以将示例中的三个输入角度全部改成 0°,重新运行并检查四元数是否为 (0, 0, 0, 1),航向是否为 0°;再设置 roll、pitch 为 0°,yaw 为 90°,检查输出是否接近 (0, 0, 0.707107, 0.707107)。这两组测试可以快速核对分量顺序、角度单位和绕 Z 轴的旋转方向。
在 ROS 2 中传递完整姿态时保留单位四元数,仅在需要平面转向控制或显示航向时提取 yaw,并始终检查参考坐标系与旋转约定。