2021.07.08-模型预测轨迹生成

本文代码来自于开源项目Cpprobotics中的model_predictive_trajectory_generator。

目的：学习基于模型预测的轨迹生成算法。为进一步学习无人车轨迹优化打基础。

 

model_predictive_trajectory_generator 包含两个文件：

-------motion_model.h 

-------trajectory_optimizer.h

 

主要Class：

1. State类

成员包括x,y,yaw,v

2. TrajState 类

成员包括x,y,yaw

这里没有限制终点的速度。

这个BVP问题因为是车模型，不存在闭式解，如果求最优解，用Nlopt应该也可以的，有空试一下。

3. Traj类

using Traj = std::vector<TrajState>;

4. MotionModel类

成员变量base_l(轴距)，ds(航迹距离), State(初始状态)

 

 

 

有四个成员函数：

update--根据状态，时间间隔，计算下一个State;

generate_trajectory，根据参数Parameter,计算轨迹，轨迹就是一个State的集合

5. Parameter类

float distance;

//不可以扩容，属于固定大小的数组。

std::array<float, 3> steering_sequence{{0,0,0}};

6. TrajectoryOptimizer类

核心类

 

main函数主要过程：

1. 设定起点和终点状态

State init_state(0, 0, 0, CONST_V);
TrajState target_(5, 2.0, 0);

2. 初始化MotionModel

MotionModel m_model(L, DS, init_state);

3. 声明Parameter

Parameter p_(6, {{0,0,0}});

4. 初始化optimizier及求解

 //一些优化参数
float cost_th_ = 0.1;
 std::vector<float> h_step_{0.2, 0.005, 0.005};
 int max_iter = 100;
//构造
 TrajectoryOptimizer traj_opti_obj(m_model, p_, target_);
//求解
 Traj traj = traj_opti_obj.optimizer_traj(max_iter, cost_th_, h_step_, true, true);

 

求解细节：

1.主要过程：

• 根据起点，终点，初始参数Parameter，计算出一条采样轨迹；
• 求采样轨迹终点跟实际终点的偏差，更新参数Patameter;
• 用新的参数Parameter生成一条采样轨迹。
• 循环迭代

　　如下图所示，迭代了4次就满足阈值要求了：

 

2.定义一段轨迹

这段轨迹通过迭代，不断跟终点接近，最终停止

Traj sample_traj;

3.循环迭代max_iter次

for(int i=0; i<max_iter; i++) {
　　//p是参数Parameter,初始为{0，0，0}
　　sample_traj = m_model.generate_trajectory(p);
　　//sample_traj终点和真正的终点之间的位置和角度差值
　　TrajState dc = calc_diff(sample_traj.back());
　　//这里判断一下，满足阈值的话，就可以直接返回sample_traj
　　
　　//这个偏差很重要，需要用来计算并更新Parameter p
　　Eigen::Vector3f dc_vct;
　　dc_vct<< dc.x , dc.y, dc.yaw;
　　
　　//计算J，这个J是什么含义
　　Eigen::Matrix3f J = calc_J(h_step);
　　Eigen::Vector3f dp = - J.inverse() * dc_vct;

　　//计算alpha
　　float alpha = selection_learning_param(dp);
　　
　　//用alpha更新Parameter,然后p可以用于下一次迭代生成sample_traj。
　　p.distance += alpha * dp(0);
　　p.steering_sequence[1] += alpha * dp(1);
　　p.steering_sequence[2] += alpha * dp(2);
　
}

 

缺点：

对于起点状态角度任意时，不一定能求解出来！