运动控制——模型预测控制MPC

算法解析

模型预测控制（Model Predictive Control）指一类算法，周期性基于当帧测量信息在线求解一个有限时间开环优化问题，并将结果的前部分控制序列作用于被控对象。根据所用模型不同，分为动态矩阵控制（DMC），模型算法控制（MAC）、广义预测控制（GPC）。在智能驾驶方向，重点在于基于状态空间模型的模型预测控制。

模型预测控制的基本思想为利用一个已有的模型、系统当前的状态和未来的控制量，来预测系统未来的输出，然后与期望的系统输出做比较，得到一个损失函数，即：
$$\text{损失函数}=[\text{未来输出}(\text{模型},\text{未来控制量},\text{当前状态})-\text{期望输出}{]}^2$$
标准形式如下：

与标准的QP形式不同，但属于QP问题，可转化

$$\begin{array}{rl}\text{minimize}& (x_N-x_r{)}^T{Q}_N(x_N-x_r)+\sum _{k=0}^{N-1}(x_k-x_r{)}^{T}Q(x_k-x_r)+u_k^{T}R{u}_k\\ \text{subject to}& x_{k+1}=A{x}_k+B{u}_k\\ & x_{\mathrm{m}\mathrm{i}\mathrm{n}}\le x_k\le x_{\mathrm{m}\mathrm{a}\mathrm{x}}\\ & u_{\mathrm{m}\mathrm{i}\mathrm{n}}\le u_k\le u_{\mathrm{m}\mathrm{a}\mathrm{x}}\\ & x_0=\bar{x}\end{array}$$

$x_k\in {\mathbf{R}}^{n_x}$为状态，$x_r$为期望/参考状态，$u_k\in {\mathbf{R}}^{n_u}$为输入。

$x_{k+1}=A{x}_k+B{u}_k$为离散化后的模型。

$x_N$为当前状态，若$Q_N$与$Q$一致，则可以简化为
$$\text{minimize}\sum _{k=0}^N(x_k-x_r{)}^{T}Q(x_k-x_r)+u_k^{T}R{u}_k$$
Q为状态惩罚矩阵，R为输入惩罚矩阵，可通过修改这两个矩阵调整不同状态和输入的在优化过程中的权重。

算法流程

1. 建立模型
2. 构建系统状态方程
3. 状态方程离散化
4. 构建预测方程
5. 优化
6. 更新状态值，更新预测方程，重复步骤5

倒立摆模型

建立模型

符号	意义
$x$	小车位置
$\theta$	倒立摆偏角
$F$	施加到小车上的外力，为输入u
$M$	小车质量
$m$	倒立摆质量
$l$	摆的长度
$g$	重力加速度

倒立摆运动方程

推导参考倒单摆

$$\begin{array}{c}(M+m)\ddot{x}-ml\ddot{\theta }cos\theta +ml{\dot{\theta }}^{2}sin\theta =F\\ l\ddot{\theta }-gsin\theta =\ddot{x}cos\theta \end{array}$$

构建系统状态方程

状态向量为$X={\left[\begin{array}{cccc}x& \dot{x}& \theta & \dot{\theta }\end{array}\right]}^T$，控制输入量为$u=F$。

系统状态方程为：

状态方程为对状态向量求导，即$\dot{X}$

$$x_1=x,x_2=\dot{x},x_3=\theta ,x_4=\dot{\theta }$$

$$\begin{array}{rl}\dot{x_1}& =x_2\\ \dot{x_2}& =\frac{F-ml{x}_4^2\sin x_3+mg\sin x_3\cos x_3}{M+m-m{\cos }^2{x}_3}\\ & =\frac{F-ml{x}_4^2\sin x_3+mg\sin x_3\cos x_3}{M+(1-{\cos }^2{x}_3)m}\\ \dot{x_3}& =x_4\\ \dot{x_4}& =\frac{(F-ml{x}_4^2\sin x_3+mg\sin x_3\cos x_3)\cos x_3+\left[M+m(1-{\cos }^2{x}_3)\right]g\sin x_3}{l\left[M+(1-{\cos }^2{x}_3)m)\right]}\\ & =\frac{(F-ml{x}_4^2\sin x_3)\cos x_3+(M+m)g\sin x_3}{l\left[M+(1-{\cos }^2{x}_3)m\right]}\end{array}$$

