本文代码来自于开源项目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); }