【自动驾驶】模型预测控制(MPC)实现轨迹跟踪

springboot+vue

文章目录

cin

参考资料

1. 基本概念

  • 模型预测控制(MPC)的核心思想就是以优化方法求解最优控制器,其中优化方法大多时候采用二次规划(Quadratic Programming)。

    android13

  • MPC控制器优化得到的控制输出也是系统在未来有限时间步的控制序列。 当然,由于理论构建的模型与系统真实模型都有误差,所以,实际上更远未来的控制输出对系统控制的价值很低,故MPC仅执行输出序列中的第一个控制输出

    xr

模型(Model)

导入导出

分为机理模型基于数据的模型(例如用神经网络训练的一个model)使用基于数据的模型的MPC可以结合model based RL使用。

secureCRT

预测(Predict)

非同质化代币

模型就是用来预测的,预测的目的是为了更好的决策

python数据分析

控制(Control)

opengles

控制即决策,根据预测来作出决策。

Android12蓝牙

MPC利用一个已有的模型系统当前的状态未来的控制量,来预测系统未来的输出,然后与我们期望的系统输出做比较,得到一个损失函数(代价函数),即:

没有方法签名

损失函数

=

(

未来输出

(

模型,当前状态,未来控制量

)

期望输出

)

2

损失函数 = (未来输出(模型,当前状态,未来控制量)-期望输出)^2

损失函数=(未来输出(模型,当前状态,未来控制量)期望输出)2

增删改查

由于上式中模型、当前状态、期望输出都是已知的,因此只有未来控制量一个自变量。采用二次规划的方法求解出某个未来控制量,使得损失函数最小,前面提到,这个未来控制量的第一个元素就是当前控制周期的控制量

python解释器

1.1 MPC vs optimal control

最优控制(optimal control)指的是在一定的约束情况下达到最优状态的系统表现,其中约束情况通常是实际环境所带来的限制,例如汽车中的油门不能无限大等。

配置gradle签名信息

最优控制强调的是“最优”,一般最优控制需要在整个时间域上进行求优化(从0时刻到正无穷时刻的积分),这样才能保证最优性,这是一种很贪婪的行为,需要消耗大量算力。同时,系统如果是一个时变系统,或者面临扰动的话,前一时刻得到的最优并不一定是下一时刻的最优值。

梯度下降

J

=

0

E

T

Q

E

+

U

T

R

U

d

t

J=\int_{0}^{\infty} E^{T} Q E+U^{T} R U d t

J=0ETQE+UTRUdt

微信登录小程序

最优控制常用解法有: 变分法,极大值原理,动态规划,LQR(LQR可以参考博客)。

Flutter苹果端启动图适配

MPC仅考虑未来几个时间步,一定程度上牺牲了最优性。

分享到朋友圈

1.2 MPC优点

  • MPC善于处理多输入多输出系统(MIMO);

    MODBUS

  • MPC可以处理约束,如安全性约束,上下阈值;

    http

  • MPC是一种向前考虑未来时间步的有限时域优化方法(一定的预测能力)

    Java异常处理

    最优控制要求在整个时间优化

    数据库

    实际上MPC采用了一个折中的策略,既不是像最优控制那样考虑整个时域,也不是仅仅考虑当前,而是考虑未来的有限时间域。

    旅游

2. MPC整体流程

2.1 预测区间与控制区间

对于一般的离散化系统(因为实际计算机实现的控制系统都是离散的系统,连续系统可以进行离散化操作),在k时刻,我们可以测量出系统的当前状态

y

(

k

)

y(k)

y(k),再通过计算得到的

u

(

k

)

,

u

(

k

+

1

)

,

u

(

k

+

2

)

.

.

.

u

(

k

+

j

)

u(k),u(k+1),u(k+2)…u(k+j)

u(k),u(k+1),u(k+2)u(k+j)得到系统未来状态的估计值

y

(

k

+

1

)

,

y

(

k

+

2

)

.

.

.

y

(

k

+

j

)

y(k+1),y(k+2)…y(k+j)

y(k+1),y(k+2)y(k+j)

副本集