对模型（非线性）在平衡点（$X_{eq}={\left[\begin{array}{cccc}0& 0& 0& 0\end{array}\right]}^T$，为此处的期望状态）处进行线性化，利用一阶泰勒展开实现：

非线性方程线性化只在线性点附近才有比较好的效果

$$\dot{X}=f(X_{e}q,0)+{\frac{\partial f}{\partial X}|}_{X=X_{eq}}(X-X_{eq})+{\frac{\partial f}{\partial u}|}_{u=u_0}(u-u_0)$$

线性化后的模型如下：
$$\begin{gathered} \dot{X}=A\text{X}+B\text{u} \\ =\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& 0& \frac{mg}{M}& 0\\ 0& 0& 0& 1\\ 0& 0& \frac{(M+m)g}{lM}& 0\end{array}\right]\left[\begin{array}{c}x\\ \dot{x}\\ \theta \\ \dot{\theta }\end{array}\right]+\left[\begin{array}{c}0\\ \frac{1}{M}\\ 0\\ \frac{1}{lM}\end{array}\right]u \end{gathered}$$

状态方程离散化

采用前向欧拉法将状态方程离散化，T为离散时间间隔

欧拉方法，是一种一阶数值方法，用以对给定初值的常微分方程（即初值问题）求解。它是一种解决数值常微分方程的最基本的一类显型方法（Explicit method）。几何上看为切线拟合。

微分方程：$\dot{y}(t)=f(t,y(t)),y(t_0)=y_0$

前向欧拉: $y_{n+1}=y_n+h(y_n,t_n)$

后向欧拉: $y_{n+1}=y_n+h(y_{n+1},t_{n+1})$

$$\begin{array}{c}\dot{X}=\frac{X_{k+1}-X_k}{T}=A{X}_k+B{u}_k\\ X_{k+1}=(I+TA)X_k+TB{u}_k\\ X_{k+1}=\bar{A}{X}_k+\bar{B}{u}_k\end{array}$$

其中

I为单位矩阵

$$\begin{array}{c}\bar{A}=\left(\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right)+T\left(\begin{array}{cccc}0& 1& 0& 0\\ 0& 0& \frac{mg}{M}& 0\\ 0& 0& 0& 1\\ 0& 0& \frac{(M+m)g}{lM}& 0\end{array}\right)\\ \bar{B}=\left(\begin{array}{c}0\\ \frac{1}{M}\\ 0\\ \frac{1}{lM}\end{array}\right)T\end{array}$$

构建预测方程

设预测时域为未来n个周期，预测系统状态为：

$X_k$包含n个周期内每一个运动周期的状态

$$X_k={\left[\begin{array}{cccc}X(k+1|k{)}^T& X(k+2|k{)}^T& \cdots & X(k+n|k{)}^T\end{array}\right]}^T$$

预测时域内的控制量$U_k$​​

控制量为预测状态n-1

$$U_k={\left[\begin{array}{cccc}u(k|k{)}^T& u(k+1|k{)}^T& \cdots & u(k+n-1|k{)}^T\end{array}\right]}^T$$

通过离散化状态方程依次对未来$p$​个控制周期的系统状态进行预测
$$\begin{array}{c}X(k+1|k)=\bar{A}X(k)+\bar{B}u(k|k)\\ X(k+2|k)={\bar{A}}^{2}X(k)+\bar{A}\bar{B}u(k|k)+\bar{B}u(k+1|k)\\ X(k+3|k)={\bar{A}}^{3}X(k)+{\bar{A}}^2\bar{B}u(k|k)+\bar{A}\bar{B}u(k+1|k)+\bar{B}u(k+2|k)\\ ⋮\\ X(k+n|k)={\bar{A}}^{n}X(k)+{\sum }_{i=0}^{n-1}{\bar{A}}^{n-1-i}\bar{B}u(k+i|k)\end{array}$$

优化

