# 无人车系统（十一）：轨迹跟踪模型预测控制(MPC)原理与python实现【40行代码】

## 3. MPC控制器设计

### 3.1 线性模型

$x_{k+1}=Ax_k+Bu_k+C \tag{1}$

$x_{k+1}=Ax_k+Bu_k+C$
$x_{k+2}=Ax_{k+1}+Bu_{k+1}+C=A(Ax_k+Bu_k+C)+Bu_{k+1}+C=A^2x_{k+1}+ABu_k+Bu_{k+1}+AC+C$
$x_{k+3}=A^3x_k+A^2Bu_{k}+AB_{k+1}+Bu_{k+2}+A^2C+AC+C$
$...$
$x_{k+T}=A^{T}x_{k}+A^{T-1}Bu_k+A^{T-2}Bu_{k+1}+...+A^{T-i}Bu_{k+i-1}+...+Bu_{k+T-1}+A^{T-1}C+A^{T-2}C+...+C$

$\mathcal{X}=\mathcal{A}x_k+\mathcal{B}\mathbf{u}+\mathcal{C} \tag{2}$

$\mathcal{X}=\left[\begin{matrix} x_{k+1} & x_{k+2} & x_{k+3}& ... & x_{k+T}\end{matrix}\right]^T$,
$\mathbf{u}=\left[\begin{matrix} u_k & u_{k+1} & u_{k+2} &...&u_{k+T-1}\end{matrix}\right]^T$,
$\mathcal{A}=\left[\begin{matrix}A & A^2 & A^3 & ... & A^T\end{matrix}\right]^T$,
$\mathcal{B}=\left[\begin{matrix}B & 0 & 0 & ... & 0 \\AB & B & 0 & ... & 0 \\ A^2B & AB & B & ... & 0 \\...& ...&...&...&...\\A^{t-1}B & A^{T-2}B& A^{T-3}B &...&B\end{matrix}\right]$,
$\mathcal{C}=\left[\begin{matrix}C\\AC+C\\A^2C+AC+C\\...\\A^{k+T-1}C+...+C\end{matrix}\right]$

$\min \mathcal{J}=\mathcal{E}^T Q \mathcal{E}+\mathbf{u}^T R \mathbf{u} \\ \text{s.t. } u_{min}\leq \mathbf{u}\leq u_{max} \tag{3}$

### 3.2 非线性模型

#### 3.2.1 无人车轨迹跟踪MPC问题

$\begin{matrix} \dot{x}=vcos(\theta) \\ \dot{y}=vsin(\theta) \\ \dot{\theta}=v\frac{tan(\delta)}{L} \\ \dot{v}=a \end{matrix} \tag{1}$

$\begin{matrix} x_{k+1}=x_k+v_k\cos(\theta_k)d_t \\ y_{k+1}=y_k+v_k\sin(\theta_k)d_t \\ \theta_{k+1}=\theta_{k}+v_k \frac{\tan(\delta_k)}{L}d_t \\ v_{k+1} = v_k+a_kd_t \\ \text{cte}_{k+1} = \text{cte}_k+v_k \sin (\theta_k)d_t \\ \text{epsi}_{k+1}=\text{epsi}_k+v_k \frac{\tan(\delta_k)}{L}d_t \end{matrix} \tag{2}$

