一、背景意義

??無人機在執(zhí)行一段飛行任務或者在一段簡單的飛行中應用到軌跡規(guī)劃器，所飛行的軌跡應該是平滑的、安全的、動態(tài)可行的。然而，無人機通過路徑規(guī)劃器得到的軌跡是一段段折線軌跡，而且有時路徑規(guī)劃的算法（如A*、RRT、RRT*等）未考慮到機體本身的大小、執(zhí)行軌跡的能力和環(huán)境的因素，使得軌跡非?？拷系K物，實際飛行中可能會碰撞障礙物。這時，需要對得到的軌跡進行二次優(yōu)化，使得軌跡能夠適合無人機的自主移動，不會產(chǎn)生速度、高階動力學狀態(tài)突變，同時也可以節(jié)約飛行時的能源。

二、前提

??軌跡一般用n階多項式進行表示，即：

其中有p0到pn共（n+1）個軌跡參數(shù)，設參數(shù)向量p=[p0,p1...,pn]可以將其寫成向量的形式：

接下來對于任意時刻t，可以根據(jù)參數(shù)計算出軌跡的位置P，速度v，加速度a，加加速度jerk，加加加速度snap等。

接著對軌跡按時間進行分段，這樣更好表達一段復雜的多項式，每段各用一條多項式曲線表示：

說明，在實際問題中軌跡往往在二維、三維甚至高維，這時需要對每個維度進行單獨求解。

三、Minimum Snap Optimization

?在進行軌跡優(yōu)化時往往需要軌跡滿足一系列約束，比如：得到軌跡需要滿足相鄰連接處平滑（位置、速度、加速度等連續(xù)），軌跡需要滿足經(jīng)歷某些路徑節(jié)點，軌跡需要滿足在規(guī)定的走廊范圍內(nèi)等。然而，滿足約束的軌跡有無數(shù)條，實際問題中，只需要一條最優(yōu)的軌跡，因此需要構(gòu)建一個最優(yōu)函數(shù)，將問題建模成一個約束優(yōu)化問題，形如：

其中f(p)為需要優(yōu)化的函數(shù)，s.t.為等式約束和不等式約束。這樣可以通過最優(yōu)求解得到軌跡參數(shù)p。Minimum Snap顧名思義就是最小化加加加速度，即

在無人機中Snap表示差動推力，也就是在給定的時間內(nèi)平滑地通過關(guān)鍵幀，同時保持在安全走廊內(nèi)。

四、基本步驟

1.將軌跡按照時間進行分段

將無人機的狀態(tài)用m個時間間隔的n階分段多項式函數(shù)表達：

需注意，時間應該怎樣分配才能獲得更加的軌跡，常用的方法有勻速分配或梯形分配，假設每段軌跡內(nèi)速度滿足勻速或梯形速度變化，根據(jù)每段的距離將總時間T分配到每段。另外，對于初始分配的軌跡和時間，在迭代算法中，如果不滿足安全走廊約束和可行性約束，可以通過插值或者增大某一段時間等進行重新分段和分配時間。

??2.構(gòu)建需要優(yōu)化的函數(shù)

四旋翼無人機的動力學是差分平坦的，也就是說其狀態(tài)和輸入可以寫成四個精心（x，y，z，Ψ）選擇的平坦輸出及其導數(shù)的代數(shù)函數(shù)。這有利于軌跡的自動生成，因為在平坦輸出空間中的任何平滑軌跡(具有合理有界導數(shù))都可以跟隨欠驅(qū)動四旋翼飛行器。因此，無人機最小化函數(shù)通過最小化位置平方的kr階導數(shù)和偏航角平方的kΨ階導數(shù)：

其中μr和μΨ為常數(shù)，這里σT=[XT,YT,ZT,ΨT]的裝置，σi=[Xi,Yi,Zi,Ψi]的裝置

然后將其二次規(guī)劃QP問題：

進行一個簡單的推導來解釋轉(zhuǎn)化為QP問題：

3.?添加約束條件

①設定某點的位置、速度、加速度等為一個特定指，比如起點和終點的p=某值、v=0、a=0，這樣可以構(gòu)成等式約束。其中p、v、a由第二節(jié)前提的公式提供。

②相鄰兩端之間的連續(xù)性約束，可以表達為在端點處，其函數(shù)的導數(shù)等于0。例如第i、i+1段的位置連續(xù)構(gòu)成的等式約束為：

③安全走廊約束:把ti定義為從ri到ri+1段的單位向量。與段i的垂直距離向量di(t)被定義為：

定義走廊的寬度為：

通過引入nc個中間點，可以將這個約束融入到QP中

?④其他特定的約束，比如無人機在執(zhí)行某個任務時，需要多大的距離來執(zhí)行，飛行中需要避開某個軌跡節(jié)點等等。

4.?求解

??將優(yōu)化函數(shù)、不等式約束、等式約束和某些特定約束利用QP求解器進行求解得到相應的軌跡。對軌跡還需一個初步判斷，是否符合要求，是否能滿足無人機控制需求等。

?

五、參考文獻

[1] D. Mellinger and V. Kumar, “Minimum snap trajectory generation and control for quadrotors,” in Proc. of the IEEE Intl. Conf. on Robot. and Autom., Shanghai, China, May 2011, pp. 2520–2525.