基于MPC的二阶倒立摆稳定控制

石转转，郭开玺，张 品，张占东

（山西大同大学 机电工程学院，大同 037003）

倒立摆系统是一个不稳定、多变量、非线性的机械系统，在控制过程中，倒立摆系统可以有效地反映出控制系统的非线性、鲁棒性等问题，因此，在控制领域中经常使用倒立摆来检验新的控制策略和算法的有效性[1-2]。 倒立摆为机器人系统中的大量技术应用提供了足够的模型[3]，它们被用作许多实际控制问题的代表性模型： 比如人的手臂运动、火箭的发射[4]、机器人等。

二阶倒立摆为一阶倒立摆的扩展版本，因为它在一阶倒立摆系统中增加了一个摆。 因此，与一阶倒立摆相比，二阶倒立摆是一个高阶、绝对非线性、不稳定的系统[5]。 这使得二阶倒立摆的控制和稳定成为一个非常具有挑战性的问题。 研究人员为此提出了不同的控制技术来控制二阶倒立摆系统。 模型预测控制技术是一种处理多变量约束系统最优控制问题的最有效方法之一， 与其他控制算法相比，模型预测控制（MPC）提供了一种使输出误差和控制工作量最小化的最优控制方案。MPC 使用系统的状态空间模型预测未来输出，并将预测的输出误差合并到二次成本函数中。 在控制过程中，利用二次规划方法使成本函数最小化，并使其服从系统的硬约束。 因此，MPC 可以有效地处理系统的约束[6-7]。

本文提出使用模型预测控制对二阶倒立摆系统进行稳定控制，在Python 中建立倒立摆拉格朗日系统模型， 同时设计模型预测控制快速预测控制器，对处于不同情况下二阶倒立摆系统的稳定控制做了数值仿真计算分析。

1 理论

1.1 系统数学模型

二阶倒立摆系统是一种典型的欠驱动机械系统，其主要由小车和两根摆杆组成，两个摆杆连接到水平面自由移动的小车上。 它有3 个自由变量，即拉力F（水平位置x），第一摆角θ1和第二摆角θ2，二阶倒立摆系统示意图如图1 所示。

图1 机械式二阶倒立摆系统Fig.1 Mechanical double inverted pendulum system

其中F 是控制输入，表示水平拉力；θ1∈（-π，π）表示摆杆1（即下摆杆，链接在小车上）与竖直垂线之间的夹角；θ2∈（-π，π）表示摆杆2（即上摆杆，链接在摆杆1 的另一端）与竖直垂线之间的夹角。 此外；m，m1和m2分别代表小车、摆杆1 和摆杆2 的质量；L1和L2分别代表摆杆1 和摆杆2 的长度；l1和l2分别表示摆杆1 和摆杆2 摆杆质心到低端的长度；J1和J2分别表示摆杆1 和摆杆2 的转动惯量。此外，二阶倒立摆系统的机械参数取值如表1 所示，g 表示重力加速度，参数取自于文献[8]。

表1 二阶倒立摆系统仿真参数Tab.1 Simulation parameters of the double-inverted-pendulum system

式中：z 为广义坐标；La 为拉格朗日函数；T，V 和Fi分别代表系统总的动能、势能和沿广义坐标方向上的力。 选取x，θ1和θ2作为广义坐标。 基于公式La=T-V，可以得出：

二阶倒立摆系统总动能为T=Tm+Tm1+Tm2， 其中Tm为小车的动能，Tm1和Tm2分别为摆杆1 和摆杆2的动能。 小车和两根摆杆的动能分别为

则系统总动能T 为

二阶倒立摆系统的总势能为V=Vm+Vm1+Vm2，其中Vm为小车的势能，Vm1和Vm2分别为摆杆1 和摆杆2 的势能。

小车和两根摆杆的势能分别为

式中：y1=l1cosθ1，y2=L1cosθ1+l2cosθ2则系统的总势能V 为

将式（4）和式（6）代入式（2）以获得拉格朗日算子：

最后，将公式（7）带入到公式（2），可以得到直线二阶倒立摆系统的数学模型，其形式如下：

其中：

1.2 模型线性化

上述方程（8）描述了系统的运动，显然是非线性的，因此，需要将其线性化。 选取系统的状态向量控制输入量u=F，输出变量为最后可得到倒立摆系统的线性状态空间方程为

式中：I 为单位矩阵，系数矩阵A，B，C 分别为

2 模型预测控制

MPC 是一种反馈控制方法，使用状态变量的当前状态值和目标值来预测并提前优化系统响应。 目标是使系统响应接近目标值。 通常情况下，MPC 会优化计算机上的一系列状态轨迹通过提供最佳输入来减少成本[9-10]。它使用当前状态变量来寻找一系列最佳输入，在下一个采样周期更新状态变量，以及在下一时间步中重复计算。 以此得到一系列控制序列，控制序列将按照一个特定时间间隔进行计算，称为控制时域，同时，它将在较长的时间间隔内确定系统的进度，称为预测时域[11]。 MPC 的基本原理如图2 所示。

图2 MPC 基本原理图Fig.2 Basic schematic diagram of MPC

模型预测控制解决问题的第一步就是模型的离散化， 随后将在每个采样时刻求解离散化问题，通常使用以下形式的离散模型：

