人形机器人建模与控制（四）

1. Repeat在机器人控制中，任务空间和配置空间分别代表了不同的控制目标和控制方法。以下是对任务空间和配置空间控制目标的详细描述和解释。

1.1 任务空间控制任务空间控制目标是指对机器人末端执行器的位置和姿态进行控制。任务空间控制的主要目标是使机器人末端执行器按照期望的轨迹进行运动。

任务空间控制公式(笛卡尔空间控制)

期望的末端执行器状态：

这里， 表示期望的末端执行器状态，属于特殊欧几里得群 ，包括位置和姿态。

期望的末端执行器加速度：$$\ddot{\boldsymbol{X}}^{d} = \ddot{\boldsymbol{X}}^{r} - \mathbf{K}{d}\left(\dot{\boldsymbol{X}} - \dot{\boldsymbol{X}}^{r}\right) - \mathbf{K}{p}\left(\boldsymbol{X} \circleddash \boldsymbol{X}^{r}\right)$$

这里， 表示期望的末端执行器加速度。

是期望的加速度。

$\mathbf{K}{d}\mathbf{K}{p}$ 分别是速度和位置的反馈增益矩阵。

表示位姿误差的操作，通常在欧几里得群 中定义。

期望的关节加速度：

这里， 表示期望的关节加速度。

是雅可比矩阵的伪逆。

表示雅可比矩阵的时间导数项。

控制律：

这里， 是控制力矩。

是惯性矩阵。

包括科里奥利力和重力项。

1.2 配置空间控制（关节空间控制）配置空间控制目标是指对机器人的关节位置进行控制，使其达到期望的位置和速度。

配置空间控制公式

期望的关节位置：

这里， 表示期望的关节位置，属于 ，即关节空间。

期望的关节加速度：$$\ddot{\mathbf{q}}^{d} = \ddot{\mathbf{q}}^{r} - \mathbf{K}{d}\left(\dot{\mathbf{q}} - \dot{\mathbf{q}}^{r}\right) - \mathbf{K}{p}\left(\mathbf{q} - \mathbf{q}^{r}\right)$$

这里， 表示期望的关节加速度。

是期望的关节加速度。

$\mathbf{K}{d}\mathbf{K}{p}$ 分别是速度和位置的反馈增益矩阵。

控制律：

这里， 是控制力矩。

是惯性矩阵。

包括科里奥利力和重力项。

1.3 优化控制再次查看关节空间控制律：

$$\boldsymbol{\tau}=\mathbf{M}(\mathbf{q})\left[\ddot{\mathbf{q}}^{r}-\mathbf{K}{d}\left(\dot{\mathbf{q}}-\dot{\mathbf{q}}^{r}\right)-\mathbf{K}{p}\left(\mathbf{q}-\mathbf{q}^{r}\right)\right]+\mathbf{h}(\mathbf{q}, \dot{\mathbf{q}})$$

我们知道这实际上可以表示为以下两个部分的和：

机器人动力学方程：

期望的关节加速度：

$$\ddot{\mathbf{q}}^{d}=\ddot{\boldsymbol{q}}^{r}-\mathbf{K}{d}\left(\dot{\mathbf{q}}-\dot{\mathbf{q}}^{r}\right)-\mathbf{K}{p}\left(\mathbf{q}-\mathbf{q}^{r}\right)$$

因此，可以将这个问题看作是以下优化问题的求解：

这个优化问题的目标是通过最小化关节加速度误差的平方范数，找到最优的控制力矩。

将控制问题表示为优化问题的形式，虽然在目前的情况下没有直接的优势，但它允许我们在控制器的制定中包含更多有用的因素，如约束。

我们可以将最优控制看作一个最小二乘问题（LSP）

一般形式：

$$\underset{\mathbf{x}}{\arg \min } \underbrace{|\mathbf{A x}-\mathbf{a}|{2}^{2}}{\text{Squared norm of linear cost}} \text { st. } \underbrace{\mathbf{Bx}=\mathbf{b}}_{\text{Linear Equality constraints}}$$

也可以看做更一般的二次规划问题（QP）

于是我们可以引入以下约束：

