考慮到近期可能要出去實(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í)交流哈!5咝矮燎!
無(wú)名小哥:余義
2018.03.01