齐次变换矩阵的原理与应用
通过齐次变换矩阵，可以描述机械臂末端执行器（法兰）在三维空间中的平移和旋转操作。该矩阵结合了旋转和平移信息，用于坐标变换。
1. 齐次变换矩阵的基本形式
一个齐次变换矩阵 T是一个 4x4 矩阵，表示刚体的旋转和平移：
T = R t 0 1 = r 11 r 12 r 13 x r 21 r 22 r 23 y r 31 r 32 r 33 z 0 0 0 1 T = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix} = \begin{bmatrix} r_{11} & r_{12} & r_{13} & x \\ r_{21} & r_{22} & r_{23} & y \\ r_{31} & r_{32} & r_{33} & z \\ 0 & 0 & 0 & 1 \end{bmatrix} T=R0t1= r11r21r310r12r22r320r13r23r330xyz1
- R是 3×3 的旋转矩阵，描述物体的姿态。
- t = x, y, z^T 的平移向量，描述物体的位置。
- 最后一行 0,0,0,1 保持矩阵的数学性质。
2. 平移和旋转的数学表达
平移矩阵
平移矩阵 Tmove 用于描述物体在空间中的移动：
T move = 1 0 0 Δ x 0 1 0 Δ y 0 0 1 Δ z 0 0 0 1 T_{\text{move}} = \begin{bmatrix} 1 & 0 & 0 & \Delta x \\ 0 & 1 & 0 & \Delta y \\ 0 & 0 & 1 & \Delta z \\ 0 & 0 & 0 & 1 \end{bmatrix} Tmove= 100001000010ΔxΔyΔz1
其中，Δx,Δy,Δz是沿 X、Y、Z 方向的移动距离。
旋转矩阵
旋转矩阵用于描述物体在各轴上的旋转。常见的旋转矩阵包括：
-
绕 X 轴旋转的旋转矩阵Rx(θ)：
R x ( θ ) = 1 0 0 0 cos θ − sin θ 0 sin θ cos θ R_x(\theta) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos \theta & -\sin \theta \\ 0 & \sin \theta & \cos \theta \end{bmatrix} Rx(θ)= 1000cosθsinθ0−sinθcosθ -
绕 Y 轴旋转的旋转矩阵 Ry(θ)：
R y ( θ ) = cos θ 0 sin θ 0 1 0 − sin θ 0 cos θ R_y(\theta) = \begin{bmatrix} \cos \theta & 0 & \sin \theta \\ 0 & 1 & 0 \\ -\sin \theta & 0 & \cos \theta \end{bmatrix} Ry(θ)= cosθ0−sinθ010sinθ0cosθ -
绕 Z 轴旋转的旋转矩阵 Rz(θ)：
R z ( θ ) = cos θ − sin θ 0 sin θ cos θ 0 0 0 1 R_z(\theta) = \begin{bmatrix} \cos \theta & -\sin \theta & 0 \\ \sin \theta & \cos \theta & 0 \\ 0 & 0 & 1 \end{bmatrix} Rz(θ)= cosθsinθ0−sinθcosθ0001
综合旋转矩阵 R
综合旋转矩阵 RRR 是三个单轴旋转矩阵的乘积：
R = R z ( θ z ) ⋅ R y ( θ y ) ⋅ R x ( θ x ) R = R_z(\theta_z) \cdot R_y(\theta_y) \cdot R_x(\theta_x) R=Rz(θz)⋅Ry(θy)⋅Rx(θx)
3. 位姿变换的数学运算
假设当前位姿由齐次变换矩阵 Tnow 表示，可以通过乘以平移或旋转矩阵，得到新的目标位姿 Ttarget：
T target = T now ⋅ T move T_{\text{target}} = T_{\text{now}} \cdot T_{\text{move}} Ttarget=Tnow⋅Tmove
矩阵的乘法顺序表示变换的执行顺序，顺序不同，结果会有所不同。
4. 从变换矩阵中提取位姿
计算目标变换矩阵后，可以从矩阵中提取出新的位置和姿态：
- 平移位置：从 Ttarget 的右上角元素 x, y, z^T 提取平移分量。
- 姿态（欧拉角）：从旋转矩阵部分提取 RX、RY、RZ 角度。
提取 RX、RY、RZ 的公式如下：
rx = arctan 2 ( T 32 , T 33 ) \text{rx} = \arctan2(T_{32}, T_{33}) rx=arctan2(T32,T33)
ry = arcsin ( − T 31 ) \text{ry} = \arcsin(-T_{31}) ry=arcsin(−T31)
rz = arctan 2 ( T 21 , T 11 ) \text{rz} = \arctan2(T_{21}, T_{11}) rz=arctan2(T21,T11)