对于一般的离散化系统(因为实际计算机实现的控制系统都是离散的系统,连续系统可以进行离散化操作),在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);
设线性模型为以下形式:
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+3...xk+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+Am−1Buk+Am−2Buk+1+...+Am−iBuk+i−1+...+Buk+m−1+Am−1C+Am−2C+...+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+m−1]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=⎝⎛0BAB...Am−1B00B...Am−2B...............000...B⎠⎞
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+C…Ak+m−1C+…+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+3…xˉ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. umin≤u≤umax(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=X−X=[xk+1−xˉk+1xk+2−xˉk+2…xk+m−xˉ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+m−1}。
当转换成式(3)后,可以利用凸优化库进行二次规划求解,例如python的cvxopt,OSQP: An Operator Splitting Solver for Quadratic Programs,Casdi等。
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~=[v−vrδ−δ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)=⎣⎡100010−Tvrsinφ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=0∑N−1(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=QT≥0,Qf=QfT≥0,R=RT>0
Q
Q_(f)
R
给定状态代价矩阵
最终状态代价矩阵
输入代价矩阵
N
N
N : 时间范围(Time Horizon)
Q
,
R
Q , R
Q,R : 分别设定状态偏差和输入的相对权重
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
classKinematicModel_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
defupdate_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
defget_state(self):return self.x, self.y, self.psi, self.v
defstate_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 参考轨迹
classMyReferencePath: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 inrange(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计算defcalc_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 inrange(len(self.refer_path))]
d_y =[self.refer_path[i,1]-y for i inrange(len(self.refer_path))]
d =[np.sqrt(d_x[i]**2+d_y[i]**2)for i inrange(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 *=-1return e, k, yaw, s
defcalc_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.0for i inrange(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