对多连杆机器人进行运动学正解与逆解的建模过程

对多连杆机器人进行运动学正解与逆解的建模

机械臂不同于移动质点机器人，是一种多关节组成的多连杆机器人，每个关节的角度所构成的向量也被称作机器人的广义坐标，即为机器人的构型空间（Comfiguration Space,C-Space）,也称为C-Space。C-Space与机器人实际形状、尺寸有关以及关节数量相关，以至于构型空间的描述则与笛卡尔空间有极大的不同。机器人的规划与控制一般在机器人C-Space开展，但是具体的工作任务一般在笛卡尔空间中进行，所以建立机械臂联系构型空间与笛卡尔空间的运动学计算模型是必要的。

一、首先使用DH参数建立机械臂的正解模型（Denavit-Hartenberg,DH）

在机器人学中，使用DH参数法来描述机器人连杆和关节之间的相对位置和方向。从i-1轴到i轴的变换矩阵是DH参数法的核心内容，它描述了如何将一个连杆的坐标系转换到下一个连杆的坐标系。第 iii 个坐标系相对于第 i−1i-1i−1 个坐标系的齐次变换矩阵可表示为四个基本变换的复合。

绕 zi−1z_{i-1}zi−1 轴旋转 θi\theta_iθi，使xi−1x_{i-1}xi−1轴转向x_i轴：

Rz(θi)=[cos⁡θi−sin⁡θi0sin⁡θicos⁡θi0001]R_z(\theta_i) = \begin{bmatrix} \cos\theta_i & -\sin\theta_i & 0 \\ \sin\theta_i & \cos\theta_i & 0 \\ 0 & 0 & 1 \end{bmatrix}Rz(θi)= cosθisinθi0−sinθicosθi0001

沿 zi−1z_{i-1}zi−1 轴平移 did_idi，将旋转后的坐标系沿 zi−1z_{i-1}zi−1轴平移 did_idi，使 xi−1x_{i-1}xi−1轴与xix_ixi轴共线：
Tz(di)=[10di]T_z(d_i) = \begin{bmatrix} 1 \\ 0 \\ d_i \end{bmatrix}Tz(di)= 10di

（在齐次坐标中表示为对角矩阵，非齐次部分为此向量）
沿 xix_ixi 轴平移 aia_iai，将平移后的坐标系沿x_i轴平移a_i，使原点o_{i-1}与o_i重合：

Tx(ai)=[ai00]T_x(a_i) = \begin{bmatrix} a_i \\ 0 \\ 0 \end{bmatrix}Tx(ai)= ai00

（同样在齐次坐标中表示）

绕 xix_ixi 轴旋转 αi\alpha_iαi，使zi−1z_{i-1}zi−1轴转向ziz_izi轴：

Rx(αi)=[1000cos⁡αi−sin⁡αi0sin⁡αicos⁡αi]R_x(\alpha_i) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\alpha_i & -\sin\alpha_i \\ 0 & \sin\alpha_i & \cos\alpha_i \end{bmatrix}Rx(αi)= 1000cosαisinαi0−sinαicosαi

将这四个变换矩阵相乘，我们得到从i-1轴到i轴的齐次变换矩阵：

Tii−1=Rz(θi)⋅Tz(di)⋅Tx(ai)⋅Rx(αi)T_i^{i-1} = R_z(\theta_i) \cdot T_z(d_i) \cdot T_x(a_i) \cdot R_x(\alpha_i)Tii−1=Rz(θi)⋅Tz(di)⋅Tx(ai)⋅Rx(αi)

在齐次坐标表示中，这个变换矩阵为：
Tii−1=[cos⁡θi−sin⁡θicos⁡αisin⁡θisin⁡αiaicos⁡θisin⁡θicos⁡θicos⁡αi−cos⁡θisin⁡αiaisin⁡θi0sin⁡αicos⁡αidi0001]T_i^{i-1} = \begin{bmatrix} \cos\theta_i & -\sin\theta_i\cos\alpha_i & \sin\theta_i\sin\alpha_i & a_i\cos\theta_i \\ \sin\theta_i & \cos\theta_i\cos\alpha_i & -\cos\theta_i\sin\alpha_i & a_i\sin\theta_i \\ 0 & \sin\alpha_i & \cos\alpha_i & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix}Tii−1= cosθisinθi00−sinθicosαicosθicosαisinαi0sinθisinαi−cosθisinαicosαi0aicosθiaisinθidi1

其中：

θi\theta_iθi：关节旋转角（绕前一连杆的 zzz 轴）
did_idi：关节沿 zi−1z_{i-1}zi−1 轴的平移距离
aia_iai：连杆长度，即沿当前连杆的 xix_ixi 轴的平移
αi\alpha_iαi：连杆扭转角，即绕当前连杆的 xix_ixi 轴的旋转

