【DQ Robotics】二次规划控制

二次规划 (Quadratic Programming)

我们将讨论的第二种优化问题称为二次规划（简称 QP）。QP 是一种线性约束的二次优化问题，其数学形式如下：

$$u\in \text{arg}\underset{\dot{q}}{\min }\mathcal{F}(\dot{q})$$

受限于：

$$W\dot{q}⪯w$$

其中，$W\in {\mathbb{R}}^{n\times p}$ 和 $w\in {\mathbb{R}}^p$ 表示与机器人 $n$ 自由度相关的 $p$ 个线性约束。需要注意，我们使用符号 $⪯$ 而不是 $\le$，因为我们表示的是逐元素的不等式。

你可以将这个 QP 问题理解为：“寻找所有使得 $\mathcal{F}(\dot{q})$ 最小化的 $\dot{q}$，在满足 $W\dot{q}⪯w$ 的条件下，并返回其中一个解，称为 $u$”。通常情况下，需要使用数值方法来求解此类 QP 问题。类似于无约束情况下的情况，在本节中我们将把这样的最小化解称为 $u$。

解释：

1. 定义：
   二次规划是一种优化问题，目标是最小化一个关于 $\dot{q}$ 的函数 $\mathcal{F}(\dot{q})$，同时受到一组线性不等式 $W\dot{q}⪯w$ 的约束。

2. 符号说明：

   • $W$：一个 $n\times p$ 的矩阵，定义了约束的系数。
   • $w$：一个 $p$ 维向量，定义了约束的边界值。
   • $\dot{q}$：待优化的变量（例如机器人的关节速度）。
   • $\mathcal{F}(\dot{q})$：一个待最小化的目标函数，通常是一个二次函数。

3. 目标：
   优化目标是找到一个最优的 $\dot{q}$，既满足约束条件，又使目标函数 $\mathcal{F}(\dot{q})$ 达到最小值。

4. 约束解释：
   约束 $W\dot{q}⪯w$ 表示 $W$ 与 $\dot{q}$ 的矩阵乘积的每个元素都小于或等于 $w$ 的对应元素。这种约束在许多优化问题（如机器人控制、经济模型等）中非常常见。

5. 实际意义：
   在机器人控制中，优化变量 $\dot{q}$ 可能代表关节速度，而 $\mathcal{F}(\dot{q})$ 可能是能量消耗或控制误差的度量。约束 $W\dot{q}⪯w$ 则可能是运动范围或力的限制。

关节限制 (Joint Limits)

在某些情况下，使用无约束优化来解决机器人控制任务是有用的，但大多数实际任务都存在所谓的“硬限制”（Hard Limits）。这些限制指的是机器人关节空间或任务空间中的限制，无法被越过。例如，关节限制是一种物理上的硬性限制，控制器无法命令机器人超越其关节限制。

在进行运动学控制时，决策变量是关节速度，而不是关节位置。因此，我们需要将关节限制表示为依赖于关节速度的线性不等式。

假设关节的下限为 $q^{-}\in {\mathbb{R}}^n$，上限为 $q^{+}\in {\mathbb{R}}^n$。使用二次规划（QP）实现关节限制的最常见方法如下：

$$u\in \text{arg}\underset{\dot{q}}{\min }\Vert J\dot{q}+\eta \tilde{x}{\Vert }^2+{\lambda }^2\Vert \dot{q}{\Vert }^2$$

受限于：

$$W_{JL}\dot{q}⪯w_{JL}$$

其中：

$$W_{JL}=\left[\begin{array}{c}-I\\ I\end{array}\right]$$

$$w_{JL}=\left[\begin{array}{c}-{\eta }_{JL}(q^{-}-q)\\ {\eta }_{JL}(q^{+}-q)\end{array}\right]$$

η J L ∈ R + − { 0 } \eta_{JL} \in \mathbb{R}^+ - \{0\} ηJL​∈R+−{0} 是一个可配置的增益值。通常，我们选择 ${\eta }_{JL}=1$。

解释

1. 定义和背景：

   • 硬限制：机器人在实际操作中，关节的物理限制需要严格遵守。这些限制无法被超越，否则会导致物理损坏或功能失效。
   • 运动学控制：主要关注的是关节速度 $\dot{q}$ 而非关节位置 $q$，因此限制条件需要从关节位置转换为关节速度的形式。

2. 公式解读：

   • 目标函数：
     $$\Vert J\dot{q}+\eta \tilde{x}{\Vert }^2+{\lambda }^2\Vert \dot{q}{\Vert }^2$$

     • 第一部分 $\Vert J\dot{q}+\eta \tilde{x}{\Vert }^2$ 旨在最小化任务误差（$\tilde{x}$ 表示误差）。
     • 第二部分 ${\lambda }^2\Vert \dot{q}{\Vert }^2$ 用于正则化，限制关节速度的幅度，避免不稳定或过大的速度值。

   • 约束条件：
     $$W_{JL}\dot{q}⪯w_{JL}$$

     • $W_{JL}$ 定义了限制矩阵，包含关节速度上下限的线性约束。
     • $w_{JL}$ 表示关节速度的上下边界，根据当前关节位置 $q$ 和硬性限制 $q^{-}$、$q^{+}$ 确定。

3. 物理意义：

   • 关节速度限制：通过 $\dot{q}$ 的线性不等式，确保关节速度不会超出物理边界。
   • 增益值 ${\eta }_{JL}$：调整关节速度约束的响应速率，典型选择 ${\eta }_{JL}=1$，表示等比例映射当前的限制。