将预测状态估计的部分称为预测区间(Predictive Horizon),指的是一次优化后预测未来输出的时间步的个数。

青龙面板常见问题报错

将控制估计的部分称为控制区间(Control Horizon),在得到最优输入之后,我们只施加当前时刻的输入u(k),即控制区间的第一位控制输入。

安卓app

如下图

[

k

,

k

+

m

]

[k, k+m]

[k,k+m]范围为控制区间,之后的红色部分称为 held constant,其中控制区间是要通过优化器来进行优化的参数。

服务器

过小的控制区间,可能无法做到较好的控制,而较大的控制区间,比如与预测区间相等,则会导致只有前一部分的控制范围才会有较好的效果,而后一部分的控制范围则收效甚微,而且将带来大量的计算开销。

地理信息系统

2.2 约束

对于约束,一般分为Hard约束Soft约束,Hard约束是不可违背必须遵守的,在控制系统中,输入输出都可能会有约束限制,但是在设计时不建议将输入输出都给予Hard约束,因为这两部的约束有可能是有重叠的,导致优化器会产生不可行解。

CDC

Hard约束不能违反,Soft约束可以;比如Hard约束是刹车踩的幅度;Soft约束是速度

线性代数

建议输出采用Soft约束,而输入的话建议输入和输入参数变化率二者之间不要同时为Hard约束,可以一个Hard一个Soft。

2.3 MPC流程

模型预测控制在k时刻共需三步;

  • 第一步:获取系统的当前状态;

  • 第二步:基于

    u

    (

    k

    )

    ,

    u

    (

    k

    +

    1

    )

    ,

    u

    (

    k

    +

    2

    )

    .

    .

    .

    u

    (

    k

    +

    m

    )

    u(k),u(k+1),u(k+2)…u(k+m)

    u(k),u(k+1),u(k+2)u(k+m)进行最优化处理;

    离散系统的代价函数可以参考

    J

    =

    k

    m

    1

    E

    k

    T

    Q

    E

    k

    +

    u

    k

    T

    R

    u

    k

    +

    E

    N

    T

    F

    E

    N

    J=\sum_{k}^{m-1}E_k^TQE_k+u_k^TRu_k+E_N^TFE_N

    J=km1EkTQEk+ukTRuk+ENTFEN

    其中

    E

    N

    E_N

    EN表示误差的终值,也是衡量优劣的一种标准。

  • 第三步:只取

    u

    (

    k

    )

    u(k)

    u(k)作为控制输入施加在系统上。

在下一时刻重复以上三步,在下一步进行预测时使用的就是下一步的状态值,我们将这样的方案称为滚动优化控制(Receding Horizon Control)。

预测控制的优化不是一次离线进行,而是随着采样时刻的前进反复地在线进行,故称为滚动优化。这种滚动优化虽然得不到理想的全局最优解,但是反复对每一采样时刻的偏差进行优化计算,将可及时地校正控制过程中出现的各种复杂情况。

2.4 MPC vs. LQR

从以下几个方面进行阐述:

  • 研究对象:是否线性化
  • 状态方程:是否离散化
  • 目标函数:误差和控制量的极小值
  • 工作时域:预测时域,控制时域,滚动优化,求解一次
  • 求解方法:QP求解器,变分法求解黎卡提方程
  • LQR和MPC的优缺点:滚动优化,求解时域,实时性,算力,工程常用方法

具体可参考博客

3. MPC设计

当模型是线性的时候(非线性系统可以线性化),MPC的设计求解一般使用二次规划方法。

设线性模型为以下形式:

x

k

+

1

=

A

x

k

+

B

u

k

+

C

(1)

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

xk+1=Axk+Buk+C(1)

假定未来

m

m

m步的控制输入已知,为

u

k

,

u

k

+

1

,

u

k

+

2

,

.

.

.

,

u

k

+

m

u_k, u_{k+1}, u_{k+2}, …, u_{k+m}

uk,uk+1,uk+2,,uk+m​,根据以上模型与输入,我们可以计算未来

m

m

m步的状态:

x

k

+

1

=

A

x

k

+

B

u

k