这个变换矩阵 Tii−1T_i^{i-1}Tii−1 完整地描述了从连杆i-1的坐标系到连杆i的坐标系的变换关系，是机器人运动学分析的基础。当机器人有n个关节和连杆组成，其基座标下末端的矩阵可以被表示为：
Tn0=T10⋅T21⋅T23⋅....⋅Tn0T_n^{0} = T_1^{0} \cdot T_2^{1}\cdot T_2^{3}\cdot.... \cdot T_n^{0}Tn0=T10⋅T21⋅T23⋅....⋅Tn0

其中Tn0T_n^{0}Tn0就是末端的旋转矩阵，至此机器人正运动模型建立完毕。

二、然后使用阻尼最小二乘法(DLS)建立机械臂逆运动学模型

1. 问题描述

关节变量：θ∈Rn\boldsymbol{\theta}\in\mathbb{R}^nθ∈Rn
末端执行器在笛卡尔空间的位置/姿态（用同质变换矩阵表示）：
Tn0(θ)=[R(θ)p(θ)0T1] T_n^{0}(\boldsymbol{\theta}) = \begin{bmatrix} R(\boldsymbol{\theta}) & \mathbf{p}(\boldsymbol{\theta}) \\ \mathbf{0}^\mathsf{T} & 1 \end{bmatrix} Tn0(θ)=[R(θ)0Tp(θ)1]
给定目标末端位姿 Td=[Rd, pd]T_d = [R_d,\; \mathbf{p}_d]Td=[Rd,pd]，求使 Tn0(θ)≈TdT_n^{0}(\boldsymbol{\theta}) \approx T_dTn0(θ)≈Td 的 θ\boldsymbol{\theta}θ。

2. 线性化误差

在当前关节 θk\boldsymbol{\theta}_kθk 处，末端小增量（由泰勒定理展开获得）：

Td=Tn0(θk+Δθ)≈Tn0(θk)+J(θk) Δθ T_d = T_n^{0}(\boldsymbol{\theta}_k +\Delta\boldsymbol{\theta}) \approx T_n^{0}(\boldsymbol{\theta}_k )+ J(\boldsymbol{\theta}_k)\,\Delta\boldsymbol{\theta} Td=Tn0(θk+Δθ)≈Tn0(θk)+J(θk)Δθ

由于矩阵Tn0(θk+Δθ)T_n^{0}(\boldsymbol{\theta}_k+\Delta\boldsymbol{\theta})Tn0(θk+Δθ)与Tn0(θk)T_n^{0}(\boldsymbol{\theta}_k )Tn0(θk)并不能直接作差所以我们引如Δx\Delta \mathbf{x}Δx来表示两个矩阵之间的姿态与位置之前的误差项（Tn0(θk+Δθ)−Tn0(θk)T_n^{0}(\boldsymbol{\theta}_k+\Delta\boldsymbol{\theta})-T_n^{0}(\boldsymbol{\theta}_k )Tn0(θk+Δθ)−Tn0(θk),这是示意表示）：

Δx=[ΔpΔϕ]≈J(θk) Δθ, \Delta \mathbf{x} = \begin{bmatrix} \Delta \mathbf{p} \\[4pt] \Delta \boldsymbol{\phi} \end{bmatrix} \approx J(\boldsymbol{\theta}_k)\,\Delta\boldsymbol{\theta}, Δx=[ΔpΔϕ]≈J(θk)Δθ,

其中

