【MATLAB】无人驾驶车辆的模型预测控制技术（精简讲解和代码）【运动学轨迹规划】_无人驾驶车辆模型预测控制第二版pdf

\qquad

汽车在变道时，给定期望的轨迹

(

x

r

,

y

r

)

(x_r,y_r)

(xr​,yr​)，并通过控制变量使汽车在时域上经过的路径逼近期望轨迹。当然，我们期望汽车的朝向

θ

\theta

θ最终逼近于0（即直线行驶），期望的状态量使用

S

r

S_r

Sr​表示，则我们的目的是使

∣

S

−

S

r

∣

|S-S_r|

∣S−Sr​∣即

(

S

−

S

r

)

2

/

2

(S-S_r)^2/2

(S−Sr​)2/2最小。当期望状态向量与实际状态向量相差较小且期望控制量与实际控制量相差较小时，可采用在

S

r

S_r

Sr​处的导数

d

y

/

d

x

dy/dx

dy/dx来代替变化率

Δ

y

/

Δ

x

\Delta y/ \Delta x

Δy/Δx：

S

′

−

S

r

′

=

f

(

S

,

U

)

−

f

(

S

r

,

U

r

)

≈

∂

f

∂

S

∣

S

r

⋅

(

S

−

S

r

)

∂

f

∂

U

∣

U

r

⋅

(

U

−

U

r

)

=

A

⋅

(

S

−

S

r

)

B

⋅

(

U

−

U

r

)

\begin{array}{l} S’-S’r=f(S,U)-f(S_r,U_r)\[2ex] ≈\frac {\partial f}{\partial S}|{S_r}\cdot(S-S_r)+\frac {\partial f}{\partial U}|_{U_r}\cdot(U-U_r)\[2ex] =A\cdot(S-S_r)+B\cdot(U-U_r) \end{array}

S′−Sr′​=f(S,U)−f(Sr​,Ur​)≈∂S∂f​∣Sr​​⋅(S−Sr​)+∂U∂f​∣Ur​​⋅(U−Ur​)=A⋅(S−Sr​)+B⋅(U−Ur​)​
其中

A

=

∂

f

∂

S

∣

S

r

=

[

∂

f

1

∂

S

1

∂

f

1

∂

S

2

∂

f

1

∂

S

3

∂

f

2

∂

S

1

∂

f

2

∂

S

2

∂

f

2

∂

S

3

∂

f

3

∂

S

1

∂

f

3

∂

S

2

∂

f

3

∂

S

3

]

=

[

1

0

−

v

d

r

∗

s

i

n

(

θ

r

)

0

1

v

d

r

∗

c

o

s

(

θ

r

)

0

0

1

]

B

=

∂

f

∂

U

∣

U

r

=

[

∂

f

1

∂

U

1

∂

f

1

∂

U

2

∂

f

2

∂

U

1

∂

f

2

∂

U

2

∂

f

3

∂

U

1

∂

f

3

∂

U

2

]

=

[

c

o

s

(

θ

r

)

0

s

i

n

(

θ

r

)

0

t

a

n

(

ϕ

r

)

/

l

v

d

r

∗

s

e

c

2

(

ϕ

r

)

/

l

]

A=\frac {\partial f}{\partial S}|{S_r}= = \ B=\frac {\partial f}{\partial U}|{U_r}= =

A=∂S∂f​∣Sr​​=

​∂S1​∂f1​​∂S1​∂f2​​∂S1​∂f3​​​∂S2​∂f1​​∂S2​∂f2​​∂S2​∂f3​​​∂S3​∂f1​​∂S3​∂f2​​∂S3​∂f3​​​

​=

​100​010​−vdr​∗sin(θr​)vdr​∗cos(θr​)1​

​B=∂U∂f​∣Ur​​=

​∂U1​∂f1​​∂U1​∂f2​​∂U1​∂f3​​​∂U2​∂f1​​∂U2​∂f2​​∂U2​∂f3​​​

​=

​cos(θr​)sin(θr​)tan(ϕr​)/l​00vdr​∗sec2(ϕr​)/l​

​
记

S

′

~

=

S

′

−

S

r

′

,

S

~

=

S

−

S

r

,

U

~

=

U

−

U

r

\widetilde{S’}=S’-S’_r,\widetilde{S}=S-S_r,\widetilde{U}=U-U_r

S′

=S′−Sr′​,S

=S−Sr​,U

