几何与空间变换¶
几何与空间变换是机器人学的核心，用于描述机器人在三维空间中的位置和姿态。
学习目标¶
完成本章后，你将能够：
- 理解坐标系和坐标变换
- 掌握旋转的多种表示方法
- 理解齐次变换矩阵
- 应用空间变换解决机器人学问题
1. 坐标系基础¶
1.1 坐标系的定义¶
坐标系由以下要素定义：
- 原点 (Origin): 参考点
- 基向量 (Basis Vectors): 方向
- 维度 (Dimension): 通常是 2D 或 3D
在机器人学中常用的坐标系：
- 世界坐标系 (World Frame): 全局参考
- 机器人基座标系 (Robot Base Frame): 机器人参考
- 末端执行器坐标系 (End-Effector Frame): 工具参考
- 传感器坐标系 (Sensor Frame): 传感器参考
1.2 坐标表示¶
import numpy as np
# 2D 坐标
point_2d = np.array([3, 4])
# 3D 坐标
point_3d = np.array([1, 2, 3])
# 齐次坐标 (用于统一变换)
# 2D: [x, y, 1]
point_2d_homogeneous = np.array([3, 4, 1])
# 3D: [x, y, z, 1]
point_3d_homogeneous = np.array([1, 2, 3, 1])
2. 旋转表示¶
2.1 旋转矩阵¶
旋转矩阵是最直观的旋转表示方法：
性质：
- 正交矩阵: R^T R = I
- 行列式为 1: det(R) = 1
- 保持长度和角度
2D 旋转矩阵：
R(θ) = [cos(θ) -sin(θ)]
[sin(θ) cos(θ)]
3D 旋转矩阵（绕 Z 轴）：
Rz(θ) = [cos(θ) -sin(θ) 0]
[sin(θ) cos(θ) 0]
[0 0 1]
import numpy as np
def rotation_matrix_2d(theta):
"""2D 旋转矩阵"""
return np.array([
[np.cos(theta), -np.sin(theta)],
[np.sin(theta), np.cos(theta)]
])
def rotation_matrix_z(theta):
"""绕 Z 轴旋转"""
return np.array([
[np.cos(theta), -np.sin(theta), 0],
[np.sin(theta), np.cos(theta), 0],
[0, 0, 1]
])
def rotation_matrix_y(theta):
"""绕 Y 轴旋转"""
return np.array([
[ np.cos(theta), 0, np.sin(theta)],
[ 0, 1, 0 ],
[-np.sin(theta), 0, np.cos(theta)]
])
def rotation_matrix_x(theta):
"""绕 X 轴旋转"""
return np.array([
[1, 0, 0 ],
[0, np.cos(theta), -np.sin(theta)],
[0, np.sin(theta), np.cos(theta)]
])
# 示例
theta = np.radians(45) # 45度
R = rotation_matrix_z(theta)
print("旋转矩阵:")
print(R)
# 验证正交性
print("R^T R = I:", np.allclose(R.T @ R, np.eye(3)))
print("det(R) = 1:", np.isclose(np.linalg.det(R), 1))
2.2 欧拉角¶
欧拉角用三个角度描述旋转：
常见约定：
- ZYX: 先绕 Z，再绕 Y，最后绕 X (Roll-Pitch-Yaw)
- XYZ: 先绕 X，再绕 Y，最后绕 Z
- ZYZ: 先绕 Z，再绕 Y，最后绕 Z
ZYX 欧拉角：
R = Rz(α) Ry(β) Rx(γ)
其中：
- α: 偏航角 (Yaw)
- β: 俯仰角 (Pitch)
- γ: 滚转角 (Roll)
import numpy as np
from scipy.spatial.transform import Rotation
def euler_to_rotation_matrix(roll, pitch, yaw):
"""欧拉角转旋转矩阵 (ZYX 约定)"""
Rz = rotation_matrix_z(yaw)
Ry = rotation_matrix_y(pitch)
Rx = rotation_matrix_x(roll)
return Rz @ Ry @ Rx
def rotation_matrix_to_euler(R):
"""旋转矩阵转欧拉角 (ZYX 约定)"""
# 提取角度
pitch = np.arctan2(-R[2, 0], np.sqrt(R[0, 0]**2 + R[1, 0]**2))
if np.isclose(np.cos(pitch), 0):
# 万向锁
yaw = np.arctan2(R[0, 1], R[1, 1])
roll = 0
else:
yaw = np.arctan2(R[1, 0], R[0, 0])
roll = np.arctan2(R[2, 1], R[2, 2])
return roll, pitch, yaw
# 示例
roll = np.radians(30)
pitch = np.radians(45)
yaw = np.radians(60)
# 欧拉角转旋转矩阵
R = euler_to_rotation_matrix(roll, pitch, yaw)
print("旋转矩阵:")
print(R)
# 旋转矩阵转欧拉角
roll2, pitch2, yaw2 = rotation_matrix_to_euler(R)
print(f"\n欧拉角 (度):")
print(f"Roll: {np.degrees(roll2):.2f}")
print(f"Pitch: {np.degrees(pitch2):.2f}")
print(f"Yaw: {np.degrees(yaw2):.2f}")
# 使用 scipy
r = Rotation.from_euler('zyx', [yaw, pitch, roll])
print("\n使用 scipy:")
print("旋转矩阵:", r.as_matrix())
print("欧拉角:", r.as_euler('zyx', degrees=True))
2.3 万向锁问题¶
import numpy as np
def demonstrate_gimbal_lock():
"""演示万向锁问题"""
# 当 pitch = 90度时，发生万向锁
roll = np.radians(30)
pitch = np.radians(90) # 接近 90 度
yaw = np.radians(45)
R = euler_to_rotation_matrix(roll, pitch, yaw)
# 尝试恢复欧拉角
roll2, pitch2, yaw2 = rotation_matrix_to_euler(R)
print(f"原始欧拉角: roll={np.degrees(roll):.1f}, "
f"pitch={np.degrees(pitch):.1f}, yaw={np.degrees(yaw):.1f}")
print(f"恢复欧拉角: roll={np.degrees(roll2):.1f}, "
f"pitch={np.degrees(pitch2):.1f}, yaw={np.degrees(yaw2):.1f}")
# 注意：roll 和 yaw 会变化，但旋转矩阵相同
# 这就是万向锁问题
demonstrate_gimbal_lock()
2.4 轴角表示¶
轴角表示用一个旋转轴和一个旋转角描述旋转：
定义：
- 旋转轴: ω = [ωx, ωy, ωz] (单位向量)
- 旋转角: θ
旋转矩阵：
R = I + sin(θ)[ω]× + (1 - cos(θ))[ω]ײ
其中 [ω]× 是 ω 的反对称矩阵
import numpy as np
def skew_symmetric(v):
"""反对称矩阵"""
return np.array([
[0, -v[2], v[1]],
[v[2], 0, -v[0]],
[-v[1], v[0], 0]
])
def axis_angle_to_rotation(axis, theta):
"""轴角转旋转矩阵"""
# 单位化旋转轴
axis = axis / np.linalg.norm(axis)
# 反对称矩阵
K = skew_symmetric(axis)
# 罗德里格斯公式
R = np.eye(3) + np.sin(theta) * K + (1 - np.cos(theta)) * K @ K
return R
def rotation_to_axis_angle(R):
"""旋转矩阵转轴角"""
# 计算旋转角
theta = np.arccos((np.trace(R) - 1) / 2)
if np.isclose(theta, 0):
# 无旋转
return np.array([0, 0, 1]), 0
if np.isclose(theta, np.pi):
# 180度旋转
# 需要特殊处理
pass
# 计算旋转轴
axis = np.array([
R[2, 1] - R[1, 2],
R[0, 2] - R[2, 0],
R[1, 0] - R[0, 1]
]) / (2 * np.sin(theta))
return axis, theta
# 示例
axis = np.array([0, 0, 1]) # 绕 Z 轴
theta = np.radians(45) # 45度
# 轴角转旋转矩阵
R = axis_angle_to_rotation(axis, theta)
print("旋转矩阵:")
print(R)
# 旋转矩阵转轴角
axis2, theta2 = rotation_to_axis_angle(R)
print(f"\n旋转轴: {axis2}")
print(f"旋转角: {np.degrees(theta2):.2f} 度")
2.5 四元数¶
四元数是旋转的 4 参数表示，避免了万向锁问题：
定义：
q = [w, x, y, z] = [cos(θ/2), sin(θ/2)ω]
其中：
- w: 标量部分
- [x, y, z]: 向量部分
- ω: 旋转轴
- θ: 旋转角
性质：
- ||q|| = 1 (单位四元数)
- q* = [w, -x, -y, -z] (共轭)
- q⁻¹ = q* / ||q||² (逆)
import numpy as np
from scipy.spatial.transform import Rotation
def quaternion_multiply(q1, q2):
"""四元数乘法"""
w1, x1, y1, z1 = q1
w2, x2, y2, z2 = q2
w = w1*w2 - x1*x2 - y1*y2 - z1*z2
x = w1*x2 + x1*w2 + y1*z2 - z1*y2
y = w1*y2 - x1*z2 + y1*w2 + z1*x2
z = w1*z2 + x1*y2 - y1*x2 + z1*w2
return np.array([w, x, y, z])
def quaternion_conjugate(q):
"""四元数共轭"""
w, x, y, z = q
return np.array([w, -x, -y, -z])
def quaternion_to_rotation_matrix(q):
"""四元数转旋转矩阵"""
w, x, y, z = q
R = 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]
])
return R
def rotation_matrix_to_quaternion(R):
"""旋转矩阵转四元数"""
trace = np.trace(R)
if trace > 0:
s = 0.5 / np.sqrt(trace + 1.0)
w = 0.25 / s
x = (R[2, 1] - R[1, 2]) * s
y = (R[0, 2] - R[2, 0]) * s
z = (R[1, 0] - R[0, 1]) * s
elif R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:
s = 2.0 * np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2])
w = (R[2, 1] - R[1, 2]) / s
x = 0.25 * s
y = (R[0, 1] + R[1, 0]) / s
z = (R[0, 2] + R[2, 0]) / s
elif R[1, 1] > R[2, 2]:
s = 2.0 * np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2])
w = (R[0, 2] - R[2, 0]) / s
x = (R[0, 1] + R[1, 0]) / s
y = 0.25 * s
z = (R[1, 2] + R[2, 1]) / s
else:
s = 2.0 * np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1])
w = (R[1, 0] - R[0, 1]) / s
x = (R[0, 2] + R[2, 0]) / s
y = (R[1, 2] + R[2, 1]) / s
z = 0.25 * s
return np.array([w, x, y, z])
# 示例
# 创建旋转
r = Rotation.from_euler('z', 45, degrees=True)
# 四元数
q = r.as_quat() # [x, y, z, w] 格式
print("四元数 (scipy):", q)
# 转换为 [w, x, y, z] 格式
q_wxyz = np.array([q[3], q[0], q[1], q[2]])
print("四元数 [w,x,y,z]:", q_wxyz)
# 四元数转旋转矩阵
R = quaternion_to_rotation_matrix(q_wxyz)
print("\n旋转矩阵:")
print(R)
# 验证
print("\n验证旋转矩阵:", np.allclose(R, r.as_matrix()))
2.6 四元数插值 (SLERP)¶
import numpy as np
from scipy.spatial.transform import Rotation, Slerp
def 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
# 如果四元数非常接近，使用线性插值
if dot > 0.9995:
result = q1 + t * (q2 - q1)
return result / np.linalg.norm(result)
# 计算角度
theta = np.arccos(dot)
# 计算插值
q1_factor = np.sin((1 - t) * theta) / np.sin(theta)
q2_factor = np.sin(t * theta) / np.sin(theta)
return q1_factor * q1 + q2_factor * q2
# 示例
# 两个旋转
r1 = Rotation.from_euler('z', 0, degrees=True)
r2 = Rotation.from_euler('z', 90, degrees=True)
# 插值
key_times = [0, 1]
key_rots = Rotation.concatenate([r1, r2])
slerp_interp = Slerp(key_times, key_rots)
# 在 t=0.5 处插值
t = 0.5
r_interp = slerp_interp(t)
print(f"旋转 1: {r1.as_euler('zyx', degrees=True)}")
print(f"旋转 2: {r2.as_euler('zyx', degrees=True)}")
print(f"插值 (t={t}): {r_interp.as_euler('zyx', degrees=True)}")
3. 齐次变换¶
3.1 齐次变换矩阵¶
齐次变换矩阵统一了旋转和平移：
定义：
T = [R d]
[0 1]
其中：
- R: 3×3 旋转矩阵
- d: 3×1 平移向量
- 0: 1×3 零向量
- 1: 标量
性质：
- 统一表示旋转和平移
- 可以链式相乘
- 逆变换有解析形式
import numpy as np
def homogeneous_transform(R, d):
"""创建齐次变换矩阵"""
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = d
return T
def transform_point(T, point):
"""变换点"""
# 转换为齐次坐标
point_h = np.append(point, 1)
# 变换
transformed_h = T @ point_h
# 转换回笛卡尔坐标
return transformed_h[:3]
def transform_inverse(T):
"""计算逆变换"""
R = T[:3, :3]
d = T[:3, 3]
T_inv = np.eye(4)
T_inv[:3, :3] = R.T
T_inv[:3, 3] = -R.T @ d
return T_inv
# 示例
# 创建变换：绕 Z 轴旋转 45 度，平移 [1, 2, 3]
theta = np.radians(45)
R = rotation_matrix_z(theta)
d = np.array([1, 2, 3])
T = homogeneous_transform(R, d)
print("齐次变换矩阵:")
print(T)
# 变换点
point = np.array([1, 0, 0])
transformed_point = transform_point(T, point)
print(f"\n原始点: {point}")
print(f"变换后: {transformed_point}")
# 计算逆变换
T_inv = transform_inverse(T)
print("\n逆变换矩阵:")
print(T_inv)
# 验证：T * T_inv = I
print("\n验证 T * T_inv = I:", np.allclose(T @ T_inv, np.eye(4)))
3.2 变换的组合¶
import numpy as np
def compose_transforms(T1, T2):
"""组合两个变换"""
return T1 @ T2
# 示例：多个变换的组合
# 变换 1：绕 Z 轴旋转 30 度
theta1 = np.radians(30)
R1 = rotation_matrix_z(theta1)
d1 = np.array([1, 0, 0])
T1 = homogeneous_transform(R1, d1)
# 变换 2：绕 Y 轴旋转 45 度
theta2 = np.radians(45)
R2 = rotation_matrix_y(theta2)
d2 = np.array([0, 2, 0])
T2 = homogeneous_transform(R2, d2)
# 变换 3：绕 X 轴旋转 60 度
theta3 = np.radians(60)
R3 = rotation_matrix_x(theta3)
d3 = np.array([0, 0, 3])
T3 = homogeneous_transform(R3, d3)
# 组合变换
T_combined = T1 @ T2 @ T3
print("变换 1:")
print(T1)
print("\n变换 2:")
print(T2)
print("\n变换 3:")
print(T3)
print("\n组合变换:")
print(T_combined)
# 变换点
point = np.array([1, 1, 1])
transformed = transform_point(T_combined, point)
print(f"\n原始点: {point}")
print(f"变换后: {transformed}")
3.3 变换的分解¶
import numpy as np
def decompose_transform(T):
"""分解齐次变换矩阵"""
R = T[:3, :3]
d = T[:3, 3]
# 提取平移
translation = d
# 提取旋转（欧拉角）
from scipy.spatial.transform import Rotation
r = Rotation.from_matrix(R)
euler = r.as_euler('zyx', degrees=True)
return translation, euler
# 示例
T = homogeneous_transform(
rotation_matrix_z(np.radians(45)),
np.array([1, 2, 3])
)
translation, euler = decompose_transform(T)
print("平移:", translation)
print("欧拉角 (ZYX):", euler)
4. 应用示例¶
4.1 机器人正运动学¶
import numpy as np
def forward_kinematics_dh(theta, d, a, alpha):
"""使用 DH 参数计算正运动学"""
# 创建变换矩阵
T = np.eye(4)
for i in range(len(theta)):
# 计算单个关节的变换
ct = np.cos(theta[i])
st = np.sin(theta[i])
ca = np.cos(alpha[i])
sa = np.sin(alpha[i])
Ti = np.array([
[ct, -st*ca, st*sa, a[i]*ct],
[st, ct*ca, -ct*sa, a[i]*st],
[0, sa, ca, d[i] ],
[0, 0, 0, 1 ]
])
T = T @ Ti
return T
# 示例：2 连杆机器人
theta = [np.pi/4, np.pi/6] # 关节角度
d = [0, 0] # 连杆偏移
a = [1, 0.8] # 连杆长度
alpha = [0, 0] # 连杆扭角
T = forward_kinematics_dh(theta, d, a, alpha)
print("末端执行器位姿:")
print(T)
# 提取位置
position = T[:3, 3]
print(f"\n位置: {position}")
4.2 坐标系之间的变换¶
import numpy as np
def transform_between_frames(T_world_A, T_world_B):
"""计算两个坐标系之间的变换"""
# T_A_B = T_world_A^(-1) * T_world_B
T_A_world = transform_inverse(T_world_A)
T_A_B = T_A_world @ T_world_B
return T_A_B
# 示例
# 世界坐标系到坐标系 A 的变换
T_world_A = homogeneous_transform(
rotation_matrix_z(np.radians(30)),
np.array([1, 2, 0])
)
# 世界坐标系到坐标系 B 的变换
T_world_B = homogeneous_transform(
rotation_matrix_z(np.radians(60)),
np.array([3, 4, 0])
)
# 计算 A 到 B 的变换
T_A_B = transform_between_frames(T_world_A, T_world_B)
print("从坐标系 A 到坐标系 B 的变换:")
print(T_A_B)
# 在坐标系 A 中的点
point_A = np.array([1, 0, 0])
# 转换到坐标系 B
point_B = transform_point(T_A_B, point_A)
print(f"\n在坐标系 A 中: {point_A}")
print(f"在坐标系 B 中: {point_B}")
5. 练习题¶
基础练习¶
- 创建绕 Z 轴旋转 90 度的旋转矩阵
- 计算两个四元数的乘积
- 创建齐次变换矩阵并变换一个点
进阶练习¶
- 实现欧拉角到旋转矩阵的转换
- 实现四元数的球面线性插值 (SLERP)
- 使用 DH 参数计算 3 连杆机器人的正运动学
应用练习¶
- 实现机器人坐标系之间的变换
- 使用四元数进行姿态插值
- 实现简单的逆运动学求解器