Δp=pd−p(θk)\Delta \mathbf{p} = \mathbf{p}_d - \mathbf{p}(\boldsymbol{\theta}_k)Δp=pd−p(θk)，
姿态误差可用旋转向量：
Δϕ=vee(RT(θk) Rd−I)\Delta\boldsymbol{\phi} = \mathsf{vee}\bigl(R^T(\boldsymbol{\theta}_k)\,R_d - I\bigr)Δϕ=vee(RT(θk)Rd−I)，
J(θk)∈Rn×mJ(\boldsymbol{\theta}_k)\in\mathbb{R}^{n\times m}J(θk)∈Rn×m 为此时的雅可比矩阵(这里的雅克比矩阵的计算比较复杂，后文将用R3的例子详细介绍整个计算过程）。

当前已经获得了构型空间Δθ\Delta\boldsymbol{\theta}Δθ与笛卡尔空间Δx\Delta \mathbf{x}Δx关联表达式，将其改写后可得到当前问题的目标表达式：
Δθ=J(θk)+Δx \Delta\boldsymbol{\theta} = J(\boldsymbol{\theta}_k)^+ \Delta \mathbf{x} Δθ=J(θk)+Δx

其中 J(θk)+J(\boldsymbol{\theta}_k)^+J(θk)+ 是雅可比矩阵的伪逆（Moore-Penrose逆）：
J(θk)+=(J(θk)TJ(θk))−1J(θk)T\mathbf{J(\boldsymbol{\theta}_k)}^+ = (\mathbf{J(\boldsymbol{\theta}_k)}^T \mathbf{J(\boldsymbol{\theta}_k)})^{-1} \mathbf{J(\boldsymbol{\theta}_k)}^TJ(θk)+=(J(θk)TJ(θk))−1J(θk)T

在获得Δθ\Delta\boldsymbol{\theta}Δθ表达式的同时，问题已经被转化为如何求解Δθ\Delta\boldsymbol{\theta}Δθ方程组。解析法求解方程组是最为通用且快速的，这类方法也被《机器人导论》等教科书广泛介绍，比较著名的IKFAST运算库也集成了该方法，但是从实际使用来看，多数位置将无法获得解析解。所以后文，介绍使用迭代的数值解法。

3. 数值解法------阻尼最小二乘法求解

标准的最小二乘法求解时可能会导致机器人在奇异构型附近，计算J(θk)T\mathbf{J(\boldsymbol{\theta}_k)}^TJ(θk)T矩阵时接近奇异值，导致数值迭代不稳定，所以引入DLS算法中的阻尼项，构造以下优化问题：

min⁡Δθ ∥J Δθ−Δx∥2+λ2 ∥Δθ∥2\min_{\Delta\boldsymbol{\theta}}\;\bigl\|J\,\Delta\boldsymbol{\theta} - \Delta\mathbf{x}\bigr\|^2+ \lambda^2\,\bigl\|\Delta\boldsymbol{\theta}\bigr\|^2Δθmin JΔθ−Δx 2+λ2 Δθ 2

其中 λ>0\lambda>0λ>0 为阻尼因子，用于平衡位置精度和关节移动幅度。

对上式对 Δθ\Delta\boldsymbol{\theta}Δθ 求导并令 =0=0=0：

JT(J Δθ−Δx)+λ2 Δθ=0⟹JTJΔθ−JTΔx+λ2Δθ=0J^\mathsf{T}(J\,\Delta\boldsymbol{\theta} - \Delta\mathbf{x})+ \lambda^2\,\Delta\boldsymbol{\theta} = 0\quad\Longrightarrow\quad \mathbf{J}^T \mathbf{J}\Delta\boldsymbol{\theta} - \mathbf{J}^T\Delta\mathbf{x} + \lambda^2 \Delta\boldsymbol{\theta} = \mathbf{0}JT(JΔθ−Δx)+λ2Δθ=0⟹JTJΔθ−JTΔx+λ2Δθ=0

整理后得：

(JTJ+λ2I) Δθ=JT Δx \bigl(J^\mathsf{T}J + \lambda^2 I\bigr)\,\Delta\boldsymbol{\theta}= J^\mathsf{T}\,\Delta\mathbf{x}(JTJ+λ2I)Δθ=JTΔx

故得：

Δθ=(JTJ+λ2I)−1JT Δx\Delta\boldsymbol{\theta} = \bigl(J^\mathsf{T}J + \lambda^2 I\bigr)^{-1}J^\mathsf{T}\,\Delta\mathbf{x}Δθ=(JTJ+λ2I)−1JTΔx

4. 迭代更新

令关节在第 kkk 次迭代为 θk\boldsymbol{\theta}_kθk，则：

θk+1=θk+Δθ. \boldsymbol{\theta}_{k+1} = \boldsymbol{\theta}_k + \Delta\boldsymbol{\theta}. θk+1=θk+Δθ.

重复直到：∥Δx∥\|\Delta\mathbf{x}\|∥Δx∥ 或 ∥Δθ∥\|\Delta\boldsymbol{\theta}\|∥Δθ∥ 小于阈值。

阻尼最小二乘法的迭代过程如下：

输入：

目标末端位置 pd∈R3\mathbf{p}_d\in\mathbb{R}^3pd∈R3（及若需姿态，可给定目标旋转矩阵 Rd∈SO(3)R_d\in SO(3)Rd∈SO(3)）

初始关节角 q0∈R6\mathbf{q}_0\in\mathbb{R}^6q0∈R6

阻尼因子 λ>0\lambda>0λ>0，收敛阈值 ϵ>0\epsilon>0ϵ>0，最大迭代次数 Nmax⁡N_{\max}Nmax

输出：

满足 ∥Δx∥<ϵ\|\Delta\mathbf{x}\|<\epsilon∥Δx∥<ϵ 的关节角解 q∗\mathbf{q}^*q∗

迭代流程

初始化

q ← q0,k←0 \mathbf{q} \;\leftarrow\;\mathbf{q}_0,\quad k\leftarrow 0 q←q0,k←0
主循环 （当 k<Nmax⁡k<N_{\max}k<Nmax 且尚未收敛时重复）

a. 正向运动学

T(q)=[R(q)p(q)01] ⟹ p=p(q),R=R(q) T(\mathbf{q}) = \begin{bmatrix} R(\mathbf{q}) & \mathbf{p}(\mathbf{q})\\[4pt] 0 & 1 \end{bmatrix} \;\;\Longrightarrow\;\; \mathbf{p}=\mathbf{p}(\mathbf{q}),\quad R=R(\mathbf{q}) T(q)=[R(q)0p(q)1]⟹p=p(q),R=R(q)

b. 误差向量
- 位置误差
  Δp=pd−p∈R3\;\Delta\mathbf{p} = \mathbf{p}_d - \mathbf{p}\in\mathbb{R}^3Δp=pd−p∈R3.
- 姿态误差（如需）
  
  ΔR=RT Rd,Δϕ=12 [ΔR32−ΔR23ΔR13−ΔR31ΔR21−ΔR12] ∈R3. \Delta R = R^T\,R_d,\quad \Delta\boldsymbol{\phi} = \tfrac12\, \begin{bmatrix} \Delta R_{32}-\Delta R_{23}\\[2pt] \Delta R_{13}-\Delta R_{31}\\[2pt] \Delta R_{21}-\Delta R_{12} \end{bmatrix} \,\in\mathbb{R}^3. ΔR=RTRd,Δϕ=21 ΔR32−ΔR23ΔR13−ΔR31ΔR21−ΔR12 ∈R3.
- 组合成全误差
  
  Δx=[ΔpΔϕ] ∈R6. \Delta\mathbf{x} = \begin{bmatrix}\Delta\mathbf{p}\\[3pt]\Delta\boldsymbol{\phi}\end{bmatrix} \;\in\mathbb{R}^6. Δx=[ΔpΔϕ]∈R6.
c. 收敛判断

∥Δx∥<ϵ⟹收敛，输出 q∗=q \|\Delta\mathbf{x}\| < \epsilon \quad\Longrightarrow\quad \text{收敛，输出 }\mathbf{q}^*=\mathbf{q} ∥Δx∥<ϵ⟹收敛，输出 q∗=q

d. 雅可比矩阵

J(q)=[z0×(on−o0)...z5×(on−o5)z0...z5] ∈R6×6. J(\mathbf{q}) = \begin{bmatrix} z_0\times(o_n-o_0) & \dots & z_5\times(o_n-o_5)\\[5pt] z_0 & \dots & z_5 \end{bmatrix} \;\in\mathbb{R}^{6\times6}. J(q)=[z0×(on−o0)z0......z5×(on−o5)z5]∈R6×6.

e. DLS 伪逆

由于 JJJ 方阵，可直接用

JDLS+ = (JTJ+λ2I)−1JT(= JT(JJT+λ2I)−1). J_{DLS}^+ \;=\;\bigl(J^T J + \lambda^2 I\bigr)^{-1} J^T \quad\Bigl(=\;J^T\bigl(J J^T + \lambda^2 I\bigr)^{-1}\Bigr). JDLS+=(JTJ+λ2I)−1JT(=JT(JJT+λ2I)−1).

f. 关节增量

Δq=JDLS+ Δx=(JTJ+λ2I)−1JT Δx. \Delta\mathbf{q} = J_{DLS}^+ \,\Delta\mathbf{x} = \bigl(J^T J + \lambda^2 I\bigr)^{-1} J^T\,\Delta\mathbf{x}. Δq=JDLS+Δx=(JTJ+λ2I)−1JTΔx.

g. 更新关节角

q ← q+Δq. \mathbf{q}\;\leftarrow\;\mathbf{q} + \Delta\mathbf{q}. q←q+Δq.

h. 归一化

为防止关节角越界，可将每个分量
qi\,q_iqi 约束到 [− π,π][-\,\pi,\pi][−π,π]（或 [qimin,qimax][q_i^\text{min},q_i^\text{max}][qimin,qimax]）区间：

qi ← wrap(qi,−π,π)(i=1,...,6). q_i \;\leftarrow\; \mathrm{wrap}\bigl(q_i,-\pi,\pi\bigr) \quad(i=1,\dots,6). qi←wrap(qi,−π,π)(i=1,...,6).

i. 迭代计数

k ← k+1 k \;\leftarrow\; k+1 k←k+1
结束条件

若达到 Nmax⁡N_{\max}Nmax 仍未满足 ∥Δx∥<ϵ\|\Delta\mathbf{x}\|<\epsilon∥Δx∥<ϵ，则返回当前 q\mathbf{q}q 作为近似解，并报告"未收敛"。

说明：

本流程可同时处理位置与姿态误差，也可仅取位置分量 Δx=Δp∈R3\Delta\mathbf{x}=\Delta\mathbf{p}\in\mathbb{R}^3Δx=Δp∈R3；对应的 JJJ 则取前 3 行。
阻尼 λ\lambdaλ 通常随误差大小调整，可用自适应公式 λ=λ0 e−α∥Δx∥\lambda = \lambda_0 \,e^{-\alpha\|\Delta\mathbf{x}\|}λ=λ0e−α∥Δx∥ 进一步提升收敛性。
当 JJJ 接近奇异时，DLS 保证了 (JTJ+λ2I)\,(J^T J + \lambda^2 I)(JTJ+λ2I) 的良定性，从而避免了数值爆炸。

三、使用平面R3机器人为计算示例

我们的目标是解算空间6轴机器人UR5，但是其计算过程较为复杂，我们用R3机器人进行代替示例，整个过程与UR5计算没有差异，只需在算法中替换DH参数即可。

第一步定义机器人DH参数并建立正解模型

对于平面三轴机器人，DH参数表如下：

连杆 i	ai−1a_{i-1}ai−1 (连杆长度)	θi\theta_iθi (关节角)
1	0	θ1\theta_1θ1
2	l1l_1l1	θ2\theta_2θ2
3	l2l_2l2	θ3\theta_3θ3
末端	l3l_3l3	0

说明：

ai−1a_{i-1}ai−1：沿着 xi−1x_{i-1}xi−1 轴的距离，即连杆长度
αi−1\alpha_{i-1}αi−1：绕着 xi−1x_{i-1}xi−1 轴的旋转角度，平面机器人中通常为0
did_idi：沿着 ziz_izi 轴的距离，平面机器人中通常为0
θi\theta_iθi：绕着 ziz_izi 轴的旋转角度，即关节角

由于这是一个平面三轴机器人，所有运动都在同一平面内，因此所有的 αi−1\alpha_{i-1}αi−1 和 did_idi 均为0。每个连杆的长度由 ai−1a_{i-1}ai−1 表示，而关节变量则是 θi\theta_iθi。

将定义机器人DH参数代入这四个变换矩阵相乘，得到从i-1轴到i轴的齐次变换矩阵：

Tii−1=Rz(θi)⋅Tz(di)⋅Tx(ai)⋅Rx(αi)T_i^{i-1} = R_z(\theta_i) \cdot T_z(d_i) \cdot T_x(a_i) \cdot R_x(\alpha_i)Tii−1=Rz(θi)⋅Tz(di)⋅Tx(ai)⋅Rx(αi)

首先，我将计算 T10⋅T21T_1^0 \cdot T_2^1T10⋅T21：

T10⋅T21=[cos⁡θ1−sin⁡θ100sin⁡θ1cos⁡θ10000100001]⋅[cos⁡θ2−sin⁡θ20l1cos⁡θ2sin⁡θ2cos⁡θ20l1sin⁡θ200100001]T_1^0 \cdot T_2^1 = \begin{bmatrix} \cos\theta_1 & -\sin\theta_1 & 0 & 0 \\ \sin\theta_1 & \cos\theta_1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \cdot \begin{bmatrix} \cos\theta_2 & -\sin\theta_2 & 0 & l_1\cos\theta_2 \\ \sin\theta_2 & \cos\theta_2 & 0 & l_1\sin\theta_2 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}T10⋅T21= cosθ1sinθ100−sinθ1cosθ10000100001 ⋅ cosθ2sinθ200−sinθ2cosθ2000010l1cosθ2l1sinθ201

