人工势场法

Conclusion

• 计算时只用 Jacobian 矩阵的前三个参数（也就是位置）因为定义的势场只产生了力（线速度），没有产生力矩（角速度）
• 要把力从笛卡尔空间转到关节空间之后再相加，否则可能会丢失力矩——比如两个相反但作用于两个角的力，直接相加能够抵消，但是转换再相加会旋转
• 使用梯度下降法，走一步算一步，像控制器，容易陷入局部最优

Notes

核心思想：用“力”来引导机器人

想象一下，机器人的末端执行器是一个小球。

• 目标点 像一个磁铁，产生吸引力 ${\mathbf{F}}_{\text{att}}$。
• 障碍物 像一个电荷，产生排斥力 ${\mathbf{F}}_{\text{rep}}$。

机器人的运动在这个虚拟的“力场”中，被合力驱动，自然地“滚向”目标点并避开障碍物。 用来建模这两个“力场”的函数也与“磁铁”和“电荷”的直观想象相符。

---

公式推导详解

一步步构建关节扭矩 $\mathbf{\tau }$ 的公式。

第 1 步：定义势能场

首先定义虚拟的势能场 $U(\mathbf{q})$。在物理学中，力是势能场的负梯度 ($\mathbf{F}=-\nabla U$)。

• 吸引力场（由目标点 ${\mathbf{q}}_{\text{goal}}$ 产生）：

  $$U_{\text{att}}(\mathbf{q})=\frac{1}{2}{k}_{\text{att}}\Vert \mathbf{q}-{\mathbf{q}}_{\text{goal}}{\Vert }^2$$

  其中 $k_{\text{att}}$ 是吸引力增益常数。

