四軸軌跡規劃-Minimum snap總結

理論知識

參考：1. kummar 2011年 論文 Minimum Snap Trajectory Generation and Control for Quadrotors.
2. 香港科技大沈邵劫老師的相關論文；
3. 深藍學院 高飛老師課件。
微分平坦：系統狀態可以被平坦空間的狀態以及他們的導數所組成的代數組合所表示。

系統狀態12維，首先可以看出 x,y,z 和 dot_x, dot_y, dot_z， 以及 \phi , \theta, \psi和 \omega_x \omega_y \omega_z之間的導數關係，所以整個系統可以先規劃成6個，然後無人機的角度和\dot\dot_x相關，所以，最終可以縮減爲4個輸入，x,y,z和\psi

Minimum jerk: minimize angular velocity, good for visual tracking
Minimum snap: minimize differential thrust, save energy
注：如果是針對minimum jerk，則需要提供位置，速度，角速度3個狀態量。如果只考慮起點和終點，則有23個狀態。同時考慮多項式形式包含有x_{0}(也就是常數項），所以最終的N=5
同樣的道理，如果minimum snap，則需要提供位置，速度，加速度，以及角速度，4個狀態量。所以N = 24-1 = 7
注2： 如果考慮多段的情況，例如K段，則minimum jerk ，需要 (k-1) +3 +3= k+5 , 這裏只要求能夠連續的到達中間點，至於以怎樣的速度，怎樣的加速度到達這個點，是優化出來的，不屬於約束 。
假設每一段的階數爲N ,則每一段軌跡所能提供的自由度爲爲N+1。N階多項式可以提供N次導數，加上原多項式，即爲N+1。 所以，總計 （N+1）*k.
(N+1)*k>= k+5. 則 N_{min} = 5/k. 表明段數越多，則提供的階次越低。
每一個分段都是多項式；每個分段的多項式都是相同的階次，這樣對於問題的求解比較簡單；每一段的時間間隔都是已知的

上圖顯示了，對於Minimum snap問題的限制條件：初始條件以及結束條件的限制（包含有位置，速度，加速度等）；中間節點連續（包含有位置，速度，加速度）

優化過程包含有等式約束與導數約束：1.等式約束，start point 以及 end point的條件。只和當前段相關

2. 導數約束, 在中間點滿足位置連續，速度連續，加速度連續，jerk連續。和前後兩段相關

所以，最後minimum snap變成了一種二次型QP求解問題，典型的凸優化問題
Disciplined convex optimization programs

實踐過程

1. 在ROS中，設定起始點，終止點和中間點的座標。
2. 通過C++/ROS 計算出最有軌跡。其實生成的爲多項式的係數矩陣以及對應的每一段的時間點。
3. 然後我將係數矩陣以及對應的時間點導入到Matlab中畫圖，可以看到整個軌跡的樣子。一個簡單的圓形的軌跡如下圖所示。
   
4. 同樣的方法，新建一個ROS_node，然後類似matlab程序，將生成的軌跡按照軌跡時間，發送到mavros中，完成無人機按照目標軌跡的跟蹤問題。
5. 最終實現樣例程序，爲無人機首先起飛到空中固定座標點。然後發送消息，完成起飛動作。最後無人機再次按照minimum snap生成的軌跡，完成飛行。視頻截圖如下圖所示。視頻鏈接傳送門。