执行器限制

关节限制

速度限制

自碰撞约束

此外，这些优化问题还可以扩展为模型预测控制（MPC），在一系列离散时间步中进行优化，实现更复杂的控制策略。

再看一次笛卡尔空间控制律：

$$\boldsymbol{\tau}=\mathbf{M} \mathbf{J}^{\sharp}\left(\ddot{\boldsymbol{X}}^{r}-\mathbf{K}{d}\left(\dot{\boldsymbol{X}}-\dot{\mathbf{X}}^{r}\right)-\mathbf{K}{p}\left(\boldsymbol{X} \circleddash \boldsymbol{X}^{r}\right)-\dot{\mathbf{J}} \dot{\mathbf{q}}\right)+\mathbf{h}$$

我们知道这实际上可以表示为：

机器人动力学：

任务空间映射：

反馈控制律：

$$\ddot{\boldsymbol{X}}^{d}=\ddot{\boldsymbol{X}}^{r}-\mathbf{K}{d}\left(\dot{\boldsymbol{X}}-\dot{\mathbf{X}}^{r}\right)-\mathbf{K}{p}\left(\boldsymbol{X} \circleddash \boldsymbol{X}^{r}\right)$$

这个控制律等价于以下最小二乘问题：

这也是LSP的形式，也可以表示为QP.

2. 任务模型2.1 任务的广义公式在机器人控制中，任务（Task）通常表示为控制目标（Control Objective），例如质心运动、角动量、足部运动、手部运动、肘部运动和头部运动等。任务可以用任务变量或任务坐标表示，这些任务变量描述了控制目标在不同状态和时间下的期望行为。

任务变量/任务坐标公式任务变量 通常表示为实际输出与期望输出之间的误差：

其中：

是关节位置（广义坐标）。

是关节速度。

是控制输入（如关节力矩）。

是时间。

是实际输出，取决于系统的状态和控制输入。

是期望输出，通常是一个随时间变化的函数。

优化问题中的仿射性和非线性在优化问题中，我们通常需要将控制问题转化为一个优化问题，这需要考虑控制变量的仿射性和状态变量的非线性。

控制变量的仿射性

仿射性表示系统的控制输入与系统状态之间的关系可以表示为仿射方程。仿射方程是指一种线性方程加上一常数项。对于控制变量 ，我们希望能够写成如下形式：

其中：

是雅可比矩阵，表示任务变量对控制变量的敏感度。

是常数向量，通常包含期望输出和当前误差的相关信息。

例如，在力控制中，力矩 的控制律可以表示为：

如果我们将加速度 表示为任务变量的仿射函数，我们有：

状态变量的非线性

非线性表示系统的状态变量与控制变量之间的关系是非线性的，这通常体现在动力学方程中。状态变量的非线性使得控制问题复杂化，但通过非线性控制理论，我们可以设计有效的控制策略。

例如，在机器人动力学方程中：

这里，惯性矩阵 和科里奥利力项 都是状态变量 和 的非线性函数。

综合示例将任务变量和优化问题结合起来，我们可以将控制问题转化为如下的二次规划问题（QP）：

$$\begin{aligned}& \underset{\boldsymbol{\tau}}{\arg \min } \left|\mathbf{J} \ddot{\mathbf{q}} + \dot{\mathbf{J}} \dot{\mathbf{q}} - \ddot{\mathbf{X}}^{d}\right|{2}^{2} \& \text {st.} \quad \mathbf{M}(\mathbf{q}) \ddot{\mathbf{q}} + \mathbf{h}(\mathbf{q}, \dot{\mathbf{q}}) = \boldsymbol{\tau} \& \tau{\min} \leq \tau \leq \tau_{\max}\end{aligned}$$

这里：

是雅可比矩阵。

是期望的加速度。

是惯性矩阵，表示状态变量的非线性依赖关系。

是控制变量，要求满足仿射关系和一定的约束条件。

通过这种方法，可以有效地将复杂的非线性控制问题转化为优化问题，并引入各种约束条件，从而设计出更加鲁棒和灵活的控制策略。

2.2 基于位置的任务位置任务公式为：