+

C

x

k

+

2

=

A

x

k

+

1

+

B

u

k

+

1

+

C

=

A

(

A

x

k

+

B

u

k

+

C

)

+

B

u

k

+

1

+

C

=

A

2

x

k

+

A

B

u

k

+

B

u

k

+

1

+

A

C

+

C

x

k

+

3

=

A

3

x

k

+

A

2

B

u

k

+

A

B

k

+

1

+

B

u

k

+

2

+

A

2

C

+

A

C

+

C

.

.

.

x

k

+

m

=

A

m

x

k

+

A

m

1

B

u

k

+

A

m

2

B

u

k

+

1

+

.

.

.

+

A

m

i

B

u

k

+

i

1

+

.

.

.

+

B

u

k

+

m

1

+

A

m

1

C

+

A

m

2

C

+

.

.

.

+

C

\begin{aligned} 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}+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+m}&=A^{m}x_{k}+A^{m-1}Bu_k+A^{m-2}Bu_{k+1}+…+A^{m-i}Bu_{k+i-1}+…+Bu_{k+m-1}+A^{m-1}C+A^{m-2}C+…+C \end{aligned}

xk+1xk+2xk+3xk+m=Axk+Buk+C=Axk+1+Buk+1+C=A(Axk+Buk+C)+Buk+1+C=A2xk+ABuk+Buk+1+AC+C=A3xk+A2Buk+ABk+1+Buk+2+A2C+AC+C=Amxk+Am1Buk+Am2Buk+1++AmiBuk+i1++Buk+m1+Am1C+Am2C++C

将上面

m

m

m步写成矩阵向量形式:

X

=

A

x

k

+

B

u

+

C

(2)

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

X=Axk+Bu+C(2)

其中,

X

=

[

x

k

+

1

,

x

k

+

2

,

x

k

+

3

,

.

.

.

x

k

+

m

]

T

\mathcal{X}=\left[x_{k+1}, x_{k+2}, x_{k+3},…x_{k+m}\right]^T

X=[xk+1,xk+2,xk+3,xk+m]T

u

=

[

u

k

,

u

k

+

1

,

u

k

+

2

,

.

.

.

,

u

k

+

m

1

]

T

\mathbf{u}=\left[u_k,u_{k+1},u_{k+2},…,u_{k+m-1}\right]^T

u=[uk,uk+1,uk+2,,uk+m1]T

A

=

[

A

,

A

2

,

A

3

,

.

.

.

,

A

m

]

T

\mathcal{A}=\left[A, A^2 ,A^3 ,… ,A^m\right]^T

A=[A,A2,A3,,Am]T

B

=

(

0

0

.

.

.

0

B

0

.

.

.

0

A

B

B

.

.

.

0

.

.

.

.

.

.

.

.

.

.

.

.

A

m

1

B

A

m

2

B

.

.

.

B

)

\mathcal{B}=\begin{pmatrix}0&0&…&0\\ B&0&…&0\\ AB&B&…&0\\ …&…&…&…\\ A^{m-1}B&A^{m-2}B&…&B\end{pmatrix}

B=

0BABAm1B00BAm2B000B

C

=

[

C

A

C

+

C

A

2

C

+

A

C

+

C

A

k

+

m

1

C

+

+

C

]

\mathcal{C}=\left[\begin{array}{c} C \\ A C+C \\ A^{2} C+A C+C \\ \ldots \\ A^{k+m-1} C+\ldots+C \end{array}\right]

C=

CAC+CA2C+AC+CAk+m1C++C

上式

B

\mathcal{B}

B中的下三角形式,直接反映了系统在时间上的因果关系,即

k

+

1

k+1

k+1时刻的输入对

k

k

k 时刻的输出没有影响,

k

+

2

k+2

k+2 时刻的输入对

k

k

k

k

+

1

k+1

k+1 时刻没有影响。

假定参考轨迹为

X

=

[

x

ˉ

k

+

1

x

ˉ

k

+

2

x

ˉ

k

+

3

x

ˉ

k

+

m

]

T

