🧭 一、欧拉角、四元数、旋转矩阵之间的关系
1.1 欧拉角（Euler Angles）
欧拉角是描述三维空间旋转的最直观方式，用三个角度表示依次绕三个坐标轴的旋转：
- 常见顺序：ZYX，即依次绕 Z轴（航向 yaw）→ Y轴（俯仰 pitch）→ X轴（横滚 roll）。
- 通常记作： ( ϕ , θ , ψ ) (\phi, \theta, \psi) (ϕ,θ,ψ)（roll, pitch, yaw）
1.2 四元数（Quaternion）
四元数是一种避免欧拉角万向锁问题的姿态表示方法，形式为：
q = ( w , x , y , z ) q = (w, x, y, z) q=(w,x,y,z)
其中 w w w 是实部， ( x , y , z ) (x, y, z) (x,y,z) 是虚部。
特点：
- 无奇异点
- 可用于插值
- 可以转换为旋转矩阵用于实际运算
1.3 旋转矩阵（Rotation Matrix）
旋转矩阵 R ∈ R 3 × 3 R \in \mathbb{R}^{3\times3} R∈R3×3 表示一个正交矩阵，它将向量从一个坐标系旋转到另一个坐标系：
- R w b R_{wb} Rwb：表示从机体坐标系到世界坐标系的旋转。
- 向量变换： v world = R w b ⋅ v body \mathbf{v}_{\text{world}} = R_{wb} \cdot \mathbf{v}_{\text{body}} vworld=Rwb⋅vbody
🔁 二、三者之间的转换关系
2.1 四元数 → 旋转矩阵
四元数 q = ( w , x , y , z ) q = (w, x, y, z) q=(w,x,y,z) 对应的旋转矩阵为：
R = [ 1 − 2 ( y 2 + z 2 ) 2 ( x y − w z ) 2 ( x z + w y ) 2 ( x y + w z ) 1 − 2 ( x 2 + z 2 ) 2 ( y z − w x ) 2 ( x z − w y ) 2 ( y z + w x ) 1 − 2 ( x 2 + y 2 ) ] R = \begin{bmatrix} 1 - 2(y^2 + z^2) & 2(xy - wz) & 2(xz + wy) \\ 2(xy + wz) & 1 - 2(x^2 + z^2) & 2(yz - wx) \\ 2(xz - wy) & 2(yz + wx) & 1 - 2(x^2 + y^2) \end{bmatrix} R= 1−2(y2+z2)2(xy+wz)2(xz−wy)2(xy−wz)1−2(x2+z2)2(yz+wx)2(xz+wy)2(yz−wx)1−2(x2+y2)
你也可以用库函数自动转换，如：
from scipy.spatial.transform import Rotation as R
rot_matrix = R.from_quat([x, y, z, w]).as_matrix()
2.2 欧拉角 ↔ 四元数
- 欧拉角 → 四元数：
R.from_euler('xyz', [roll, pitch, yaw]).as_quat()
- 四元数 → 欧拉角：
R.from_quat([x, y, z, w]).as_euler('xyz')
2.3 欧拉角 ↔ 旋转矩阵
R.from_euler('xyz', [roll, pitch, yaw]).as_matrix()
或反过来 .from_matrix().as_euler(...)
🛰️ 三、将无人机坐标系中的向量转换到世界坐标系
假设
- 有一个四元数 q w b q_{wb} qwb：表示 从世界坐标系到机体坐标系的旋转
- 要将无人机坐标系下的向量（例如速度、加速度）变换到世界坐标系
错误做法是：
使用四元数的共轭或逆 q b w = q w b − 1 q_{bw} = q_{wb}^{-1} qbw=qwb−1
使用 Python 示例（Scipy）：
from scipy.spatial.transform import Rotation as R
import numpy as np
w, x, y, z = 0.7071, 0.0, 0.7071, 0.0
# 四元数（w, x, y, z）格式
q_wb = [w, x, y, z] # 世界到机体的旋转
v_body = np.array([1, 0, 0]) # 无人机坐标系下的向量
# 变换：将向量从 body 转换到 world
r = R.from_quat([x, y, z, w]) # 注意 scipy 用的是 [x, y, z, w]
v_world_right = r.apply(v_body)
v_world_wrong = r.inv().apply(v_body)
print("Body → World(right): ", v_world_right)
print("Body → World(wrong): ", v_world_wrong)
输出结果：
Body → World(right): [ 0. 0. -1.]
Body → World(wrong): [0. 0. 1.]
附：利用四元数实现无人机坐标系与世界坐标系的变换
效果说明：
粗坐标轴坐标系：世界坐标系
细坐标轴坐标系：机体坐标系
X：无人机前方（红）
Y：左侧（绿）
Z：上方（蓝）
黑点：无人机坐标系下的点 p o i n t b = ( 1.0 , 0.0 , 0.0 ) point_b = (1.0, 0.0, 0.0) pointb=(1.0,0.0,0.0) 在世界坐标系下的正确投影
红点：无人机坐标系下的点 p o i n t b = ( 1.0 , 0.0 , 0.0 ) point_b = (1.0, 0.0, 0.0) pointb=(1.0,0.0,0.0) 在世界坐标系下的错误投影
判断方式：无人机坐标系下的点投影到世界坐标系后进行可视化一定在无人机坐标的 X X X 轴上
import numpy as np
import open3d as o3d
from scipy.spatial.transform import Rotation as R
# ---------- Step 1: 定义无人机姿态 ----------
# 无人机在世界坐标系的位置
# translation = np.array([1.0, 2.0, 1.5])
translation = np.array([0.0, 0.0, 0.0])
# 四元数表示姿态（绕 Y 轴旋转 90°）
quat_wxyz = [0.7071, 0.0, 0.7071, 0.0] # [w, x, y, z]
rotation = R.from_quat([quat_wxyz[1], quat_wxyz[2], quat_wxyz[3], quat_wxyz[0]])
rotation_matrix = rotation.as_matrix()
# 变换矩阵：从 body → world（R_wb, T_wb）
T_wb = np.eye(4)
T_wb[:3, :3] = rotation_matrix
T_wb[:3, 3] = translation
# 求逆变换：从 world → body
T_bw = np.linalg.inv(T_wb)
# ---------- Step 2: 创建两个坐标系 ----------
frame_world = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.7, origin=[0, 0, 0])
frame_body = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.3)
frame_body.transform(T_wb)
# ---------- Step 3: 验证坐标变换 ----------
# 定义一个无人机坐标系下的点
point_body = np.array([1.0, 0.0, 0.0, 1.0]) # 齐次坐标
# 转换到世界坐标系(正确版本)
point_world = T_wb @ point_body
print("Body → World(right):", point_world[:3])
# 再转回无人机坐标系(正确版本)
point_body_recovered = T_bw @ point_world
print("World → Body(right):", point_body_recovered[:3]) # 应该接近 [1, 0, 0]
# 转换到世界坐标系(错误版本)
point_world_wrong = T_bw @ point_body
print("Body → World(wrong):", point_world_wrong[:3])
# 再转回无人机坐标系(错误版本)
point_body_recovered_wrong = T_wb @ point_world_wrong
print("World → Body(wrong):", point_body_recovered_wrong[:3])
# ---------- Step 4: 可视化 ----------
# 创建一个小球表示 point_world 的位置
sphere = o3d.geometry.TriangleMesh.create_sphere(radius=0.05)
sphere.translate(point_world[:3])
sphere.paint_uniform_color([0, 0, 0]) # 黑色
sphere1 = o3d.geometry.TriangleMesh.create_sphere(radius=0.05)
sphere1.translate(point_world_wrong[:3])
sphere1.paint_uniform_color([1, 0, 0]) # 红色
o3d.visualization.draw_geometries(
[frame_world, frame_body, sphere, sphere1],
window_name="Coordinate transformation visualization",
width=800,
height=600,
zoom=0.7
)
输出结果为：
Body → World(right): [ 0. 0. -1.]
World → Body(right): [1. 0. 0.]
Body → World(wrong): [0. 0. 1.]
World → Body(wrong): [1. 0. 0.]
可视化效果为：