4. 实现方法：

   • 在二次规划框架中，通过目标函数和约束条件，求解最优的 $\dot{q}$，以实现机器人任务的精准控制，同时满足硬性限制。

如何将关节限制表示为依赖于关节速度的线性不等式

1. 背景：关节限制的定义

在机器人控制中，关节限制通常以关节位置 $q$ 的上下界形式给出：

• 关节的下限：$q^{-}\in {\mathbb{R}}^n$
• 关节的上限：$q^{+}\in {\mathbb{R}}^n$

这些限制表示机器人关节的位置 $q$ 必须满足：
$$q^{-}\le q\le q^{+}$$

然而，在运动学控制中，决策变量是关节速度 $\dot{q}$ 而不是关节位置 $q$，因此需要将这些限制转换为关于 $\dot{q}$ 的形式。

2. 关节位置与速度的关系

假设机器人每个控制循环的时间间隔为 $\Delta t$，关节位置的变化可以近似表示为：
$$q_{\text{new}}=q_{\text{current}}+\dot{q}\Delta t$$

为保证关节位置 $q_{\text{new}}$ 在限制范围内，必须满足：
$$q^{-}\le q_{\text{current}}+\dot{q}\Delta t\le q^{+}$$

3. 将位置限制转化为速度限制

对上述不等式重新整理，得到：
$$q^{-}-q_{\text{current}}\le \dot{q}\Delta t\le q^{+}-q_{\text{current}}$$

将 $\Delta t$ 提到分母上（假设 $\Delta t>0$），不等式变为：
$$\frac{q^{-}-q}{\Delta t}\le \dot{q}\le \frac{q^{+}-q}{\Delta t}$$

其中：

• $q^{-}-q=q^{-}-q_{\text{current}}$ 表示当前距离下限的位置。
• $q^{+}-q=q^{+}-q_{\text{current}}$ 表示当前距离上限的位置。

这就得到了依赖于速度 $\dot{q}$ 的关节限制。

4. 将速度限制转换为矩阵形式

为了简化表示，将上述关于 $\dot{q}$ 的上下限不等式统一表示为矩阵形式。

1. 将上下限拆分成两个独立的不等式：

   • 下界：$\dot{q}\ge \frac{q^{-}-q}{\Delta t}$
   • 上界：$\dot{q}\le \frac{q^{+}-q}{\Delta t}$

2. 将下界重新表示为：
   $$-\dot{q}\le -\frac{q^{-}-q}{\Delta t}$$

3. 将这两组不等式统一为矩阵形式：
   $$W_{JL}\dot{q}⪯w_{JL}$$

5. 构造矩阵和向量

1. 构造 $W_{JL}$：

   • $W_{JL}$ 是一个 $2n\times n$ 的矩阵，用于表示每个关节速度的上下限关系。
   • 具体形式为：
     $$W_{JL}=\left[\begin{array}{c}-I\\ I\end{array}\right]$$
     其中：
   • $-I$ 是 $n\times n$ 的负单位矩阵，对应下界约束。
   • $I$ 是 $n\times n$ 的正单位矩阵，对应上界约束。

2. 构造 $w_{JL}$：

   • $w_{JL}$ 是一个 $2n$ 维向量，用于定义每个关节速度的上下限。
   • 具体形式为：
     $$w_{JL}=\left[\begin{array}{c}-\frac{q^{-}-q}{\Delta t}\\ \frac{q^{+}-q}{\Delta t}\end{array}\right]$$

6. 引入增益因子 ${\eta }_{JL}$

为了增强控制灵活性和约束的稳定性，时间间隔 $\frac{1}{\Delta t}$ 通常被替换为一个可调节的增益因子 ${\eta }_{JL}$。最终形式为：

1. $W_{JL}$ 不变：
   $$W_{JL}=\left[\begin{array}{c}-I\\ I\end{array}\right]$$

2. $w_{JL}$ 替换为：
   $$w_{JL}=\left[\begin{array}{c}-{\eta }_{JL}(q^{-}-q)\\ {\eta }_{JL}(q^{+}-q)\end{array}\right]$$

其中 ${\eta }_{JL}>0$ 是一个用户可配置的增益值，通常选择 ${\eta }_{JL}=1$。

7. 最终结果

关节速度的上下限约束可以用矩阵形式统一表示为：
$$W_{JL}\dot{q}⪯w_{JL}$$

具体展开为：
$$\left[\begin{array}{c}-I\\ I\end{array}\right]\dot{q}⪯\left[\begin{array}{c}-{\eta }_{JL}(q^{-}-q)\\ {\eta }_{JL}(q^{+}-q)\end{array}\right]$$

8. 总结

1. 起始于关节位置的上下限 $q^{-}\le q\le q^{+}$。
2. 通过时间间隔 $\Delta t$ 转换为关于关节速度 $\dot{q}$ 的限制。
3. 将限制写成矩阵形式：
   • 矩阵 $W_{JL}$ 定义了约束的结构（单位矩阵）。
   • 向量 $w_{JL}$ 表示速度的上下限。
4. 引入增益因子 ${\eta }_{JL}$ 提升灵活性和可控性。

这种矩阵形式方便在二次规划（QP）或其他优化框架中直接使用，适用于多自由度机器人的运动学控制问题。