平面三轴机器人的阻尼最小二乘法(DLS)逆运动学求解

平面三轴机器人的阻尼最小二乘法(DLS)逆运动学求解

1. 问题定义

对于平面三轴机器人，我们有：

• 三个连杆，长度分别为 l1l_1l1​, l2l_2l2​, l3l_3l3​
• 三个关节角 θ1\theta_1θ1​, θ2\theta_2θ2​, θ3\theta_3θ3​
• 末端执行器的目标位置 ptarget=[xtarget,ytarget]T\mathbf{p}_{target} = [x_{target}, y_{target}]^Tptarget​=[xtarget​,ytarget​]T

逆运动学问题就是：给定目标位置 ptarget\mathbf{p}_{target}ptarget​，求解关节角度 q=[θ1,θ2,θ3]T\mathbf{q} = [\theta_1, \theta_2, \theta_3]^Tq=[θ1​,θ2​,θ3​]T，使末端执行器位置 p(q)\mathbf{p}(\mathbf{q})p(q) 尽可能接近目标位置。

2. 前向运动学

首先，让我们定义机器人的前向运动学。基座位于原点 (0,0)(0,0)(0,0)，各关节和末端执行器的位置计算如下：

关节1的位置：
p1=[l1cos⁡(θ1)l1sin⁡(θ1)]\mathbf{p}_1 = \begin{bmatrix} l_1\cos(\theta_1) \\ l_1\sin(\theta_1) \end{bmatrix}p1​=[l1​cos(θ1​)l1​sin(θ1​)​]

关节2的位置：
p2=p1+[l2cos⁡(θ1+θ2)l2sin⁡(θ1+θ2)]\mathbf{p}_2 = \mathbf{p}_1 + \begin{bmatrix} l_2\cos(\theta_1 + \theta_2) \\ l_2\sin(\theta_1 + \theta_2) \end{bmatrix}p2​=p1​+[l2​cos(θ1​+θ2​)l2​sin(θ1​+θ2​)​]

末端执行器的位置：
p(q)=p2+[l3cos⁡(θ1+θ2+θ3)l3sin⁡(θ1+θ2+θ3)]\mathbf{p}(\mathbf{q}) = \mathbf{p}_2 + \begin{bmatrix} l_3\cos(\theta_1 + \theta_2 + \theta_3) \\ l_3\sin(\theta_1 + \theta_2 + \theta_3) \end{bmatrix}p(q)=p2​+[l3​cos(θ1​+θ2​+θ3​)l3​sin(θ1​+θ2​+θ3​)​]

3. 雅可比矩阵

雅可比矩阵 J(q)\mathbf{J}(\mathbf{q})J(q) 表示末端执行器位置对关节角的偏导数：

J(q)=∂p∂q=[∂x∂θ1∂x∂θ2∂x∂θ3∂y∂θ1∂y∂θ2∂y∂θ3]\mathbf{J}(\mathbf{q}) = \frac{\partial \mathbf{p}}{\partial \mathbf{q}} = \begin{bmatrix} \frac{\partial x}{\partial \theta_1} & \frac{\partial x}{\partial \theta_2} & \frac{\partial x}{\partial \theta_3} \\ \frac{\partial y}{\partial \theta_1} & \frac{\partial y}{\partial \theta_2} & \frac{\partial y}{\partial \theta_3} \end{bmatrix}J(q)=∂q∂p​=[∂θ1​∂x​∂θ1​∂y​​∂θ2​∂x​∂θ2​∂y​​∂θ3​∂x​∂θ3​∂y​​]

对于平面三轴机器人，计算各偏导数：

设 θ12=θ1+θ2\theta_{12} = \theta_1 + \theta_2θ12​=θ1​+θ2​ 和 θ123=θ1+θ2+θ3\theta_{123} = \theta_1 + \theta_2 + \theta_3θ123​=θ1​+θ2​+θ3​

对 θ1\theta_1θ1​ 的偏导数：
∂x∂θ1=−l1sin⁡(θ1)−l2sin⁡(θ12)−l3sin⁡(θ123)\frac{\partial x}{\partial \theta_1} = -l_1\sin(\theta_1) - l_2\sin(\theta_{12}) - l_3\sin(\theta_{123})∂θ1​∂x​=−l1​sin(θ1​)−l2​sin(θ12​)−l3​sin(θ123​)
∂y∂θ1=l1cos⁡(θ1)+l2cos⁡(θ12)+l3cos⁡(θ123)\frac{\partial y}{\partial \theta_1} = l_1\cos(\theta_1) + l_2\cos(\theta_{12}) + l_3\cos(\theta_{123})∂θ1​∂y​=l1​cos(θ1​)+l2​cos(θ12​)+l3​cos(θ123​)

