9. 多输入物理系统控制

9.1 机器人学概述

动力学方程

\boldsymbol{H}(\boldsymbol{q})\boldsymbol{\ddot{q}}+\boldsymbol{C}(\boldsymbol{q},\boldsymbol{\dot{q}})\boldsymbol{\dot{q}}+\boldsymbol{D}(\boldsymbol{q},\boldsymbol{\dot{q}})\boldsymbol{\dot{q}}+\boldsymbol{g}(\boldsymbol{q})=\boldsymbol{\tau } \tag{9.1}

$\boldsymbol{q}$ 是关节角向量， $\boldsymbol{\dot{q}}$ 是关节速度向量
$\boldsymbol{H}(\boldsymbol{q})$ 是惯性矩阵(inertial matrix)
- 是对称正定的
- 是一致正定的，即 $\exists \alpha>0,\forall \boldsymbol{q}, \boldsymbol{H}( \boldsymbol{q}) \geqslant \alpha I$
$\boldsymbol{C}(\boldsymbol{q},\boldsymbol{\dot{q}})\boldsymbol{\dot{q}}$ 是向心力和科里奥利力矩(centripetal and coriolis torques)
- depends on $\boldsymbol{q}$
- quadratic in $\boldsymbol{\dot{q}}$
$\boldsymbol{D}(\boldsymbol{q},\boldsymbol{\dot{q}})$ 是粘滞摩擦(viscous friction)
- $\boldsymbol{D}(\boldsymbol{q},\boldsymbol{\dot{q}}) \geqslant 0$
$\boldsymbol{g}(\boldsymbol{q})$ 是重力矩(gravitational torque)
$\boldsymbol{\tau }$ 是执行器输入的力矩向量

9.1.1 位置控制

假设机械手在水平面上( $\boldsymbol{g}(\boldsymbol{q})=0$ )，并且任务就是把它移动到给定的目标位置，例如由一个期望的关节角的常向量 $\boldsymbol{q}_d$ 所确定的最后位置。
物理上清楚的是，关节的比例一微分控制器，即反馈控制规律将能完成希望未知控制任务，具体地说，基于位置误差 $\tilde{q}_j=q_j-q_{d_j}$ 和关节速度 $\dot{q}_j$ ( $j$ =1,2)的局部测量，反馈控制规律独立地选择每个执行器输入

\tau_j=-k_{P_j}\tilde{q}_j-k_{D_j}\dot{q}_j \tag{9.2}

将能完成希望的位置控制任务。实际上，控制规律(9.2)(其中， $k_{P_j}$ 和 $k_{D_j}$ 是严格的正常数)仅仅是模拟了在每个机械手关节处安装一个耗散机械装置的作用。该装置由一个弹簧和一个阻尼器组成，并且以期望的 $\boldsymbol{q}_{d_j}$ 作为静止位置，所产生的无源物理系统简单地表示了摆向静止位置 $\boldsymbol{q}_{d_j}$ 的阻尼振动。

virtual physic,virtual spring(term P),virtual damper(term D)

下面讨论这种控制器设计为何有效

机械手动能 $T=\frac{1}{2}\dot{\boldsymbol{q}}^T\boldsymbol{H}(\boldsymbol{q})\dot{\boldsymbol{q}}$ ，根据能量守恒写成如下形式

\frac{1}{2}\frac{\mathrm{d} }{\mathrm{d} t}[\dot{\boldsymbol{q}}^T\boldsymbol{H}\dot{\boldsymbol{q}}]=\dot{\boldsymbol{q}}^T(\boldsymbol{\tau}-\boldsymbol{D}\boldsymbol{\dot{q}}-\boldsymbol{g}) \tag{9.3}

式中左边是机械手动能的导数，右边表示执行器的功率输入。

取比(9.2)式稍一般的控制输入，即取

\boldsymbol{\tau}=-\boldsymbol{K}_{P}\tilde{\boldsymbol{q}}-\boldsymbol{K}_{D}\dot{\boldsymbol{q}} \tag{9.4}

其中， $K_P$ 和 $K_D$ 是定常对称正定矩阵(通常的比例一微分控制器(9.2)对应的 $K_P$ 和 $K_D$ 是对角矩阵)。

