本文程式碼來自於開源專案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類

4. MotionModel類

成員變數base_l(軸距)，ds(航跡距離), State(初始狀態)

有四個成員函式：

update--根據狀態，時間間隔，計算下一個State;

generate_trajectory，根據引數Parameter,計算軌跡，軌跡就是一個State的集合

5. Parameter類

float distance;

//不可以擴容，屬於固定大小的陣列。

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);
　
}

缺點：

對於起點狀態角度任意時，不一定能求解出來！