对 θ2\theta_2θ2​ 的偏导数：
∂x∂θ2=−l2sin⁡(θ12)−l3sin⁡(θ123)\frac{\partial x}{\partial \theta_2} = -l_2\sin(\theta_{12}) - l_3\sin(\theta_{123})∂θ2​∂x​=−l2​sin(θ12​)−l3​sin(θ123​)
∂y∂θ2=l2cos⁡(θ12)+l3cos⁡(θ123)\frac{\partial y}{\partial \theta_2} = l_2\cos(\theta_{12}) + l_3\cos(\theta_{123})∂θ2​∂y​=l2​cos(θ12​)+l3​cos(θ123​)

对 θ3\theta_3θ3​ 的偏导数：
∂x∂θ3=−l3sin⁡(θ123)\frac{\partial x}{\partial \theta_3} = -l_3\sin(\theta_{123})∂θ3​∂x​=−l3​sin(θ123​)
∂y∂θ3=l3cos⁡(θ123)\frac{\partial y}{\partial \theta_3} = l_3\cos(\theta_{123})∂θ3​∂y​=l3​cos(θ123​)

因此雅可比矩阵为：

J(q)=[−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{q}) = \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(q)=[−l1​sin(θ1​)−l2​sin(θ12​)−l3​sin(θ123​)l1​cos(θ1​)+l2​cos(θ12​)+l3​cos(θ123​)​−l2​sin(θ12​)−l3​sin(θ123​)l2​cos(θ12​)+l3​cos(θ123​)​−l3​sin(θ123​)l3​cos(θ123​)​]

4. 阻尼最小二乘法（DLS）推导

4.1 基本思路

逆运动学问题可以表示为：给定当前关节角 q\mathbf{q}q 和目标位置 ptarget\mathbf{p}_{target}ptarget​，找到关节角的微小变化 Δq\Delta \mathbf{q}Δq，使末端执行器位置更接近目标位置。

根据微小变化的线性近似：
p(q+Δq)≈p(q)+J(q)Δq\mathbf{p}(\mathbf{q} + \Delta \mathbf{q}) \approx \mathbf{p}(\mathbf{q}) + \mathbf{J}(\mathbf{q}) \Delta \mathbf{q}p(q+Δq)≈p(q)+J(q)Δq

因此我们希望解决：
J(q)Δq=e\mathbf{J}(\mathbf{q}) \Delta \mathbf{q} = \mathbf{e}J(q)Δq=e

其中 e=ptarget−p(q)\mathbf{e} = \mathbf{p}_{target} - \mathbf{p}(\mathbf{q})e=ptarget​−p(q) 是当前位置误差。

4.2 最小二乘解法

对于上述方程，标准最小二乘解为：
Δq=J+e\Delta \mathbf{q} = \mathbf{J}^+ \mathbf{e}Δq=J+e

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

然而，当机器人接近奇异构型时，JTJ\mathbf{J}^T \mathbf{J}JTJ 接近奇异，导致数值不稳定。

4.3 阻尼最小二乘法

阻尼最小二乘法通过在目标函数中引入正则化项来解决这个问题：

min⁡Δq∥JΔq−e∥2+λ2∥Δq∥2\min_{\Delta \mathbf{q}} \|\mathbf{J} \Delta \mathbf{q} - \mathbf{e}\|^2 + \lambda^2 \|\Delta \mathbf{q}\|^2Δqmin​∥JΔq−e∥2+λ2∥Δq∥2

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

对上述式子求导并令其等于零：

JT(JΔq−e)+λ2Δq=0\mathbf{J}^T (\mathbf{J} \Delta \mathbf{q} - \mathbf{e}) + \lambda^2 \Delta \mathbf{q} = \mathbf{0}JT(JΔq−e)+λ2Δq=0

JTJΔq−JTe+λ2Δq=0\mathbf{J}^T \mathbf{J} \Delta \mathbf{q} - \mathbf{J}^T \mathbf{e} + \lambda^2 \Delta \mathbf{q} = \mathbf{0}JTJΔq−JTe+λ2Δq=0

(JTJ+λ2I)Δq=JTe(\mathbf{J}^T \mathbf{J} + \lambda^2 \mathbf{I}) \Delta \mathbf{q} = \mathbf{J}^T \mathbf{e}(JTJ+λ2I)Δq=JTe

因此，DLS的解为：

Δq=(JTJ+λ2I)−1JTe\Delta \mathbf{q} = (\mathbf{J}^T \mathbf{J} + \lambda^2 \mathbf{I})^{-1} \mathbf{J}^T \mathbf{e}Δq=(JTJ+λ2I)−1JTe

这可以重写为：

Δq=JT(JJT+λ2I)−1e\Delta \mathbf{q} = \mathbf{J}^T (\mathbf{J} \mathbf{J}^T + \lambda^2 \mathbf{I})^{-1} \mathbf{e}Δq=JT(JJT+λ2I)−1e