考查系统的总机械能V，这里设系统的控制律(9.4)是根据物理弹簧和阻尼器原理得到的，即总机械能是

V=\frac{1}{2}[\dot{\boldsymbol{q}}^T\boldsymbol{H}\dot{\boldsymbol{q}}+\boldsymbol{\tilde{q}}^T\boldsymbol{K}_P\boldsymbol{\tilde{q}}] \tag{9.5}

为了分析控制系统的闭环性态，我们将使用这个有效的机械能V作为李雅普诺夫函数，根据(9.3)，V对时间的导数为

\begin{align*} \dot{V}&=\dot{\boldsymbol{q}}^T(\boldsymbol{\tau}-\boldsymbol{D}\boldsymbol{\dot{q}})+ \dot{\boldsymbol{q}}^T\boldsymbol{K}_P\boldsymbol{\tilde{q}}\\ &=\boldsymbol{\dot{q}}^T(-\boldsymbol{K}_{P}\tilde{\boldsymbol{q}}-\boldsymbol{K}_{D}\dot{\boldsymbol{q}} -\boldsymbol{D}\boldsymbol{\dot{q}})+\dot{\boldsymbol{q}}^T\boldsymbol{K}_P\boldsymbol{\tilde{q}}\\ &=-\boldsymbol{\dot{q}}^T(\boldsymbol{K}_D+D)\boldsymbol{\dot{q}} \leqslant 0 \end{align*}

可知 $\dot{V}$ 就是阻尼器所耗散的功率(virtual dissipation)。
现在我们只要检验系统不能“停留”在V=0的任何一点 $\boldsymbol{q}\neq \boldsymbol{q}_d$ 处
利用不变集定理

\dot{V} =0 \Rightarrow \dot{\boldsymbol{q}} =0 \Rightarrow \boldsymbol{H}\boldsymbol{\ddot{q}}=-\boldsymbol{K}_{P}\boldsymbol{\tilde{q}} ~(9.1) \Rightarrow \boldsymbol{\ddot{q}}= -\boldsymbol{H}^{-1}\boldsymbol{K}_{P}\tilde{\boldsymbol{q}}\neq 0 ~unless~ \boldsymbol{\tilde{q}} =0 \Leftrightarrow \boldsymbol{q} =\boldsymbol{q}_d

所以，正如物理推理所得到的，系统收敛于期望的状态。

9.1.2 轨线控制(trajectory tracking)