控制目标是使系统的状态达到期望值，该值可作为参考值：
$$X_{rk}={\left[\begin{array}{cccc}{X}_r{\left(k+1\right)}^T& X_r{\left(k+2\right)}^T& \cdots & X_r{\left(k+n\right)}^T\end{array}\right]}^T$$

在倒立摆模型中期望一直为平衡点
$$X_{rk}={\left[\begin{array}{ccc}[0,0,0,0{]}^T& \cdots & r[0,0,0,0{]}^T\end{array}\right]}^T$$
优化的目标为寻找最优控制量$U_k$​，使得预测时域内的状态向量与参考值之间的差距和控制量尽可能得小，这属于开环最优控制。

MPC相对于传统的PID需要k+p时刻的参考值

$$J\left(U_k\right)={\left(X_k-X_{rk}\right)}^{T}Q\left(X_k-X_{rk}\right)+U_k^{T}R{U}_k$$

更新

MPC输出控制为优化结果的第一个结果，输出控制后获取反馈，更新初始状态的约束值，再继续优化。

程序实现

优化库CVXPY

CVXPY is a Python-embedded modeling language for convex optimization problems. It automatically transforms the problem into standard form, calls a solver, and unpacks the results.

自动转换优化问题为标准形式，并调用优化器求解，支持优化器如下，标注x的为优化器支持问题类型。

	LP	QP	SOCP	SDP	EXP	POW	MIP
CBC	X						X
GLPK	X
GLPK_MI	X						X
OSQP	X	X
CPLEX	X	X	X				X
NAG	X	X	X
ECOS	X	X	X		X
GUROBI	X	X	X				X
MOSEK	X	X	X	X	X	X	X*
CVXOPT	X	X	X	X
SCS	X	X	X	X	X	X
SCIP	X	X	X				X
XPRESS	X	X	X				X
SCIPY	X

python程序

"""
https://github.com/AtsushiSakai/PythonRobotics/blob/master/InvertedPendulumCart/InvertedPendulumMPCControl/inverted_pendulum_mpc_control.py

Inverted Pendulum MPC control
author: Atsushi Sakai
"""

import math
import time

import cvxpy
import matplotlib.pyplot as plt
import numpy as np

# Model parameters
l_bar = 2.0  	# length of bar
M = 1.0  		# [kg]
m = 0.3  		# [kg]
g = 9.8  		# [m/s^2]

Q = np.diag([0.0, 1.0, 1.0, 0.0]) # 状态惩罚矩阵
R = np.diag([0.01])               # 输入惩罚矩阵
nx = 4  	  	# 状态数量
nu = 1  		# 控制输入数量
T = 30         	# 预测周期
delta_t = 0.1 	# 离散时间间隔

animation = False

def main():
    # 初始状态向量 x,x',theta,theta'
    x0 = np.array([
        [0.0],
        [0.0],
        [0.3],
        [0.0]
    ])
    x = np.copy(x0)
	
    # 控制次数限制为50次
    for i in range(50):

        # 通过mpc计算控制输出
        opt_x, opt_delta_x, opt_theta, opt_delta_theta, opt_input = mpc_control(x)

        # 获得第一个控制量
        u = opt_input[0]

        # 更新当前状态
        x = simulation(x, u)

        if animation:
            plt.clf()
            px = float(x[0])
            theta = float(x[2])
            plot_cart(px, theta)
            plt.xlim([-5.0, 2.0])
            plt.pause(0.001)


def simulation(x, u):
    # 获得线性化模型
    A, B = get_model_matrix()
	
    # 计算下一状态
    x = np.dot(A, x) + np.dot(B, u)

    return x