\overline{\mathcal{X}}=\left[\begin{array}{lllll}\bar{x}_{k+1} & \bar{x}_{k+2} & \bar{x}_{k+3} & \ldots & \bar{x}_{k+m}\end{array}\right]^{T}

X=[xˉk+1xˉk+2xˉk+3xˉk+m]T,则MPC的一个简单的目标代价函数如下:

min

J

=

E

T

Q

E

+

u

T

R

u

s.t. 

u

m

i

n

u

u

m

a

x

(3)

\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}

minJ=ETQE+uTRus.t. uminuumax(3)

其中,

E

=

X

X

=

[

x

k

+

1

x

ˉ

k

+

1

x

k

+

2

x

ˉ

k

+

2

x

k

+

m

x

ˉ

k

+

m

]

T

\mathcal{E}=\mathcal{X}-\overline{\mathcal{X}}=\left[\begin{array}{llll}x_{k+1}-\bar{x}_{k+1} & x_{k+2}-\bar{x}_{k+2} & \ldots & x_{k+m}-\bar{x}_{k+m}\end{array}\right]^{T}

E=XX=[xk+1xˉk+1xk+2xˉk+2xk+mxˉk+m]T

u

T

R

u

\mathbf{u}^T R \mathbf{u}

uTRu这一项是为了让控制输入不会太大,因此代价函数中添加了一项对控制量的约束。

将式(2)代入式(3),则优化变量仅剩

u

\mathbf{u}

u。以上最优化问题可用二次规划方法求解,得到满足目标代价函数的最优控制序列

u

=

{

u

k

,

​​

u

k

+

1

,

​​

u

k

+

2

​​

.

.

.

u

k

+

m

1

}

\mathbf{u}=\left\{u_k,​​u_{k+1},​​u_{k+2}​​…​u_{k+m−1}\right\}

u={uk,​​uk+1,​​uk+2​​…​uk+m1}

当转换成式(3)后,可以利用凸优化库进行二次规划求解,例如python的cvxoptOSQP: An Operator Splitting Solver for Quadratic ProgramsCasdi等。

4. MPC应用——无人车轨迹跟踪

4.1 MPC建模

设车辆的状态量偏差和控制量偏差如式 ( 4 ) 所示:

x

~

=

[

x

˙

x

˙

r

y

˙

y

˙

r

φ

˙

φ

˙

r

]

,

u

~

=

[

v

v

r

δ

δ

r

]

(4)

\tag{4} \tilde{\boldsymbol{x}}=\left[\begin{array}{c} \dot{x}-\dot{x}_{r} \\ \dot{y}-\dot{y}_{r} \\ \dot{\varphi}-\dot{\varphi}_{r} \end{array}\right], \tilde{\boldsymbol{u}}=\left[\begin{array}{c} v-v_{r} \\ \delta-\delta_{r} \end{array}\right]

x~=

x˙x˙ry˙y˙rφ˙φ˙r

,u~=[vvrδδr](4)
基于先前的运动学模型的离散状态空间方程如下,

x

~

(

k

+

1

)

=

[

1

0

T

v

r

sin

φ

r

0

1

T

v

r

cos

φ

r

0

0

1

]

x

~

(

k

)

+

[

T

cos

φ

r

0

T

sin

φ

r

0

T

tan

δ

r

l

T

v

r

l

cos

2

δ

r

]

u

~

(

k

)

=

A

x

~

(

k

)

+

B

u

~

(

k

)

(5)

\tag{5} \tilde{\boldsymbol{x}}(k+1)=\left[\begin{array}{ccc} 1 & 0 & -T v_{r} \sin \varphi_{r} \\ 0 & 1 & T v_{r} \cos \varphi_{r} \\ 0 & 0 & 1 \end{array}\right] \tilde{\boldsymbol{x}}(k)+\left[\begin{array}{cc} T \cos \varphi_{r} & 0 \\ T \sin \varphi_{r} & 0 \\ T \frac{\tan \delta_{r}}{l} & T \frac{v_{r}}{l \cos ^{2} \delta_{r}} \end{array}\right] \tilde{\boldsymbol{u}}(k)=\boldsymbol{A} \tilde{\boldsymbol{x}}(k)+\boldsymbol{B} \tilde{\boldsymbol{u}}(k)