后一种形式在我们的问题中更有效，因为雅可比矩阵 J\mathbf{J}J 是 2×32 \times 32×3 的，所以 JJT\mathbf{J} \mathbf{J}^TJJT 是 2×22 \times 22×2 的，计算逆矩阵更高效。

4.4 阻尼因子的选择

阻尼因子 λ\lambdaλ 的选择很关键：

• 较大的 λ\lambdaλ 会导致稳定但缓慢的收敛
• 较小的 λ\lambdaλ 会导致快速但可能不稳定的收敛

常见的选择包括：

1. 固定值（如我们的实现）
2. 基于奇异值的自适应选择
3. 基于误差大小的自适应选择

5. 迭代算法

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

1. 初始化关节角 q\mathbf{q}q
2. 重复以下步骤，直到收敛或达到最大迭代次数：
   a. 计算当前末端执行器位置 p(q)\mathbf{p}(\mathbf{q})p(q)
   b. 计算位置误差 e=ptarget−p(q)\mathbf{e} = \mathbf{p}_{target} - \mathbf{p}(\mathbf{q})e=ptarget​−p(q)
   c. 如果 ∥e∥<ϵ\|\mathbf{e}\| < \epsilon∥e∥<ϵ（预设阈值），则收敛，退出
   d. 计算雅可比矩阵 J(q)\mathbf{J}(\mathbf{q})J(q)
   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. 计算关节角增量 Δq=JDLS+e\Delta \mathbf{q} = \mathbf{J}_{DLS}^+ \mathbf{e}Δq=JDLS+​e
   g. 更新关节角 q←q+Δq\mathbf{q} \leftarrow \mathbf{q} + \Delta \mathbf{q}q←q+Δq
   h. 将关节角归一化到合理范围（例如 [−π,π][-\pi, \pi][−π,π]）

6. 具体算例迭代过程

假设我们有以下参数：

• 连杆长度：l1=1.0l_1 = 1.0l1​=1.0, l2=0.8l_2 = 0.8l2​=0.8, l3=0.6l_3 = 0.6l3​=0.6
• 初始关节角：θ1=π/6\theta_1 = \pi/6θ1​=π/6, θ2=π/4\theta_2 = \pi/4θ2​=π/4, θ3=π/3\theta_3 = \pi/3θ3​=π/3
• 目标位置：ptarget=[1.5,1.0]T\mathbf{p}_{target} = [1.5, 1.0]^Tptarget​=[1.5,1.0]T
• 阻尼因子：λ=0.1\lambda = 0.1λ=0.1
• 收敛阈值：ϵ=10−4\epsilon = 10^{-4}ϵ=10−4

第1次迭代

1. 计算初始位置
   p(q0)=[1.830,0.897]T\mathbf{p}(\mathbf{q}_0) = [1.830, 0.897]^Tp(q0​)=[1.830,0.897]T
2. 计算位置误差
   e0=ptarget−p(q0)=[1.5,1.0]T−[1.830,0.897]T=[−0.330,0.103]T\mathbf{e}_0 = \mathbf{p}_{target} - \mathbf{p}(\mathbf{q}_0) = [1.5, 1.0]^T - [1.830, 0.897]^T = [-0.330, 0.103]^Te0​=ptarget​−p(q0​)=[1.5,1.0]T−[1.830,0.897]T=[−0.330,0.103]T
   ∥e0∥=0.345\|\mathbf{e}_0\| = 0.345∥e0​∥=0.345
3. 计算雅可比矩阵 J(q0)\mathbf{J}(\mathbf{q}_0)J(q0​)
   （略，根据前面的公式计算）
4. 计算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
5. 计算关节角增量
   Δq0=JDLS+e0\Delta \mathbf{q}_0 = \mathbf{J}_{DLS}^+ \mathbf{e}_0Δq0​=JDLS+​e0​
   （具体数值计算略）
6. 更新关节角
   q1=q0+Δq0\mathbf{q}_1 = \mathbf{q}_0 + \Delta \mathbf{q}_0q1​=q0​+Δq0​

第2次迭代

1. 计算更新后的位置
   p(q1)\mathbf{p}(\mathbf{q}_1)p(q1​)
2. 计算新的位置误差
   e1=ptarget−p(q1)\mathbf{e}_1 = \mathbf{p}_{target} - \mathbf{p}(\mathbf{q}_1)e1​=ptarget​−p(q1​)
   ∥e1∥\|\mathbf{e}_1\|∥e1​∥

继续迭代直到 ∥ek∥<ϵ\|\mathbf{e}_k\| < \epsilon∥ek​∥<ϵ 或达到最大迭代次数。

典型的迭代过程中，误差会呈现指数衰减的趋势，但具体收敛速度取决于：

1. 初始关节角与目标位置的距离
2. 阻尼因子 λ\lambdaλ 的大小
3. 目标位置是否在工作空间内
4. 是否接近奇异构型

在实际应用中，可能需要尝试不同的初始值和阻尼因子以获得最佳结果。