如果存在外部的重力距 $\boldsymbol{g}(\boldsymbol{q})$ ，能量守恒可写为(推广的(9.3)式）

\begin{align*} \dot{\boldsymbol{q}}^T(\boldsymbol{\tau}-\boldsymbol{D}\boldsymbol{\dot{q}}-\boldsymbol{g}) & =\frac{1}{2}\frac{\mathrm{d} }{\mathrm{d} t}[\dot{\boldsymbol{q}}^T\boldsymbol{H}\dot{\boldsymbol{q}}]\\ &=\dot{\boldsymbol{q}}^T\boldsymbol{H}\ddot{\boldsymbol{q}}+\frac{1}{2}\dot{\boldsymbol{q}}^T\dot{\boldsymbol{H}}\dot{\boldsymbol{q}} \tag{9.6} \end{align*}

代入式(9.1)展开 $\boldsymbol{H}\ddot{\boldsymbol{q}}$

\begin{align*} \dot{\boldsymbol{q}}^T(\boldsymbol{\tau}-\boldsymbol{D}\boldsymbol{\dot{q}}-\boldsymbol{g})&=\dot{\boldsymbol{q}}^T\boldsymbol{H}\ddot{\boldsymbol{q}}+\frac{1}{2}\dot{\boldsymbol{q}}^T\dot{\boldsymbol{H}}\dot{\boldsymbol{q}}\\ &=\dot{\boldsymbol{q}}^T(\boldsymbol{\tau}-\boldsymbol{D}\boldsymbol{\dot{q}}-\boldsymbol{g}-\boldsymbol{C}\boldsymbol{\dot{q}})+\frac{1}{2}\dot{\boldsymbol{q}}^T\dot{\boldsymbol{H}}\dot{\boldsymbol{q}} \end{align*}

观察到等式两侧同时出现了 $\dot{\boldsymbol{q}}^T(\boldsymbol{\tau}-\boldsymbol{D}\boldsymbol{\dot{q}}-\boldsymbol{g})$ ,整理化简可得

\dot{\boldsymbol{q}}^T(\dot{\boldsymbol{H}}-2\boldsymbol{C})\dot{\boldsymbol{q}}=0 \tag{9.7}

这个结果表明矩阵 $\dot{\boldsymbol{H}}-2\boldsymbol{C}$ 是反对称的——这不完全是(9.7)的直接结果，因为矩阵 $\dot{\boldsymbol{H}}-2\boldsymbol{C}$ 自身与 $\boldsymbol{\dot{q}}$ 有关。实际上，虽然科里奥利力矩和向心力矩向量 $\boldsymbol{C}\dot{\boldsymbol{q}}$ 与关节速度 $\boldsymbol{\dot{q}}$ 都是惟一性的物理量，但这里没有惟一定义矩阵 $\boldsymbol{C}$ 。然而，下面将说明，如果 $\boldsymbol{C}$ 的元素定义为

\boldsymbol{C}_{ij}=\frac{1}{2}\sum_{k=1}^{n}\frac{\partial \boldsymbol{H}_{ij}}{\partial q_k} \dot{q}_k+\frac{1}{2}\sum_{k=1}^{n}(\frac{\partial \boldsymbol{H}_{ik}}{\partial q_j}-\frac{\partial \boldsymbol{H}_{jk}}{\partial q_i}) \dot{q}_k \tag{9.8}

那么矩阵 $\dot{\boldsymbol{H}}-2\boldsymbol{C}$ 确实是反对称的。因此** $\dot{\boldsymbol{H}}-2\boldsymbol{C}$ 可看作是能量守恒的矩阵表示**。

“matrix vertion” of energy conservation

定义变量 $\boldsymbol{s}$

\boldsymbol{s}= \boldsymbol{\dot{\tilde{q}}}+\Lambda \boldsymbol{\tilde{q}} \tag{9.9}

其中， $\Lambda$ 是对称正定矩阵。
事实上，在这个二阶系统中，我们可以把(9.9)式中的s解释为“速度误差”项

\boldsymbol{s}= \boldsymbol{\dot{\tilde{q}}}+\Lambda \boldsymbol{\tilde{q}} =\boldsymbol{\dot{q}}-\boldsymbol{\dot{q}}_r \tag{9.10}

其中 $\boldsymbol{\dot{q}}_r =\boldsymbol{\dot{q}}_d-\Lambda \boldsymbol{\tilde{q}}$

根据位置误差 $\boldsymbol{\tilde{q}}$ 改变期望的速度 $\boldsymbol{\dot{q}}_d$ ，以形成"参照速度"向量 $\boldsymbol{\dot{q}}_r$ ，这仅仅是一种符号处理，它将与能量相关的性质(它用实际关节速度向量 $\boldsymbol{\dot{q}}$ 来表示)转化为轨线控制性质(由实际速度误差向量 $\boldsymbol{s}$ 表示)。
类似于单输入的情况，向量** $\boldsymbol{s}$ 包含了 $\boldsymbol{q}$ 和 $\boldsymbol{\dot{q}}$ 的有界性和收敛性的信息**。
这是因为s的定义(9.9)也可以看作是关于 $\boldsymbol{\tilde{q}}$ 的一个稳定的一阶微分方程(其中 $\boldsymbol{s}$ 看作是输入)。于是，假设了初始状态是有界时，只要说明了 $\boldsymbol{s}$ 的有界性就证明了 $\boldsymbol{\tilde{q}}$ 和 $\boldsymbol{\dot{\tilde{q}}}$ 的有界性，从而证明了 $\boldsymbol{q}$ 和 $\boldsymbol{\dot{q}}$ 的有界性；类似地，如果当 $t \rightarrow \infty$ 时， $\boldsymbol{s} \rightarrow 0$ ，那么有 $\boldsymbol{\tilde{q}} \rightarrow 0$ ，且 $\boldsymbol{\dot{\tilde{q}}} \rightarrow 0$ 。