x~(k+1)=

100010TvrsinφrTvrcosφr1

x~(k)+

TcosφrTsinφrTltanδr00Tlcos2δrvr

u~(k)=Ax~(k)+Bu~(k)(5)

为了表示控制系统达到稳定控制所付出的代价,MPC控制的代价函数定义如下:

min

J

(

U

)

=

k

=

0

N

1

(

x

~

(

k

)

T

Q

x

~

(

k

)

+

u

~

(

k

)

T

R

u

~

(

k

)

)

+

x

~

(

N

)

T

Q

f

x

~

(

N

)

(6)

\tag{6} \min J(\boldsymbol{U})=\sum_{k=0}^{N-1}\left(\tilde{\boldsymbol{x}}(k)^{T} Q \tilde{\boldsymbol{x}}(k)+\tilde{\boldsymbol{u}}(k)^{T} R \tilde{\boldsymbol{u}}(k)\right)+\tilde{\boldsymbol{x}}(N)^{T} Q_{f} \tilde{\boldsymbol{x}}(N)

minJ(U)=k=0N1(x~(k)TQx~(k)+u~(k)TRu~(k))+x~(N)TQfx~(N)(6)
其中函数参数

U

=

(

u

0

,

u

1

,

,

u

N

)

U=\left(u_{0}, u_{1}, \ldots, u_{N}\right)

U=(u0,u1,,uN) ,并且矩阵

Q

,

Q

f

,

R

Q, Q_{f}, R

Q,Qf,R 为正定矩阵,即

Q

=

Q

T

0

,

Q

f

=

Q

f

T

0

,

R

=

R

T

>

0

Q=Q^{T} \geq 0, \quad Q_{f}=Q_{f}^{T} \geq 0, \quad R=R^{T}>0

Q=QT0,Qf=QfT0,R=RT>0

Q Q_(f) R
给定状态代价矩阵 最终状态代价矩阵 输入代价矩阵
  • N

    N

    N : 时间范围(Time Horizon)

  • Q

    R

    Q , R

    QR : 分别设定状态偏差和输入的相对权重

  • R

    >

    0

    R>0

    R>0 : 意味着任何非零输入都增加

    J

    J

    J 的代价

  • x

    ~

    (

    k

    )

    T

    Q

    x

    ~

    (

    k

    )

    \tilde{\boldsymbol{x}}(k)^{T} Q \tilde{\boldsymbol{x}}(k)

    x~(k)TQx~(k) : 衡量状态偏差

  • u

    ~

    (

    k

    )

    T

    R

    u

    ~

    (

    k

    )

    \tilde{\boldsymbol{u}}(k)^{T} R \tilde{\boldsymbol{u}}(k)

    u~(k)TRu~(k) : 衡量输入大小

  • x

    ~

    (

    N

    )

    T

    Q

    f

    x

    ~

    (

    N

    )

    \tilde{\boldsymbol{x}}(N)^{T} Q_{f} \tilde{\boldsymbol{x}}(N)

    x~(N)TQfx~(N) : 衡量最终状态偏差

对于公式(6),它需要服从的约束条件包括