该任务公式在 中是非线性的。解决方案是将任务公式应用于动力学，例如 和 。

2.3 基于速度的任务在基于速度的任务中，我们关心的是系统输出的速度如何跟踪期望的速度。通过将任务公式和线性动力学结合起来，我们可以得到一个控制律，描述系统速度的演变。

基本任务公式首先，定义基于速度的误差：

这里：

是误差向量，表示实际输出 与期望输出 之间的差异。

是系统的状态变量。

是系统的输出，取决于状态变量 。

是期望输出，随时间变化。

线性动力学施加线性动力学来描述误差的演变：

这里 是一个正定增益矩阵，用于调节误差收敛速度。这个方程表示误差随着时间的变化速率与当前误差成比例，且方向相反，以实现误差的逐步减少。

将线性动力学代入误差公式将误差公式 和线性动力学结合起来：

根据线性动力学方程：

雅可比矩阵假设 是状态变量 的函数，使用雅可比矩阵 将状态变量的变化与输出的变化关联起来：

于是有：

代入上述误差方程：

整理得到：

仿射函数形式我们将这个方程表示为仿射函数的形式：

这里定义：

所以最终的形式是：

这个方程表示状态变量的变化速率 是输出期望变化速率 和当前误差 的仿射函数。通过求解这个方程，我们可以确定系统在每个时刻的状态变化速率，使得系统输出尽可能快地跟踪期望输出。

2.4 基于位置的任务基于位置的任务关注的是机器人系统的位置如何跟踪期望位置。通过将任务公式和线性动力学结合起来，我们可以得到一个控制律，描述系统位置的演变。

基本任务公式首先，定义基于位置的误差：

其中：

是误差向量，表示实际位置 与期望位置 之间的差异。

是关节位置（广义坐标）。

是系统的实际位置输出，取决于关节位置 。

是期望位置，随时间变化。

线性动力学施加线性动力学来描述误差的演变：

$$\ddot{\mathbf{e}} = -\mathbf{K}{p} \mathbf{e} - \mathbf{K}{d} \dot{\mathbf{e}}$$

这里 $\mathbf{K}{p}\mathbf{K}{d}$ 是正定增益矩阵，用于调节误差收敛速度。这个方程表示误差的加速度与当前误差和误差速度成比例，且方向相反，以实现误差的逐步减少。

将线性动力学代入误差公式首先，计算误差的加速度：

根据线性动力学方程：

$$\ddot{\mathbf{y}}(\mathbf{q}) - \ddot{\mathbf{y}}^{d}(t) = -\mathbf{K}{p} \mathbf{e} - \mathbf{K}{d} \dot{\mathbf{e}}$$

雅可比矩阵假设 是关节位置 的函数，使用雅可比矩阵 将关节位置的变化与输出的变化关联起来：

于是有：

代入上述误差方程：

$$\mathbf{J} \ddot{\mathbf{q}} + \dot{\mathbf{J}} \dot{\mathbf{q}} - \ddot{\mathbf{y}}^{d}(t) = -\mathbf{K}{p} \mathbf{e} - \mathbf{K}{d} \dot{\mathbf{e}}$$

整理得到：

$$\mathbf{J} \ddot{\mathbf{q}} = \ddot{\mathbf{y}}^{d}(t) - \dot{\mathbf{J}} \dot{\mathbf{q}} - \mathbf{K}{p} \mathbf{e} - \mathbf{K}{d} \dot{\mathbf{e}}$$

仿射函数形式我们将这个方程表示为仿射函数的形式：

这里定义：

$\mathbf{a} = \ddot{\mathbf{y}}^{d}(t) - \dot{\mathbf{J}} \dot{\mathbf{q}} - \mathbf{K}{p} \mathbf{e} - \mathbf{K}{d} \dot{\mathbf{e}}$

所以最终的形式是：

这个方程表示关节加速度 是输出期望加速度 、当前误差 和误差速度 的仿射函数。通过求解这个方程，我们可以确定系统在每个时刻的关节加速度，使得系统输出尽可能快地跟踪期望输出。

