【控制control】机器人运动控制器----基于模型预测控制MPC方法

系列文章目录

提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
TODO:写完再整理

---

前言

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！ **（1）基于无模型的控制方法** 如：PID反馈控制器、前馈控制器、前馈+反馈控制器等等

（2）基于模型的控制方法
如：机械臂模型（几何解算）控制器、小车底盘运动学模型（几何解算）的控制器、四足机器人模型（预测迭代）的控制器等等

（3）基于先进控制方法
如：微分平坦（无人机的轨迹跟随）【防盗标记–盒子君hzj】等等

（4）基于模型预测控制MPC（MPCC）控制方法
如：MPC优化控制算法

（5）基于线性二次调节器（LQR）控制方法
如：线性二次调节器（LQR）是基于模型的控制器，使用车辆状态来使误差最小化，Apollo使用LQR进行横向控制。
横向控制包含四个组件：横向误差、横向误差的变化率、朝向误差和朝向的变化率。
除状态外，还有是三个控制输入：转向、加速和制动

本文先对基于模型预测控制MPC方法做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章

---

提示：以下是本篇文章正文内容

一、模型预测控制(MPC)的介绍及构成

1.介绍

MPC不是一种具体的方法，而是一种思想

模型预测控制实际上是以优化方法来求解控制问题，或者说是以优化问题的求解来给出控制器的动作。所以说模型预测控制是外边套着一层控制的皮，内部包含着优化的思想。模型预测控制它本就是属于优化和控制两个领域的交叉；MPC一般用于多输入多输出，且输入输出相互也会有影响，希望输出尽可能的接近我们的设定值（参考轨迹）【防盗标记–盒子君hzj】，同时希望输出是平稳的可控的；理解上这PID的目的是一样的，不过只是PID是针对单输入单输出的系统，而MPC是针对多输入多输出且相互耦合的系统

模型预测控制属于反馈控制的一种
过马路时，你脑子里会建立起一个此时此刻你的速度和边上过来的车行驶速度的模型，然后你每走一步，都会修正一下这个模型，调整自己的速度，以确定自己不会被撞上。这里就涉及到了预测控制的3个最基本组成部分：模型预测，反馈校正，滚动优化。
.
.

2.构成

1） 模型：建立模型的目的是是用来做预测用的
2） 预测：预测的目的是为了让我们更好的去决策，【防盗标记–盒子君hzj】例如你过马路的时候，会预测一下是否有车能撞到你
3） 控制：控制就是我需要做出动作了，例如你过马路的时候，会预测一下是否有车能撞到你，如果没有你就赶快过马路
.
.

---

二、模型预测控制(MPC)的运行过程对应的原理

1.模型预测原理

利用一个已有的模型、系统当前的状态和未来的控制量去预测系统未来的输出。预测模型是模型预测控制的基础。其主要功能是根据对象的历史信息和未来输入【防盗标记–盒子君hzj】,预测系统未来的输出。对预测模型的形式没有做严格的限定,状态方程、传递函数这类传统的模型都可以作为预测模型。对于线性稳定系统,阶跃响应、脉冲响应这类非参数模型,也可以直接作为预测模型使用

2.滚动优化原理

这个输出的长度是控制周期的整数倍，由于未来的控制量是未知的,还需要根据一定的优化条件进行优化求解,以得到未来的控制量序列。模型预测控制通过某一性能指标的最优来确定控制作用,但优化不是一次离线进行,而是反复在线进行的。这就是滚动优化的含义,也是模型预測控制区别于传统最优控制的根本点

3.反馈校正原理

在每一个控制周期结束后,系统根据当前实际状态重新预测系统未来输出,为了防止模型失配或者环境干扰引起控制对理想状态的偏离,在新的采样时刻【防盗标记–盒子君hzj】,首先检测对象的实际输出,并利用这一实时信息对基于模型的预测结果进行修正,然后再进行新的优化
.
.

---

三、模型预测控制(MPC)的各个模块及原理流程

模型预測控制原理框图如上图所示,包含了MPC控制器、被控平台和状态估计器这3个模块。（定义形成优化问题后，最优求解用到二次规划）

1.MPC控制器

结合预测模型、目标函数和约束条件进行最优化求解,【防盗标记–盒子君hzj】得到当前时刻的最优控制序列,输入到被控平台,被控平台按照当前的控制量进行控制,然后将当前的状态量观测值输入给状态估计器

2.被控平台

当前时刻的最优控制序列u输入到被控平台,被控平台按照当前的控制量进行控制

3.状态估计器

将当前的状态量观测值x输入给状态估计器,状态估计器对于那些无法通过传感器观测得到或者观测成本过高的状态量进行估计。比较常用的方法有 Kalman滤波、粒子滤波等。

