MPC+pybullet仿真双轮机器人的移动，附带键盘控制

MPC+pybullet仿真双轮机器人的移动，附带键盘控制

基础代码见之前的博客

使用MPC对小车进行运动控制，为什么抛弃了LQR呢？

MPC的好处在于

（1）它可以求解对状态空间有限制的系统，普通的PID，LQR只能对输入和输出做限制

（2）求解多输入多输出的系统

（3）反复的在线优化，使用范围广

代码的基础模板是参考了OSQP官网的示例教程的python代码，但这只是一个暂时的替代品，由于实时性要求，代码应该写在C++平台上面，经个人测算，使用C++对小车进行深度为20的预测需要的时间为0.8ms左右，而python上却达到了惊人的5ms，会占用很大一部分的时间，再加上MPC算法最终会在算力不那么充足的平台上使用，所以在最近我也会将Python转移到C++上

关于MPC，详细看DR_CAN的bilibili主页MPC相关视频，能够跟着一起推导公式，完成一次深入的理解是阅读此文的基础，这里指路一下

不多说了，开始上代码，解析就放在里面

---

从这里开始直到下一个分隔符，都是MPC的代码

1.import进所有的需要的文件

import osqp
import numpy as np
import scipy as sp
from scipy import sparse
import time

2.定义离散的状态空间方程，从连续的状态空间方程转化得到

指路一位大佬的非常详实的博客

连续的状态空间方程的离散化

在这里，两个时刻的时间间隔是0.05S，这个时间间隔根据自己的控制的时间尺度自己把握，如果是一个大工程的话，那么控制的时间尺度大概在10~100S，那么完全可以把dt设置得大一点

下列的Ad和Bd由X(t+1)=(I+Adt)X(t)+Bdtu(t)得到

Ad=I+Adt

Bd=Bdt

# Discrete time model of a quadcopter
def get_trq(data, x_ref):
    time_start=time.time()
    Ad = sparse.csc_matrix([
        [1., 0.05, 0., 0., 0., 0.],
        [0., 1 - 0.366446, 0., 0., 0., 0.],
        [0., 0., 1., 0.05, 0., 0.],
        [0., 0., 17.0575,  1., 0.,  0.],
        [0., 0., 0., 0., 1., 0.05],
        [0., 0., 0., 0., 0., 1.]
    ])
    Bd = sparse.csc_matrix([
        [0., 0.],
        [14.6215, 14.6215],
        [0., 0.],
        [-436.853, -436.853],
        [0., 0.],
        [-178.678, 178.678]
    ])
    [nx, nu] = Bd.shape

3.设置上下限，这里咱不做要求，np.inf表示正的无穷大，如果实际工程有需要，就可以在相应的位置设置上下限。

    # Constraints
    umin = [-np.inf, -np.inf]
    umax = [np.inf, np.inf]
    xmin = np.array([-np.inf,-np.inf,-np.inf,-np.inf,-np.inf,-np.inf])
    xmax = np.array([np.inf, np.inf, np.inf, np.inf,np.inf, np.inf])

4.系数矩阵，是各位调参侠需要反复琢磨的部分，主要设置Q与R，F矩阵的话由于和Q矩阵作用差不多，在实际意义上参考价值不算很大，这里OSQP官方没有给出F矩阵的考量，默认Q=F，Q，R的作用详细的可以参考前文

# Objective function
    Q = sparse.diags([1., 100., 1., 100., 1., 100.])
    QN = Q
    R = 1*sparse.eye(2)

5.MPC的各种参数设置

（1）设置状态的初始值和参考值，系统会根据初始值来完成

（2）设置预测的深度N

********上面的可以手动调，下面的别管了，都是语法要求，官方文档已经给出来了不要动了

（3）求解器搭建

（4）约束的录入

（5）开始解决问题

    # Initial and reference states
    x0 = data
    xr = x_ref

    # Prediction horizon
    N = 20

    # Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
    # - quadratic objective
    P = sparse.block_diag([sparse.kron(sparse.eye(N), Q), QN,
                           sparse.kron(sparse.eye(N), R)], format='csc')
    # - linear objective
    q = np.hstack([np.kron(np.ones(N), -Q.dot(xr)), -QN.dot(xr),
                   np.zeros(N*nu)])
    # - linear dynamics
    Ax = sparse.kron(sparse.eye(N+1), -sparse.eye(nx)) + sparse.kron(sparse.eye(N+1, k=-1), Ad)
    Bu = sparse.kron(sparse.vstack([sparse.csc_matrix((1, N)), sparse.eye(N)]), Bd)
    Aeq = sparse.hstack([Ax, Bu])
    leq = np.hstack([-x0, np.zeros(N*nx)])
    ueq = leq
    # - input and state constraints
    Aineq = sparse.eye((N+1)*nx + N*nu)
    lineq = np.hstack([np.kron(np.ones(N+1), xmin), np.kron(np.ones(N), umin)])
    uineq = np.hstack([np.kron(np.ones(N+1), xmax), np.kron(np.ones(N), umax)])
    # - OSQP constraints
    A = sparse.vstack([Aeq, Aineq], format='csc')
    l = np.hstack([leq, lineq])
    u = np.hstack([ueq, uineq])

    # Create an OSQP object
    prob = osqp.OSQP()

    # Setup workspace
    prob.setup(P, q, A, l, u, warm_start=True)

    # Solve
    res = prob.solve()

    # Check solver status
    if res.info.status != 'solved':
        raise ValueError('OSQP did not solve the problem!')

    # Apply first control input to the plant
    ctrl = res.x[-N*nu:-(N-1)*nu]
    x0 = Ad.dot(x0) + Bd.dot(ctrl)

    # Update initial state
    l[:nx] = -x0
    u[:nx] = -x0
    time_end = time.time()
    time_c = time_end-time_start
    print("Once MPC costs:", time_c, "S")

6.得到结果并返回，这个U就是输出，我们MPC预测N步，只取第一步的输入值作为系统的输入。

    return(u[0:2])
    prob.update(l=l, u=u)

---

总结以上的步骤：

预设了离散的状态空间方程

输入了当前状态量与期望值

输出了控制量