式中：x（k）和u（k）分别代表系统当前状态和控制输入，根据当前状态x（k）和输入u（k）映射到下一个状态x（k+1），使系统预测得到状态在预测时域上的轨迹[12]，通过离散化状态方程依次对未来整个控制周期的系统状态进行预测，可以得到：

显然，系统在未来某个时刻K 的状态，取决于初始状态x（1）和输入u（k）。当对动态系统未来状态进行预测时，需求得整个预测时域内的控制输入量U，即为后续将要求解的优化问题的独立变量：

在每个采样时刻都将进行优化， 寻找最优解，通过最小化决定控制性能的成本函数来实现，成本函数定义为

式中：Q 是半正定状态加权矩阵，用于约束系统状态偏差；R 是输入加权矩阵，是指约束施加给被控对象的控制量u 的大小。

由于在倒立摆系统运行过程中，对于小车和双摆稳定性，动力性等方面的考虑，需要添加对控制输入量力的约束：

式中：xub和xlb分别为状态的上限和下限，uub和ulb分别输入量拉力的最大值和最小值。 通过在未来每个采样时刻的控制动作中最小化J 来确定U，得到一系列控制序列：

将第一个控制动作应用于系统，作为本控制周期的控制量，下一次根据新的样本重复该过程[13]。

3 仿真研究

为了评估MPC 控制方法的控制性能，本文通过利用Python 编程对二阶倒立摆进行了数值仿真研究，忽略空气阻力和各种摩擦，将二阶倒立摆系统抽象成小车和均匀质杆组成的系统并建立直角坐标系，如图3 所示。 控制目标是小车在初始位置附近运动时， 双摆能够精确的稳定在垂直向上的位置，本文使用基于能量的公式来实现目标。 从势能和动能的角度来考虑能量，当双摆稳定时势能达到最大并且动能最小（向上位置）。

图3 无障碍二阶倒立摆系统仿真模型Fig.3 Simulation model of barrier-free double-inverted-pendulum system

首先将双摆位置初始化为垂直向下（角度θ 为1 π），得到二阶倒立摆无障碍模型预测控制结果，如图4所示。

图4 研究1：无障碍非稳定初始的双倒立摆的输入输出各变量变化Fig.4 Study 1：Changes in the input and output variables of the double-inverted-pendulum with the barrier-free and unstable initial

图4 显示了无障碍倒立摆系统的输入输出各状态变量的变化，其中图4（a）为摆杆动能在模型预测控制下的大小变化，从初始位置动能为零到稳定时动能恢复到零；图4（b）显示了摆杆势能的大小变化，由于初始位置为垂直向下，此时势能为相反数，且数值达到最大， 随着小车的运动及时间的推移，当双摆位置稳定到垂直向上时， 此时势能达到最大，且为正值；图4（c）为双摆角度θ 的变化过程，以及图4（d）在控制过程中施加力F 大小的变化。结果显示，在模型预测控制下，系统在4 s 时达到稳定状态，并且各状态变量也能够限制在约束之内。

为了增加控制的复杂度，在原有的倒立摆系统中增加了一个球形障碍物，半径大小设置为0.2 m，位置位于小车正上方0.5 m 处， 包括设定点跟踪任务，使得双摆在所需位置必须出错，系统模型如图5所示，仿真结果如图6 所示。

图5 有障碍二阶倒立摆系统仿真模型Fig.5 Simulation model of double-inverted-pendulum system with obstacles

图6 研究2：有障碍非稳定初始的双倒立摆的输入输出各变量变化Fig.6 Study 2：Changes in the input and output variables of the double-inverted-pendulum with the obstacle and unstable initial stage

为了与上述实验保持一致性，首先讨论了当双摆初始位置位于垂直向下时各状态变量的变化。 由于增加了一个球形的障碍物，双摆在小车运动过程中需要避免触碰到障碍物，增加了整体的控制复杂度。 从仿真结果图6 中可以看出，相较于无障碍物，系统达到稳定时所需要的时间增加了接近1 s，说明控制小车来回运动多次才能使得双摆处于稳定状态，图6（a）动能和图6（b）势能变化也能体现出。 另外为了更好的突出小车的运动过程，增加了对小车位置的探讨，如图6（c）所示，小车从初始位置0 m运动到最终稳定时的位置-0.8 m；图6（d）给出了控制过程中输入力F 的变化曲线。

另外，在接下来讨论了当双摆初始位置为垂直向上时，双摆各状态变量的预测变化，如图7 所示，当初始位置为垂直向上（稳定状态）时，通过施加外力F 打破稳定状态后，可以看到系统相对于初始位置垂直向下的情况来说可以在更快的时间内再次恢复到稳定状态。 即从稳定状态再次恢复到稳定状态所需时间比从不稳定状态到稳定状态所需时间少1.5 s 左右。

图7 研究3：有障碍稳定初始的双倒立摆的输入输出各变量变化Fig.7 Study 3：Changes in the input and output variables of the initial double-inverted-pendulum with obstacles to stabilize

4 结语

本文利用Python 编程，首先基于拉格朗日方程对二阶倒立摆系统进行了线性化建模，然后根据模型预测控制的控制原理进行了分析并设计了快速预测控制器。 在模型预测控制下，倒立摆系统的双摆能够稳定保持在垂直向上的平衡位置，同时为了更好的评估控制性能，在原有的系统上进行了复杂化设计。 从仿真结果来看，模型预测控制可以准确的控制系统稳定，并对各参量做出预测。