[欠驱动机器人]3,Acobot,Cart-pole和Quadrotors

摘要

介绍了三种经典的非线性机器人模型，分析了它们的动力学模型和控制方法。

Acrobot

介绍

Acrobot是一种两连杆机器人，如上图所示，但是这个机器人只有在肘关节有驱动源，这个东西如果直译过来是体操机器人，取名的愿意大概是平衡木项目中，人倒立在平衡木上，手部和平衡木形成一个铰链，但是这个铰链并不受力，主要是依靠腰部调整重心来保持平衡。

动力学方程

对于复杂机器人，使用牛顿法建立动力学方程就太阴间了，使用拉格朗日方程，得到两连杆的动能为*(第二根杆动能的计算方法：刚体平面运动的动能为质心的平动能量和绕质心的转动能量之和，可以先计算第二根杆的质心的平动能量，而后根据肘部关节和质心的速度差得到相对转动速度，求解整理可得下式，公式中的转动惯量为绕端点的转动惯量)*：
$$\begin{gathered} T_1=\frac{1}{2}{I}_1{\dot{q}}_1^2 \\ {T}_2=\frac{1}{2}\left(m_2{l}_1^2+I_2+2m_2{l}_1{l}_{c2}{c}_2\right){\dot{q}}_1^2+\frac{1}{2}{I}_2{\dot{q}}_2^2+\left(I_2+m_2{l}_1{l}_{c2}{c}_2\right){\dot{q}}_1{\dot{q}}_2 \end{gathered}$$

势能为：
$$U=-m_{1}g{l}_{c1}{c}_1-m_{2}g\left(l_1{c}_1+l_{c2}{c}_{1+2}\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}$$
式中，
$$\begin{array}{c}\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}g{l}_{c1}{s}_1-m_{2}g\left(l_1{s}_1+l_{c2}{s}_{1+2}\right)\\ -m_{2}g{l}_{c2}{s}_{1+2}\end{array}\right],\mathbf{B}=\left[\begin{array}{l}0\\ 1\end{array}\right].\end{array}$$

Cart-Pole

驱动小车来控制单摆的角度。

动力学方程

系统能量如下所示：
$$\begin{array}{l}T=\frac{1}{2}\left(m_c+m_p\right){\dot{x}}^2+m_p\dot{x}\dot{\theta }l\cos \theta +\frac{1}{2}{m}_p{l}^2{\dot{\theta }}^2\\ U=-m_{p}gl\cos \theta \end{array}$$
带入拉格朗日方程并整理得到：
$$\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}\text{\hspace{1em}(1)}$$

$$\begin{array}{c}\mathbf{M}(\mathbf{q})=\left[\begin{array}{lc}{m}_c+m_p& m_{p}l\cos \theta \\ m_{p}l\cos \theta & m_p{l}^2\end{array}\right],\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})=\left[\begin{array}{cc}0& -m_{p}l\dot{\theta }\sin \theta \\ 0& 0\end{array}\right]\\ {\tau }_g(\mathbf{q})=\left[\begin{array}{c}0\\ -m_{p}gl\sin \theta \end{array}\right],\mathbf{B}=\left[\begin{array}{l}1\\ 0\end{array}\right]\end{array}$$

平面四旋翼

这个动力学方程很简单，假设两个螺旋桨的力为u1，u2，则动力学模型如下
$$\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}$$

控制系统

本节要做的事情是对于Acrobot和Cart-pole，控制他们在不稳定不动点附近的行为，为了完成这一点，我们可以在不稳定不定点周围进行线性化，而后使用LQR算法设计控制器。

线性化系统

主要针对的是不稳定不动点进行线性化，可以得到下边的式子：
$$\dot{\mathbf{x}}=\mathbf{f}(\mathbf{x},\mathbf{u})\approx \mathbf{f}\left({\mathbf{x}}^{\ast },{\mathbf{u}}^{\ast }\right)+{\left[\frac{\partial \mathbf{f}}{\partial \mathbf{x}}\right]}_{\mathbf{x}={\mathbf{x}}^{\ast },\mathbf{u}={\mathbf{u}}^{\ast }}\left(\mathbf{x}-{\mathbf{x}}^{\ast }\right)+{\left[\frac{\partial \mathbf{f}}{\partial \mathbf{u}}\right]}_{\mathbf{x}={\mathbf{x}}^{\ast },\mathbf{u}={\mathbf{u}}^{\ast }}\left(\mathbf{u}-{\mathbf{u}}^{\ast }\right)$$