转化为代价函数为了优化控制策略，我们通常将上述仿射方程转化为代价函数形式：

这个代价函数表示我们希望最小化 和 之间的二范数误差，从而找到最优的关节加速度 。

通过引入基于位置的任务公式，并施加线性动力学，我们得到了一个控制律，描述了系统位置的演变。这个控制律利用雅可比矩阵将关节位置的变化与输出的变化联系起来，使得我们能够计算出所需的关节加速度，以实现对期望位置的跟踪。将仿射方程转化为代价函数的形式，可以通过优化方法找到最优的关节加速度，从而提高控制精度和系统响应速度。

3. 任务空间逆动力学目标是最小化任务函数：

$$\min {\mathbf{y}=[\dot{\mathbf{v}}, \boldsymbol{\tau}]}|\mathbf{A} \mathbf{y}-\mathbf{a}|{2}^{2} \quad \text { st. }\left[\mathbf{M} \quad-\mathbf{S}^{\top}\right] \mathbf{y}=-\mathbf{h}$$

3.1 接触力模型在机器人与环境接触的场景中，处理接触力是关键问题。接触力可以分为软接触和硬接触两种情况。在这两种情况下，系统的动力学方程可以表示为：

其中：

是惯性矩阵。

是包括科里奥利力和重力项的广义力。

是选择矩阵，用于选择控制的关节力矩。

是关节力矩向量。

是雅可比矩阵。

是接触力。

3.2 软接触模型在软接触模型中，没有显式的运动约束。如果接触是软的，系统可以接受轻微的约束违反。这种情况下，我们通过优化问题来求解系统状态。

优化问题我们设定优化目标最小化以下代价函数：

约束条件为：

其中：

是优化变量，包括关节速度和关节力矩。

和 是根据具体控制任务定义的矩阵和向量。

这种优化问题没有显式的运动约束，允许系统在软接触情况下灵活调整。

3.3 刚性接触模型在刚性接触模型中，接触是完全约束的。接触边界由触发函数（Witness Function）定义：

为了确保机器人不加速进入占用空间，我们需要施加一个新的线性约束在加速度上：

3.4 约束条件将这个新的约束条件转化为系统动力学中的线性约束：

将雅可比矩阵引入，可以重写为：

所以，接触模型的优化问题为：

软接触模型

在软接触模型中，我们通过求解优化问题来调整系统状态，允许轻微的约束违反，优化问题为：

刚性接触模型

在刚性接触模型中，接触是完全约束的，需要确保机器人不加速进入占用空间，约束条件为：

这种情况下，通过施加严格的线性约束，可以保证系统的稳定性和准确性。

总之，这两种模型处理接触力的方式不同，适用于不同的应用场景。软接触模型更灵活，适用于允许轻微约束违反的情况，而刚性接触模型更严格，适用于要求严格约束的情况。

接触稳定性对于点接触，稳定性条件为：

对于面接触，稳定性条件为：

刚性接触模型中的优化问题将接触约束包含在先前的问题中：

$$\min {\mathbf{y}=[\dot{\mathbf{v}}, \mathbf{W}, \boldsymbol{\tau}]}|\mathbf{A} \mathbf{y}-\mathbf{a}|{2}^{2} \quad \text { st. }\left[\right] \mathbf{y}=\left[\right]$$

添加不等式约束：

$$\begin{array}{l}\tau_{\min } \leq \tau \leq \tau_{\max } \\dot{\mathbf{v}}{\text {min }} \leq \dot{\mathbf{v}} \leq \dot{\mathbf{v}}{\text {max }} \\mathbf{B W}<\mathbf{0}\end{array}$$

4. 多任务控制人形机器人具有许多自由度（DOF），因此可以同时实现多个任务。这被称为冗余解决（Redundancy resolution）。

多任务控制中有两种方法：

加权方法（Weighted Approach）

严格/优先级方法（Strict/Prioritized Approach）

加权方法假设有 个任务，每个任务的成本函数为：

$$g_{i}(\mathbf{y})=\left|\mathbf{A}{i} \mathbf{y}-\mathbf{a}{i}\right|_{2}^{2} \quad i=1, \ldots, N$$

