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

算法解析

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

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

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

minimize ( x N − x r ) T Q N ( x N − x r ) + ∑ k = 0 N − 1 ( x k − x r ) T Q ( x k − x r ) + u k T R u k subject to x k + 1 = A x k + B u k x m i n ≤ x k ≤ x m a x u m i n ≤ u k ≤ u m a x x 0 = x ˉ \begin{aligned} \text{minimize}\quad &(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}\quad & x_{k+1} = A x_k + B u_k \\ & x_{\rm min} \le x_k \le x_{\rm max} \\ & u_{\rm min} \le u_k \le u_{\rm max} \\ & x_0 = \bar{x} \end{aligned} minimizesubject to(xNxr)TQN(xNxr)+k=0N1(xkxr)TQ(xkxr)+ukTRukxk+1=Axk+Bukxminxkxmaxuminukumaxx0=xˉ

x k ∈ R n x x_k \in \mathbf{R}^{n_x} xkRnx为状态, x r x_r xr为期望/参考状态, u k ∈ R n u u_k \in \mathbf{R}^{n_u} ukRnu为输入。

x k + 1 = A x k + B u k x_{k+1} = A x_k + B u_k xk+1=Axk+Buk为离散化后的模型。

x N x_N xN为当前状态,若 Q N Q_N QN Q Q Q一致,则可以简化为
minimize ∑ k = 0 N ( x k − x r ) T Q ( x k − x r ) + u k T R u k \text{minimize} \sum_{k=0}^{N} (x_k-x_r)^T Q (x_k-x_r) + u_k^T R u_k minimizek=0N(xkxr)TQ(xkxr)+ukTRuk
Q为状态惩罚矩阵,R为输入惩罚矩阵,可通过修改这两个矩阵调整不同状态和输入的在优化过程中的权重。

算法流程

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

倒立摆模型

运动控制——模型预测控制MPC_第1张图片

建立模型

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

倒立摆运动方程

推导参考倒单摆

( M + m ) x ¨ − m l θ ¨ c o s θ + m l θ ˙ 2 s i n θ = F l θ ¨ − g s i n θ = x ¨ c o s θ \begin{matrix} (M + m)\ddot{x} - ml\ddot{\theta}cos\theta + ml\dot{\theta}^2sin\theta = F\\ l\ddot{\theta} - gsin\theta = \ddot{x}cos\theta \end{matrix} (M+m)x¨mlθ¨cosθ+mlθ˙2sinθ=Flθ¨gsinθ=x¨cosθ

构建系统状态方程

状态向量为 X = [ x x ˙ θ θ ˙ ] T X = \begin{bmatrix} x & \dot{x} & \theta & \dot{\theta} \end{bmatrix}^T X=[xx˙θθ˙]T,控制输入量为 u = F u=F u=F

系统状态方程为:

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

x 1 = x , x 2 = x ˙ , x 3 = θ , x 4 = θ ˙ x_1=x,x_2=\dot{x},x_3=\theta,x_4=\dot{\theta} x1=x,x2=x˙,x3=θ,x4=θ˙

