关于MPC控制算法,在控制部分,已经有详细的MPC算法的详细解释和说明,这里就不再多说了,可以自行查阅。
这里主要针对Apollo中,现有的部分进行一下相关的说明。
MPC使用的模型
Apollo中使用的是车辆动力学模型,具体内容请参考这里。
横纵向的综合动力学模型,其控制的状态量就是在单横向的基础上,加上了 station_error,speed_error两个量。
m
a
t
r
i
x
−
s
t
a
t
e
=
[
l
a
t
e
r
a
l
−
e
r
r
o
r
l
a
t
e
r
a
l
−
e
r
r
o
r
−
r
a
t
e
h
e
a
d
i
n
g
−
e
r
r
o
r
h
e
a
d
i
n
g
−
e
r
r
o
r
−
r
a
t
e
s
t
a
t
i
o
n
−
e
r
r
o
r
s
p
e
e
d
−
e
r
r
o
r
]
matrix_-state = \begin{bmatrix} lateral_-error \\ lateral_-error_-rate \\ heading_-error \\ heading_-error_-rate \\ station_-error \\ speed_-error \\ \end{bmatrix}
matrix−state=⎣⎢⎢⎢⎢⎢⎢⎡lateral−errorlateral−error−rateheading−errorheading−error−ratestation−errorspeed−error⎦⎥⎥⎥⎥⎥⎥⎤
具体这六个状态量,在纵向控制和横向控制的代码里面均有详细的说明,可以自行查阅下。
控制变量有2个:
m
a
t
r
i
x
−
c
o
n
t
r
o
l
=
[
δ
f
a
]
matrix_-control=\begin{bmatrix} \delta_f \\ a \\ \end{bmatrix}
matrix−control=[δfa]
其中,
δ
f
\delta_f
δf为前轮转角,
a
a
a为车辆加速度。
加上之前的,动力学模型,结合Apollo中的相关代码可知:
d
d
t
[
l
a
t
e
r
a
l
−
e
r
r
o
r
l
a
t
e
r
a
l
−
e
r
r
o
r
−
r
a
t
e
h
e
a
d
i
n
g
−
e
r
r
o
r
h
e
a
d
i
n
g
−
e
r
r
o
r
−
r
a
t
e
s
t
a
t
i
o
n
−
e
r
r
o
r
s
p
e
e
d
−
e
r
r
o
r
]
=
[
0
1
0
0
0
0
0
−
2
C
a
f
+
2
C
a
r
m
V
2
C
a
f
+
2
C
a
r
m
−
2
C
a
f
ℓ
f
−
2
C
a
r
ℓ
r
m
V
0
0
0
0
0
1
0
0
0
−
2
C
a
f
ℓ
f
−
2
C
a
r
ℓ
r
I
z
V
2
C
a
f
ℓ
f
−
2
C
a
r
ℓ
r
I
z
−
2
C
a
f
ℓ
f
2
+
2
C
a
r
ℓ
r
2
I
z
V
0
0
0
0
0
0
0
1
0
0
0
0
0
0
]
[
l
a
t
e
r
a
l
−
e
r
r
o
r
l
a
t
e
r
a
l
−
e
r
r
o
r
−
r
a
t
e
h
e
a
d
i
n
g
−
e
r
r
o
r
h
e
a
d
i
n
g
−
e
r
r
o
r
−
r
a
t
e
s
t
a
t
i
o
n
−
e
r
r
o
r
s
p
e
e
d
−
e
r
r
o
r
]
+
[
0
0
2
C
a
f
m
0
0
0
2
C
a
f
ℓ
f
I
z
0
0
0
0
−
1
]
[
δ
f
a
]
+
[
0
−
V
−
2
C
a
f
ℓ
f
−
2
C
a
r
ℓ
r
m
V
0
−
2
C
a
f
ℓ
f
2
+
2
C
a
r
ℓ
r
2
I
z
V
0
1
]
φ
˙
(1)
\frac{d}{dt} \begin{bmatrix} lateral_-error \\ lateral_-error_-rate \\ heading_-error \\ heading_-error_-rate \\ station_-error \\ speed_-error \\ \end{bmatrix} \\ = \begin{bmatrix} 0 && 1 && 0 && 0 && 0 && 0 \\ 0 && -\frac{2C_{af}+2C_{ar}}{mV} && \frac{2C_{af}+2C_{ar}}{m} && - \frac{2C_{af}\ell_{f}-2C_{ar}\ell_{r}}{mV} && 0 && 0 \\ 0 && 0 && 0 && 1 && 0 && 0 \\ 0 && -\frac{2C_{af}\ell_{f}-2C_{ar}\ell_{r}}{I_{z}V} && \frac{2C_{af}\ell_{f}-2C_{ar}\ell_{r}}{I_{z}} && -\frac{2C_{af}\ell_{f}^{2}+2C_{ar}\ell_{r}^{2}}{I_{z}V} && 0 && 0 \\ 0 && 0 && 0 && 0 && 0 && 1 \\ 0 && 0 && 0 && 0 && 0 && 0 \\ \end{bmatrix}\begin{bmatrix} lateral_-error \\ lateral_-error_-rate \\ heading_-error \\ heading_-error_-rate \\ station_-error \\ speed_-error \\ \end{bmatrix} \\ +\begin{bmatrix} 0 && 0 \\ \frac{2C_{af}}{m} && 0 \\ 0 && 0 \\ \frac{2C_{af}\ell_{f}}{I_{z}} && 0 \\ 0 && 0 \\ 0 && -1 \\ \end{bmatrix} \begin{bmatrix} \delta_f \\ a \\ \end{bmatrix}+ \begin{bmatrix} 0 \\ -V- \frac{2C_{af}\ell_{f}-2C_{ar}\ell_{r}}{mV} \\ 0 \\ -\frac{2C_{af}\ell_{f}^{2}+2C_{ar}\ell_{r}^{2}}{I_{z}V} \\ 0 \\ 1 \\ \end{bmatrix}\dot{\varphi} \tag{1}
dtd⎣⎢⎢⎢⎢⎢⎢⎡lateral−errorlateral−error−rateheading−errorheading−error−ratestation−errorspeed−error⎦⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎢⎡0000001−mV2Caf+2Car0−IzV2Cafℓf−2Carℓr000m2Caf+2Car0Iz2Cafℓf−2Carℓr000−mV2Cafℓf−2Carℓr1−IzV2Cafℓf2+2Carℓr200000000000010⎦⎥⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎡lateral−errorlateral−error−rateheading−errorheading−error−ratestation−errorspeed−error⎦⎥⎥⎥⎥⎥⎥⎤+⎣⎢⎢⎢⎢⎢⎢⎡0m2Caf0Iz2Cafℓf0000000−1⎦⎥⎥⎥⎥⎥⎥⎤[δfa]+⎣⎢⎢⎢⎢⎢⎢⎢⎡0−V−mV2Cafℓf−2Carℓr0−IzV2Cafℓf2+2Carℓr201⎦⎥⎥⎥⎥⎥⎥⎥⎤φ˙(1)
式中,
C
f
,
C
r
C_f,C_r
Cf,Cr分别为前、后轮的侧偏刚度,
ℓ
f
,
ℓ
r
\ell_f,\ell_r
ℓf,ℓr分别为前、后悬长度,
m
m
m为车身质量,
V
V
V为车身速度,
I
z
I_z
Iz为车辆绕z轴的转动惯量,
φ
˙
\dot{\varphi}
φ˙为车辆
y
a
w
yaw
yaw角度的变化率,也就是横摆角速度。
Apollo中使用的车身坐标系为,车辆右侧为x轴正向,车头前向为y轴正向,z轴垂直向上。
将式(1)化为线性形式
x
(
k
+
1
)
=
A
x
(
k
)
+
B
u
(
k
)
+
C
(2)
x(k+1)=Ax(k)+Bu(k)+C \tag{2}
x(k+1)=Ax(k)+Bu(k)+C(2)
式中,
A
=
[
0
1
0
0
0
0
0
−
2
C
a
f
+
2
C
a
r
m
V
2
C
a
f
+
2
C
a
r
m
−
2
C
a
f
ℓ
f
−
2
C
a
r
ℓ
r
m
V
0
0
0
0
0
1
0
0
0
−
2
C
a
f
ℓ
f
−
2
C
a
r
ℓ
r
I
z
V
2
C
a
f
ℓ
f
−
2
C
a
r
ℓ
r
I
z
−
2
C
a
f
ℓ
f
2
+
2
C
a
r
ℓ
r
2
I
z
V
0
0
0
0
0
0
0
1
0
0
0
0
0
0
]
,
A=\begin{bmatrix} 0 && 1 && 0 && 0 && 0 && 0 \\ 0 && -\frac{2C_{af}+2C_{ar}}{mV} && \frac{2C_{af}+2C_{ar}}{m} && - \frac{2C_{af}\ell_{f}-2C_{ar}\ell_{r}}{mV} && 0 && 0 \\ 0 && 0 && 0 && 1 && 0 && 0 \\ 0 && -\frac{2C_{af}\ell_{f}-2C_{ar}\ell_{r}}{I_{z}V} && \frac{2C_{af}\ell_{f}-2C_{ar}\ell_{r}}{I_{z}} && -\frac{2C_{af}\ell_{f}^{2}+2C_{ar}\ell_{r}^{2}}{I_{z}V} && 0 && 0 \\ 0 && 0 && 0 && 0 && 0 && 1 \\ 0 && 0 && 0 && 0 && 0 && 0 \\ \end{bmatrix},
A=⎣⎢⎢⎢⎢⎢⎢⎢⎡0000001−mV2Caf+2Car0−IzV2Cafℓf−2Carℓr000m2Caf+2Car0Iz2Cafℓf−2Carℓr000−mV2Cafℓf−2Carℓr1−IzV2Cafℓf2+2Carℓr200000000000010⎦⎥⎥⎥⎥⎥⎥⎥⎤,
B
=
[
0
0
2
C
a
f
m
0
0
0
2
C
a
f
ℓ
f
I
z
0
0
0
0
−
1
]
,
B=\begin{bmatrix} 0 && 0 \\ \frac{2C_{af}}{m} && 0 \\ 0 && 0 \\ \frac{2C_{af}\ell_{f}}{I_{z}} && 0 \\ 0 && 0 \\ 0 && -1 \\ \end{bmatrix},
B=⎣⎢⎢⎢⎢⎢⎢⎡0m2Caf0Iz2Cafℓf0000000−1⎦⎥⎥⎥⎥⎥⎥⎤,
C
=
[
0
−
V
−
2
C
a
f
ℓ
f
−
2
C
a
r
ℓ
r
m
V
0
−
2
C
a
f
ℓ
f
2
+
2
C
a
r
ℓ
r
2
I
z
V
0
1
]
C=\begin{bmatrix} 0 \\ -V- \frac{2C_{af}\ell_{f}-2C_{ar}\ell_{r}}{mV} \\ 0 \\ -\frac{2C_{af}\ell_{f}^{2}+2C_{ar}\ell_{r}^{2}}{I_{z}V} \\ 0 \\ 1 \\ \end{bmatrix}
C=⎣⎢⎢⎢⎢⎢⎢⎢⎡0−V−mV2Cafℓf−2Carℓr0−IzV2Cafℓf2+2Carℓr201⎦⎥⎥⎥⎥⎥⎥⎥⎤
代码中线性化系数(双线性变换离散法):
matrix_ad_ = (matrix_i - ts_ * 0.5 * matrix_a_).inverse() * (matrix_i + ts_ * 0.5 * matrix_a_);
matrix_bd_ = matrix_b_ * ts_;
A
~
=
(
I
−
t
s
A
/
2
)
−
1
(
I
+
t
s
A
/
2
)
B
~
=
B
t
s
C
~
=
t
s
C
φ
˙
(3)
\widetilde{A}=(I-t_sA/2)^{-1}(I+t_sA/2) \\ \widetilde{B}=Bt_s \\ \widetilde{C}=t_sC\dot{\varphi} \tag{3}
A
=(I−tsA/2)−1(I+tsA/2)B
=BtsC
=tsCφ˙(3)
另一种离散化方法连续系统离散化方法,我没有仔细看,后期再说吧。
模型预测控制模型的部分大体上就这些。
增益系数的插值
由于车辆运行时速不是按照一个固定值,所以在不同的车速情况下,会选择不同的系数,Apollo中主要对四个参数进行了插值补偿:
- 侧向误差
- 朝向误差
- 方向盘前馈
- 方向盘控制矩阵权重
见代码
matrix_q_updated_(0, 0) =
matrix_q_(0, 0) *
lat_err_interpolation_->Interpolate(VehicleStateProvider::Instance()->linear_velocity());
matrix_q_updated_(2, 2) =
matrix_q_(2, 2) *
heading_err_interpolation_->Interpolate(VehicleStateProvider::Instance()->linear_velocity());
steer_angle_feedforwardterm_updated_ =
steer_angle_feedforwardterm_ *
feedforwardterm_interpolation_->Interpolate(VehicleStateProvider::Instance()->linear_velocity());
matrix_r_updated_(0, 0) =
matrix_r_(0, 0) *
steer_weight_interpolation_->Interpolate(VehicleStateProvider::Instance()->linear_velocity());
个人也很认同这种思路,但是Apollo中control_conf.pb.txt文件中的增益系数,设置的感觉有点不太好,还是需要自己根据实际工程情况去进行相关标定的。
求解器
目前用的比较多的求解器,
- qpOASES
- OSQP
- ceres
- Ipopt
之前用过Ipopt,感觉挺好使的,按照给定的接口就可以很快的求解,Apollo中现在使用的是OSQP,代码如下:
MpcOsqp::MpcOsqp(const Eigen::MatrixXd &matrix_a,
const Eigen::MatrixXd &matrix_b,
const Eigen::MatrixXd &matrix_q,
const Eigen::MatrixXd &matrix_r,
const Eigen::MatrixXd &matrix_initial_x,
const Eigen::MatrixXd &matrix_u_lower,
const Eigen::MatrixXd &matrix_u_upper,
const Eigen::MatrixXd &matrix_x_lower,
const Eigen::MatrixXd &matrix_x_upper,
const Eigen::MatrixXd &matrix_x_ref, const int max_iter,
const int horizon, const double eps_abs)
: matrix_a_(matrix_a),
matrix_b_(matrix_b),
matrix_q_(matrix_q),
matrix_r_(matrix_r),
matrix_initial_x_(matrix_initial_x),
matrix_u_lower_(matrix_u_lower),
matrix_u_upper_(matrix_u_upper),
matrix_x_lower_(matrix_x_lower),
matrix_x_upper_(matrix_x_upper),
matrix_x_ref_(matrix_x_ref),
max_iteration_(max_iter),
horizon_(horizon),
eps_abs_(eps_abs) {
state_dim_ = matrix_b.rows();
control_dim_ = matrix_b.cols();
ADEBUG << "state_dim" << state_dim_;
ADEBUG << "control_dim_" << control_dim_;
num_param_ = state_dim_ * (horizon_ + 1) + control_dim_ * horizon_;
}
在求解MPC时,Apollo封装好了OSQP的类,这里直接调用就行,具体的用法
apollo::common::math::MpcOsqp mpc_osqp(
matrix_ad_, matrix_bd_, matrix_q_updated_, matrix_r_updated_,
matrix_state_, lower_bound, upper_bound, lower_state_bound,
upper_state_bound, reference_state, mpc_max_iteration_, horizon_,
mpc_eps_);
if (!mpc_osqp.Solve(&control_cmd))
{
AERROR << "MPC OSQP solver failed";
}
else
{
ADEBUG << "MPC OSQP problem solved! ";
control[0](0, 0) = control_cmd.at(0);
control[0](1, 0) = control_cmd.at(1);
}
我们需要做的就是计算好对应的参数,把值付给求解器即可,
- matrix_ad_ – 离散化后的系数A
- matrix_bd_ – 离散化后的系数B
- matrix_q_updated_ – 权重q矩阵
- matrix_r_updated_ – 权重r矩阵
- matrix_state_ – 当前时刻的状态矩阵
- lower_bound – 控制量下限
- upper_bound – 控制量上限
- lower_state_bound – 状态量下限
- upper_state_bound – 状态量上限
- reference_state – 参考状态量
- mpc_max_iteration_ – 求解器最大求解迭代次数
- horizon_ – 预测步长
- mpc_eps_ – 预测周期
这里对reference_state为零做一下说明,我们选取的模型是误差模型,所以当然希望最终的误差为零。
具体的OSQP网上资料有很多,搜一下就好。