根据用户定义的权重 对这些成本进行求和：

$$\begin{array}{l}\min {\mathbf{y}=[\dot{\mathbf{v}}, \mathbf{W}, \boldsymbol{\tau}]} \sum{i=1}^{N} w_{i}\left|\mathbf{A}{i} \mathbf{y}-\mathbf{a}{i}\right|_{2}^{2} \ \\operatorname{st.}\left[\right] \mathbf{y}=\left[\right]\end{array}$$

加权方法的优点是没有增加复杂性（仍然是最小二乘问题），但缺点是任务之间相互妥协，并且需要用户定义权重。

优先级方法假设有 个任务，每个任务的成本函数为：

$$g_{i}(\mathbf{y})=\left|\mathbf{A}{i} \mathbf{y}-\mathbf{a}{i}\right|_{2}^{2} \quad i=1, \ldots, N$$

定义优先级列表：

这需要解决一系列 个优化问题：

第一级：$$\begin{aligned}g_{1}^{\star} & =\min {\mathbf{y}=[\dot{\mathbf{v}}, \mathbf{W}, \boldsymbol{\tau}]} g{1}(\mathbf{y}) \& \text { st. }\left[\right] \mathbf{y}=\left[\right]\end{aligned}$$

第二级：$$\begin{aligned}g_{2}^{\star} =&\min {\mathbf{y}=[\dot{\mathbf{i}}, \mathbf{W}, \tau]} g{2}(\mathbf{y}) & \\text { st. }&\left[\right] \mathbf{y}=\left[\right] \& g_{1}(\mathbf{y})=g_{1}^{\star}\end{aligned}$$

第 级：$$\begin{aligned}g_{i}^{\star}= & \min {\mathbf{y}=[\dot{\mathbf{v}}, \mathbf{W}, \boldsymbol{\tau}]} g{i}(\mathbf{y}) \\text { st. } & {\left[\right] \mathbf{y}=\left[\right] } \& g_{j}(\mathbf{y})=g_{j}^{\star} \quad \forall j

优先级方法的优点是优先级更容易定义，但计算量较大。

5. 性能优化问题的维度可以描述如下：

目标是最小化二范数误差，受到等式约束和不等式约束：

其中，决策变量 包含 （加速度）、（接触力）和 （关节力矩）。具体维度如下：

决策变量维度：

等式约束维度：

不等式约束维度：

其中 代表速度变量的维度， 代表接触力的维度， 代表关节力矩的维度， 是某个比例系数， 代表某些附加因素的数量。

解决这个优化问题的复杂度主要受限于 Cholesky 分解，其复杂度为 。

为了提高求解效率，可以通过分解机器人动力学为非执行和执行动力学来减少计算量：

$$\begin{array}{ll}\mathbf{M}{u} \ddot{\mathbf{q}}+\mathbf{h}{u}=\mathbf{J}{u}^{\top} \mathbf{W} & \in \mathbb{R}^{6} \\mathbf{M}{a} \ddot{\mathbf{q}}+\mathbf{h}{a}=\boldsymbol{\tau}+\mathbf{J}{a}^{\top} \mathbf{W} & \in \mathbb{R}^{n_{a}}\end{array}$$

其中 和 分别为非执行和执行部分的质量矩阵， 和 为相应的非执行和执行部分的向量， 和 为相应的雅可比矩阵。

通过将关节力矩与加速度和接触力的关系明确，可以将关节力矩从决策变量中移除：

$$\boldsymbol{\tau} = \mathbf{M}{a} \ddot{\mathbf{q}} + \mathbf{h}{a} - \mathbf{J}_{a}^{\top} \mathbf{W}$$

因此可以重新表示决策变量：

$$\left[\right] = \left[\begin{array}{cc}\mathbf{I} & \mathbf{0} \\mathbf{0} & \mathbf{I} \\mathbf{M}{a} & -\mathbf{J}{a}^{\top}\end{array}\right]\left[\right] + \left[\right]$$

记为：

这样，优化问题就变为：

通过这种变换，决策变量的数量减少了 ，从而提高了优化问题的求解效率。