考慮到近期可能要出去實(shí)習(xí)了巩剖，抓緊多整理點(diǎn)干貨給大家两曼，目前正在把V2.0版本的部分代碼往1,0里面整合澳窑，博客一直來(lái)不及更新斧散，今天抽空先寫(xiě)一點(diǎn)，針對(duì)實(shí)現(xiàn)GPS定點(diǎn)的算法流程部分摊聋，也是售后群之前一些人理不太清楚的地方鸡捐，給大家梳理下。首先這里給出無(wú)名飛控GPS定點(diǎn)部分的流程圖麻裁，這里只摳出了位置+速度+姿態(tài)控制這部分闯参。
最新的定點(diǎn)測(cè)試視頻見(jiàn)文末。
完整流程圖與飛控代碼見(jiàn)個(gè)人Github托管悲立，鏈接：https://github.com/wustyuyi/NamelessCotrunQuad_V1.0
這里對(duì)位置+速度+姿態(tài)控制流程圖里面每一部分進(jìn)行了標(biāo)號(hào)鹿寨，下面來(lái)足一解釋?zhuān)?/p>
1、根據(jù)GPS狀態(tài)：星數(shù)+定位質(zhì)量薪夕、Home點(diǎn)是否刷新來(lái)決定是否允許飛控進(jìn)入GPS定點(diǎn)模式脚草。
2、當(dāng)允許飛控進(jìn)入GPS定點(diǎn)模式時(shí)原献，判斷遙控桿位（俯仰馏慨、橫滾桿是否回中）埂淮，來(lái)決定是否加入位置控制。
3写隶、當(dāng)不滿足定點(diǎn)條件時(shí)倔撞，此時(shí)水平姿態(tài)期望角直接來(lái)源于水平遙感給定，即水平方向無(wú)位置+速度控制慕趴，與自穩(wěn)模式一致痪蝇。
4、當(dāng)允許飛控進(jìn)入GPS定點(diǎn)模式后冕房，水平遙感桿位不在中位躏啰，此時(shí)放棄水平位置控制。
此時(shí)遙感動(dòng)作是希望飛機(jī)進(jìn)行水平運(yùn)動(dòng)耙册，此時(shí)有兩種方式：a给僵、水平打桿的行程量對(duì)應(yīng)期望姿態(tài)角；b详拙、水平打桿行程量對(duì)應(yīng)期望運(yùn)動(dòng)速度帝际。其中a模式與自穩(wěn)狀態(tài)下，打桿給定期望姿態(tài)角模式一致饶辙，打桿時(shí)無(wú)位置+速度控制蹲诀，也是APM在PosHold模式下采取的方式，DJI精靈3之前的也都是采取此模式畸悬，b種模式為水平控速模式，比如精靈4水平最大運(yùn)動(dòng)速度達(dá)到20m/s珊佣，也就是這么來(lái)的蹋宦。a，b兩種模式直觀的差異在于咒锻，a模式是直接給期望角度冷冗，會(huì)一直保持姿態(tài)角，即給定飛機(jī)水平運(yùn)動(dòng)的加速度惑艇，飛機(jī)速度在沒(méi)有大到水平阻力與水平推力抵消時(shí)蒿辙，飛機(jī)是會(huì)一直做加速運(yùn)動(dòng)的。b模式給定的是期望水平運(yùn)動(dòng)速度滨巴，控制上是水平速度+姿態(tài)控制器思灌，剛開(kāi)始加速運(yùn)動(dòng)時(shí)，飛機(jī)會(huì)有一定姿態(tài)角恭取，隨著速度增加泰偿，姿態(tài)角度會(huì)逐漸回中，勻速后會(huì)維持有一定小角度蜈垮，去克服水平阻力耗跛。精靈3以上的控a裕照，b模式都有，需要用App設(shè)置才能開(kāi)啟a模式调塌。
5晋南、水平打桿時(shí)，直接給定期望姿態(tài)角羔砾，與自穩(wěn)模式下打桿一致负间。
6、水平打桿行程量蜒茄，對(duì)應(yīng)期望水平運(yùn)動(dòng)速度唉擂，進(jìn)行水平速度+姿態(tài)控制。
7檀葛、8玩祟、水平打桿后，把水平位置期望給0（7）屿聋，這里目的是用作8回中時(shí)刻空扎，有且只刷新一次位置懸停點(diǎn)。
9润讥、滿足水平方向桿從動(dòng)作到回中條件转锈，刷新一次位置懸停點(diǎn)。
10楚殿、進(jìn)行水平位置+速度+姿態(tài)控制撮慨。
以上為無(wú)名飛控的GPS定點(diǎn)實(shí)現(xiàn)部分的位置流程圖解析，其中6里面脆粥，水平遙桿行程量到期望運(yùn)動(dòng)速度的映射可以用一個(gè)非線性函數(shù)來(lái)處理砌溺，7、8变隔、9部分給定懸停點(diǎn)這里规伐，無(wú)名飛控是遙感一回中即賦值，實(shí)際可以結(jié)合當(dāng)前運(yùn)動(dòng)速度匣缘，當(dāng)前姿態(tài)角來(lái)決策剎車(chē)距離猖闪、懸停點(diǎn)實(shí)現(xiàn)緩沖的效果，視覺(jué)感柔和肌厨，避免翻機(jī)培慌，比如Phantom 4最大飛行20m/s后，需要接近15m的緩沖距離才能剎停柑爸，剎停之后是懸停在剎停點(diǎn)检柬，而不是剎停之后，又回到初始剎車(chē)點(diǎn)，這部分代碼可以參考APM何址、Pixhawk煮纵。
以上梳理了定點(diǎn)實(shí)現(xiàn)流程挑豌，下面來(lái)對(duì)具體定點(diǎn)原理進(jìn)行講解:
首先，GPS定點(diǎn)控制有：水平位置+水平速度+姿態(tài)控制，這里水平位置+水平速度得到的是期望運(yùn)動(dòng)加速度基显，在多旋翼里面闽寡，螺旋槳輸出的總推力到運(yùn)動(dòng)加速度志群，線運(yùn)動(dòng)模型如下雕沉，單個(gè)軸來(lái)看，運(yùn)動(dòng)加速度與該方向的姿態(tài)角度的正弦值呈正相關(guān)颇玷，小角度情況下笨农，姿態(tài)傾角即對(duì)應(yīng)運(yùn)動(dòng)加速度。故這里不再設(shè)計(jì)一個(gè)水平加速度控制器帖渠，直接把期望運(yùn)動(dòng)加速度谒亦，通過(guò)航向分解到俯仰橫滾方向姿態(tài)期望即可。
其中水平位置空郊、水平速度控制器反饋來(lái)源于慣導(dǎo)融合份招，根據(jù)前幾篇博客講解，對(duì)于GPS與慣導(dǎo)融合得到的是導(dǎo)航系下狞甚，正東（E）锁摔、北（N）方向上的運(yùn)動(dòng)速度，相對(duì)Home點(diǎn)正東（E）哼审、北（N）方向的運(yùn)動(dòng)速度谐腰。
假設(shè)你的機(jī)頭嚴(yán)格朝著正北（不考慮磁偏角影響），如上圖涩盾，這個(gè)時(shí)候的定點(diǎn)控制變得很簡(jiǎn)單十气，不需做要航向分解，直接對(duì)正東方向做位置控制旁赊，正東方向位置控制器輸出給正東方向期望運(yùn)動(dòng)速度桦踊，再進(jìn)行正東方向速度控制椅野，得到正東方向期望運(yùn)動(dòng)加速度终畅，即得到橫滾方向Roll姿態(tài)期望角（正北同理）。
對(duì)于實(shí)現(xiàn)GPS定點(diǎn)控制這里有兩種殊途同歸的實(shí)現(xiàn)辦法：
1竟闪、a离福、對(duì)于定點(diǎn)（不打桿）控制來(lái)講，位置炼蛤、速度控制都是在正東妖爷、正北方向上計(jì)算誤差，最后得到正東（E）、正北（N）方向上期望運(yùn)動(dòng)加速度絮识，然后E绿聘、N方向運(yùn)動(dòng)加速度分解到載體航向上。b次舌、對(duì)于打桿時(shí)候速度控制來(lái)講熄攘，遙桿給定的是導(dǎo)航系上，載體機(jī)頭（Pitch）與橫滾（Roll）方向上的期望運(yùn)動(dòng)速度（并非E彼念、N）挪圾，但是慣性導(dǎo)航得到的是正東、正北方向的運(yùn)動(dòng)速度逐沙，因此同樣需要把E哲思、N方向上的運(yùn)動(dòng)速度，分解到Pitch吩案、Roll方向上來(lái)棚赔，見(jiàn)代碼注釋。
這里需要注意的一點(diǎn)务热，也是很多人容易搞混的是忆嗜，此時(shí)分解到載體航向上的V_Pitch,V_Roll依然是在導(dǎo)航系上，只是所用的直角坐標(biāo)基底不再是E崎岂、N捆毫，而是隨著航向旋轉(zhuǎn)的一組游動(dòng)坐標(biāo)基底。
介紹完畢方式1冲甘，大家可能不覺(jué)得有什么問(wèn)題绩卤，這里提出最直接的一點(diǎn)，方式1從a到b時(shí)江醇，對(duì)速度控制器來(lái)講濒憋，前者為導(dǎo)航系E、N方向的速度控制陶夜，后者為導(dǎo)航系Pitch凛驮、Roll方向的速度控制，使用起來(lái)不方便条辟，需要設(shè)計(jì)兩個(gè)速度控制器黔夭，單獨(dú)只有一個(gè)速度控制器的話，需要單獨(dú)處理下切換時(shí)速度控制器的積分項(xiàng)羽嫡。
2本姥、第二種GPS定點(diǎn)模式：方式1來(lái)講是直接利用慣性導(dǎo)航E、N方向上的位置偏移杭棵、速度
去做控制婚惫，這是最直觀的方式，只需要把速度控制器的輸出項(xiàng)——E、N方向的期望運(yùn)動(dòng)加速度通過(guò)航向分解給期望姿態(tài)角即可先舷，問(wèn)題是其在打桿與不打桿是速度控制器所處的坐標(biāo)基底不一致艰管，需要特殊處理。受到啟發(fā)蒋川，我們想能不能把位置蛙婴、速度控制也放在導(dǎo)航系Pitch、Roll方向上去做控制尔破，這樣就直接一路串級(jí)控制到底就可街图，這樣就和我們初始假設(shè)機(jī)頭朝著正北時(shí)一樣，不用再中間做航向分解懒构，只用提前把位置期望餐济、速度期望、位置反饋胆剧、速度反饋分解到導(dǎo)航系Pitch絮姆、Roll方向上即可，顯然這樣的處理方式就可以避免設(shè)計(jì)兩個(gè)速度控制器秩霍、單速度控制器切換時(shí)積分處理的問(wèn)題篙悯。
相關(guān)測(cè)試視頻：
四旋翼飛行器的自抗擾控制：設(shè)計(jì)、實(shí)現(xiàn)與實(shí)例
無(wú)名飛控新西達(dá)電機(jī)震動(dòng)測(cè)試
無(wú)名科創(chuàng)開(kāi)源飛控群：540707961
技術(shù)博客在畢業(yè)前（2019年6月）铃绒，會(huì)持續(xù)更新鸽照，歡迎大家一起學(xué)習(xí)交流哈！５咝矮燎！
無(wú)名小哥：余義
2018.03.01
