MPC功能包梳理

最新推荐文章于 2022-08-21 20:44:50 发布

小朱智能驾驶

最新推荐文章于 2022-08-21 20:44:50 发布

阅读量1.4k

点赞数 2

分类专栏： E100

本文链接：https://blog.youkuaiyun.com/weixin_37532614/article/details/106727942

版权

E100 专栏收录该内容

3 篇文章

订阅专栏

1 MPC 节点输入输出接口

MPC节点接收的参数	参数的物理含义
vector <double> ptsx	全局路径 x
vector <double> ptsy	全局路径 y
double px	当前x坐标
double py	当前y坐标
double psi	当前航向角
double v	当前车辆速度
double delta	当前方向盘转角
double a	当前节气门开度

MPC节点输出的参数	参数的物理含义

2. 车辆模型（约束条件）（状态方程）

考虑道路曲率变化的车辆运动学模型

$x_{t+1} = x_t + v_t cos(\psi_t) * dt \\ y_{t+1} = y_t + v_t sin(\psi_t) * dt \\ \psi_{t+1} = \psi_t + \frac {v_t} { L_f} \delta_t * dt \\ v_{t+1} = v_t + a_t * dtv \\ cte_{t+1} = f(x_t) - y_t + (v_t sin(e\psi_t) dt)cte \\ e\psi_{t+1} = \psi_t - \psi{des}_t + (\frac{v_t} { L_f} \delta_t dt)\\ \text{where:}\psi{des}_t=\arctan\left[f'\left(x_{t}\right)\right]$

状态变量：
$\begin{bmatrix} x\\ y\\ \psi\\ v\\ cte\\ e\psi \end{bmatrix}$

输入变量：

$\begin{bmatrix} \delta\\ a \end{bmatrix}$

3. MPC 控制器使用说明

MPC控制模块的参考轨迹是用xy坐标系下的多项式系数表达的。
- polyfit函数可以使用(x,y)点集进行拟合得到。
- polyeval函数，根据x坐标和多项式系数，拟合出y坐标
实车无法获取节气门开度，因此使用车辆加速度作为节气门开度

4. Ipopt优化求解器构造

4.1 求解器标准形式

$\underset {x ∈ R^n}{min}f(x)\\ s.t. g_L ≤ g(x) ≤ g_U\\ x_L ≤ x ≤ x_U$
where f(x): Rⁿ --> R is the objective function, and g(x): Rⁿ --> Rᵐ are the constraint functions. The vectors g_L and g_U denote the lower and upper bounds on the constraints, and the vectors x_L and x_U are the bounds on the variables x. The functions f(x) and g(x) can be nonlinear and nonconvex, but should be twice continuously differentiable. Note that equality constraints can be formulated in the above formulation by setting the corresponding components of g_L and g_U to the same value.
优化器每次求解完成得到的结果是控制时域内的所有状态变量的值以及控制量的值

4.2 cost function里的项

cross track error
headin error
velocity error
控制信号幅值
控制信号变化率

4.3 约束条件 void operator()(ADvector& fg, const ADvector& vars)

fg：存放目标函数和车辆模型/约束条件(车辆模型是动态规划的等式约束条件）
fg[0] 存放，目标函数，将目标函数的各个组成部分加起来，然后存在这里：cross_error heading_error, velocity_error

车辆模型：
f(x(t))是参考路径
$x_{t+1} = x_t + v_t cos(\psi_t) * dt\\ y_{t+1} = y_t + v_t sin(\psi_t) * dt\\ \psi_{t+1} = \psi_t + \frac {v_t} { L_f} \delta_t * dt\\ v_{t+1} = v_t + a_t * dt\\ cte_{t+1} = f(x_t) - y_t + (v_t sin(e\psi_t) dt)\\ e\psi_{t+1} = \psi_t - \psi{des}_t + (\frac{v_t} { L_f} \delta_t dt)$

vars：存放目标函数和模型需要用到的参数
$\psi, v, cte, e\psi]$
$[\delta, a]$
vars 是一个长的一维向量，当N的值为25的时候，
$\longrightarrow x_1, ..., x_{25}}\\ vars[25], ..., vars[49] \longrightarrow y_1, ..., y_{25}\\ vars[50], ..., vars[74] \longrightarrow \psi_1, ..., \psi_{25}\\ vars[75], ..., vars[99] \longrightarrow v_1, ..., v_{25}\\ vars[100], ..., vars[124] \longrightarrow cte_1, ..., cte_{25}\\ vars[125], ..., vars[149] \longrightarrow e\psi_1, ..., e\psi_{25}\\ vars[150], ..., vars[173] \longrightarrow \delta_1, ..., \delta_{24}\\ vars[174], ..., vars[197] \longrightarrow a_1, ..., a_{24}\\$

目标函数

 // The part of the cost based on the reference state.
 // minimize our cross track, heading, and velocity errors
    for (int t = 0; t < N; t++) {
      fg[0] += CppAD::pow(vars[cte_start + t], 2);
      fg[0] += CppAD::pow(vars[epsi_start + t], 2);
      fg[0] += CppAD::pow(vars[v_start + t] - ref_v, 2);
    }

    // Minimize the use of actuators.
    // A further enhancement is to constrain erratic control inputs.
    //For example, if we're making a turn, we'd like the turn to be smooth, not sharp. Additionally, the vehicle velocity should not change too radically
    for (int t = 0; t < N - 1; t++) {
      fg[0] += CppAD::pow(vars[delta_start + t], 2);
      fg[0] += CppAD::pow(vars[a_start + t], 2);
    }

    // Minimize the value gap between sequential actuations.
    // The goal of this final loop is to make control decisions more consistent, or smoother. The next control input should be similar to the current one.
    for (int t = 0; t < N - 2; t++) {
      fg[0] += CppAD::pow(vars[delta_start + t + 1] - vars[delta_start + t], 2);
      fg[0] += CppAD::pow(vars[a_start + t + 1] - vars[a_start + t], 2);
    }