将估计的状态量输入到MPC控制器,再次进行最优化求解,以得到未来一段时间的控制序列。如此循环,就构成了完整的模型预测控制过程。MPC不过是在每个采样时刻解一个轨迹优化问题，但是只用其中的一部分解.
.
.

---

四、模型预测控制(MPC)的运行过程介绍

图注解：基于上面3个要素,模型预测控制的基本原理可以用上图来表示。

控制过程中,始终存在一条期望参考轨迹，如曲线1所示（狗的姿态轨迹）。以时刻作为当前时刻(坐标系纵轴所在位置),控制器结合当前的测量值和预测模型,预测系统未来一段时域内系统的输出，如曲线2所示（反作用力的大小何方向）。
通过求解满足目标函数以及各种约束的优化问题,得到在控制时域内一系列的控制序列,如矩形波4所示（电机WBC的指令输出）,并将该控制序列的第一个元素作为受控对象的实际控制量。
当来到下一个时刻时,重复上述过程,如此滚动地完成一个【防盗标记–盒子君hzj】个带约束的优化问题,以实现对被控对象的持续控制。

运行描述：控制过程中,始终存在一条期望参考轨迹。以时刻作为当前时刻(坐标系纵轴所在位置),控制器结合当前的测量值和预测模型,预测系统未来一段时域内系统的输出，通过求解满足目标函数以及各种约束的优化问题,得到在控制时域内一系列的控制序列，并将该控制序列的第一个元素作为受控对象的实际控制量。当来到下一个时刻时,重复上述过程,如此滚动地完成一个个带约束的优化问题,以实现对被控对象的持续控制。

1.模型预测

根据已有的知识和经验可以预计任务的完成情况。如团队以往完成任务的能力就是已有的经验模型

预测模型是模型预测控制的基础。【防盗标记–盒子君hzj】其主要功能是根据对象的历史信息和未来输入,预测系统未来的输出。对预测模型的形式没有做严格的限定,状态方程、传递函数这类传统的模型都可以作为预测模型。对于线性稳定系统,阶跃响应、脉冲响应这类非参数模型,也可以直接作为预测模型使用。

2.滚动优化

只规划未来固定一段时间的任务计划,而不是整个工作周期

模型预测控制通过某一性能指标的最优来确定控制作用,但优化不是一次离线进行,而是反复在线进行的。这就是滚动优化的含义,也是模型预測控制区别于传统最优控制的根本点。

3.反馈校正

每一固定时间段结東后检查任务完成情况,【防盗标记–盒子君hzj】然后根据现有状态重新规划下一段时间的任务计划

为了防止模型失配或者环境干扰引起控制对理想状态的偏离,在新的采样时刻,首先检测对象的实际输出,并利用这一实时信息对基于模型的预测结果进行修正,然后再进行新的优化

.
.

---

五、模型预测控制(MPC)的工程实现一般步骤

1.给定使用的模型，并对模型进行简化、线性化、离散化

1）模型简化：把复杂的系统合理的降维简化，使计算的维度更低
2）模型线性化：非线性系统进行凸优化，变成线性MPC系统
3）模型离散化：把这个连续问题在时间域上离散化，【防盗标记–盒子君hzj】从而变成可以数值求解的形式，需要用到数值积分的方法

---

2.输入线性约束条件

第一种约束是动力学模型
从最简单的线性倒立摆到完整的刚体动力学模型都有被使用过。不同模型除了对机器人动力学特性的简化程度不同，还会影响形成的优化问题的形式，也进而影响求解速度。

第二种约束是足式机器人里的稳定性判据
这和控制论里的稳定性判据不同，主要用到的就是 zero moment point (ZMP)，friction cone 或者 contact wrench cone (CWC)。同样，不同的稳定性判据对问题的忽略程度不同，也会影响最终优化问题的形式。其他一些约束也很常见【防盗标记–盒子君hzj】,比如初始和终止的状态、关节的位置或者力矩、落脚点的位置、支撑脚不动、非支撑脚上的作用力等等

---

3.输入规划生成的目标轨迹（Reference）

---

4.建立优化问题【用状态空间和状态方程表示】

我们可以得到一个连续的优化问题，它的优化变量一般是系统输入随时间变化的连续函数。
优化问题的目标函数都大致含有两种项：
第一种是对参考轨迹的跟踪，比如跟踪重心在 x，y 方向的速度等等。
第二种是 regulation（回归），比如希望重心的加速度或者绕质心的角动量不要太大等等，【防盗标记–盒子君hzj】这一项主要是为了提高机器人的稳定性或者是让优化问题解得更好。

---

5.用QP求解上面的优化问题

