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

1. 问题定义

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

• 三个连杆，长度分别为 $l_1$, $l_2$, $l_3$
• 三个关节角 ${\theta }_1$, ${\theta }_2$, ${\theta }_3$
• 末端执行器的目标位置 ${\mathbf{p}}_{target}=[x_{target},y_{target}{]}^T$

逆运动学问题就是：给定目标位置 ${\mathbf{p}}_{target}$，求解关节角度 $\mathbf{q}=[{\theta }_1,{\theta }_2,{\theta }_3{]}^T$，使末端执行器位置 $\mathbf{p}(\mathbf{q})$ 尽可能接近目标位置。

2. 前向运动学

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

关节1的位置：
$${\mathbf{p}}_1=\left[\begin{array}{c}{l}_1\cos ({\theta }_1)\\ l_1\sin ({\theta }_1)\end{array}\right]$$

关节2的位置：
$${\mathbf{p}}_2={\mathbf{p}}_1+\left[\begin{array}{c}{l}_2\cos ({\theta }_1+{\theta }_2)\\ l_2\sin ({\theta }_1+{\theta }_2)\end{array}\right]$$

末端执行器的位置：
$$\mathbf{p}(\mathbf{q})={\mathbf{p}}_2+\left[\begin{array}{c}{l}_3\cos ({\theta }_1+{\theta }_2+{\theta }_3)\\ l_3\sin ({\theta }_1+{\theta }_2+{\theta }_3)\end{array}\right]$$

3. 雅可比矩阵

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

$$\mathbf{J}(\mathbf{q})=\frac{\partial \mathbf{p}}{\partial \mathbf{q}}=\left[\begin{array}{ccc}\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{array}\right]$$

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

设 ${\theta }_{12}={\theta }_1+{\theta }_2$ 和 ${\theta }_{123}={\theta }_1+{\theta }_2+{\theta }_3$

对 ${\theta }_1$ 的偏导数：
$$\frac{\partial x}{\partial {\theta }_1}=-l_1\sin ({\theta }_1)-l_2\sin ({\theta }_{12})-l_3\sin ({\theta }_{123})$$
$$\frac{\partial y}{\partial {\theta }_1}=l_1\cos ({\theta }_1)+l_2\cos ({\theta }_{12})+l_3\cos ({\theta }_{123})$$

对 ${\theta }_2$ 的偏导数：
$$\frac{\partial x}{\partial {\theta }_2}=-l_2\sin ({\theta }_{12})-l_3\sin ({\theta }_{123})$$
$$\frac{\partial y}{\partial {\theta }_2}=l_2\cos ({\theta }_{12})+l_3\cos ({\theta }_{123})$$

对 ${\theta }_3$ 的偏导数：
$$\frac{\partial x}{\partial {\theta }_3}=-l_3\sin ({\theta }_{123})$$
$$\frac{\partial y}{\partial {\theta }_3}=l_3\cos ({\theta }_{123})$$

因此雅可比矩阵为：

$$\mathbf{J}(\mathbf{q})=\left[\begin{array}{ccc}-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{array}\right]$$

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

4.1 基本思路

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

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

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

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

4.2 最小二乘解法

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

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

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

4.3 阻尼最小二乘法

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

$$\underset{\Delta \mathbf{q}}{\min }\Vert \mathbf{J}\Delta \mathbf{q}-\mathbf{e}{\Vert }^2+{\lambda }^2\Vert \Delta \mathbf{q}{\Vert }^2$$

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

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

$${\mathbf{J}}^T(\mathbf{J}\Delta \mathbf{q}-\mathbf{e})+{\lambda }^2\Delta \mathbf{q}=0$$

$${\mathbf{J}}^T\mathbf{J}\Delta \mathbf{q}-{\mathbf{J}}^T\mathbf{e}+{\lambda }^2\Delta \mathbf{q}=0$$

$$({\mathbf{J}}^T\mathbf{J}+{\lambda }^2\mathbf{I})\Delta \mathbf{q}={\mathbf{J}}^T\mathbf{e}$$

因此，DLS的解为：

$$\Delta \mathbf{q}=({\mathbf{J}}^T\mathbf{J}+{\lambda }^2\mathbf{I}{)}^{-1}{\mathbf{J}}^T\mathbf{e}$$

这可以重写为：

$$\Delta \mathbf{q}={\mathbf{J}}^T(\mathbf{J}{\mathbf{J}}^T+{\lambda }^2\mathbf{I}{)}^{-1}\mathbf{e}$$

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

4.4 阻尼因子的选择

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

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

常见的选择包括：

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

5. 迭代算法

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

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

6. 具体算例迭代过程

假设我们有以下参数：

• 连杆长度：$l_1=1.0$, $l_2=0.8$, $l_3=0.6$
• 初始关节角：${\theta }_1=\pi /6$, ${\theta }_2=\pi /4$, ${\theta }_3=\pi /3$
• 目标位置：${\mathbf{p}}_{target}=[1.5,1.0{]}^T$
• 阻尼因子：$\lambda =0.1$
• 收敛阈值：$ϵ=10^{-4}$

第1次迭代

1. 计算初始位置
   $\mathbf{p}({\mathbf{q}}_0)=[1.830,0.897{]}^T$

2. 计算位置误差
   ${\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{]}^T$
   $\Vert {\mathbf{e}}_0\Vert =0.345$

3. 计算雅可比矩阵 $\mathbf{J}({\mathbf{q}}_0)$
   （略，根据前面的公式计算）

4. 计算DLS逆
   ${\mathbf{J}}_{DLS}^{+}={\mathbf{J}}^T(\mathbf{J}{\mathbf{J}}^T+{\lambda }^2\mathbf{I}{)}^{-1}$

5. 计算关节角增量
   $\Delta {\mathbf{q}}_0={\mathbf{J}}_{DLS}^{+}{\mathbf{e}}_0$
   （具体数值计算略）

6. 更新关节角
   ${\mathbf{q}}_1={\mathbf{q}}_0+\Delta {\mathbf{q}}_0$

第2次迭代

1. 计算更新后的位置
   $\mathbf{p}({\mathbf{q}}_1)$

2. 计算新的位置误差
   ${\mathbf{e}}_1={\mathbf{p}}_{target}-\mathbf{p}({\mathbf{q}}_1)$
   $\Vert {\mathbf{e}}_1\Vert$

继续迭代直到 $\Vert {\mathbf{e}}_k\Vert <ϵ$ 或达到最大迭代次数。

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

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

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