矩阵相乘后得到：

T20=[cos⁡θ1cos⁡θ2−sin⁡θ1sin⁡θ2−cos⁡θ1sin⁡θ2−sin⁡θ1cos⁡θ20l1cos⁡θ1cos⁡θ2−l1sin⁡θ1sin⁡θ2sin⁡θ1cos⁡θ2+cos⁡θ1sin⁡θ2−sin⁡θ1sin⁡θ2+cos⁡θ1cos⁡θ20l1sin⁡θ1cos⁡θ2+l1cos⁡θ1sin⁡θ200100001]T_2^0 = \begin{bmatrix} \cos\theta_1\cos\theta_2 - \sin\theta_1\sin\theta_2 & -\cos\theta_1\sin\theta_2 - \sin\theta_1\cos\theta_2 & 0 & l_1\cos\theta_1\cos\theta_2 - l_1\sin\theta_1\sin\theta_2 \\ \sin\theta_1\cos\theta_2 + \cos\theta_1\sin\theta_2 & -\sin\theta_1\sin\theta_2 + \cos\theta_1\cos\theta_2 & 0 & l_1\sin\theta_1\cos\theta_2 + l_1\cos\theta_1\sin\theta_2 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}T20= cosθ1cosθ2−sinθ1sinθ2sinθ1cosθ2+cosθ1sinθ200−cosθ1sinθ2−sinθ1cosθ2−sinθ1sinθ2+cosθ1cosθ2000010l1cosθ1cosθ2−l1sinθ1sinθ2l1sinθ1cosθ2+l1cosθ1sinθ201

