链接地址如下：http://underactuated.csail.mit.edu/underactuated.html
本系列博客写作的初衷是为了督促自身在学习机器人控制过程中加强理论方面的训练，写作的内容主要是对原文的翻译，以及掺杂一些个人的简单体会。当然，我也会尽可能表述清晰，帮助读者降低一些语言方面的压力。
对于许多低维的欠驱动机器人系统，目前已经取得了许多令人瞩目的进展。尽管这些系统相对简单，但是都刻画了欠驱动系统控制问题的本质。这一章主要从几款经典的欠驱动机器人系统出发，介绍了其控制的基本工具。这些机器人分别是：Acrobots，Cart-Poles还有Quadrotors(四旋翼)。
Acrobot是一款平面二连杆机械臂，它的肘关节处有驱动器，而肩关节处则没有，显然这是一个欠驱动系统。对于Acrobot而言，最常见的控制任务就是起摆控制(swing-upcontrol)，即通过控制肘关节的力矩，使得整个机械臂摆到垂直向上的状态，并保持平衡。以下是Acrobot的示意图：
Acrobot的控制问题是一类欠驱动机器人系统的典型问题，它非常重要，因为它实际上非常类似于walkingrobot的最简化的模型。
我们定义肩关节的角度为q_1=\theta_1，肘关节的角度为q_2=\theta_2，定义位置向量为：\mathbf{q}=[\theta_1,\theta_2]^T。因为角度以及角速度都是我们所关心的，所以定义机器人的状态向量为：\mathbf{x}=[\mathbf{q},\dot{\mathbf{q}}]^T。此外，定义机器人的零状态是，机械臂垂直向下的情况。而我们期望将机器人控制到\mathbf{x}=[\pi,0,0,0]^T。
动力学方程可以表示如下：
\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}}+\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}=\tau_{g}(\mathbf{q})+\mathbf{B}\mathbf{u}
其中：
\begin{aligned}&\mathbf{M}(\mathbf{q})=\left[\begin{array}{cc}I_{1}+I_{2}+m_{2}l_{1}^{2}+2m_{2}l_{1}l_{c2}c_{2}&I_{2}+m_{2}l_{1}l_{c2}c_{2}\\I_{2}+m_{2}l_{1}l_{c2}c_{2}&I_{2}\end{array}\right]\\&\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})=\left[\begin{array}{cc}-2m_{2}l_{1}l_{c2}s_{2}\dot{q}_{2}&-m_{2}l_{1}l_{c2}s_{2}\dot{q}_{2}\\m_{2}l_{1}l_{c2}s_{2}\dot{q}_{1}&0\end{array}\right]\\&\tau_{g}(\mathbf{q})=\left[\begin{array}{c}-m_{1}gl_{c1}s_{1}-m_{2}g\left(l_{1}s_{1}+l_{c2}s_{1+2}\right)\\-m_{2}gl_{c2}s_{1+2}\end{array}\right],\mathbf{B}=\left[\begin{array}{l}0\\1\end{array}\right]\end{aligned}
其中，c_1=\cos(q_1),c_2=\cos(q_2),s_1=\sin(q_1),s_2=\sin(q_2)
另外，\mathbf{u}=\tau，即肘关节力矩。
动力学方程的推导，读者可参考原文。
这个例子展示了Acrobot的运动
Note：这里官方给了一个例子，目录为：\example\acrobot\constant_torque.ipynb
Cart-pole系统由一个水平运动的小车和一根与小车铰接的连杆(倒立摆)构成，通常其主要的控制任务是，通过对小车施加水平方向的力，来控制连杆竖直向上，并保持平衡。由于Cart-pole系统的平衡任务相对简单，利用简单的线性控制技术也能实现，所以在许多控制入门课程中都有出现。而本章则会进一步关注到Cart-pole的起摆控制与平衡控制的结合，这就需要用到较为复杂的非线性控制技术了。Cart-pole的示意图如下：
在这里，我们将系统的状态参数化为：x表示水平位置，\theta为倒立摆的角度。因此，广义的位置向量可以表示为\mathbf{q}=[x,\theta]^T，系统状态向量表示为：\mathbf{x}=[\mathbf{q},\dot{\mathbf{q}}]^T。因此，控制任务可以表示为，要将系统稳定在：\mathbf{x}=[0,\pi,0,0]^T。
由此，得到机器人动力学方程为：
\begin{array}{l}\ddot{x}=\frac{1}{m_{c}+m_{p}\sin^{2}\theta}\left[f+m_{p}\sin\theta\left(l\dot{\theta}^{2}+g\cos\theta\right)\right]\\\ddot{\theta}=\frac{1}{l\left(m_{c}+m_{p}\sin^{2}\theta\right)}\left[-f\cos\theta-m_{p}l\dot{\theta}^{2}\cos\theta\sin\theta-\left(m_{c}+m_{p}\right)g\sin\theta\right]\end{array}
为了研究系统的本质，我们可以假设所有常量都为1，那么有：
\begin{array}{c}2\ddot{x}+\ddot{\theta}\cos\theta-\dot{\theta}^{2}\sin\theta=f\\\ddot{x}\cos\theta+\ddot{\theta}+\sin\theta=0\end{array}
关于动力学方程的推导，读者可参考原文。
这个例子展示了Cart-pole的运动
Note：这里官方给了一个例子，目录为：\example\cartpole\constant_force.ipynb
这款机器人就无须更多介绍了。
这里仅在二维平面上考虑一个简单的四旋翼模型。不过因为是在平面上，所以只要两个旋翼就够了。其示意图如下：
其动力学方程如下：
\begin{array}{c}m\ddot{x}=-\left(u_{1}+u_{2}\right)\sin\theta\\m\ddot{y}=\left(u_{1}+u_{2}\right)\cos\theta-mg\\I\ddot{\theta}=r\left(u_{1}-u_{2}\right)\end{array}
其中：m代表质量，I代表围绕其中心的转动惯量，r代表两个旋翼间的距离。其余量可以参考图片中的定义。
在这一小节中，我们将会针对Acrobot和Cart-Pole系统设计线性控制器，使系统在不稳定的平衡点附近保持平衡。首先，我们需要对系统在平衡点处进行线性化，然后检查线性系统的可控性，最后使用Linearquadraticregulator(LQR)方法来设计反馈控制器。
在平衡点(\mathbf{x}^*,\mathbf{u}^*)附近的线性化过程基于泰勒展开，可以表达为如下形式：
\dot{\mathbf{x}}=\mathbf{f}(\mathbf{x},\mathbf{u})\approx\mathbf{f}\left(\mathbf{x}^{*},\mathbf{u}^{*}\right)+\left[\frac{\partial\mathbf{f}}{\partial\mathbf{x}}\right]_{\mathbf{x}=\mathbf{x}^{*},\mathbf{u}=\mathbf{u}^{*}}\left(\mathbf{x}-\mathbf{x}^{*}\right)+\left[\frac{\partial\mathbf{f}}{\partial\mathbf{u}}\right]_{\mathbf{x}=\mathbf{x}^{*},\mathbf{u}=\mathbf{u}^{*}}\left(\mathbf{u}-\mathbf{u}^{*}\right)
以一般的机械臂方程为例，他的动力学方程为：
\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}}+\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}=\tau_{g}(\mathbf{q})+\mathbf{B}\mathbf{u}
事实上大多数，机器人系统的动力学方程都可以写成这种形式。
定义\mathbf{x}=[\mathbf{q},\dot{\mathbf{q}}]^T，我们在平衡点处对系统进行线性化，由于是平衡点，说明\mathbf{f}\left(\mathbf{x}^{*},\mathbf{u}^{*}\right)=0。所以，线性化的结果为：
\begin{aligned}\dot{\mathbf{x}}&=\left[\begin{array}{c}\dot{\mathbf{q}}\\\mathbf{M}^{-1}(\mathbf{q})\left[\tau_{g}(\mathbf{q})+\mathbf{B}(\mathbf{q})\mathbf{u}-\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}\right]\end{array}\right]\\&\approx\mathbf{A}_{lin}\left(\mathbf{x}-\mathbf{x}^{*}\right)+\mathbf{B}_{lin}\left(\mathbf{u}-\mathbf{u}^{*}\right)\end{aligned}
其中，\mathbf{A}_{lin},\mathbf{B}_{lin}是常数矩阵，定义为：
\begin{aligned}&\mathbf{A}_{lin}=\left[\begin{array}{cc}\mathbf{0}&\mathbf{I}\\\mathbf{M}^{-1}\frac{\partial\tau_{g}}{\partial\mathbf{q}}+\sum_{j}\mathbf{M}^{-1}\frac{\partial\mathbf{B}_{j}}{\partial\mathbf{q}}u_{j}&\mathbf{0}\end{array}\right]_{\mathbf{x}-\mathbf{x}^{*},\mathbf{u}=\mathbf{u}^{*}}\\&\mathbf{B}_{lin}=\left[\begin{array}{c}\mathbf{0}\\\mathbf{M}^{-1}\mathbf{B}\end{array}\right]_{\mathbf{x}=\mathbf{x}^{*},\mathbf{u}=\mathbf{u}^{*}}\end{aligned}
定义\mathbf{x}=\mathbf{x}-\mathbf{x}^{*},\mathbf{u}=\mathbf{u}-\mathbf{u}^{*}，则有：
\dot{\overline{\mathbf{x}}}=\mathbf{A}_{lin}\overline{\mathbf{x}}+\mathbf{B}_{lin}\overline{\mathbf{u}}
以上便是在平衡点处对系统进行线性化的结果。
研究线性化系统的性质可以告诉我们有关非线性系统(局部)性质的一些信息。例如，线性化系统是稳定的则意味着非线性系统的局部指数稳定性。反之，线性化系统不稳定也意味着系统是局部不稳定的。不过需要注意，当线性化系统是临界稳定的，则无法判断非线性系统的稳定性。
如果能够找到一个无约束的输入信号\mathbf{u}(t),t\in[0,t_f]，使得系统在有限时间0
对于一般的线性系统\dot{\mathbf{x}}=\mathbf{A}\mathbf{x}+\mathbf{B}\mathbf{u}而言，由于我们可以写出其闭合解，因此，它的能控性条件是可以解析表达的，即：
\operatorname{rank}\left[\begin{array}{lllll}\mathbf{B}&\mathbf{AB}&\mathbf{A}^{2}\mathbf{B}&\cdots&\mathbf{A}^{n-1}\mathbf{B}\end{array}\right]_{n\times(nm)}=n
其中n是\mathbf{x}的维度。
原文中，作者提供了一个例子，是关于系统可控性的更加直观的理解。即假设系统矩阵\mathbf{A}没有重复的特征根，可以变换为对角矩阵，即把系统状态转换到了模态坐标上，然后再分析系统的控制输入对每一个模态的影响。读者如果对这部分内容感兴趣，可以进一步阅读原文。
通过对Acrobot和Cart-Pole系统可控性的分析，我们知道二者对应的线性化系统是可控的。这是非常有趣的现象，尽管这两个系统都是欠驱动，但是在它的非稳定平衡点附近是可控的。
这意味着，欠驱动系统不一定是不可控的系统。欠驱动系统并不意味着系统无法到达状态空间中的某些位置，而是意味着系统不能以任意的轨迹去达到目标位置。有时候，欠驱动系统需要采用极其复杂的轨迹才能达到目标位置
最后，可控性分析通常是对于线性时不变系统而言的。对于非线性系统，可控性分析可以进一步扩展为可达性(reachability)分析。
可控性分析告诉我们确实存在一条轨迹可以到达平衡点，但是并没有说，怎么进行控制。
进行这类控制的方法有非常多，而其中有一种常用的控制器被称为：LQR控制器。这是一类最优控制器，并且其本质上是一个状态反馈控制器。控制器形式为：
\mathbf{u}(t)=-\mathbf{K}\mathbf{x}(t)
其中矩阵\mathbf{K}是基于LQR的理论计算出来的状态反馈控制矩阵。我们可以理解为，LQR控制实际上提供给我们一种计算反馈矩阵的方法，而且这种方法在某种评价标准下还是最优的。关于LQR的具体理论在后面的章节中将会进行详细的介绍。
这里展示了使用LQR控制器控制Acrobot和Cart-Pole的效果
这里官方给了两个例子，目录为：
\example\acrobot\lqr.ipynb
\example\cartpole\
最后，关于LQR，它对于线性化后的系统是最优的，然而从最大化平衡点附近的吸引域的角度而言，就未必是最优的了。从这个意义上看，鲁棒控制要优于LQR，因为它充分考虑了线性化后的系统与原非线性系统的差异。
一般来说，欠驱动系统无法反馈等效于\ddot{\mathbf{q}}=\mathbf{u}。但是虽然无法将完整的系统动力学等效为线性系统，其中一部分系统动态还是可以通过反馈线性化的。这个技术叫做Partialfeedbacklinearization(部分反馈线性化)。
例如cart-pole系统，小车的运动时受到倒立摆的影响的，因此如果我们设计一种反馈控制律，抵消倒立摆带来的影响，那么就相当于对小车的动力学进行了线性化。不仅如此，我们甚至还可以利用反馈控制器，线性化倒立摆的动力学。
可以线性化驱动关节的动力学的控制器，我们称其为：collocatedpartialfeedbacklinearization。此外，可以线性化非驱动关节的动力学的控制器，我们称其为：non-collocatedpartialfeedbacklinearization。
回顾Cart-pole系统的动力学方程如下：
\begin{array}{c}2\ddot{x}+\ddot{\theta}\cos\theta-\dot{\theta}^{2}\sin\theta=f\\\ddot{x}\cos\theta+\ddot{\theta}+\sin\theta=0\end{array}
6.1.1Collocated
如果采用如下反馈控制：
f=\left(2-c^{2}\right)\ddot{x}^{d}-sc-\dot{\theta}^{2}s
原系统可以化作：
\begin{aligned}&\ddot{x}=\ddot{x}^{d}\\&\ddot{\theta}=-\ddot{x}^{d}c-s\end{aligned}
其中\ddot{\mathbf{x}}_d代表期望的小车加速度，c代表\cos\theta，s代表\sin\theta。这是针对小车加速度的线性化，所以是collocatedPFL。
经过反馈控制的改造以后，可以看出，小车的加速度可以被我们任意的配置了。此外，倒立摆的方程也变成了一种非常简单的形式，倒立摆摆动的角速度只与我们期望的加速度有关。因此，它就等价于一个简单的倒立摆了，力矩输入为\ddot{x}^{d}c。因此我们也可以对倒立摆的力矩进行控制，不过这个力矩会受到\cos\theta的调制，尤其是当\cos\theta=0时，我们将无法对倒立摆的力矩进行控制。此时倒立摆处于水平位置，很显然，小车的运动无法产生对倒立摆的力矩。
6.2.2Non-collocated
我们将系统做一个简单的变换：
\begin{array}{c}\ddot{x}=-\frac{\ddot{\theta}+s}{c}\\\ddot{\theta}\left(c-\frac{2}{c}\right)-2\tan\theta-\dot{\theta}^{2}s=f\end{array}
因此，很容易可以得到如下反馈控制律：
f=\left(c-\frac{2}{c}\right)\ddot{\theta}^{d}-2\tan\theta-\dot{\theta}^{2}s
原系统可以化作：
\begin{aligned}&\ddot{\theta}=\ddot{\theta}^{d}\\&\ddot{x}=-\frac{1}{c}\ddot{\theta}^{d}-\tan\theta\end{aligned}
其中\ddot{\mathbf{\theta}}_d代表期望的倒立摆角加速度。注意，上述形式在\cos\theta=0时无效，因为分母不能为0。这是针对倒立摆角加速度的线性化，所以是non-collocatedPFL。
值得注意的是，这个控制器存在数值爆炸的风险，尤其是当\cos\theta=0时。因此在实现时，需要做饱和的处理。
假设系统是最简单的欠驱动形式，即某些关节有控制力拒，某些关节没有的这种形式。那么系统动力学方程可以表示为：
\begin{array}{l}\mathbf{M}_{11}\ddot{\mathbf{q}}_{1}+\mathbf{M}_{12}\ddot{\mathbf{q}}_{2}=\tau_{1}\\\mathbf{M}_{21}\ddot{\mathbf{q}}_{1}+\mathbf{M}_{22}\ddot{\mathbf{q}}_{2}=\tau_{2}+\mathbf{u}\end{array}
我们对\mathbf{q}向量的顺序做了重新排列，被动关节表示为\mathbf{q}_1，主动关节表示为\mathbf{q}_2。另外这里的\tau=\tau_g-\mathbf{C}\dot{\mathbf{q}}。注意到，这里的\mathbf{M}_{11}和\mathbf{M}_{22}是正定矩阵。因为本身的质量矩阵是正定的，根据Schurcomplementconditionforpositivedefiniteness，可以得到此结论。
6.2.1Collocatedlinearization
我们对动力学方程稍作变换：
\begin{array}{c}\ddot{\mathbf{q}}_{1}=\mathbf{M}_{11}^{-1}\left[\tau_{1}-\mathbf{M}_{12}\ddot{\mathbf{q}}_{2}\right]\\\left(\mathbf{M}_{22}-\mathbf{M}_{21}\mathbf{M}_{11}^{-1}\mathbf{M}_{12}\right)\ddot{\mathbf{q}}_{2}-\tau_{2}+\mathbf{M}_{21}\mathbf{M}_{11}^{-1}\tau_{1}=\mathbf{u}\end{array}
所以，反馈控制律可以设计为：
\mathbf{u}=\left(\mathbf{M}_{22}-\mathbf{M}_{21}\mathbf{M}_{11}^{-1}\mathbf{M}_{12}\right)\ddot{\mathbf{q}}_{2}^d-\tau_{2}+\mathbf{M}_{21}\mathbf{M}_{11}^{-1}\tau_{1}
其中\ddot{\mathbf{q}}_2^d是主动关节期望的加速度。
6.2.2Non-collocatedlinearization
我们对动力学方程稍作变换：
\begin{array}{c}\ddot{\mathbf{q}}_{2}=\mathbf{M}_{12}^{+}\left[\tau_{1}-\mathbf{M}_{11}\ddot{\mathbf{q}}_{1}\right]\\\left(\mathbf{M}_{21}-\mathbf{M}_{22}\mathbf{M}_{12}^{+}\mathbf{M}_{11}\right)\ddot{\mathbf{q}}_{1}-\tau_{2}+\mathbf{M}_{22}\mathbf{M}_{12}^{+}\tau_{1}=\mathbf{u}\end{array}
其中，\mathbf{M}_{12}^{+}是Moore-Penrosepseudo-inverse(广义逆)。假设\mathbf{q}_1的维数为l，则当rank({\mathbf{M}_{12}})=l时，广义逆有唯一的解。此时，也称此系统具有StrongInertialCoupling的属性。
所以，反馈控制律可以设计为：
\mathbf{u}=\left(\mathbf{M}_{21}-\mathbf{M}_{22}\mathbf{M}_{12}^{+}\mathbf{M}_{11}\right)\ddot{\mathbf{q}}_{1}^d-\tau_{2}+\mathbf{M}_{22}\mathbf{M}_{12}^{+}\tau_{1}
6.2.3Task-spacepartialfeedbacklinearization(任务空间部分反馈线性化)
通常情况下，我们只对一部分的主动关节以及被动关节的控制感兴趣，我们称感兴趣的这部分关节的运动空间为任务空间(taskspace)。我们考虑如下输出函数：
\mathbf{y}=\mathbf{h}(\mathbf{q})
其中，\mathbf{y}\in\mathbb{R}^p表示任务空间。定义\mathbf{H}_{1}=\frac{\partial\mathbf{h}}{\partial\mathbf{q}_{1}},\mathbf{H}_{2}=\frac{\partial\mathbf{h}}{\partial\mathbf{q}_{2}},\mathbf{H}=\left[\mathbf{H}_{1},\mathbf{H}_{2}\right]。
首先，要明确我们反馈线性化目的是令：\ddot{\mathbf{y}}=\ddot{\mathbf{y}}^d。
接下来开始推导：
\begin{array}{c}\dot{\mathbf{y}}=\mathbf{H}\dot{\mathbf{q}}\\\ddot{\mathbf{y}}=\dot{\mathbf{H}}\dot{\mathbf{q}}+\mathbf{H}_{\mathbf{1}}\ddot{\mathbf{q}}_{1}+\mathbf{H}_{\mathbf{2}}\ddot{\mathbf{q}}_{2}\end{array}
由于根据动力学方程，可得：\ddot{\mathbf{q}}_{1}=\mathbf{M}_{11}^{-1}\left(\tau_{1}-\mathbf{M}_{12}\ddot{\mathbf{q}}_{2}\right)
将其代入上式，可得：
\begin{aligned}\ddot{\mathbf{y}}&=\dot{\mathbf{H}}\dot{\mathbf{q}}+\mathbf{H}_{1}\mathbf{M}_{11}^{-1}\left(\tau_{1}-\mathbf{M}_{12}\ddot{\mathbf{q}}_{2}\right)+\mathbf{H}_{2}\ddot{\mathbf{q}}_{2}\\&=\dot{\mathbf{H}}\dot{\mathbf{q}}+\overline{\mathbf{H}}\ddot{\mathbf{q}}_{2}+\mathbf{H}_{1}\mathbf{M}_{11}^{-1}\tau_{1}\end{aligned}
其中：\overline{\mathbf{H}}=\mathbf{H}_{2}-\mathbf{H}_{1}\mathbf{M}_{11}^{-1}\mathbf{M}_{12}，另外，我们定义该矩阵的Moore逆为：\overline{\mathbf{H}}^{+}=\overline{\mathbf{H}}^{T}\left(\overline{\mathbf{H}}\overline{\mathbf{H}}^{T}\right)^{-1}。
由于\ddot{\mathbf{q}}_2是主动关节的加速度，我们非常容易控制。所以，我们如果想令\ddot{\mathbf{y}}=\ddot{\mathbf{y}}^d，只需令\ddot{\mathbf{q}}_2趋近于如下形式：
\ddot{\mathbf{q}}_{2}=\overline{\mathbf{H}}^{+}\left[\ddot{\mathbf{y}}^{d}-\dot{\mathbf{H}}\dot{\mathbf{q}}-\mathbf{H}_{1}\mathbf{M}_{11}^{-1}\tau_{1}\right]
读者可以把上式代入\ddot{\mathbf{y}}的表达式，很容易验证其正确性。
此时，我们仅需要设置6.2.1Collocatedlinearization中的目标角加速度为\ddot{\mathbf{q}}_{2}^d=\overline{\mathbf{H}}^{+}\left[\ddot{\mathbf{y}}^{d}-\dot{\mathbf{H}}\dot{\mathbf{q}}-\mathbf{H}_{1}\mathbf{M}_{11}^{-1}\tau_{1}\right]。
最后，如果我们希望跟踪任务空间的轨迹，那么我们可以设置：
\ddot{\mathbf{y}}^{d}=\ddot{\mathbf{\bar{y}}}^{d}+\mathbf{K}_{d}\left(\dot{\mathbf{\bar{y}}}^{d}-\dot{\mathbf{y}}\right)+\mathbf{K}_{p}\left(\mathbf{\bar{y}}^{d}-\mathbf{y}\right)
这样只要K_p,K_d>0，那么轨迹跟踪的误差会逐渐收敛。
在这个例子中，展示了我们如何利用任务空间部分反馈线性化来实现轨迹跟踪。
我们希望通过控制，保证倒立摆的末端的高度跟踪某个轨迹。因此，定义输出方程为：
\begin{array}{c}y=h(\mathbf{q})=-l\cos\theta\\\dot{y}=l\dot{\theta}\sin\theta\end{array}
定义期望的轨迹为：\bar{y}^{d}(t)=\frac{l}{2}+\frac{l}{4}\sin(t)
基于以下三个公式，可以设计控制器：
\mathbf{u}=\left(\mathbf{M}_{22}-\mathbf{M}_{21}\mathbf{M}_{11}^{-1}\mathbf{M}_{12}\right)\ddot{\mathbf{q}}_{2}^d-\tau_{2}+\mathbf{M}_{21}\mathbf{M}_{11}^{-1}\tau_{1}
\ddot{\mathbf{q}}_{2}^d=\overline{\mathbf{H}}^{+}\left[\ddot{\mathbf{y}}^{d}-\dot{\mathbf{H}}\dot{\mathbf{q}}-\mathbf{H}_{1}\mathbf{M}_{11}^{-1}\tau_{1}\right]
\ddot{\mathbf{y}}^{d}=\ddot{\mathbf{\bar{y}}}^{d}+\mathbf{K}_{d}\left(\dot{\mathbf{\bar{y}}}^{d}-\dot{\mathbf{y}}\right)+\mathbf{K}_{p}\left(\mathbf{\bar{y}}^{d}-\mathbf{y}\right)
6.2.4任务空间PFL与CollocatedPFL，Non-collocatedPFL的关系
任务空间PFL可以看做是CollocatedPFL，Non-collocatedPFL的泛化形式。
当我们定义输出函数\mathbf{y}=\mathbf{q}_2时，我们就可以从任务空间PFL得到CollocatedPFL。
当我们定义输出函数\mathbf{y}=\mathbf{q}_1时，我们就可以从任务空间PFL得到Non-CollocatedPFL。
关于这部分内容更详细的讨论，读者可以参考原文。
上一章中我们有讨论过基于Energyshaping技术实现单摆的起摆控制。同样的，对于Acrobot和Cart-pole系统，也可以利用这种方法。原文中给出了很多参考文献，读者感兴趣的话可以前往查看。
这部分内容展示了如何利用EnergyShaping的方法来控制Cart-Pole系统起摆。
由于我们之前已经介绍了如何对欠驱动系统做部分反馈线性化，因此，我们现在可以直接基于线性化后的系统进行控制。这里我们选用的是CollocatedPFL的形式，因为这样可以避免由于\cos\theta=0引起的问题。
由于我们假设了所有参数均为1，因此系统方程为：
\begin{aligned}&\ddot{x}=u\\&\ddot{\theta}=-uc-s\end{aligned}
系统的总能量可以表示为：
E(\mathbf{x})=\frac{1}{2}\dot{\theta}^{2}-\cos\theta
倒立摆处在垂直向上的平衡点的能量为：E^d=1(此时的\theta=\pi,\dot{\theta}=0)。这也是我们期望达到的能量。
定义能量的误差为：\tilde{E}(\mathbf{x})={E}(\mathbf{x})-E^d，所以其导数为：
\begin{aligned}\dot{\tilde{E}}(\mathbf{x})&=\dot{E}(\mathbf{x})=\dot{\theta}\ddot{\theta}+\dot{\theta}s\\&=\dot{\theta}[-uc-s]+\dot{\theta}s\\&=-u\dot{\theta}\cos\theta\end{aligned}
因此，我们设计控制器为：
u=k\dot{\theta}\cos\theta\tilde{E}
则可得：\dot{\tilde{E}}=-k\dot{\theta}^2\cos^2\theta\tilde{E}
可知能量误差|\tilde{E}|是不会下降的，但是不能保证会趋于0，这是因为当\theta=\dot{\theta}=0时，系统不会动。但是，我们通过LaSalle'stheorem还是可以证明，一般情况下|\tilde{E}|会趋于0。关于这部分内容，也会在后面章节详细介绍。
最后，为了在起摆控制的同时稳定小车，还增加了一个额外的PD控制器，于是期望的小车加速度可以设计为：
\ddot{x}^{d}=k_{E}\dot{\theta}\cos\theta\tilde{E}-k_{p}x-k_{d}\dot{x}
Energyshaping控制技术是非线性欠驱动系统的控制中相当具有代表性的技术。尽管这样的控制设计方法看起来需要一些小技巧，后续章节也会继续介绍一些更加通用的方法。
在这一部分中，我们将会以平面四旋翼为例，介绍一个新的概念。
平面四旋翼的悬停控制是相对简单的，而在这里我们将要讨论是关于轨迹规划的问题。也就是说，如何在状态空间中找到一条可行的轨迹，甚至能做到实时检测到障碍物并进行躲避。
实际上，关于轨迹优化的问题，后面的章节也会详细的介绍。然而，在这里突然提到这一点是因为这里即将介绍的轨迹规划方法，与部分反馈线性化(PFL)在思想上有某种异曲同工之处，这就是所谓的differentialflatness(微分平坦性)。
部分反馈线性化的技术告诉我们，如果我们有m个驱动器，那么我们至少可以控制机器人系统中的m个量。进一步地，任务空间PFL告诉我们，这m个量还可以自己来挑选。(当然这需要满足某些条件，比如说一些矩阵要是可逆的，或者秩要满足一些条件，或者就是要用inertialcoupling的属性)。
微分平坦性理论则稍有不同，你只是给定了一条m维状态的轨迹，但很有可能这已经决定了机器人系统的所有状态，决定了驱动器必须采取某种控制。
我们知道平面四旋翼有三个自由度(x,y,\theta)，但是只有两个驱动器。
如果我们给定了四旋翼质心的轨迹x(t),y(t),\forallt\in\left[t_{0},t_{f}\right]，那么实际上我们就完全可以推断出轨迹对应的可行的\theta(t)了。甚至，对应的控制u(t)也可以直接推导出来了。
当然，这需要一些条件，即x(t),y(t)要连续可导(至少有四阶导数)。下面将会详细的论证这一点。
平面四旋翼的动力学方程为：
\begin{array}{c}m\ddot{x}=-\left(u_{1}+u_{2}\right)\sin\theta\\m\ddot{y}=\left(u_{1}+u_{2}\right)\cos\theta-mg\\I\ddot{\theta}=r\left(u_{1}-u_{2}\right)\end{array}
我们拿前两个式子相除，可得：
\frac{-m\ddot{x}}{my+mg}=\frac{\left(u_{1}+u_{2}\right)\sin\theta}{\left(u_{1}+u_{2}\right)\cos\theta}=\tan\theta
换句话说，如果给定了\ddot{\mathbf{x}}(t),\ddot{\mathbf{y}}(t)，那么\theta(t)完全可以计算出来。在此基础上，对\theta(t)求二阶导\ddot{\mathbf{\theta}}(t)，那么根据三个动力学方程，我们可以解出u_1,u_2。
这里我们可以解释为什么需要x(t),y(t)连续可导，至少有四阶导数了。因为\mathbf{u}(t)依赖于\ddot{\mathbf{\theta}}(t)，而\ddot{\mathbf{\theta}}(t)依赖于\frac{d^4x(t)}{dt^4},\frac{d^4y(t)}{dt^4}。
而且，如果我们假设控制力矩没有任何限制，那么任何足够光滑的轨迹x(t),y(t)都是可行的，我们都能基于上面的方法，设计出全状态轨迹。这也就意味着，遇到障碍物后，只要你能得到一条可行的x(t),y(t)，我们都能快速的计算出状态轨迹，甚至控制\mathbf{u}(t)
以下是显示平面四旋翼利用微分平坦性进行轨迹规划的图
这里官方给了一个例子，目录为：\example\quadrotor2d\differential_flatness.ipynb
有了上面的这个例子的引入，我们可以正式阐述微分平坦性的定义了。
即对于系统：
\dot{\mathbf{x}}=f(\mathbf{x},\mathbf{u})
我们设计其输出坐标(任务空间)为：
\mathbf{z}(t)=h\left(\mathbf{x},\mathbf{u},\frac{d\mathbf{u}}{dt},\ldots,\frac{d^{k}\mathbf{u}}{dt^{k}}\right)
假如我们可以将\mathbf{x},\mathbf{u}写作输出\mathbf{z}以及其导数的函数，即：
\begin{array}{l}\mathbf{x}(t)=\mathbf{x}\left(\mathbf{z},\frac{d\mathbf{z}}{dt},\dots,\frac{d^{k}\mathbf{z}}{dt^{k}}\right)\\\mathbf{u}(t)=\mathbf{u}\left(\mathbf{z},\frac{d\mathbf{z}}{dt},\dots,\frac{d^{k}\mathbf{z}}{dt^{k}}\right)\end{array}
那么，我们称系统\mathbf{f}对于输出\mathbf{z}是differentiallyflat(微分平坦)的。
最后是原文作者想说的原话：
最后的最后，利用微分平坦性来设计轨迹是很好的，但是需要额外的工作来使系统稳定工作在轨迹上，而这一点会在后面章节中进行介绍。
OK，以上是关于第三章的内容，基于更加复杂的Acrobot和Cart-Pole系统介绍了关于系统线性化，可控性分析，部分反馈线性化的内容。基于平面四旋翼模型介绍了微分平坦性的相关知识。需要说明的是，在我的博客里面只提及了我认为重要的部分，一些例子以及更加详细的说明被略过了。读者可以继续阅读原文，以获得更好的理解。
0
全部评论 （0）