x 1 ˙ = x 2 x 2 ˙ = F − m l x 4 2 sin ⁡ x 3 + m g sin ⁡ x 3 cos ⁡ x 3 M + m − m cos ⁡ 2 x 3 = F − m l x 4 2 sin ⁡ x 3 + m g sin ⁡ x 3 cos ⁡ x 3 M + ( 1 − cos ⁡ 2 x 3 ) m x 3 ˙ = x 4 x 4 ˙ = ( F − m l x 4 2 sin ⁡ x 3 + m g sin ⁡ x 3 cos ⁡ x 3 ) cos ⁡ x 3 + [ M + m ( 1 − cos ⁡ 2 x 3 ) ] g sin ⁡ x 3 l [ M + ( 1 − cos ⁡ 2 x 3 ) m ) ] = ( F − m l x 4 2 sin ⁡ x 3 ) cos ⁡ x 3 + ( M + m ) g sin ⁡ x 3 l [ M + ( 1 − cos ⁡ 2 x 3 ) m ] \begin{aligned} \dot{x_1} &= x_2\\ \dot{x_2} &= \frac{F-m l x_4^2 \sin{x_3}+mg \sin{x_3} \cos{x_3}}{M+m-m\cos^2{x_3}}\\ &= \frac{F-m l 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-m l 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-m l 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{aligned} x1˙x2˙x3˙x4˙=x2=M+mmcos2x3Fmlx42sinx3+mgsinx3cosx3=M+(1cos2x3)mFmlx42sinx3+mgsinx3cosx3=x4=l[M+(1cos2x3)m)](Fmlx42sinx3+mgsinx3cosx3)cosx3+[M+m(1cos2x3)]gsinx3=l[M+(1cos2x3)m](Fmlx42sinx3)cosx3+(M+m)gsinx3

对模型(非线性)在平衡点( X e q = [ 0 0 0 0 ] T X_{eq}=\begin{bmatrix}0 & 0 & 0 &0\end{bmatrix}^T Xeq=[0000]T,为此处的期望状态)处进行线性化,利用一阶泰勒展开实现:

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

X ˙ = f ( X e q , 0 ) + ∂ f ∂ X ∣ X = X e q ( X − X e q ) + ∂ f ∂ u ∣ u = u 0 ( u − u 0 ) \dot{X} = f(X_eq,0) + \left.\frac{\partial f}{\partial X} \right|_{X=X_{eq}} (X-X_{eq})+\left.\frac{\partial f}{\partial u} \right|_{u=u_0} (u-u_0)\\ X˙=f(Xeq,0)+XfX=Xeq(XXeq)+ufu=u0(uu0)

线性化后的模型如下:
X ˙ = A X + B u = [ 0 1 0 0 0 0 m g M 0 0 0 0 1 0 0 ( M + m ) g l M 0 ] [ x x ˙ θ θ ˙ ] + [ 0 1 M 0 1 l M ] u \dot{X} = A \text{X}+B \text{u}\\ =\begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & 0 & \frac{mg}{M} & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & \frac{(M + m)g}{lM} & 0 \end{bmatrix} \begin{bmatrix} x \\ \dot{x} \\ \theta \\ \dot{\theta} \end{bmatrix} + \begin{bmatrix} 0 \\ \frac{1}{M} \\ 0 \\ \frac{1}{lM} \end{bmatrix} u X˙=AX+Bu=000010000Mmg0lM(M+m)g0010xx˙θθ˙+0M10lM1u

状态方程离散化

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

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

微分方程: y ˙ ( t ) = f ( t , y ( t ) ) , y ( t 0 ) = y 0 \dot{y}(t)=f(t,y(t)),y(t_0) = y_0 y˙(t)=f(t,y(t)),y(t0)=y0

前向欧拉: y n + 1 = y n + h ( y n , t n ) y_{n+1} = y_n+h(y_n,t_n) yn+1=yn+h(yn,tn)

后向欧拉: y n + 1 = y n + h ( y n + 1 , t n + 1 ) y_{n+1} = y_n+h(y_{n+1},t_{n+1}) yn+1=yn+h(yn+1,tn+1)

X ˙ = X k + 1 − X k T = A X k + B u k X k + 1 = ( I + T A ) X k + T B u k X k + 1 = A ˉ X k + B ˉ u k \begin{matrix} \dot{X} = \frac{X_{k+1}-X_k}{T} =A X_k+B u_k\\ X_{k+1} = (I+TA)X_k+TBu_k\\ X_{k+1} = \bar{A}X_k+\bar{B}u_k \end{matrix} X˙=TXk+1Xk=AXk+BukXk+1=(I+TA)Xk+TBukXk+1=AˉXk+Bˉuk

其中

I为单位矩阵