使用三角恒等式，我们可以简化：
cos⁡θ1cos⁡θ2−sin⁡θ1sin⁡θ2=cos⁡(θ1+θ2)\cos\theta_1\cos\theta_2 - \sin\theta_1\sin\theta_2 = \cos(\theta_1 + \theta_2)cosθ1cosθ2−sinθ1sinθ2=cos(θ1+θ2)
sin⁡θ1cos⁡θ2+cos⁡θ1sin⁡θ2=sin⁡(θ1+θ2)\sin\theta_1\cos\theta_2 + \cos\theta_1\sin\theta_2 = \sin(\theta_1 + \theta_2)sinθ1cosθ2+cosθ1sinθ2=sin(θ1+θ2)
−cos⁡θ1sin⁡θ2−sin⁡θ1cos⁡θ2=−sin⁡(θ1+θ2)-\cos\theta_1\sin\theta_2 - \sin\theta_1\cos\theta_2 = -\sin(\theta_1 + \theta_2)−cosθ1sinθ2−sinθ1cosθ2=−sin(θ1+θ2)
−sin⁡θ1sin⁡θ2+cos⁡θ1cos⁡θ2=cos⁡(θ1+θ2)-\sin\theta_1\sin\theta_2 + \cos\theta_1\cos\theta_2 = \cos(\theta_1 + \theta_2)−sinθ1sinθ2+cosθ1cosθ2=cos(θ1+θ2)