def mpc_control(x0):
    # 基于状态变量x和输入变量u构建方程
	# 通过等式约束添加离散状态之间的连续约束和初始状态为当前状态的约束
    # 此处理想状态为[0 0 0 0]，因此构造优化方程时不需要减去理想值
    
    # 定义变量
    x = cvxpy.Variable((nx, T + 1)) # 状态
    u = cvxpy.Variable((nu, T))	    # 输入

    A, B = get_model_matrix()

    cost = 0.0
    constr = []
    for t in range(T):
        # 最小化误差方程
        cost += cvxpy.quad_form(x[:, t + 1], Q) # x[:, t + 1] Q x[:, t + 1]^T
        cost += cvxpy.quad_form(u[:, t], R)     # u[:, t] R u[:, t]^T
        
        # 模型状态约束
        constr += [x[:, t + 1] == A * x[:, t] + B * u[:, t]]

    # 设置初始状态为当前状态
    constr += [x[:, 0] == x0[:, 0]]
    
    # 优化
    prob = cvxpy.Problem(cvxpy.Minimize(cost), constr)

    start = time.time()
    prob.solve(verbose=False)
    elapsed_time = time.time() - start
    print("calc time:{0} [sec]".format(elapsed_time))
	
    if prob.status == cvxpy.OPTIMAL: # 优化成功输出结果
        ox = get_numpy_array_from_matrix(x.value[0, :])
        dx = get_numpy_array_from_matrix(x.value[1, :])
        theta = get_numpy_array_from_matrix(x.value[2, :])
        d_theta = get_numpy_array_from_matrix(x.value[3, :])

        ou = get_numpy_array_from_matrix(u.value[0, :])
    else: # 优化失败
        ox, dx, theta, d_theta, ou = None, None, None, None, None

    return ox, dx, theta, d_theta, ou


def get_numpy_array_from_matrix(x):
    """
    get build-in list from matrix
    """
    return np.array(x).flatten()


def get_model_matrix():
    """
    线性化离散模型
    """
    A = np.array([
        [0.0, 1.0, 0.0, 0.0],
        [0.0, 0.0, m * g / M, 0.0],
        [0.0, 0.0, 0.0, 1.0],
        [0.0, 0.0, g * (M + m) / (l_bar * M), 0.0]
    ])
    A = np.eye(nx) + delta_t * A

    B = np.array([
        [0.0],
        [1.0 / M],
        [0.0],
        [1.0 / (l_bar * M)]
    ])
    B = delta_t * B

    return A, B


def flatten(a):
    """
    返回一维数组
    """
    return np.array(a).flatten()


def plot_cart(xt, theta):
    cart_w = 1.0
    cart_h = 0.5
    radius = 0.1

    cx = np.array([-cart_w / 2.0, cart_w / 2.0, cart_w /
                   2.0, -cart_w / 2.0, -cart_w / 2.0])
    cy = np.array([0.0, 0.0, cart_h, cart_h, 0.0])
    cy += radius * 2.0

    cx = cx + xt

    bx = np.array([0.0, l_bar * math.sin(-theta)])
    bx += xt
    by = np.array([cart_h, l_bar * math.cos(-theta) + cart_h])
    by += radius * 2.0

    angles = np.arange(0.0, math.pi * 2.0, math.radians(3.0))
    ox = np.array([radius * math.cos(a) for a in angles])
    oy = np.array([radius * math.sin(a) for a in angles])

    rwx = np.copy(ox) + cart_w / 4.0 + xt
    rwy = np.copy(oy) + radius
    lwx = np.copy(ox) - cart_w / 4.0 + xt
    lwy = np.copy(oy) + radius

    wx = np.copy(ox) + bx[-1]
    wy = np.copy(oy) + by[-1]

    plt.plot(flatten(cx), flatten(cy), "-b")
    plt.plot(flatten(bx), flatten(by), "-k")
    plt.plot(flatten(rwx), flatten(rwy), "-k")
    plt.plot(flatten(lwx), flatten(lwy), "-k")
    plt.plot(flatten(wx), flatten(wy), "-k")
    plt.title("x:" + str(round(xt, 2)) + ",theta:" +
              str(round(math.degrees(theta), 2)))

    plt.axis("equal")


if __name__ == '__main__':
    main()

参考

模型预测控制（MPC）倒立摆作业实操（一）

一个模型预测控制（MPC）的简单实现

cvxpy

Askari M , Mohamed H , Moghavvemi M , et al. Model predictive control of an inverted pendulum[C]// IEEE. IEEE, 2009.

モデル予測制御による倒立振子