{

运动学模型约束——

x

~

(

k

+

1

)

=

A

x

~

(

k

)

+

B

u

~

(

k

)

控制量约束——

u

~

(

k

)

u

~

m

a

x

初始状态——

x

~

(

0

)

=

x

~

0

(7)

\tag{7} \left\{ \begin{aligned} &\text{运动学模型约束——}&\tilde{\boldsymbol{x}}(k+1)=\boldsymbol{A} \tilde{\boldsymbol{x}}(k)+\boldsymbol{B} \tilde{\boldsymbol{u}}(k)\\ &\text{控制量约束——}&\left|\tilde{\boldsymbol{u}}(k)\right| \leq \tilde{\boldsymbol{u}}_{max}\\ &\text{初始状态——}&\tilde{\boldsymbol{x}}(0)=\tilde{\boldsymbol{x}}_0 \end{aligned} \right.

运动学模型约束——控制量约束——初始状态——x~(k+1)=Ax~(k)+Bu~(k)u~(k)u~maxx~(0)=x~0(7)

4.2 python代码实现

完整程序见GitHub仓库

4.2.1 参数

# mpc parameters
NX = 3  # x = x, y, yaw
NU = 2  # u = [v,delta]
T = 8  # horizon length
R = np.diag([0.1, 0.1])  # input cost matrix
Rd = np.diag([0.1, 0.1])  # input difference cost matrix
Q = np.diag([1, 1, 1])  # state cost matrix
Qf = Q  # state final matrix



#车辆
dt=0.1 # 时间间隔,单位:s
L=2 # 车辆轴距,单位:m
v = 2 # 初始速度
x_0=0 # 初始x
y_0=-3 #初始y
psi_0=0 # 初始航向角

MAX_STEER = np.deg2rad(45.0)  # maximum steering angle [rad]
MAX_DSTEER = np.deg2rad(45.0)  # maximum steering speed [rad/s]

MAX_VEL = 2.0  # maximum accel [m/s]

4.2.2 运动学模型

import math


class KinematicModel_3:
  """假设控制量为转向角delta_f和加速度a
  """

  def __init__(self, x, y, psi, v, L, dt):
    self.x = x
    self.y = y
    self.psi = psi
    self.v = v
    self.L = L
    # 实现是离散的模型
    self.dt = dt

  def update_state(self, a, delta_f):
    self.x = self.x+self.v*math.cos(self.psi)*self.dt
    self.y = self.y+self.v*math.sin(self.psi)*self.dt
    self.psi = self.psi+self.v/self.L*math.tan(delta_f)*self.dt
    self.v = self.v+a*self.dt

  def get_state(self):
    return self.x, self.y, self.psi, self.v

    def state_space(self, ref_delta, ref_yaw):
    """将模型离散化后的状态空间表达

    Args:
        ref_delta (_type_): 参考的转角控制量
        ref_yaw (_type_): 参考的偏航角

    Returns:
        _type_: _description_
    """

    A = np.matrix([
        [1.0, 0.0, -self.v*self.dt*math.sin(ref_yaw)],
        [0.0, 1.0, self.v*self.dt*math.cos(ref_yaw)],
        [0.0, 0.0, 1.0]])

    B = np.matrix([
        [self.dt*math.cos(ref_yaw), 0],
        [self.dt*math.sin(ref_yaw), 0],
        [self.dt*math.tan(ref_delta)/self.L, self.v*self.dt /(self.L*math.cos(ref_delta)*math.cos(ref_delta))]
    ])

    C = np.eye(3)
    return A, B, C

4.2.3 参考轨迹

class MyReferencePath:
    def __init__(self):
        # set reference trajectory
        # refer_path包括4维:位置x, 位置y, 轨迹点的切线方向, 曲率k 
        self.refer_path = np.zeros((1000, 4))
        self.refer_path[:,0] = np.linspace(0, 100, 1000) # x
        self.refer_path[:,1] = 2*np.sin(self.refer_path[:,0]/3.0)+2.5*np.cos(self.refer_path[:,0]/2.0) # y
        # 使用差分的方式计算路径点的一阶导和二阶导,从而得到切线方向和曲率
        for i in range(len(self.refer_path)):
            if i == 0:
                dx = self.refer_path[i+1,0] - self.refer_path[i,0]
                dy = self.refer_path[i+1,1] - self.refer_path[i,1]
                ddx = self.refer_path[2,0] + self.refer_path[0,0] - 2*self.refer_path[1,0]
                ddy = self.refer_path[2,1] + self.refer_path[0,1] - 2*self.refer_path[1,1]
            elif i == (len(self.refer_path)-1):
                dx = self.refer_path[i,0] - self.refer_path[i-1,0]
                dy = self.refer_path[i,1] - self.refer_path[i-1,1]
                ddx = self.refer_path[i,0] + self.refer_path[i-2,0] - 2*self.refer_path[i-1,0]
                ddy = self.refer_path[i,1] + self.refer_path[i-2,1] - 2*self.refer_path[i-1,1]
            else:      
                dx = self.refer_path[i+1,0] - self.refer_path[i,0]
                dy = self.refer_path[i+1,1] - self.refer_path[i,1]
                ddx = self.refer_path[i+1,0] + self.refer_path[i-1,0] - 2*self.refer_path[i,0]
                ddy = self.refer_path[i+1,1] + self.refer_path[i-1,1] - 2*self.refer_path[i,1]
            self.refer_path[i,2]=math.atan2(dy,dx) # yaw
            # 计算曲率:设曲线r(t) =(x(t),y(t)),则曲率k=(x'y" - x"y')/((x')^2 + (y')^2)^(3/2).
            # 参考:https://blog.csdn.net/weixin_46627433/article/details/123403726
            self.refer_path[i,3]=(ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2)) # 曲率k计算
            
    def calc_track_error(self, x, y):
        """计算跟踪误差

        Args:
            x (_type_): 当前车辆的位置x
            y (_type_): 当前车辆的位置y

        Returns:
            _type_: _description_
        """
        # 寻找参考轨迹最近目标点
        d_x = [self.refer_path[i,0]-x for i in range(len(self.refer_path))] 
        d_y = [self.refer_path[i,1]-y for i in range(len(self.refer_path))] 
        d = [np.sqrt(d_x[i]**2+d_y[i]**2) for i in range(len(d_x))]
        s = np.argmin(d) # 最近目标点索引


        yaw = self.refer_path[s, 2]
        k = self.refer_path[s, 3]
        angle = normalize_angle(yaw - math.atan2(d_y[s], d_x[s]))
        e = d[s]  # 误差
        if angle < 0:
            e *= -1

        return e, k, yaw, s
        
    def calc_ref_trajectory(self, robot_state, dl=1.0):
        """计算参考轨迹点,统一化变量数组,便于后面MPC优化使用
            参考自https://github.com/AtsushiSakai/PythonRobotics/blob/eb6d1cbe6fc90c7be9210bf153b3a04f177cc138/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py
        Args:
            robot_state (_type_): 车辆的状态(x,y,yaw,v)
            dl (float, optional): _description_. Defaults to 1.0.

        Returns:
            _type_: _description_
        """
        e, k, ref_yaw, ind = self.calc_track_error(robot_state[0], robot_state[1])

        xref = np.zeros((NX, T + 1))
        dref = np.zeros((NU, T)) # 参考控制量
        ncourse = len(self.refer_path)


        xref[0, 0] = self.refer_path[ind,0]
        xref[1, 0] = self.refer_path[ind, 1]
        xref[2, 0] = self.refer_path[ind, 2]

        # 参考控制量[v,delta]
        ref_delta = math.atan2(L*k, 1)
        dref[0, :] = robot_state[3]
        dref[1, :] = ref_delta

        travel = 0.0

        for i in range(T + 1):
            travel += abs(robot_state[3]) * dt
            dind = int(round(travel / dl))

            if (ind + dind) < ncourse:
                xref[0, i] = self.refer_path[ind + dind,0]
                xref[1, i] = self.refer_path[ind + dind,1]
                xref[2, i] = self.refer_path[ind + dind,2]


            else:
                xref[0, i] = self.refer_path[ncourse - 1,0]
                xref[1, i] = self.refer_path[ncourse - 1,1]
                xref[2, i] = self.refer_path[ncourse - 1,2]


        return xref, ind, dref


4.2.4 矩阵拍平

def get_nparray_from_matrix(x):
    return np.array(x).flatten()

4.2.5 角度归一化到[-pi,pi]

def normalize_angle(angle):
    """
    Normalize an angle to [-pi, pi].

    :param angle: (float)
    :return: (float) Angle in radian in [-pi, pi]
    copied from https://atsushisakai.github.io/PythonRobotics/modules/path_tracking/stanley_control/stanley_control.html
    """
    while angle > np.pi:
        angle -= 2.0 * np.pi

    while angle < -np.pi:
        angle += 2.0 * np.pi

    return angle

4.2.6 MPC控制实现

def linear_mpc_control(xref, x0, delta_ref,ugv):
    """
    linear mpc control

    xref: reference point
    x0: initial state
    delta_ref: reference steer angle
    ugv:车辆对象
    """

    x = cvxpy.Variable((NX, T + 1))
    u = cvxpy.Variable((NU, T)) 

    cost = 0.0  # 代价函数
    constraints = []  # 约束条件

    for t in range(T):
        cost += cvxpy.quad_form(u[:, t]-delta_ref[:,t], R)

        if t != 0:
            cost += cvxpy.quad_form(x[:, t] - xref[:, t], Q)

        A, B, C = ugv.state_space(delta_ref[1,t], xref[2,t])
        constraints += [x[:, t + 1]-xref[:, t+1] == A @ (x[:, t]-xref[:, t]) + B @ (u[:, t]-delta_ref[:,t]) ]


    cost += cvxpy.quad_form(x[:, T] - xref[:, T], Qf)

    constraints += [(x[:, 0]) == x0]
    constraints += [cvxpy.abs(u[0, :]) <= MAX_VEL]
    constraints += [cvxpy.abs(u[1, :]) <= MAX_STEER]

    prob = cvxpy.Problem(cvxpy.Minimize(cost), constraints)
    prob.solve(solver=cvxpy.ECOS, verbose=False)

    if prob.status == cvxpy.OPTIMAL or prob.status == cvxpy.OPTIMAL_INACCURATE:
        opt_x = get_nparray_from_matrix(x.value[0, :])
        opt_y = get_nparray_from_matrix(x.value[1, :])
        opt_yaw = get_nparray_from_matrix(x.value[2, :])
        opt_v = get_nparray_from_matrix(u.value[0, :])
        opt_delta = get_nparray_from_matrix(u.value[1, :])

    else:
        print("Error: Cannot solve mpc..")
        opt_v, opt_delta, opt_x, opt_y, opt_yaw = None, None, None, None, None, 

    return opt_v, opt_delta, opt_x, opt_y, opt_yaw


4.2.7 主函数

from celluloid import Camera # 保存动图时用,pip install celluloid
# 使用随便生成的轨迹
def main():

    reference_path = MyReferencePath()
    goal = reference_path.refer_path[-1,0:2]



    # 运动学模型
    ugv = KinematicModel_3(x_0, y_0, psi_0, v, L, dt)
    x_ = []
    y_ = []
    fig = plt.figure(1)
    # 保存动图用
    camera = Camera(fig)
    # plt.ylim([-3,3])
    for i in range(500):
        robot_state = np.zeros(4)
        robot_state[0] = ugv.x
        robot_state[1] = ugv.y
        robot_state[2]=ugv.psi
        robot_state[3]=ugv.v
        x0 = robot_state[0:3]
        xref, target_ind, dref = reference_path.calc_ref_trajectory(robot_state)
        opt_v, opt_delta, opt_x, opt_y, opt_yaw = linear_mpc_control(xref, x0, dref, ugv)
        ugv.update_state(0, opt_delta[0])  # 加速度设为0,恒速

        x_.append(ugv.x)
        y_.append(ugv.y)

        # 显示动图
        plt.cla()
        plt.plot(reference_path.refer_path[:,0], reference_path.refer_path[:,1], "-.b",  linewidth=1.0, label="course")
        plt.plot(x_, y_, "-r", label="trajectory")
        plt.plot(reference_path.refer_path[target_ind,0], reference_path.refer_path[target_ind,1], "go", label="target")
        # plt.axis("equal")
        plt.grid(True)
        plt.pause(0.001)

        # camera.snap()
        # 判断是否到达最后一个点
        if np.linalg.norm(robot_state[0:2]-goal)<=0.1:
            print("reach goal")
            break
    # animation = camera.animate()
    # animation.save('trajectory.gif')

if __name__=='__main__':
    main()

跟踪效果如下:

在这里插入图片描述

(跟踪效果不是很好,我并没有进一步调整,就先这样吧···。)

5. MPC开源库/程序

发表回复

您的电子邮箱地址不会被公开。 必填项已用*标注