A ˉ = ( 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1 ) + T ( 0 1 0 0 0 0 m g M 0 0 0 0 1 0 0 ( M + m ) g l M 0 ) B ˉ = ( 0 1 M 0 1 l M ) T \begin{matrix} \bar{A} = \begin{pmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{pmatrix} + T \begin{pmatrix} 0 & 1 & 0 & 0 \\ 0 & 0 & \frac{mg}{M} & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & \frac{(M + m)g}{lM} & 0 \end{pmatrix} \\ \bar{B} = \begin{pmatrix} 0 \\ \frac{1}{M} \\ 0 \\ \frac{1}{lM} \end{pmatrix} T \end{matrix} Aˉ=1000010000100001+T000010000Mmg0lM(M+m)g0010Bˉ=0M10lM1T

构建预测方程

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

X k X_k Xk包含n个周期内每一个运动周期的状态

X k = [ X ( k + 1 ∣ k ) T X ( k + 2 ∣ k ) T ⋯ X ( k + n ∣ k ) T ] T X_k = \begin{bmatrix} X(k+1|k)^T & X(k+2|k)^T & \cdots & X(k+n|k)^T \end{bmatrix}^T Xk=[X(k+1k)TX(k+2k)TX(k+nk)T]T

预测时域内的控制量 U k U_k Uk​​

控制量为预测状态n-1

U k = [ u ( k ∣ k ) T u ( k + 1 ∣ k ) T ⋯ u ( k + n − 1 ∣ k ) T ] T U_k = \begin{bmatrix} u(k|k)^T & u(k+1|k)^T & \cdots & u(k+n-1|k)^T \end{bmatrix}^T Uk=[u(kk)Tu(k+1k)Tu(k+n1k)T]T

通过离散化状态方程依次对未来 p p p​个控制周期的系统状态进行预测
X ( k + 1 ∣ k ) = A ˉ X ( k ) + B ˉ u ( k ∣ k ) X ( k + 2 ∣ k ) = A ˉ 2 X ( k ) + A ˉ B ˉ u ( k ∣ k ) + B ˉ u ( k + 1 ∣ k ) X ( k + 3 ∣ k ) = A ˉ 3 X ( k ) + A ˉ 2 B ˉ u ( k ∣ k ) + A ˉ B ˉ u ( k + 1 ∣ k ) + B ˉ u ( k + 2 ∣ k ) ⋮ X ( k + n ∣ k ) = A ˉ n X ( k ) + ∑ i = 0 n − 1 A ˉ n − 1 − i B ˉ u ( k + i ∣ k ) \begin{matrix} 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)\\ \vdots\\ 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{matrix} X(k+1k)=AˉX(k)+Bˉu(kk)X(k+2k)=Aˉ2X(k)+AˉBˉu(kk)+Bˉu(k+1k)X(k+3k)=Aˉ3X(k)+Aˉ2Bˉu(kk)+AˉBˉu(k+1k)+Bˉu(k+2k)X(k+nk)=AˉnX(k)+i=0n1Aˉn1iBˉu(k+ik)

优化

控制目标是使系统的状态达到期望值,该值可作为参考值:
X r k = [ X r ( k + 1 ) T X r ( k + 2 ) T ⋯ X r ( k + n ) T ] T X_{rk}=\left[ \begin{matrix} X_r\left( k+1 \right) ^T& X_r\left( k+2 \right) ^T& \cdots& X_r\left( k+n \right) ^T\\ \end{matrix} \right] ^T Xrk=[Xr(k+1)TXr(k+2)TXr(k+n)T]T

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

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

J ( U k ) = ( X k − X r k ) T Q ( X k − X r k ) + U k T R U k J\left( U_k \right) =\left( X_k-X_{rk} \right) ^TQ\left( X_k-X_{rk} \right) +U_{k}^{T}RU_k J(Uk)=(XkXrk)TQ(XkXrk)+UkTRUk

更新

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.

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

你可能感兴趣的:(运动控制)