四旋翼GPS定點流程梳理與原理淺析

考慮到近期可能要出去實習了，抓緊多整理點乾貨給大家，目前正在把V2.0版本的部分代碼往1,0裏面整合，博客一直來不及更新，今天抽空先寫一點，針對實現GPS定點的算法流程部分，也是售後羣之前一些人理不太清楚的地方，給大家梳理下。首先這裏給出無名飛控GPS定點部分的流程圖，這裏只摳出了位置+速度+姿態控制這部分。

最新的定點測試視頻見文末。

完整流程圖與飛控代碼見個人Github託管，鏈接：https://github.com/wustyuyi/NamelessCotrunQuad_V1.0

這裏對位置+速度+姿態控制流程圖裏面每一部分進行了標號，下面來足一解釋：

1、根據GPS狀態：星數+定位質量、Home點是否刷新來決定是否允許飛控進入GPS定點模式。

2、當允許飛控進入GPS定點模式時，判斷遙控杆位（俯仰、橫滾杆是否回中），來決定是否加入位置控制。

3、當不滿足定點條件時，此時水平姿態期望角直接來源於水平遙感給定，即水平方向無位置+速度控制，與自穩模式一致。

4、當允許飛控進入GPS定點模式後，水平遙感杆位不在中位，此時放棄水平位置控制。

此時遙感動作是希望飛機進行水平運動，此時有兩種方式：a、水平打杆的行程量對應期望姿態角；b、水平打杆行程量對應期望運動速度。其中a模式與自穩狀態下，打杆給定期望姿態角模式一致，打杆時無位置+速度控制，也是APM在PosHold模式下采取的方式，DJI精靈3之前的也都是採取此模式，b種模式爲水平控速模式，比如精靈4水平最大運動速度達到20m/s，也就是這麼來的。a，b兩種模式直觀的差異在於，a模式是直接給期望角度，會一直保持姿態角，即給定飛機水平運動的加速度，飛機速度在沒有大到水平阻力與水平推力抵消時，飛機是會一直做加速運動的。b模式給定的是期望水平運動速度，控制上是水平速度+姿態控制器，剛開始加速運動時，飛機會有一定姿態角，隨着速度增加，姿態角度會逐漸回中，勻速後會維持有一定小角度，去克服水平阻力。精靈3以上的控a，b模式都有，需要用App設置才能開啓a模式。

5、水平打杆時，直接給定期望姿態角，與自穩模式下打杆一致。

6、水平打杆行程量，對應期望水平運動速度，進行水平速度+姿態控制。

7、8、水平打杆後，把水平位置期望給0（7），這裏目的是用作8回中時刻，有且只刷新一次位置懸停點。

9、滿足水平方向杆從動作到回中條件，刷新一次位置懸停點。

10、進行水平位置+速度+姿態控制。

以上爲無名飛控的GPS定點實現部分的位置流程圖解析，其中6裏面，水平遙杆行程量到期望運動速度的映射可以用一個非線性函數來處理，7、8、9部分給定懸停點這裏，無名飛控是遙感一回中即賦值，實際可以結合當前運動速度，當前姿態角來決策剎車距離、懸停點實現緩衝的效果，視覺感柔和，避免翻機，比如Phantom 4最大飛行20m/s後，需要接近15m的緩衝距離才能剎停，剎停之後是懸停在剎停點，而不是剎停之後，又回到初始剎車點，這部分代碼可以參考APM、Pixhawk。

以上梳理了定點實現流程，下面來對具體定點原理進行講解:

首先，GPS定點控制有：水平位置+水平速度+姿態控制，這裏水平位置+水平速度得到的是期望運動加速度，在多旋翼裏面，螺旋槳輸出的總推力到運動加速度，線運動模型如下，單個軸來看，運動加速度與該方向的姿態角度的正弦值呈正相關，小角度情況下，姿態傾角即對應運動加速度。故這裏不再設計一個水平加速度控制器，直接把期望運動加速度，通過航向分解到俯仰橫滾方向姿態期望即可。

其中水平位置、水平速度控制器反饋來源於慣導融合，根據前幾篇博客講解，對於GPS與慣導融合得到的是導航系下，正東（E）、北（N）方向上的運動速度，相對Home點正東（E）、北（N）方向的運動速度。

假設你的機頭嚴格朝着正北（不考慮磁偏角影響），如上圖，這個時候的定點控制變得很簡單，不需做要航向分解，直接對正東方向做位置控制，正東方向位置控制器輸出給正東方向期望運動速度，再進行正東方向速度控制，得到正東方向期望運動加速度，即得到橫滾方向Roll姿態期望角（正北同理）。

對於實現GPS定點控制這裏有兩種殊途同歸的實現辦法：

1、a、對於定點（不打杆）控制來講，位置、速度控制都是在正東、正北方向上計算誤差，最後得到正東（E）、正北（N）方向上期望運動加速度，然後E、N方向運動加速度分解到載體航向上。b、對於打杆時候速度控制來講，遙杆給定的是導航繫上，載體機頭（Pitch）與橫滾（Roll）方向上的期望運動速度（並非E、N），但是慣性導航得到的是正東、正北方向的運動速度，因此同樣需要把E、N方向上的運動速度，分解到Pitch、Roll方向上來，見代碼註釋。

這裏需要注意的一點，也是很多人容易搞混的是，此時分解到載體航向上的V_Pitch,V_Roll依然是在導航繫上，只是所用的直角座標基底不再是E、N，而是隨着航向旋轉的一組遊動座標基底。

介紹完畢方式1，大家可能不覺得有什麼問題，這裏提出最直接的一點，方式1從a到b時，對速度控制器來講，前者爲導航系E、N方向的速度控制，後者爲導航系Pitch、Roll方向的速度控制，使用起來不方便，需要設計兩個速度控制器，單獨只有一個速度控制器的話，需要單獨處理下切換時速度控制器的積分項。

2、第二種GPS定點模式：方式1來講是直接利用慣性導航E、N方向上的位置偏移、速度

去做控制，這是最直觀的方式，只需要把速度控制器的輸出項——E、N方向的期望運動加速度通過航向分解給期望姿態角即可，問題是其在打杆與不打杆是速度控制器所處的座標基底不一致，需要特殊處理。受到啓發，我們想能不能把位置、速度控制也放在導航系Pitch、Roll方向上去做控制，這樣就直接一路串級控制到底就可，這樣就和我們初始假設機頭朝着正北時一樣，不用再中間做航向分解，只用提前把位置期望、速度期望、位置反饋、速度反饋分解到導航系Pitch、Roll方向上即可，顯然這樣的處理方式就可以避免設計兩個速度控制器、單速度控制器切換時積分處理的問題。