由于是不动点，所以f（x，u）= 0，整理1式，可以写成以下形式：
$$\begin{array}{rl}\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}}^{\ast }\right)+{\mathbf{B}}_{lin}\left(\mathbf{u}-{\mathbf{u}}^{\ast }\right)\end{array}$$
将矩阵求偏导，并且转化坐标可以得到：
$$\dot{\overline{\mathbf{x}}}={\mathbf{A}}_{lin}\overline{\mathbf{x}}+{\mathbf{B}}_{lin}\overline{\mathbf{u}}$$
式中，两个矩阵依次是：
$$\begin{array}{l}{\mathbf{A}}_{lin}={\left[\begin{array}{cc}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& 0\end{array}\right]}_{\mathbf{x}={\mathbf{x}}^{\ast },\mathbf{u}={\mathbf{u}}^{\ast }}\\ {\mathbf{B}}_{lin}={\left[\begin{array}{c}0\\ {\mathbf{M}}^{-1}\mathbf{B}\end{array}\right]}_{\mathbf{x}={\mathbf{x}}^{\ast },\mathbf{u}={\mathbf{u}}^{\ast }}\end{array}$$

线性系统的可控性

根据目标和上边的线性化，把复杂非线性系统转化成了线性系统，所以接下来讨论线性系统的可控性，这里不详细介绍了只做以下区分

可控性vs欠驱动

欠驱动系统未必是不可控的，可控意味着存在一条轨迹，能使系统从任意点回归到原点，而欠驱动会限制这个轨迹的某些特征，但是欠驱动并不意味着轨迹不存在。

LQR算法

这里对LQR算法只是一个初步介绍，细节下文书分解LQR算法是一种优化算法，首先定义系统的耗能函数为：
$$J\left({\mathbf{x}}_0\right)={\int }_0^{\infty }\left[{\mathbf{x}}^T(t)\mathbf{Q}\mathbf{x}(t)+{\mathbf{u}}^T(t)\mathbf{R}\mathbf{u}(t)\right]dt,\mathbf{x}(0)={\mathbf{x}}_0,\mathbf{Q}={\mathbf{Q}}^T>0,\mathbf{R}={\mathbf{R}}^T>0$$

式中，等号左边表示从x0点到原点需要消耗的函数，Q和R都是对阵正定矩阵，可以使用LQR算法找到一个线性矩阵K，使得：
$$\mathbf{u}(t)=-\mathbf{K}\mathbf{x}(t)$$
这样控制信号u和状态信号x联系起来，使得系统可控，下面使用python-control库举一个例子。

from control import lqr
import numpy as np

A = np.array([[1, 2], [3, 4]])
B = np.array([[1, 0], [0, 0]]) # A，B是系统动力学
Q = np.array([[1, 2], [2, 5]])
R = np.array([[2, 1], [1, 5]])

K, S, E = lqr(A, B, Q, R)
print(K)

输出如下：

需要注意的是LQR对线性系统来说确实是很好用的，但是对非线性系统来说，LQR算法在吸引阈上有些缺陷，离原点太远线性化水平就不够了，这点上，它不如robust control。

PFL算法

在之前的章节，我们提到了欠驱动系统不能简化成双积分器的形式，但是仍然可以将系统动力学的一部分线性化。这种技术被称为部分反馈线性化（PFL）。

考虑之前提到的Cart-pole系统，小车的动力学是受到单摆动力学的影响的，这样我们可以做一件非常神奇的事情，就是控制小车的拉力，这个力可以完全抵消单摆的影响，这样就简化了小车的动力学，而后通过小车的动力学我们也可以间接控制单摆的角度。

当我们线性化驱动关节的时候，这个方法叫做collocated PFL，当我们线性化非驱动关节的时候，叫做non-collocated PFL，接下来介绍cart-pole中的PFL

cart-pole的PFL

Collocated

对上边的公式，为了方便分析，我们可以设定所有的物理参数都是1（重力加速度也为1），可以得到下式：
$$\begin{array}{c}\ddot{\theta }=-\ddot{x}c-s\\ \ddot{x}\left(2-c^2\right)-sc-{\dot{\theta }}^{2}s=f_x\end{array}$$
可以施加外力为：
$$f_x=\left(2-c^2\right){\ddot{x}}^d-sc-{\dot{\theta }}^{2}s$$
带入可得此时系统动力学是：
$$\begin{array}{l}\ddot{x}={\ddot{x}}^d\\ \ddot{\theta }=-{\ddot{x}}^{d}c-s,\end{array}$$
仔细观察方程，发现和单摆章节提到的无阻尼情况是十分类似的，只是输入力不是u，而是x_doubledot*c，因此单摆可以可以视为没有小车的影响，通过调整小车的加速度，来控制单摆的角度也就完成了