因此：

T20=[cos⁡(θ1+θ2)−sin⁡(θ1+θ2)0l1cos⁡θ1sin⁡(θ1+θ2)cos⁡(θ1+θ2)0l1sin⁡θ100100001]T_2^0 = \begin{bmatrix} \cos(\theta_1 + \theta_2) & -\sin(\theta_1 + \theta_2) & 0 & l_1\cos\theta_1 \\ \sin(\theta_1 + \theta_2) & \cos(\theta_1 + \theta_2) & 0 & l_1\sin\theta_1 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}T20= cos(θ1+θ2)sin(θ1+θ2)00−sin(θ1+θ2)cos(θ1+θ2)000010l1cosθ1l1sinθ101

现在计算 T30=T20⋅T32T_3^0 = T_2^0 \cdot T_3^2T30=T20⋅T32：

T30=[cos⁡(θ1+θ2)−sin⁡(θ1+θ2)0l1cos⁡θ1sin⁡(θ1+θ2)cos⁡(θ1+θ2)0l1sin⁡θ100100001]⋅[cos⁡θ3−sin⁡θ30l2cos⁡θ3sin⁡θ3cos⁡θ30l2sin⁡θ300100001]T_3^0 = \begin{bmatrix} \cos(\theta_1 + \theta_2) & -\sin(\theta_1 + \theta_2) & 0 & l_1\cos\theta_1 \\ \sin(\theta_1 + \theta_2) & \cos(\theta_1 + \theta_2) & 0 & l_1\sin\theta_1 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \cdot \begin{bmatrix} \cos\theta_3 & -\sin\theta_3 & 0 & l_2\cos\theta_3 \\ \sin\theta_3 & \cos\theta_3 & 0 & l_2\sin\theta_3 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}T30= cos(θ1+θ2)sin(θ1+θ2)00−sin(θ1+θ2)cos(θ1+θ2)000010l1cosθ1l1sinθ101 ⋅ cosθ3sinθ300−sinθ3cosθ3000010l2cosθ3l2sinθ301

同样使用三角恒等式进行简化后，得到：

T30=[cos⁡(θ1+θ2+θ3)−sin⁡(θ1+θ2+θ3)0l1cos⁡θ1+l2cos⁡(θ1+θ2)sin⁡(θ1+θ2+θ3)cos⁡(θ1+θ2+θ3)0l1sin⁡θ1+l2sin⁡(θ1+θ2)00100001]T_3^0 = \begin{bmatrix} \cos(\theta_1 + \theta_2 + \theta_3) & -\sin(\theta_1 + \theta_2 + \theta_3) & 0 & l_1\cos\theta_1 + l_2\cos(\theta_1 + \theta_2) \\ \sin(\theta_1 + \theta_2 + \theta_3) & \cos(\theta_1 + \theta_2 + \theta_3) & 0 & l_1\sin\theta_1 + l_2\sin(\theta_1 + \theta_2) \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}T30= cos(θ1+θ2+θ3)sin(θ1+θ2+θ3)00−sin(θ1+θ2+θ3)cos(θ1+θ2+θ3)000010l1cosθ1+l2cos(θ1+θ2)l1sinθ1+l2sin(θ1+θ2)01

最后，为了包含第三个连杆长度 l3l_3l3，我们需要再乘以 Te3T_e^3Te3：

Te0=T30⋅Te3=[cos⁡(θ1+θ2+θ3)−sin⁡(θ1+θ2+θ3)0l1cos⁡θ1+l2cos⁡(θ1+θ2)sin⁡(θ1+θ2+θ3)cos⁡(θ1+θ2+θ3)0l1sin⁡θ1+l2sin⁡(θ1+θ2)00100001]⋅[100l3010000100001]T_e^0 = T_3^0 \cdot T_e^3 = \begin{bmatrix} \cos(\theta_1 + \theta_2 + \theta_3) & -\sin(\theta_1 + \theta_2 + \theta_3) & 0 & l_1\cos\theta_1 + l_2\cos(\theta_1 + \theta_2) \\ \sin(\theta_1 + \theta_2 + \theta_3) & \cos(\theta_1 + \theta_2 + \theta_3) & 0 & l_1\sin\theta_1 + l_2\sin(\theta_1 + \theta_2) \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \cdot \begin{bmatrix} 1 & 0 & 0 & l_3 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}Te0=T30⋅Te3= cos(θ1+θ2+θ3)sin(θ1+θ2+θ3)00−sin(θ1+θ2+θ3)cos(θ1+θ2+θ3)000010l1cosθ1+l2cos(θ1+θ2)l1sinθ1+l2sin(θ1+θ2)01 ⋅ 100001000010l3001