=U−Ur​
则线性化后的模型为

S

′

~

=

A

S

~

B

U

~

\widetilde{S’}=A\widetilde{S}+B\widetilde{U}

S′

=AS

+BU

即我们所熟知的线性状态方程。我们的目标是使得汽车按照既定的轨迹形式，因此希望实际状态逼近于期望状态，即

∣

∣

S

~

∣

∣

→

0

||\widetilde{S}||\rightarrow0

∣∣S

∣∣→0，因此输出方程即为

Y

=

S

~

Y=\widetilde{S}

Y=S

3.2.

U

r

U_r

Ur​的求取

\qquad

期望状态向量

S

r

S_r

Sr​是容易理解的，无人驾驶车辆需要提前规划形式的路径

(

x

r

,

y

r

)

(x_r,y_r)

(xr​,yr​)，有时还对车辆的朝向有所要求（

θ

r

\theta_r

θr​），那么期望控制量

U

r

U_r

Ur​又是怎么得出的呢？其实很简单，在没有约束条件的情况下，根据状态方程即可得到

U

r

U_r

Ur​的取值。根据状态方程我们可以得知：

{

x

r

′

=

v

d

r

c

o

s

(

θ

r

)

y

r

′

=

v

d

r

s

i

n

(

θ

r

)

θ

r

′

=

v

d

r

t

a

n

ϕ

r

/

l

⇒

{

v

d

r

=

(

x

r

′

)

2

(

y

r

′

)

2

ϕ

r

=

a

t

a

n

(

l

⋅

θ

r

′

/

v

d

r

)

\Rightarrow \begin{cases} v_{dr}=\sqrt{(x’_r)2+(y’_r)2}\ \phi_r=atan(l\cdot \theta’r/v{dr}) \end{cases}

⎩

⎨

⎧​xr′​=vdr​cos(θr​)yr′​=vdr​sin(θr​)θr′​=vdr​tanϕr​/l​⇒{vdr​=(xr′​)2+(yr′​)2

​ϕr​=atan(l⋅θr′​/vdr​)​

\qquad

在实际系统中，若

S

=

(

x

r

,

y

r

,

θ

r

)

S=(x_r,y_r,\theta_r)

S=(xr​,yr​,θr​)是离散取值的，可以借助三次样条插值的方法补全需要的值，并用差分替代微分求取状态量的导数。样条差值的方法在此不再赘述，仿真时建议使用MATLAB的pchip函数。
本部分对应的MATLAB代码如下

    X_r = [0,1,2,3,4,5,6]\*3;  % X的期望坐标
    Y_r = [3,3,2,1,0,0,0];  % Y的期望坐标
    L=2.5; % 车身长度
    v0 = 10; % 期望速度
    T = 20e-3; % 控制周期20ms
    dt = 1e-3; % 仿真周期1ms
    Xr = linspace(X\_r(1),X\_r(numel(X_r)),(X\_r(numel(X_r))-X\_r(1))/(T\*v0));  % 横坐标期望值的插值序列
    Yr = pchip(X_r,Y_r,Xr);  % 通过三次样条插值算出纵坐标坐标插值序列
    n = numel(Yr);  % 预测模型中所有规划点
    FYr = Yr(2:n);
    dYr = zeros(1,n);
    dYr(1:n-1) = FYr - Yr(1:n-1);
    dYr(n) = dYr(n-1);%保证导数连续
    FXr = Xr(2:n);
    dXr = zeros(1,n);
    dXr(1:n-1) = FXr - Xr(1:n-1);
    dXr(n) = dXr(n-1);
    Thr = atan(dYr./dXr);  % 车朝向角的期望值
    FThr = Thr(2:n);
    dThr = zeros(1,n);
    dThr(1:n-1) = FThr-Thr(1:n-1);
    dThr(n)=dThr(n-1);
    vdr = sqrt(dXr.^2+dYr.^2)/T;  % 车后轮的期望速度
    Phr = atan(L./vdr.\*dThr/T);  % 车前轮的偏转角
    
    
    
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
    

这里以变道为例，绘制期望轨迹

(

X

r

,

Y

r

)

(X_r,Y_r)

(Xr​,Yr​)如下图

\qquad

可以发现，车辆从上道（y=3）逐步变化至下道(y=0)。样条插值后的轨迹并不是折线，但由于不一定满足车辆的物理约束，因此不能用期望控制量作为实际控制量进行控制。

3.3.离散化与序列化

\qquad

预测控制依赖于计算机，计算机的控制信号是离散的，因此我们还需要把线性化后的状态方程离散化。假设控制周期为

T

T

T，则离散化后的状态方程为：

S

~

(

k

1

)

−

S

~

(

k

)

T

=

A

k

S

~

(

k

)

B

k

U

~

(

k

)

⇒

S

~

(

k

1

)

=

(

A

k

T

I

)

S

~

(

k

)

B

k

T

U

~

(

k

)

⇒

S

~

(

k

1

)

=

A

k

∗

S

~

(

k

)

B

k

∗

U

~

(

k

)

\begin{array}{l} \frac{\widetilde{S}(k+1)-\widetilde{S}(k)}{T}=A_k\widetilde{S}(k)+B_k\widetilde{U}(k) \[2ex] \Rightarrow \widetilde{S}(k+1)=(A_kT+I)\widetilde{S}(k)+B_kT\widetilde{U}(k) \ \Rightarrow \widetilde{S}(k+1)=A*_k\widetilde{S}(k)+B*_k\widetilde{U}(k)\ \end{array}

TS

(k+1)−S

(k)​=Ak​S

(k)+Bk​U

(k)⇒S

(k+1)=(Ak​T+I)S

(k)+Bk​TU

(k)⇒S

(k+1)=Ak∗​S

(k)+Bk∗​U

(k)​

\qquad

接着我们需要选择接下来需要预测的未来若干个采样时刻的状态量的个数和控制量个数，即预测时域

N

N

N和控制时域

M

M

M（

M

≤

N

M≤N

M≤N）。我们采取最简单的情形

M

=

N

M=N

M=N讲解。

\qquad

首先列写未来

N

N

N个时刻的离散状态方程

{

S

~

(

k

1

)

=

A

k

∗

S

~

(

k

)

B

k

∗

U

~

(

k

)

S

~

(

k

2

)

=

A

k

1

∗

S

~

(

k

1

)

B

k

1

∗

U

~

(

k

1

)

⋯

S

~

(

k

N

)

=

A

k

N

−

1

∗

S

~

(

k

N

−

1

)

B

k

N

−

1

∗

U

~

(

k

N

−

1

)

⇒

S

~

(

k

i

)

=

A

A

i

S

~

(

k

)

A

B

1

U

~

(

k

)

A

B

2

U

~

(

k

1

)

.

.

.

A

B

i

U

~

(

k

i

−

1

)

\begin{cases} \widetilde{S}(k+1)=A*k\widetilde{S}(k)+B*k\widetilde{U}(k)\ \widetilde{S}(k+2)=A*{k+1}\widetilde{S}(k+1)+B*{k+1}\widetilde{U}(k+1)\ \qquad \qquad \cdots \ \widetilde{S}(k+N)=A*{k+N-1}\widetilde{S}(k+N-1)+B*{k+N-1}\widetilde{U}(k+N-1)\ \end{cases}\ \Rightarrow \widetilde{S}(k+i)=AA_i\widetilde{S}(k)+AB_1\widetilde{U}(k)+AB_2\widetilde{U}(k+1)+…+AB_i\widetilde{U}(k+i-1)

⎩

⎨

⎧​S

(k+1)=Ak∗​S

(k)+Bk∗​U

(k)S

(k+2)=Ak+1∗​S

(k+1)+Bk+1∗​U

(k+1)⋯S

(k+N)=Ak+N−1∗​S

(k+N−1)+Bk+N−1∗​U

(k+N−1)​⇒S

(k+i)=AAi​S

(k)+AB1​U

(k)+AB2​U

(k+1)+…+ABi​U

(k+i−1)
其中

{

A

A

i

=

A

k

i

−

1

∗

A

k

i

−

2

∗

.

.

.

A

k

∗

A

B

1

(

i

)

=

A

k

i

−

1

∗

A

k

i

−

2

∗

.

.

.

A

k

1

∗

B

k

∗

A

B

2

(

i

)

=

A

k

i

−

1

∗

A

k

i

−

2

∗

.

.

.

A

k

2

∗

B

k

1

∗

⋯

A

B

i

−

1

(

i

)

=

A

k

i

−

1

∗

B

k

i

−

2

∗

A

B

i

(

i

)

=

B

k

i

−

1

∗

\begin{cases} AA_i=A*{k+i-1}A*{k+i-2}…A^_{k} \ AB_1{(i)}=A{k+i-1}A*{k+i-2}…A*{k+1}B^_k \ AB_2{(i)}=A{k+i-1}A*{k+i-2}…A*{k+2}B^{k+1} \ \qquad \cdots \ AB{i-1}{(i)}=A{k+i-1}B^*{k+i-2} \ AB_{i}{(i)}=B*_{k+i-1} \ \end{cases}

⎩

⎨

⎧​AAi​=Ak+i−1∗​Ak+i−2∗​…Ak∗​AB1(i)​=Ak+i−1∗​Ak+i−2∗​…Ak+1∗​Bk∗​AB2(i)​=Ak+i−1∗​Ak+i−2∗​…Ak+2∗​Bk+1∗​⋯ABi−1(i)​=Ak+i−1∗​Bk+i−2∗​ABi(i)​=Bk+i−1∗​​

\qquad

已知量为

S

~

(

k

)

,

A

k

∗

,

B

k

∗

\widetilde{S}(k),A*_k,B*_k

S

(k),Ak∗​,Bk∗​，后两者是因为期望状态量和期望控制量已知因此也已知，但由于期望状态量是一个时间序列，因此矩阵

A

k

∗

,

B

k

∗

A*_k,B*_k

Ak∗​,Bk∗​也是一个时间序列。输入为

U

~

(

k

)

,

U

~

(

k

1

)

,

.

.

.

,

U

~

(

k

N

)

\widetilde{U}(k),\widetilde{U}(k+1),…,\widetilde{U}(k+N)

U

(k),U

(k+1),…,U

(k+N)，输出为

S

~

(

k

1

)

,

S

~

(

k

2

)

,

.

.

.

.

,

S

~

(

k

N

)

\widetilde{S}(k+1), \widetilde{S}(k+2),…, \widetilde{S}(k+N)

S

(k+1),S

(k+2),…,S

(k+N)，记

S

~

^

=

[

S

~

(

k

1

)

,

S

~

(

k

2

)

,

.

.

.

.

,

S

~

(

k

N

)

]

T

U

~

^

=

[

U

~

(

k

)

,

U

~

(

k

1

)

,

.

.

.

,

U

~

(

k

N

−

1

)

]

T

U

^

=

[

U

(

k

)

,

U

(

k

1

)

,

.

.

.

,

U

(

k

N

−

1

)

]

T

\widehat{ \widetilde{S}}=[\widetilde{S}(k+1), \widetilde{S}(k+2),…, \widetilde{S}(k+N)]^T \ \widehat{\widetilde{U}}=[\widetilde{U}(k),\widetilde{U}(k+1),…,\widetilde{U}(k+N-1)]^T \ \widehat{U}=[U(k),U(k+1),…,U(k+N-1)]^T

S

=[S

(k+1),S

(k+2),…,S

(k+N)]TU

=[U

(k),U

(k+1),…,U

(k+N−1)]TU

=[U(k),U(k+1),…,U(k+N−1)]T
根据上面的推导，可以推出序列化后的状态方程：

S

~

^

=

A

A

⋅

S

~

(

k

)

B

B

⋅

U

~

^

s

.

t

.

{

v

m

i

n

≤

v

d

≤

v

m

a

x

−

ϕ

m

≤

ϕ

≤

ϕ

m

\widehat{ \widetilde{S}}=AA\cdot \widetilde{S}(k)+BB\cdot\widehat{\widetilde{U}} \ s.t.

S

=AA⋅S

(k)+BB⋅U

s.t.{vmin​≤vd​≤vmax​−ϕm​≤ϕ≤ϕm​​
其中

A

A

=

[

A

A

1

A

A

2

⋮

A

A

N

]

B

B

=

[

A

B

1

(

1

)

0

⋯

0

A

B

1

(

2

)

A

B

2

(

2

)

⋯

0

⋮

⋮

⋱

⋮

A

B

1

(

N

)

A

B

2

(

N

)

⋯

A

B

N

(

N

)

]

AA= \[2ex] BB = \begin{bmatrix} AB_1^{(1)} & 0 &\cdots & 0 \ AB_1^{(2)} & AB_2^{(2)} & \cdots & 0\ \vdots & \vdots & \ddots & \vdots \ AB_1^{(N)} & AB_2^{(N)} &\cdots & AB_{N}^{(N)} \end{bmatrix}

AA=

​AA1​AA2​⋮AAN​​

​BB=

​AB1(1)​AB1(2)​⋮AB1(N)​​0AB2(2)​⋮AB2(N)​​⋯⋯⋱⋯​00⋮ABN(N)​​

​
优化问题为

U

^

=

a

r

g

min

⁡

U

^

1

2

S

~

^

T

Q

S

~

^

1

2

U

^

T

P

U

^

\widehat{U}=arg\min_{\widehat{U}}{\frac{1}{2}\widehat{ \widetilde{S}}^TQ\widehat{ \widetilde{S}}+\frac{1}{2}\widehat{U}^TP\widehat{U}}

U

=argU

min​21​S

TQS

+21​U

TPU

3.4.实现增量控制

\qquad

除了保证车辆基本的约束外，为了保证车辆的舒适性，还应防止控制量的突变，即车辆的加速度和打方向盘的速度不至于过快。控制量的加速度不宜过大，需要在约束条件中增加关于控制量变化率的硬约束 和在目标函数增加关于控制量变化率的软约束 。

\qquad

硬约束很好理解，即对控制增量进行限幅

{

a

m

i

n

≤

v

d

′

≤

a

m

a

x

−

d

ϕ

m

≤

ϕ

′

≤

d

ϕ

m

{amin​≤vd′​≤amax​−dϕm​≤ϕ′≤dϕm​​

\qquad

软约束则是通过在目标函数中加入关于控制增量的二次型进行约束，目的是使得目标函数在求取极小值的过程中兼顾到控制增量的最小化。因此上述3.3.节的目标函数则需要修改为

Δ

U

^

=

a

r

g

min

⁡

Δ

U

^

1

2

S

~

^

T

Q

S

~

^

1

2

(

Δ

U

^

)

T

P

(

Δ

U

^

)

\Delta\widehat{U}=arg\min_{\Delta\widehat{U}}{\frac{1}{2}\widehat{ \widetilde{S}}^TQ\widehat{ \widetilde{S}}+\frac{1}{2}(\Delta\widehat{U})^TP(\Delta\widehat{U})}

ΔU

=argΔU

min​21​S

TQS

+21​(ΔU

)TP(ΔU

)

\qquad

仅此而已是不够的，我们还需要重写线性状态方程，记

{

G

u

=

[

u

(

k

−

1

)

−

u

r

(

k

)

,

u

(

k

−

1

)

−

u

r

(

k

1

)

,

.

.

.

,

u

(

k

−

1

)

−

u

r

(

k

N

−

1

)

]

T

E

N

=

T

r

d

(

o

n

e

s

(

1

,

N

)

)

(元素均为1的下三角阵)

U

~

^

=

G

u

T

∗

E

N

⋅

Δ

U

^

\left { \right.

⎩

⎨

⎧​Gu​=[u(k−1)−ur​(k),u(k−1)−ur​(k+1),…,u(k−1)−ur​(k+N−1)]TEN​=Trd(ones(1,N))(元素均为1的下三角阵)U

=Gu​+T∗EN​⋅ΔU

​
因此目标函数为

J

=

1

2

S

~

^

T

Q

S

~

^

1

2

(

Δ

U

^

)

T

P

(

Δ

U

^

)

=

1

2

(

A

A

⋅

S

~

(

k

)

B

B

⋅

U

~

^

)

T

Q

(

A

A

⋅

S

~

(

k

)

B

B

⋅

U

~

^

)

1

2

(

Δ

U

^

)

T

P

(

Δ

U

^

)

=

1

2

(

A

A

⋅

S

~

(

k

)

B

B

(

G

u

T

∗

E

N

Δ

U

^

)

)

T

Q

(

A

A

⋅

S

~

(

k

)

B

B

(

G

u

T

∗

E

N

Δ

U

^

)

)

1

2

(

Δ

U

^

)

T

P

(

Δ

U

^

)

J===​21​S

TQS

+21​(ΔU

)TP(ΔU

)21​(AA⋅S

(k)+BB⋅U

)TQ(AA⋅S

(k)+BB⋅U

)+21​(ΔU

)TP(ΔU

)21​(AA⋅S

(k)+BB(Gu​+T∗EN​ΔU

))TQ(AA⋅S

(k)+BB(Gu​+T∗EN​ΔU

))+21​(ΔU

)TP(ΔU

)​
令

G

A

B

=

A

A

⋅

S

~

(

k

)

B

B

⋅

G

u

E

N

B

=