$\begin{array}{c} \text{cte}_k=f(x_k)-y_k \\ \text{epsi}_k = arc\tan(f'(x_k))-\theta \end{array} \tag{3}$

$\begin{array}{cc} \text{min } &\mathcal{J}=\sum_{k=1}^N(\omega_{\text{cte}}||\text{cte}_t||^2+\omega_{\text{epsi}}||\text{epsi}_k||^2+\omega_v ||v_k-v_{\text{ref}}||^2) \\ & +\sum_{k=1}^{N-1}(\omega_{\delta}||\delta_k||^2+\omega_{a}||a_k||^2) \\ & +\sum_{k=1}^{N-2}(\omega_{\text{rate}_{\delta}}||\delta_{k+1}-\delta_{k}||^2+\omega_{\text{rate}_{a}}||a_{k+1}-a_{k}||^2) \\ \end{array}\tag{4}$

$\begin{array}{c} \text{s.t.} & x_{k+1}=x_k+v_kcos(\theta_k)d_t , k=1,2,...,N-1\\ & y_{k+1}=y_k+v_ksin(\theta_k)d_t , k=1,2,...,N-1\\ & \theta_{k+1}=\theta_{k}+v_k \frac{tan(\delta_k)}{L}d_t , k=1,2,...,N-1\\ & v_{k+1} = v_k+a_kd_t, k=1,2,...,N-1 \\ & \text{cte}_{k+1} =f(x_k)-y_k+v_k \sin (\theta_k)d_t \\ & \text{epsi}_{k+1}=arc\tan(f'(x_k))-\theta+v_k \frac{\tan(\delta_k)}{L}d_t \end{array}\tag{5}$

$\begin{array}{cc} \delta\in[\delta_{\text{min}}, \delta_{\text{max}}]\\ a\in [a_{\text{min}}, a_{\text{max}}] \end{array}\tag{6}$

#### 3.2.2 无人车轨迹跟踪MPC问题求解

src/json.hpp:6057:62: error: wrong number o

error while loading shared libraries: libcoinmumps.so.1: cannot open shared对于这个错误，是因为系统是64位的，但是程序用到的某个32版本的库。我运行以下命令就解决了。

sudo apt-get update
sudo apt-get install ia32-libs


sudo apt-get install lib32z1

##### 3.2.2.2 基于python+pyomo的实现

import numpy as np
from pyomo.environ import *
from pyomo.dae import *

N = 9 # forward predict steps
ns = 6  # state numbers / here: 1: x, 2: y, 3: psi, 4: v, 5: cte, 6: epsi
na = 2  # actuator numbers /here: 1: steering angle, 2: throttle

class MPC(object):
def __init__(self):
m = ConcreteModel()
m.sk = RangeSet(0, N-1)
m.uk = RangeSet(0, N-2)
m.uk1 = RangeSet(0, N-3)
# Parameters
m.wg       = Param(RangeSet(0, 3), initialize={0:1., 1:10., 2:100., 3:130000}, mutable=True)
m.dt       = Param(initialize=0.1, mutable=True)
m.Lf       = Param(initialize=2.67, mutable=True)
m.ref_v    = Param(initialize=75., mutable=True)
m.ref_cte  = Param(initialize=0.0, mutable=True)
m.ref_epsi = Param(initialize=0.0, mutable=True)
m.s0       = Param(RangeSet(0, ns-1), initialize={0:0., 1:0., 2:0., 3:0., 4:0., 5:0.}, mutable=True)
m.coeffs   = Param(RangeSet(0, 3),
initialize={0:-0.000458316, 1:0.00734257, 2:0.0538795, 3:0.080728}, mutable=True)

# Variables
m.s      = Var(RangeSet(0, ns-1), m.sk)
m.f      = Var(m.sk)
m.psides = Var(m.sk)
m.ua     = Var(m.uk, bounds=(-1.0, 1.0))
m.ud     = Var(m.uk, bounds=(-0.436332, 0.436332))

# 0: x, 1: y, 2: psi, 3: v, 4: cte, 5: epsi
# Constrainsts
m.s0_update      = Constraint(RangeSet(0, ns-1), rule = lambda m, i: m.s[i,0] == m.s0[i])
m.x_update       = Constraint(m.sk, rule=lambda m, k:
m.s[0,k+1]==m.s[0,k]+m.s[3,k]*cos(m.s[2,k])*m.dt
if k<N-1 else Constraint.Skip)
m.y_update       = Constraint(m.sk, rule=lambda m, k:
m.s[1,k+1]==m.s[1,k]+m.s[3,k]*sin(m.s[2,k])*m.dt
if k<N-1 else Constraint.Skip)
m.psi_update     = Constraint(m.sk, rule=lambda m, k:
m.s[2,k+1]==m.s[2,k]-m.s[3,k]*m.ud[k]/m.Lf*m.dt
if k<N-1 else Constraint.Skip)
m.v_update       = Constraint(m.sk, rule=lambda m, k:
m.s[3,k+1]==m.s[3,k]+m.ua[k]*m.dt
if k<N-1 else Constraint.Skip)
m.f_update      = Constraint(m.sk, rule=lambda m, k:
m.f[k]==m.coeffs[0]*m.s[0,k]**3+m.coeffs[1]*m.s[0,k]**2+
m.coeffs[2]*m.s[0,k]+m.coeffs[3])
m.psides_update = Constraint(m.sk, rule=lambda m, k:
m.psides[k]==atan(3*m.coeffs[0]*m.s[0,k]**2
+2*m.coeffs[1]*m.s[0,k]+m.coeffs[2]))
m.cte_update     = Constraint(m.sk, rule=lambda m, k:
m.s[4,k+1]==(m.f[k]-m.s[1,k]+m.s[3,k]*sin(m.s[5,k])*m.dt)
if k<N-1 else Constraint.Skip)

m.epsi_update    = Constraint(m.sk, rule=lambda m, k:
m.s[5, k+1]==m.s[2,k]-m.psides[k]-m.s[3,k]*m.ud[k]/m.Lf*m.dt
if k<N-1 else Constraint.Skip)
# Objective function
m.cteobj  = m.wg[2]*sum((m.s[4,k]-m.ref_cte)**2 for k in m.sk)
m.epsiobj = m.wg[2]*sum((m.s[5,k]-m.ref_epsi)**2 for k in m.sk)
m.vobj    = m.wg[0]*sum((m.s[3,k]-m.ref_v)**2 for k in m.sk)
m.udobj   = m.wg[1]*sum(m.ud[k]**2 for k in m.uk)
m.uaobj   = m.wg[1]*sum(m.ua[k]**2 for k in m.uk)
m.sudobj  = m.wg[3]*sum((m.ud[k+1]-m.ud[k])**2 for k in m.uk1)
m.suaobj  = m.wg[2]*sum((m.ua[k+1]-m.ua[k])**2 for k in m.uk1)
m.obj = Objective(expr = m.cteobj+m.epsiobj+m.vobj+m.udobj+m.uaobj+m.sudobj+m.suaobj, sense=minimize)

self.iN = m#.create_instance()

def Solve(self, state, coeffs):
self.iN.s0.reconstruct({0:state[0], 1: state[1], 2:state[2], 3:state[3], 4:state[4], 5:state[5]})
self.iN.coeffs.reconstruct({0:coeffs[0], 1:coeffs[1], 2:coeffs[2], 3:coeffs[3]})
self.iN.f_update.reconstruct()
self.iN.s0_update.reconstruct()
self.iN.psides_update.reconstruct()
SolverFactory('ipopt').solve(self.iN)
x_pred_vals = [self.iN.s[0,k]() for k in self.iN.sk]
y_pred_vals = [self.iN.s[1,k]() for k in self.iN.sk]
steering_angle = self.iN.ud[0]()
throttle = self.iN.ua[0]()
return x_pred_vals, y_pred_vals, steering_angle, throttle


## 结语

PID控制存在积分环节，控制时具有滞后性，而自动驾驶技术对实时性的要求非常高，因此PID控制不太适用于自动驾驶技术。本文介绍的是一种基于模型预测的自动驾驶汽车轨迹追踪方法，不存在滞后性且不需要像PID控制那样调节参数。

by windSeS 2019-12-20

06-27 633

02-19 1万+

06-13 3591

09-12 5万+

02-12 2121

07-12 6156

02-27 9488

01-17 2万+

04-10 1万+

01-15 3544

02-05 8252

02-08 643

04-25 3292

12-17 1万+

08-11 2万+

10-27 1573

11-14

01-07

03-19

10-02

07-29

09-26

03-08 2212

08-14

11-20 2万+

02-02 1071

03-26 4万+

03-27 1万+

07-12 3559

08-04 5415

#### 无人驾驶算法——使用Stanley method实现无人车轨迹追踪

©️2019 CSDN 皮肤主题: Age of Ai 设计师: meimeiellie