四元数与旋转矩阵互转实战：用Python代码搞定机器人姿态计算（附完整公式推导）
·
四元数与旋转矩阵互转实战：用Python代码搞定机器人姿态计算
在机器人开发和3D视觉工程中，姿态表示是一个基础但至关重要的概念。四元数和旋转矩阵作为两种最常用的姿态表示方法，各有优缺点：四元数计算高效且无万向节锁问题，而旋转矩阵则更直观且易于组合。本文将深入探讨这两种表示方法的相互转换原理，并提供可直接用于ROS、Unity等项目的Python实现代码。
1. 四元数与旋转矩阵基础
1.1 四元数的数学本质
四元数由Hamilton于1843年提出，是复数的扩展形式。一个单位四元数可以表示为：
q = w + xi + yj + zk
其中：
w为实部(x,y,z)为虚部- 满足
w² + x² + y² + z² = 1
在三维旋转中，四元数可以解释为绕单位轴n=(nx,ny,nz)旋转角度θ，其分量关系为：
w = cos(θ/2)
x = nx * sin(θ/2)
y = ny * sin(θ/2)
z = nz * sin(θ/2)
1.2 旋转矩阵的几何意义
旋转矩阵是一个3×3的正交矩阵，其每一列代表旋转后坐标系各轴在原坐标系中的方向余弦。对于绕任意轴n旋转θ角的变换，旋转矩阵的基本形式为：
R(n,θ) = [(1-cosθ)nx²+cosθ, (1-cosθ)nxny-nzsinθ, (1-cosθ)nxnz+nysinθ;
(1-cosθ)nxny+nzsinθ, (1-cosθ)ny²+cosθ, (1-cosθ)nynz-nxsinθ;
(1-cosθ)nxnz-nysinθ, (1-cosθ)nynz+nxsinθ, (1-cosθ)nz²+cosθ]
2. 四元数转旋转矩阵
2.1 数学推导
通过三角恒等式变换，我们可以将旋转矩阵完全用四元数分量表示。关键推导步骤包括：
-
对角线元素化简：
m11 = 1 - 2y² - 2z² = w² + x² - y² - z² m22 = 1 - 2x² - 2z² = w² - x² + y² - z² m33 = 1 - 2x² - 2y² = w² - x² - y² + z² -
非对角线元素化简：
m12 = 2xy - 2wz m21 = 2xy + 2wz m13 = 2xz + 2wy m31 = 2xz - 2wy m23 = 2yz - 2wx m32 = 2yz + 2wx
最终得到的旋转矩阵形式为：
R = [[1-2y²-2z², 2xy-2wz, 2xz+2wy],
[2xy+2wz, 1-2x²-2z², 2yz-2wx],
[2xz-2wy, 2yz+2wx, 1-2x²-2y²]]
2.2 Python实现
使用NumPy实现四元数到旋转矩阵的转换：
import numpy as np
def quat_to_rot(q):
""" 单位四元数转旋转矩阵 """
w, x, y, z = q
return np.array([
[1-2*y*y-2*z*z, 2*x*y-2*w*z, 2*x*z+2*w*y],
[2*x*y+2*w*z, 1-2*x*x-2*z*z, 2*y*z-2*w*x],
[2*x*z-2*w*y, 2*y*z+2*w*x, 1-2*x*x-2*y*y]
])
注意：输入四元数必须为单位四元数。实际应用中应先进行归一化：
q = q / np.linalg.norm(q)
3. 旋转矩阵转四元数
3.1 转换原理
从旋转矩阵提取四元数的关键在于矩阵对角线元素的和与差：
-
计算迹（trace）：
tr(R) = m11 + m22 + m33 = 4w² - 1 ⇒ w = √(tr(R)+1)/2 -
通过对称元素组合求其他分量：
x = (m32 - m23)/(4w) y = (m13 - m31)/(4w) z = (m21 - m12)/(4w)
3.2 数值稳定性处理
当w接近0时，上述方法会出现数值不稳定。更鲁棒的实现应选择四个分量中绝对值最大的作为计算基准：
def rot_to_quat(R):
""" 旋转矩阵转单位四元数 """
m00, m01, m02 = R[0]
m10, m11, m12 = R[1]
m20, m21, m22 = R[2]
# 计算各分量平方
w_sq = 0.25 * (1 + m00 + m11 + m22)
x_sq = 0.25 * (1 + m00 - m11 - m22)
y_sq = 0.25 * (1 - m00 + m11 - m22)
z_sq = 0.25 * (1 - m00 - m11 + m22)
# 选择最大分量避免除零
max_val = max(w_sq, x_sq, y_sq, z_sq)
if max_val == w_sq:
w = np.sqrt(w_sq)
x = (m21 - m12) / (4*w)
y = (m02 - m20) / (4*w)
z = (m10 - m01) / (4*w)
elif max_val == x_sq:
x = np.sqrt(x_sq)
w = (m21 - m12) / (4*x)
y = (m01 + m10) / (4*x)
z = (m02 + m20) / (4*x)
elif max_val == y_sq:
y = np.sqrt(y_sq)
w = (m02 - m20) / (4*y)
x = (m01 + m10) / (4*y)
z = (m12 + m21) / (4*y)
else:
z = np.sqrt(z_sq)
w = (m10 - m01) / (4*z)
x = (m02 + m20) / (4*z)
y = (m12 + m21) / (4*z)
return np.array([w, x, y, z])
4. 工程实践中的关键问题
4.1 数值误差处理
在实际应用中，累积的数值误差可能导致旋转矩阵不再严格正交。建议添加正交化步骤：
def orthonormalize(R):
""" 施密特正交化旋转矩阵 """
u, s, vh = np.linalg.svd(R)
return u @ vh
4.2 四元数规范化
从旋转矩阵转换得到的四元数应始终满足单位约束：
q = q / np.linalg.norm(q) # 强制单位化
4.3 特殊情形处理
当旋转角度接近π时，存在数值稳定性问题。此时可采用以下策略：
if abs(w) < 1e-6: # 非常小的旋转角度
angle = 2 * np.arcsin(np.sqrt(x*x + y*y + z*z))
axis = np.array([x, y, z]) / np.sin(angle/2)
# 使用轴角表示进行插值等操作
5. 完整代码示例
以下是一个集成了所有功能的Python类，适用于机器人项目：
import numpy as np
class RotationConverter:
@staticmethod
def quat_to_rot(q, normalize=True):
if normalize:
q = q / np.linalg.norm(q)
w, x, y, z = q
return np.array([
[1-2*y*y-2*z*z, 2*x*y-2*w*z, 2*x*z+2*w*y],
[2*x*y+2*w*z, 1-2*x*x-2*z*z, 2*y*z-2*w*x],
[2*x*z-2*w*y, 2*y*z+2*w*x, 1-2*x*x-2*y*y]
])
@staticmethod
def rot_to_quat(R):
R = np.asarray(R)
if R.shape != (3,3):
raise ValueError("Input must be 3x3 matrix")
m00, m01, m02 = R[0]
m10, m11, m12 = R[1]
m20, m21, m22 = R[2]
tr = m00 + m11 + m22
if tr > 0:
S = np.sqrt(tr + 1.0) * 2
w = 0.25 * S
x = (m21 - m12) / S
y = (m02 - m20) / S
z = (m10 - m01) / S
elif (m00 > m11) and (m00 > m22):
S = np.sqrt(1.0 + m00 - m11 - m22) * 2
w = (m21 - m12) / S
x = 0.25 * S
y = (m01 + m10) / S
z = (m02 + m20) / S
elif m11 > m22:
S = np.sqrt(1.0 + m11 - m00 - m22) * 2
w = (m02 - m20) / S
x = (m01 + m10) / S
y = 0.25 * S
z = (m12 + m21) / S
else:
S = np.sqrt(1.0 + m22 - m00 - m11) * 2
w = (m10 - m01) / S
x = (m02 + m20) / S
y = (m12 + m21) / S
z = 0.25 * S
q = np.array([w, x, y, z])
return q / np.linalg.norm(q)
@staticmethod
def test_conversion(q, tol=1e-6):
R = RotationConverter.quat_to_rot(q)
q_recon = RotationConverter.rot_to_quat(R)
return np.allclose(q, q_recon, atol=tol) or np.allclose(q, -q_recon, atol=tol)
6. 在ROS和Unity中的应用
6.1 ROS中的实现
ROS中常用tf库进行坐标变换。以下示例展示如何在ROS中使用我们的转换方法：
import tf
from geometry_msgs.msg import Quaternion
def ros_quat_to_numpy(ros_quat):
return np.array([ros_quat.w, ros_quat.x, ros_quat.y, ros_quat.z])
def numpy_to_ros_quat(np_quat):
q = Quaternion()
q.w, q.x, q.y, q.z = np_quat
return q
# 使用示例
ros_quat = ... # 从ROS话题获取
np_quat = ros_quat_to_numpy(ros_quat)
R = RotationConverter.quat_to_rot(np_quat)
# 进行矩阵运算后转回四元数
new_quat = RotationConverter.rot_to_quat(R)
ros_new_quat = numpy_to_ros_quat(new_quat)
6.2 Unity中的实现
Unity使用左手坐标系且四元数分量顺序略有不同：
def unity_to_standard(q_unity):
""" 转换Unity四元数到标准数学表示 """
return np.array([q_unity.w, q_unity.x, q_unity.y, q_unity.z])
def standard_to_unity(q):
""" 转换标准四元数到Unity表示 """
return {"w": q[0], "x": q[1], "y": q[2], "z": q[3]}
# 在Unity脚本中的应用示例
unity_quat = ... # 从Unity获取
np_quat = unity_to_standard(unity_quat)
R = RotationConverter.quat_to_rot(np_quat)
# 进行矩阵操作后转回四元数
new_quat = RotationConverter.rot_to_quat(R)
unity_new_quat = standard_to_unity(new_quat)
7. 性能优化与进阶技巧
7.1 利用SIMD指令加速
对于需要处理大量四元数的应用，可以使用NumPy的向量化运算：
def batch_quat_to_rot(quats):
""" 批量转换四元数到旋转矩阵 """
quats = np.asarray(quats)
if quats.ndim == 1:
quats = quats[np.newaxis, :]
w, x, y, z = quats.T
xx = x * x
yy = y * y
zz = z * z
xy = x * y
xz = x * z
yz = y * z
wx = w * x
wy = w * y
wz = w * z
R = np.empty((len(quats), 3, 3))
R[:, 0, 0] = 1 - 2 * (yy + zz)
R[:, 0, 1] = 2 * (xy - wz)
R[:, 0, 2] = 2 * (xz + wy)
R[:, 1, 0] = 2 * (xy + wz)
R[:, 1, 1] = 1 - 2 * (xx + zz)
R[:, 1, 2] = 2 * (yz - wx)
R[:, 2, 0] = 2 * (xz - wy)
R[:, 2, 1] = 2 * (yz + wx)
R[:, 2, 2] = 1 - 2 * (xx + yy)
return R.squeeze()
7.2 四元数插值（Slerp）
在动画和轨迹规划中，四元数球面线性插值（Slerp）比矩阵插值更高效：
def quat_slerp(q1, q2, t):
""" 四元数球面线性插值 """
q1 = q1 / np.linalg.norm(q1)
q2 = q2 / np.linalg.norm(q2)
dot = np.dot(q1, q2)
# 确保取最短路径
if dot < 0:
q2 = -q2
dot = -dot
DOT_THRESHOLD = 0.9995
if dot > DOT_THRESHOLD:
result = q1 + t * (q2 - q1)
return result / np.linalg.norm(result)
theta_0 = np.arccos(dot)
theta = theta_0 * t
sin_theta = np.sin(theta)
sin_theta_0 = np.sin(theta_0)
s0 = np.cos(theta) - dot * sin_theta / sin_theta_0
s1 = sin_theta / sin_theta_0
return (s0 * q1) + (s1 * q2)
7.3 四元数微分方程
对于姿态估计和控制系统，常需要处理四元数的微分：
def quat_derivative(q, omega):
""" 计算四元数导数 given角速度omega """
w, x, y, z = q
mat = np.array([
[-x, -y, -z],
[ w, -z, y],
[ z, w, -x],
[-y, x, w]
])
return 0.5 * mat @ omega
8. 测试与验证
为确保转换的正确性，建议实现以下测试用例：
def test_rotation_conversions():
# 测试随机旋转
np.random.seed(42)
for _ in range(100):
axis = np.random.randn(3)
axis /= np.linalg.norm(axis)
angle = np.random.uniform(0, 2*np.pi)
# 创建测试四元数
w = np.cos(angle/2)
x, y, z = axis * np.sin(angle/2)
q = np.array([w, x, y, z])
# 转换测试
R = RotationConverter.quat_to_rot(q)
q_recon = RotationConverter.rot_to_quat(R)
# 四元数q和-q表示相同旋转
assert np.allclose(q, q_recon) or np.allclose(q, -q_recon)
# 测试旋转一致性
v = np.random.randn(3)
v_rot1 = R @ v
q_v = np.hstack([0, v])
q_rot = quat_mult(quat_mult(q, q_v), quat_conj(q))
v_rot2 = q_rot[1:]
assert np.allclose(v_rot1, v_rot2)
print("所有测试通过！")
def quat_mult(q1, q2):
""" 四元数乘法 """
w1, x1, y1, z1 = q1
w2, x2, y2, z2 = q2
return np.array([
w1*w2 - x1*x2 - y1*y2 - z1*z2,
w1*x2 + x1*w2 + y1*z2 - z1*y2,
w1*y2 - x1*z2 + y1*w2 + z1*x2,
w1*z2 + x1*y2 - y1*x2 + z1*w2
])
def quat_conj(q):
""" 四元数共轭 """
return np.array([q[0], -q[1], -q[2], -q[3]])
更多推荐
8
0- 0
已为社区贡献1条内容
所有评论(0)