Non-collocated

在Collocated中，我们线性化了x，现在我们线性化θ，经过整理可以得到：
$$\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_x\end{array}$$
同样的，可以设置施加的外力为：
$$f_x=\left(c-\frac{2}{c}\right){\ddot{\theta }}^d-2\tan \theta -{\dot{\theta }}^{2}s$$
得到系统方程：
$$\begin{array}{rl}\ddot{\theta }& ={\ddot{\theta }}^d\\ \ddot{x}& =-\frac{1}{c}{\ddot{\theta }}^d-\tan \theta .\end{array}$$
需要注意的是，上面的线性化只有当cosθ不为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}$$
q1表示所有的欠驱动铰链，而t包含了重力和科里奥利力的影响，而且：
$$\mathbf{M}(\mathbf{q})=\left[\begin{array}{ll}{\mathbf{M}}_{11}& {\mathbf{M}}_{12}\\ {\mathbf{M}}_{21}& {\mathbf{M}}_{22}\end{array}\right]$$
M是个正定矩阵，所以M11和M22也全部是正定矩阵

Collocated线性化

这个是要线性化所有的驱动关节，因此，可以整理上边的方程，得到下面的方程：
$$\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}$$
可以整理第二个方程，得到需求的q2，然后带入1式，就使得q1和q2解耦。

Non-Collocated线性化

同样我们也可以把方程整理成下边这种形式：
$$\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}$$
式中，M+表示广义逆矩阵。（这里不在多介绍，感兴趣可以去阅读原文）

Swing-up 控制

调整能量

我们在上一章使用了调整能量的方法控制倒立摆，事实上，这个方法适用范围很广泛，可以用于控制acrobot和cart-pole系统。

cart-pole

我们可以使用PFL来控制系统的能量，让小车在原点附近摆动，而单摆在连续的转动，系统的动力学方程如下：
$$\begin{array}{c}\ddot{x}=u\\ \ddot{\theta }=-uc-s\end{array}$$
单摆的能量是**(此处忽略小车的运动对单摆能量的影响，正如前面说的控制目标是让小车在原点摆动，速度较小，所以忽略)**：
$$E(\mathbf{x})=\frac{1}{2}{\dot{\theta }}^2-\cos \theta$$
目标能量是
$$E^d=1$$
定义能量差，并对时间求导可得：
$$\begin{array}{rl}\dot{\tilde{E}}(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{array}$$
因此可以取控制信号u为：
$$u=k\dot{\theta }\cos \theta \tilde{E}$$
系统的能量方程为：
$$\dot{\tilde{E}}=-k{\dot{\theta }}^2{\cos }^2\theta \tilde{E}.$$
需要注意的是，此处只是保证了能量误差不会增大，但不一定会收敛到0，如果系数是0，能量差会保持在一个定值。但是如果系统能满足一下条件：
$${\int }_0^t{\dot{\theta }}^2\left(t^{\prime }\right){\cos }^2\theta \left(t^{\prime }\right)d{t}^{\prime }\to \infty ,t\to \infty$$
则能量差一定会回到0，这个理论是之后在李雅普诺夫章节提出的LaSalle理论，请听下回分解，但是从直觉上来看呢，这个式子说明了：对于时间，积分里的方程不恒等于0，所以斜率永远是负数，从而最终收敛。

要进一步控制系统动力学的话，我们需要使用PD控制器，使得小车的控制方程如下所示：
$${\ddot{x}}^d=k_E\dot{\theta }\cos \theta \tilde{E}-k_{p}x-k_d\dot{x}$$

总结

这种通过改变能量从而改变系统状态的控制方法是欠驱动控制方法里的代表方法，它的思路就是通过一些方法（这里是改变能量）来去除系统的非线性，而后寻找合适的李雅普诺夫方程来证明系统的稳定性。

参考文献

$$@bookunderactuated,title="UnderactuatedRobotics",subtitle="AlgorithmsforWalking,Running,Swimming,Flying,andManipulation",howpublished="CourseNotesforMIT6.832",author="Tedrake,Russ",year=2022,url="http://underactuated.mit.edu",$$