• 排斥力场（由障碍物产生）：

  $$U_{\text{rep}}(\mathbf{q})=\{\begin{array}{ll}\frac{1}{2}{k}_{\text{rep}}{\left(\frac{1}{\Vert \mathbf{q}-{\mathbf{q}}_{\text{obs}}\Vert }-\frac{1}{{\rho }_0}\right)}^2,& \text{if }\Vert \mathbf{q}-{\mathbf{q}}_{\text{obs}}\Vert \le {\rho }_0\\ 0,& \text{if }\Vert \mathbf{q}-{\mathbf{q}}_{\text{obs}}\Vert >{\rho }_0\end{array}$$

  其中 $k_{\text{rep}}$ 是排斥力增益常数，${\rho }_0$ 是障碍物的影响范围。

• 总势能场：

  $$U_{\text{total}}(\mathbf{q})=U_{\text{att}}(\mathbf{q})+\sum _i{U}_{{\text{rep}}_i}(\mathbf{q})$$

第 2 步：从势能场计算虚拟力

根据 $\mathbf{F}=-\nabla U$ 计算虚拟力。

• 吸引力：

  $${\mathbf{F}}_{\text{att}}(\mathbf{q})=-\nabla U_{\text{att}}(\mathbf{q})=-k_{\text{att}}(\mathbf{q}-{\mathbf{q}}_{\text{goal}})$$

  这个力的方向指向目标点，大小与距离成正比。

• 排斥力：

  $${\mathbf{F}}_{\text{rep}}(\mathbf{q})=-\nabla U_{\text{rep}}(\mathbf{q})$$

  具体计算为：

  $${\mathbf{F}}_{\text{rep}}(\mathbf{q})=\{\begin{array}{ll}{K}_{\text{rep}}\left(\frac{1}{\Vert \mathbf{q}-{\mathbf{q}}_{\text{obs}}\Vert }-\frac{1}{{\rho }_0}\right)\frac{1}{\Vert \mathbf{q}-{\mathbf{q}}_{\text{obs}}{\Vert }^2}\cdot \frac{\mathbf{q}-{\mathbf{q}}_{\text{obs}}}{\Vert \mathbf{q}-{\mathbf{q}}_{\text{obs}}\Vert },& \text{if }\Vert \mathbf{q}-{\mathbf{q}}_{\text{obs}}\Vert \le {\rho }_0\\ 0,& \text{if }\Vert \mathbf{q}-{\mathbf{q}}_{\text{obs}}\Vert >{\rho }_0\end{array}$$

  这个力的方向是远离障碍物，距离障碍物越近，推力越大。

• 总虚拟力：

  $${\mathbf{F}}_{\text{total}}(\mathbf{q})={\mathbf{F}}_{\text{att}}(\mathbf{q})+\sum _i{\mathbf{F}}_{{\text{rep}}_i}(\mathbf{q})$$

第 3 步：从虚拟力到关节扭矩

这是最关键的一步。${\mathbf{F}}_{\text{total}}$ 是在操作空间中的力，但我们需要关节的扭矩 $\mathbf{\tau }$。

这里需要用到雅可比矩阵 $\mathbf{J}(\mathbf{q})$，它描述了关节速度 $\dot{\mathbf{q}}$ 与末端执行器速度 $\mathbf{v}$ 之间的关系：

$$\mathbf{v}=\mathbf{J}(\mathbf{q})\dot{\mathbf{q}}$$

通过虚功原理，雅可比矩阵的转置可以将操作空间中的力映射到关节空间的广义力（扭矩）：

$$\mathbf{\tau }=\mathbf{J}(\mathbf{q}{)}^{\top }\mathbf{F}$$

• 最终公式： $$\mathbf{\tau }=\mathbf{J}(\mathbf{q}{)}^{\top }{\mathbf{F}}_{\text{total}}(\mathbf{q})=\mathbf{J}(\mathbf{q}{)}^{\top }\left[{\mathbf{F}}_{\text{att}}(\mathbf{q})+\sum _i{\mathbf{F}}_{{\text{rep}}_i}(\mathbf{q})\right]$$ $$\mathbf{\tau }=\mathbf{J}(\mathbf{q}{)}^{\top }\left[-k_{\text{att}}(\mathbf{q}-{\mathbf{q}}_{\text{goal}})+\sum _i{\mathbf{F}}_{{\text{rep}}_i}(\mathbf{q})\right]$$

什么是虚功原理？

这是一个经典的力学原理，它完美地解决了 “末端执行器上的一个虚拟力，应该如何等效为各个关节的扭矩？” 这个问题。

虚功原理说的是：如果一个系统处于平衡状态，那么所有作用力在任何“虚位移”上所做的“虚功”之和为零。

• 虚位移：一个假想的、非常微小的、符合系统约束的位移。它不是实际发生的位移，只是一个思想实验工具。
• 虚功：力在虚位移上做的功。

假设我们对机器人末端执行器施加一个力 $\mathbf{F}$（即我们计算出的虚拟合力），同时末端执行器产生了一个微小的虚位移 $\delta \mathbf{x}$。

1. 操作空间中的虚功： 末端力做的虚功是：

   $$\delta W_{\text{end}}={\mathbf{F}}^{\top }\delta \mathbf{x}$$

2. 关节空间中的虚功： 这个末端的位移是由关节的微小位移 $\delta \mathbf{q}$ 引起的。它们通过雅可比矩阵 $\mathbf{J}(\mathbf{q})$ 联系起来：

   $$\delta \mathbf{x}=\mathbf{J}(\mathbf{q})\delta \mathbf{q}$$

   同时，关节扭矩 $\mathbf{\tau }$ 在关节位移上做的虚功是：

   $$\delta W_{\text{joints}}={\mathbf{\tau }}^{\top }\delta \mathbf{q}$$

3. 建立等效关系： 根据虚功原理，为了让系统等效，末端力 $\mathbf{F}$ 所做的功，应该等于关节扭矩 $\mathbf{\tau }$ 所做的功：

   $$\delta W_{\text{end}}=\delta W_{\text{joints}}$$ $${\mathbf{F}}^{\top }\delta \mathbf{x}={\mathbf{\tau }}^{\top }\delta \mathbf{q}$$

   将 $\delta \mathbf{x}=\mathbf{J}\delta \mathbf{q}$ 代入：

   $${\mathbf{F}}^{\top }(\mathbf{J}\delta \mathbf{q})={\mathbf{\tau }}^{\top }\delta \mathbf{q}$$ $$({\mathbf{F}}^{\top }\mathbf{J})\delta \mathbf{q}={\mathbf{\tau }}^{\top }\delta \mathbf{q}$$

   由于这个等式对于任意的虚位移 $\delta \mathbf{q}$ 都成立，所以系数必须相等：

   $${\mathbf{\tau }}^{\top }={\mathbf{F}}^{\top }\mathbf{J}$$

   两边同时转置，就得到了我们最终使用的公式：

   $$\mathbf{\tau }={\mathbf{J}}^{\top }\mathbf{F}$$

把机器人想象成一组杠杆系统。

• 雅可比矩阵 $\mathbf{J}$ 就像一套传动比，它告诉你“关节转动一个很小的角度，末端会移动多少”。
• 虚功原理则说，能量是守恒的。末端力 $\mathbf{F}$ 移动一段距离 $\delta \mathbf{x}$ 所做的功，必须等于各个关节扭矩 $\mathbf{\tau }$ 转动相应角度 $\delta \mathbf{q}$ 所做的功之和。
• $\mathbf{\tau }={\mathbf{J}}^{\top }\mathbf{F}$ 这个公式，就是根据这个能量守恒原则，计算出的为了产生末端力 $\mathbf{F}$，各个关节需要出的“力”（扭矩）是多少。

所以，虚功原理是连接抽象的操作空间力与物理的关节空间扭矩之间的一个非常优美且坚实的桥梁。它确保了我们在虚拟势场中计算的力，能够被正确地转换为驱动实际机器人关节运动的指令。

---

总结与工作流程

1. 感知：获取当前关节角度 $\mathbf{q}$、目标位置 ${\mathbf{q}}_{\text{goal}}$ 和障碍物位置 ${\mathbf{q}}_{\text{obs}}$
2. 计算虚拟力：
   • 计算吸引力 ${\mathbf{F}}_{\text{att}}=-k_{\text{att}}(\mathbf{q}-{\mathbf{q}}_{\text{goal}})$
   • 计算每个障碍物的排斥力 ${\mathbf{F}}_{{\text{rep}}_i}$
   • 计算合力 ${\mathbf{F}}_{\text{total}}={\mathbf{F}}_{\text{att}}+{\sum }_i{\mathbf{F}}_{{\text{rep}}_i}$
3. 转换到关节扭矩：
   • 计算雅可比矩阵 $\mathbf{J}(\mathbf{q})$
   • 计算关节扭矩 $\mathbf{\tau }=\mathbf{J}(\mathbf{q}{)}^{\top }{\mathbf{F}}_{\text{total}}$
4. 控制：将扭矩 $\mathbf{\tau }$ 发送给关节控制器
5. 循环：重复直到到达目标

优缺点

• 优点：
  • 概念直观
  • 计算效率高，能够实时反应环境变化
• 缺点：
  • 容易陷入局部最小值，在狭窄通道中可能振荡
  • 参数调优困难