1. 基本定义
给定关节角向量：
θ = [θ₁, θ₂, θ₃, θ₄, θ₅, θ₆, θ₇]ᵀ
目标：计算末端执行器相对于基座坐标系的位姿，表示为齐次变换矩阵 ⁿ⁰T₇（即从坐标系 0 到坐标系 7 的变换）。
2. 齐次变换总公式
ⁿ⁰T₇ = ⁿ⁰T₁(θ₁) · ⁿ¹T₂(θ₂) · ⁿ²T₃(θ₃) · ⁿ³T₄(θ₄) · ⁿ⁴T₅(θ₅) · ⁿ⁵T₆(θ₆) · ⁿ⁶T₇(θ₇)
其中“·”表示矩阵乘法。
3. 单个关节的 D–H 齐次变换矩阵
对于第 i 个关节（i = 1 到 7），其从坐标系 {i−1} 到 {i} 的变换矩阵为：
ⁿⁱ⁻¹Tᵢ =
⎡ cosθᵢ −sinθᵢ·cosαᵢ₋₁ sinθᵢ·sinαᵢ₋₁ aᵢ₋₁·cosθᵢ ⎤
⎢ sinθᵢ cosθᵢ·cosαᵢ₋₁ −cosθᵢ·sinαᵢ₋₁ aᵢ₋₁·sinθᵢ ⎥
⎢ 0 sinαᵢ₋₁ cosαᵢ₋₁ dᵢ ⎥
⎣ 0 0 0 1 ⎦
其中：
- θᵢ：关节变量（旋转角，对于旋转关节）
- dᵢ：沿 zᵢ₋₁ 轴的偏移距离
- aᵢ₋₁：沿 xᵢ₋₁ 轴的连杆长度
- αᵢ₋₁：绕 xᵢ₋₁ 轴的连杆扭转角
注：对于典型的七轴协作机器人（如 KUKA LBR iiwa、UR 系列扩展型等），通常全部为旋转关节，因此 θᵢ 为变量，dᵢ、aᵢ₋₁、αᵢ₋₁ 为结构常数。
4. 计算步骤
- 根据机器人结构，确定标准 D–H 参数表（共 7 行，每行对应一个关节）。
- 对每个 i ∈ {1, 2, ..., 7}，代入当前 θᵢ 值，计算 ⁿⁱ⁻¹Tᵢ。
- 按顺序左乘所有变换矩阵，得到最终的 ⁿ⁰T₇。
- 从 ⁿ⁰T₇ 中提取：
- 位置：p = [T₁₄, T₂₄, T₃₄]ᵀ
- 姿态：旋转矩阵 R = [[T₁₁, T₁₂, T₁₃], [T₂₁, T₂₂, T₂₃], [T₃₁, T₃₂, T₃₃]]
5. 示例
若已知 D–H 参数，则可编程实现如下伪代码逻辑：
T = I₄ // 4×4 单位矩阵
for i from 1 to 7:
T_i = DH_Transform(theta[i], d[i], a[i-1], alpha[i-1])
T = T @ T_i // 矩阵乘法
end
// T 即为 ⁿ⁰T₇