计算后得到：

Te0=[cos⁡(θ1+θ2+θ3)−sin⁡(θ1+θ2+θ3)0l1cos⁡θ1+l2cos⁡(θ1+θ2)+l3cos⁡(θ1+θ2+θ3)sin⁡(θ1+θ2+θ3)cos⁡(θ1+θ2+θ3)0l1sin⁡θ1+l2sin⁡(θ1+θ2)+l3sin⁡(θ1+θ2+θ3)00100001]T_e^0 = \begin{bmatrix} \cos(\theta_1 + \theta_2 + \theta_3) & -\sin(\theta_1 + \theta_2 + \theta_3) & 0 & l_1\cos\theta_1 + l_2\cos(\theta_1 + \theta_2) + l_3\cos(\theta_1 + \theta_2 + \theta_3) \\ \sin(\theta_1 + \theta_2 + \theta_3) & \cos(\theta_1 + \theta_2 + \theta_3) & 0 & l_1\sin\theta_1 + l_2\sin(\theta_1 + \theta_2) + l_3\sin(\theta_1 + \theta_2 + \theta_3) \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}Te0= cos(θ1+θ2+θ3)sin(θ1+θ2+θ3)00−sin(θ1+θ2+θ3)cos(θ1+θ2+θ3)000010l1cosθ1+l2cos(θ1+θ2)+l3cos(θ1+θ2+θ3)l1sinθ1+l2sin(θ1+θ2)+l3sin(θ1+θ2+θ3)01

从这个最终的变换矩阵中，我们可以提取末端执行器的位置坐标：

x 坐标 = l1cos⁡θ1+l2cos⁡(θ1+θ2)+l3cos⁡(θ1+θ2+θ3)l_1\cos\theta_1 + l_2\cos(\theta_1 + \theta_2) + l_3\cos(\theta_1 + \theta_2 + \theta_3)l1cosθ1+l2cos(θ1+θ2)+l3cos(θ1+θ2+θ3)

y 坐标 = l1sin⁡θ1+l2sin⁡(θ1+θ2)+l3sin⁡(θ1+θ2+θ3)l_1\sin\theta_1 + l_2\sin(\theta_1 + \theta_2) + l_3\sin(\theta_1 + \theta_2 + \theta_3)l1sinθ1+l2sin(θ1+θ2)+l3sin(θ1+θ2+θ3)

第二步构建雅克比矩阵:

J(θk)=[−l1sin⁡(θ1)−l2sin⁡(θ12)−l3sin⁡(θ123)−l2sin⁡(θ12)−l3sin⁡(θ123)−l3sin⁡(θ123)l1cos⁡(θ1)+l2cos⁡(θ12)+l3cos⁡(θ123)l2cos⁡(θ12)+l3cos⁡(θ123)l3cos⁡(θ123)]\mathbf{J}(\mathbf{{\theta}k}) = \begin{bmatrix} -l_1\sin(\theta_1) - l_2\sin(\theta{12}) - l_3\sin(\theta_{123}) & -l_2\sin(\theta_{12}) - l_3\sin(\theta_{123}) & -l_3\sin(\theta_{123}) \\ l_1\cos(\theta_1) + l_2\cos(\theta_{12}) + l_3\cos(\theta_{123}) & l_2\cos(\theta_{12}) + l_3\cos(\theta_{123}) & l_3\cos(\theta_{123}) \end{bmatrix}J(θk)=[−l1sin(θ1)−l2sin(θ12)−l3sin(θ123)l1cos(θ1)+l2cos(θ12)+l3cos(θ123)−l2sin(θ12)−l3sin(θ123)l2cos(θ12)+l3cos(θ123)−l3sin(θ123)l3cos(θ123)]

以及按照公式 J(θk)+=(J(θk)TJ(θk))−1J(θk)T\mathbf{J}(\boldsymbol{\theta}_k)^+ = (\mathbf{J}(\boldsymbol{\theta}_k)^T \mathbf{J}(\boldsymbol{\theta}_k))^{-1} \mathbf{J}(\boldsymbol{\theta}_k)^TJ(θk)+=(J(θk)TJ(θk))−1J(θk)T 来计算的伪逆矩阵 J(θk)+\mathbf{J}(\mathbf{\theta}_k)^+J(θk)+：

J+=A−1JT=1det⁡(A)adj(A)JT\mathbf{J}^+ = \mathbf{A}^{-1}\mathbf{J}^T = \frac{1}{\det(\mathbf{A})}\text{adj}(\mathbf{A})\mathbf{J}^TJ+=A−1JT=det(A)1adj(A)JT

其中设 A=JTJ\mathbf{A} = \mathbf{J}^T\mathbf{J}A=JTJ，则 A−1=1det⁡(A)adj(A)\mathbf{A}^{-1} = \frac{1}{\det(\mathbf{A})}\text{adj}(\mathbf{A})A−1=det(A)1adj(A)，且A\mathbf{A}A为：

