一. 軌跡規划是什么？
在機器人導航過程中，如何控制機器人從A點移動到B點，通常稱之為運動規划。運動規划一般又分為兩步：
1、路徑規划：在地圖（柵格地圖、四\八叉樹、RRT地圖等）中搜索一條從A點到B點的路徑，由一系列離散的空間點（waypoint）組成。
2、軌跡規划：由於路徑點可能比較稀疏、而且不平滑，為了能更好的控制機器人運動，需要將稀疏的路徑點變成平滑的曲線或稠密的軌跡點，也就是軌跡。
2. 軌跡是什么？注意低次項在前
軌跡一般用n階多項式(polynomial)來表示，即
其中p0,p1,...,pn為軌跡參數（n+1個），設參數向量p=[p0,p1,...,pn]T，則軌跡可以寫成向量形式。
對於任意時刻t，可以根據參數計算出軌跡的位置P(osition)，速度V(elocity)，加速度A(cceleration)，jerk，snap等。
一個多項式曲線過於簡單，一段復雜的軌跡很難用一個多項式表示，所以將軌跡按時間分成多段，每段各用一條多項式曲線表示，形如：
k為軌跡的段數，
為第i段軌跡的參數向量。
此外，實際問題中的軌跡往往是二維、三維甚至更高維，通常每個維度單獨求解軌跡。
3. Minimum Snap軌跡規划
軌跡規划的目的：求軌跡的多項式參數p1,...,pk。
我們可能希望軌跡滿足一系列的約束條件，比如：希望設定起點和終點的位置、速度或加速度，希望相鄰軌跡連接處平滑（位置連續、速度連續等），希望軌跡經過某些路徑點，設定最大速度、最大加速度等，甚至是希望軌跡在規定空間內（Obstruction check）等等。
通常滿足約束條件的軌跡有無數條，而實際問題中，往往需要一條特定的軌跡，所以又需要構建一個最優的函數，在可行的軌跡中找出“最優”的那條特定的軌跡。 所以，我們將問題建模（fomulate）成一個約束優化問題，形如：
這樣，就可以通過最優化的方法求解出目標軌跡參數p。注意：這里的軌跡參數p是多端polynomial組成的大參數向量
我們要做的就是：將優化問題中的f(p)函數和Aeq,beq,Aieq,bieq參數給列出來，然后丟到優化器中求解軌跡參數p。
Minimum Snap顧名思義，Minimum Snap中的最小化目標函數是Snap（加速度的二階導），當然你也可以最小化Acceleration（加速度）或者Jerk（加速度的導數），至於它們之間有什么區別，quora上有討論。一般不會最小化速度。
4. 一個簡單的例子
給定包含起點終點在內的k+1個二維路徑點pt0,pt1,...,ptk，pti=(xi,yi)，給定起始速度和加速度為v0,a0，末端加速度為ve,ae，給定時間T，規划出經過所有路徑點的平滑軌跡。
a. 初始軌跡分段與時間分配
根據路徑點，將軌跡分為k段，計算每段的距離，按距離平分時間T(勻速時間分配)，得到時間序列t0,t1,...,tk。對x,y維度單獨規划軌跡。后面只討論一個維度。
時間分配的方法：勻速分配或梯形分配，假設每段polynomial內速度滿足勻速或梯形速度變化，根據每段的距離將總時間T分配到每段。
這里的軌跡分段和時間分配都是初始分配，在迭代算法中，如果 Obstruction check和 feasibility check不滿足條件，會插點或增大某一段的時間。
b. 構建優化函數
Minimum Snap的優化函數為：
其中，
注意：r,c為矩陣的行索引和列索引，索引從0開始，即第一行 r=0。
可以看到，問題建模成了一個數學上的二次規划（Quadratic Programming，QP）問題。
c. 構建等式約束方程
- 設定某一個點的位置、速度、加速度或者更高為一個特定的值，可以構成一個等式約束。
例如：
由於要過中間點，對中間點的位置也構建等式約束，方法同上。
2. 相鄰段之間的位置、速度、加速度連續可以構成一個等式約束
例如第i、i+1段的位置連續構成的等式約束為
合並所有等式約束，得到
等式約束個數=3（起始PVA）+k-1（中間點的p）+3（終點pva）+3(k-1)（中間點PVA連續）=4k+2。
d. 構建不等式約束
不等式約束與等式約束類似，也是設置某個點的P、V、A小於某一特定值，從而構建Aieqp=bieq
，不等式約束一般是在corridor中用的比較多，這里暫時先不使用不等式約束。
e. 求解
利用QP求解器進行求解，在MATLAB中可以使用quadprog() 函數，C++的QP求解器如OOQP，也可以自己去網上找。
參考資料：
1、深藍學院
2、https://blog.csdn.net/q597967420/article/details/76099491