1、优化问题会被转换成为二次规划问题(QP问题）
2、把 QP 问题变成求解器可以求解的形式的方法

---

6.控制过程

当得到优化问题最优解（QP）后这个解作为真正的控制器的输出作用给被控对象

---

7.加入状态估计器过程

状态估计器大部分使用了卡尔曼滤波器的控制技术

---

8.调参过程：MPC设计时需要的参数(会影响控制器的性能)

（1）Sample time【系统采样时间，用于确定控制器执行的速率，系统采样时间过大实时性不高，系统采样时间过短计算量大，考察计算机计算性能，1000hz能刚刚满足控制要求】

（2）Prediction horizon【预测范围（相对于输入），（车的话可能是车前方的距离）机器狗是预测时间范围，预测时间范围过长，动作滞后，预测时间范围过短会超调震荡，浪费计算资源】

（3）Control horizon【控制范围（相对于输出），【防盗标记–盒子君hzj】控制范围越小计算量越小，但是越不精确，控制范围越大，计算量越大，但是能得到更好的预测】

（4）Constrains【约束，包括软约束和硬约束，硬约束不能违反，软约束可能会被违反，是针对预测输出变量和输入控制变量进行约束，尽量用软约束不要用硬约束】

（5）Weight【权重，用于衡量输入输出的重要性，设定在N输入M输出系统，一个N*M的矩阵】

---

9.控制效果反思及优化：降低MPC的复杂性和计算时间

目的是优化出毫秒级的时延，很挑战内存。MPC模型在计算上是比较复杂的，原因之一就是MPC的在线优化问题，在线优化问题会被转换成为二次规划问题(QP问题），【防盗标记–盒子君hzj】随着越来越多的（机器人）输入状态、约束条件、控制范围、预测范围，MPC的计算量会变得很大，导致实时性非常差
–解决办法： 使用更短的控制时间范围、更小的预测范围、减少约束数量、使用更低精度的控制范围、降低模型维度，（减少迭代次数也可以，但是精度会下降），总之是多尝试，然后头发就没了

---

六、模型预测控制(MPC)的实现难点

1.关键就在于怎么定义这个优化问题、

很多MPC最后形成的优化问题是一个二次规划问题，那么可以借助丰富的运筹优化的理论来求解这个二次规划问题。所以MPC是外壳套着控制，内涵是一个优化方法

2.模型的选择

使用复杂模型离线优化
第一种是使用复杂模型，但是 offline 花很长时间把关节轨迹算出来，然后想一些在线跟踪的方法。比如 Hybrid Zero Dynamics（HZD）的方法会花很长时间 offline 解一个 NLP，【防盗标记–盒子君hzj】然后去跟踪产生的关节轨迹或者形成一个 gait library 在不同步态之间来回切换【不建议】

使用简化模型在线优化
第二种就是使用简化模型，用 MPC 实时地产生重心和脚的轨迹，然后再去实时地做逆运动学或者逆动力学分解，从而在得到关节轨迹的同时跟踪上层产生的运动。【建议】

---

七、模型预测控制(MPC)的优势

（1）模型预测控制可以处理约束,MPC可以处理同时多个条件约束，而PID只能逐个处理单个条件约束（PID仅仅能够做简单的限幅，抗积分饱和也是限幅的一种）

（2） 模型预测控制是有向前考虑未来时间步的有限时域优化【防盗标记–盒子君hzj】（PID的微分项也有一点点预测的作用）

（3）模型预测控制不仅仅是追求“控制”，而且追求“优化控制”。传统PID控制器的“控制”是没问题的，但它不能够用最节约的方式进行控制，最经典的优化控制器是二次规划控制器QP

（4）MPC可以轻松的将未来的参考信息纳入控制的问题，MPC有提前预知的能力，这样可以概算控制器的性能（类似于一种前馈）

（5）MPC是一个多变量控制系统，而PID是一个单变量控制系统

（6）根据传感器信息输入有预测功能（类似于前馈控制和PID微分项的影响）

（7）优先级-实现化繁为简优先级技术可将复杂的逻辑，【防盗标记–盒子君hzj】往往通过几个优先级等级来实现

（8）MPC在每个时间步骤解决了在线优化的问题

---

八、模型预测控制(MPC)的弊端

模型预测控制需要在每一个时间步迭代求解优化问题，而优化问题的求解往往是比较耗费时间的，而控制器的动作对实时性要求可能会比较强一些，这样就会产生一个矛盾

---

九、模型预测控制（MPC）与PID控制对比

传统的PID算法用于复杂环境（如无人驾驶环境）存在两个问题

1、为了保证系统稳定时，【防盗标记–盒子君hzj】参数变量设置得比较小，系统延时非常大，控制效率比较低。
2、为了保证系统动态响应比较快时，参数变量设置得比较大，系统会产生震荡得情况，造成行驶不安全，机件寿命短等等问题

---

十、模型预测控制（MPC）与最优控制对比

（1）对于最优控制

最优控制强调的是“最优”，一般最优控制需要在整个时间域上进行求优化（这样才能保证最优性）

最优控制常用的几类解法有 1）变分法，2）极大值原理，3）动态规划（我们狗用这个）
变分法和极大值原理一般来说只能处理线性的模型并且不能含有复杂一点的约束，而动态规划实际上其计算复杂度往往非常高。最优控制由于过于强调最优性，【防盗标记–盒子君hzj】而暴露出两个问题
1、对于非线性的 包含复杂约束的情况难以求解；
2、需要对系统的模型精确的知道。

（2）对于模型预测控制

模型预测控制（MPC）退而求其次仅仅去考虑未来几个时间步，一定程度上牺牲了最优性。

当我们用最优控制解决问题的时候，往往需要这样的几个东西：初值、模型、约束、目标函数，然后的工作就是解决这个最优问题，得到最优解，这其中会用到许多数学方法，但是最终的目的是一样的。值得注意的是，最优控制是一锤子买卖，从上面的例子可以看出，有了初值、模型、【防盗标记–盒子君hzj】约束和目标函数之后，最优解就确定了，不论之后系统发生什么（干扰，不确定性等等），都只按照这个最优解来处理。

模型预测控制也可以看作是最优控制的实时升级版。模型预测控制的计算过程是这样：在某个时刻，把当前时刻的状态作为初值，结合模型、约束、目标函数来解有限时间之内的最优解（也就是一个有限时域的最优控制问题），在得到最优解之后，只取一个下一时刻的控制量，后面的全部忽略。在把这个控制量作用于系统之后，采集此时的系统状态，再解一次最优问题。如此往复。

这样做的最大优点就是，较好处理了系统中可能存在的干扰、噪声等不确定问题，也就增强了系统的鲁棒性。但是缺点就是对模型的精确性要求高，试想万一模型差之毫厘，每次算出来的最优控制都差一点，最后的结果也就可能谬以千里了。通俗讲，就好像我们常说的“走一步看一步”，到当前时刻，选择对自己最有利的做法，然后到下一步再重新估计对自己最优的做法，如此往复。但是这样做，得对自己有个充分的认识，【防盗标记–盒子君hzj】不然你以为的最优解可能是最糟糕的。

---

十一、模型预测控制（MPC）评价：

该方法也是在第四种方法上进行优化的，毕竟波士顿是麻省理工的前身，波士顿那套更关注落足点终点位置的规划从而产生飞行相动态轨迹【防盗标记–盒子君hzj】，而MIT采用了模型预测控制（MPC）。先建立一个简化的动力学模型，形成一个 trajectory optimization (轨迹优化) 的问题，再使用QP求解器的对这个优化问题进行实时求解。MPC不过是在每个采样时刻解一个轨迹优化问题，但是只用其中的一部分解，优化问题的约束条件是动力学模型【防盗标记–盒子君hzj】，这比波士顿考虑的更周全，形成了这样一个优化问题之后，把它变成求解器可以求解的QP形式，因为 QP（二次规划）可以解得又快又好。
QP求解的结果产生最终的关节轨迹。主流做法是把 inverse kinematics（逆运动学 IK）或者 inverse dynamics（逆动力学 ID）融合进一个或多个 quadratic program（二次规划 QP），

从某个角度讲讲：MPC这种方法比波士顿这种解耦分层控制的方法更好，直接用非线性控制+最优控制的知识结合动力学约束来搞，不需要精确的模型，而且系统还是多输入多输出的。【防盗标记–盒子君hzj】用优化的方法解决了控制的问题。系统在未来一段时间步长上有预测的能力。但是从另外一个角度上讲，有一点还是比不上波士顿的框架，因为波士顿的方法的分层控制每个模块可以单独控制优化，而MIT这个框架，一改变就意味全部推倒重来来设计

---

十二、模型预测控制（MPC）的注意事项

1、模型预测控制使用简化的模型，不然解算困难，算力消耗大

2、作用力预测的结果要经过凸优化，找到最优解，因为mpc预测的结果也是存在比较大的噪声的，因为使用了简化的模型

3、MPC比较耗算力和内存，具有实时性不高的问题，【防盗标记–盒子君hzj】可以通过简化模型和控制参数，使用高算力设备来解决

4、非线性的MPC问题比较多，如时间的算力约束、优化陷入局部最小值，所以模型要经过线性化，后面的工作才可以进行

十三、模型预测控制（MPC）的工程参考

cheetah software:提高MPC计算效率
https://zhuanlan.zhihu.com/p/185693836

一个模型预测控制（MPC）的简单实现
https://zhuanlan.zhihu.com/p/141871796

---