JTJ=[l12+l22+l32+2l1l2cos⁡(θ2)+2l1l3cos⁡(θ2+θ3)+2l2l3cos⁡(θ3)l22+l32+l1l2cos⁡(θ2)+l1l3cos⁡(θ2+θ3)+2l2l3cos⁡(θ3)l32+l1l3cos⁡(θ2+θ3)+l2l3cos⁡(θ3)l22+l32+l1l2cos⁡(θ2)+l1l3cos⁡(θ2+θ3)+2l2l3cos⁡(θ3)l22+l32+2l2l3cos⁡(θ3)l32+l2l3cos⁡(θ3)l32+l1l3cos⁡(θ2+θ3)+l2l3cos⁡(θ3)l32+l2l3cos⁡(θ3)l32]\mathbf{J}^T\mathbf{J} = \begin{bmatrix} l_1^2 + l_2^2 + l_3^2 + 2l_1l_2\cos(\theta_2) + 2l_1l_3\cos(\theta_2+\theta_3) + 2l_2l_3\cos(\theta_3) & l_2^2 + l_3^2 + l_1l_2\cos(\theta_2) + l_1l_3\cos(\theta_2+\theta_3) + 2l_2l_3\cos(\theta_3) & l_3^2 + l_1l_3\cos(\theta_2+\theta_3) + l_2l_3\cos(\theta_3) \\ l_2^2 + l_3^2 + l_1l_2\cos(\theta_2) + l_1l_3\cos(\theta_2+\theta_3) + 2l_2l_3\cos(\theta_3) & l_2^2 + l_3^2 + 2l_2l_3\cos(\theta_3) & l_3^2 + l_2l_3\cos(\theta_3) \\ l_3^2 + l_1l_3\cos(\theta_2+\theta_3) + l_2l_3\cos(\theta_3) & l_3^2 + l_2l_3\cos(\theta_3) & l_3^2 \end{bmatrix}JTJ= l12+l22+l32+2l1l2cos(θ2)+2l1l3cos(θ2+θ3)+2l2l3cos(θ3)l22+l32+l1l2cos(θ2)+l1l3cos(θ2+θ3)+2l2l3cos(θ3)l32+l1l3cos(θ2+θ3)+l2l3cos(θ3)l22+l32+l1l2cos(θ2)+l1l3cos(θ2+θ3)+2l2l3cos(θ3)l22+l32+2l2l3cos(θ3)l32+l2l3cos(θ3)l32+l1l3cos(θ2+θ3)+l2l3cos(θ3)l32+l2l3cos(θ3)l32

求解J(θk)+\mathbf{J}(\mathbf{\theta}_k)^+J(θk)+矩阵是十分复杂的过程，需要借用计算机中计算库辅助计算，并且在6轴空间机器人的求解中使用svd对求解过程进行通用简化。

第三步代入阻尼最小二乘法阻尼最小二乘法表达式进行迭代优化

输入：

给定目标末端位姿 Td=[Rd, pd]T_d = [R_d,\; \mathbf{p}_d]Td=[Rd,pd]

初始关节角 θk∈R3\mathbf{\theta}_k\in\mathbb{R}^3θk∈R3

阻尼因子 λ>0\lambda>0λ>0，收敛阈值 ϵ>0\epsilon>0ϵ>0，最大迭代次数 Nmax⁡N_{\max}Nmax

输出：

满足 ∥Δx∥<ϵ\|\Delta\mathbf{x}\|<\epsilon∥Δx∥<ϵ 的关节角解 θ\boldsymbol{\theta}θ

阻尼最小二乘法的迭代过程如下：

初始化关节角 θ ← θ0,k←0\mathbf{\theta} \;\leftarrow\;\mathbf{\theta}_0,\quad k\leftarrow 0θ←θ0,k←0
重复以下步骤，直到收敛或达到最大迭代次数：
a. 计算当前末端执行器位置和姿态 Te0(θ)T_e^{0}(\boldsymbol{\theta})Te0(θ)
b. 计算位置和姿态误差Δx\Delta \mathbf{x}Δx
c. 如果 ∥Δx∥<ϵ\|\Delta \mathbf{x}\| < \epsilon∥Δx∥<ϵ（预设阈值），则收敛，退出
d. 计算雅可比矩阵 J\mathbf{J}J
e. 计算DLS逆 JDLS+=JT(JJT+λ2I)−1\mathbf{J}{DLS}^+ = \mathbf{J}^T (\mathbf{J} \mathbf{J}^T + \lambda^2 \mathbf{I})^{-1}JDLS+=JT(JJT+λ2I)−1
f. 计算关节角增量 Δθ=JDLS+Δx\Delta\boldsymbol{\theta} = \mathbf{J}{DLS}^+ \Delta \mathbf{x}Δθ=JDLS+Δx
g. 更新关节角 θk←θk+Δθ\mathbf{\theta}_k \leftarrow \mathbf{\theta}_k + \Delta \mathbf{\theta}θk←θk+Δθ
h. 将关节角归一化到合理范围（例如 [−π,π][-\pi, \pi][−π,π]）

对多连杆机器人进行运动学正解与逆解的建模过程