齐次变换矩阵的原理与应用

通过齐次变换矩阵，可以描述机械臂末端执行器（法兰）在三维空间中的平移和旋转操作。该矩阵结合了旋转和平移信息，用于坐标变换。

1. 齐次变换矩阵的基本形式

一个齐次变换矩阵 T是一个 4x4 矩阵，表示刚体的旋转和平移：
$$T=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]=\left[\begin{array}{cccc}{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{array}\right]$$

• R是 3×3 的旋转矩阵，描述物体的姿态。
• t = [x, y, z]^T 的平移向量，描述物体的位置。
• 最后一行 [0,0,0,1] 保持矩阵的数学性质。

2. 平移和旋转的数学表达

平移矩阵

平移矩阵 Tmove 用于描述物体在空间中的移动：
$$T_{\text{move}}=\left[\begin{array}{cccc}1& 0& 0& \Delta x\\ 0& 1& 0& \Delta y\\ 0& 0& 1& \Delta z\\ 0& 0& 0& 1\end{array}\right]$$
其中，Δx,Δy,Δz是沿 X、Y、Z 方向的移动距离。

旋转矩阵

旋转矩阵用于描述物体在各轴上的旋转。常见的旋转矩阵包括：

• 绕 X 轴旋转的旋转矩阵Rx(θ)：
  $$R_x(\theta )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & -\sin \theta \\ 0& \sin \theta & \cos \theta \end{array}\right]$$

• 绕 Y 轴旋转的旋转矩阵 Ry(θ)：
  $$R_y(\theta )=\left[\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right]$$

• 绕 Z 轴旋转的旋转矩阵 Rz(θ)：
  $$R_z(\theta )=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]$$

综合旋转矩阵 R

综合旋转矩阵 RRR 是三个单轴旋转矩阵的乘积：
$$R=R_z({\theta }_z)\cdot R_y({\theta }_y)\cdot R_x({\theta }_x)$$

3. 位姿变换的数学运算

假设当前位姿由齐次变换矩阵 Tnow 表示，可以通过乘以平移或旋转矩阵，得到新的目标位姿 Ttarget：
$$T_{\text{target}}=T_{\text{now}}\cdot T_{\text{move}}$$
矩阵的乘法顺序表示变换的执行顺序，顺序不同，结果会有所不同。

4. 从变换矩阵中提取位姿

计算目标变换矩阵后，可以从矩阵中提取出新的位置和姿态：

• 平移位置：从 Ttarget 的右上角元素 [x, y, z]^T 提取平移分量。
• 姿态（欧拉角）：从旋转矩阵部分提取 RX、RY、RZ 角度。

提取 RX、RY、RZ 的公式如下：
$$\text{rx}=\arctan 2(T_{32},T_{33})$$

$$\text{ry}=\arcsin (-T_{31})$$

$$\text{rz}=\arctan 2(T_{21},T_{11})$$