四旋翼GPS定點(diǎn)流程梳理與原理淺析

考慮到近期可能要出去實(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è)試視頻:

無(wú)名飛控江邊定點(diǎn)

四旋翼飛行器的自抗擾控制:設(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

?著作權(quán)歸作者所有,轉(zhuǎn)載或內(nèi)容合作請(qǐng)聯(lián)系作者
  • 序言:七十年代末,一起剝皮案震驚了整個(gè)濱河市赔癌,隨后出現(xiàn)的幾起案子诞外,更是在濱河造成了極大的恐慌,老刑警劉巖灾票,帶你破解...
    沈念sama閱讀 222,681評(píng)論 6 517
  • 序言:濱河連續(xù)發(fā)生了三起死亡事件峡谊,死亡現(xiàn)場(chǎng)離奇詭異,居然都是意外死亡刊苍,警方通過(guò)查閱死者的電腦和手機(jī)既们,發(fā)現(xiàn)死者居然都...
    沈念sama閱讀 95,205評(píng)論 3 399
  • 文/潘曉璐 我一進(jìn)店門(mén),熙熙樓的掌柜王于貴愁眉苦臉地迎上來(lái)班缰,“玉大人贤壁,你說(shuō)我怎么就攤上這事悼枢〔和” “怎么了?”我有些...
    開(kāi)封第一講書(shū)人閱讀 169,421評(píng)論 0 362
  • 文/不壞的土叔 我叫張陵,是天一觀的道長(zhǎng)莹妒。 經(jīng)常有香客問(wèn)我名船,道長(zhǎng),這世上最難降的妖魔是什么旨怠? 我笑而不...
    開(kāi)封第一講書(shū)人閱讀 60,114評(píng)論 1 300
  • 正文 為了忘掉前任渠驼,我火速辦了婚禮,結(jié)果婚禮上鉴腻,老公的妹妹穿的比我還像新娘迷扇。我一直安慰自己,他們只是感情好爽哎,可當(dāng)我...
    茶點(diǎn)故事閱讀 69,116評(píng)論 6 398
  • 文/花漫 我一把揭開(kāi)白布蜓席。 她就那樣靜靜地躺著,像睡著了一般课锌。 火紅的嫁衣襯著肌膚如雪厨内。 梳的紋絲不亂的頭發(fā)上,一...
    開(kāi)封第一講書(shū)人閱讀 52,713評(píng)論 1 312
  • 那天渺贤,我揣著相機(jī)與錄音雏胃,去河邊找鬼。 笑死志鞍,一個(gè)胖子當(dāng)著我的面吹牛瞭亮,可吹牛的內(nèi)容都是我干的。 我是一名探鬼主播固棚,決...
    沈念sama閱讀 41,170評(píng)論 3 422
  • 文/蒼蘭香墨 我猛地睜開(kāi)眼街州,長(zhǎng)吁一口氣:“原來(lái)是場(chǎng)噩夢(mèng)啊……” “哼!你這毒婦竟也來(lái)了玻孟?” 一聲冷哼從身側(cè)響起唆缴,我...
    開(kāi)封第一講書(shū)人閱讀 40,116評(píng)論 0 277
  • 序言:老撾萬(wàn)榮一對(duì)情侶失蹤,失蹤者是張志新(化名)和其女友劉穎黍翎,沒(méi)想到半個(gè)月后面徽,有當(dāng)?shù)厝嗽跇?shù)林里發(fā)現(xiàn)了一具尸體,經(jīng)...
    沈念sama閱讀 46,651評(píng)論 1 320
  • 正文 獨(dú)居荒郊野嶺守林人離奇死亡匣掸,尸身上長(zhǎng)有42處帶血的膿包…… 初始之章·張勛 以下內(nèi)容為張勛視角 年9月15日...
    茶點(diǎn)故事閱讀 38,714評(píng)論 3 342
  • 正文 我和宋清朗相戀三年趟紊,在試婚紗的時(shí)候發(fā)現(xiàn)自己被綠了。 大學(xué)時(shí)的朋友給我發(fā)了我未婚夫和他白月光在一起吃飯的照片碰酝。...
    茶點(diǎn)故事閱讀 40,865評(píng)論 1 353
  • 序言:一個(gè)原本活蹦亂跳的男人離奇死亡霎匈,死狀恐怖,靈堂內(nèi)的尸體忽然破棺而出送爸,到底是詐尸還是另有隱情铛嘱,我是刑警寧澤暖释,帶...
    沈念sama閱讀 36,527評(píng)論 5 351
  • 正文 年R本政府宣布,位于F島的核電站墨吓,受9級(jí)特大地震影響球匕,放射性物質(zhì)發(fā)生泄漏。R本人自食惡果不足惜帖烘,卻給世界環(huán)境...
    茶點(diǎn)故事閱讀 42,211評(píng)論 3 336
  • 文/蒙蒙 一亮曹、第九天 我趴在偏房一處隱蔽的房頂上張望。 院中可真熱鬧秘症,春花似錦照卦、人聲如沸。這莊子的主人今日做“春日...
    開(kāi)封第一講書(shū)人閱讀 32,699評(píng)論 0 25
  • 文/蒼蘭香墨 我抬頭看了看天上的太陽(yáng)。三九已至趟卸,卻和暖如春蹄葱,著一層夾襖步出監(jiān)牢的瞬間,已是汗流浹背锄列。 一陣腳步聲響...
    開(kāi)封第一講書(shū)人閱讀 33,814評(píng)論 1 274
  • 我被黑心中介騙來(lái)泰國(guó)打工图云, 沒(méi)想到剛下飛機(jī)就差點(diǎn)兒被人妖公主榨干…… 1. 我叫王不留,地道東北人邻邮。 一個(gè)月前我還...
    沈念sama閱讀 49,299評(píng)論 3 379
  • 正文 我出身青樓竣况,卻偏偏與公主長(zhǎng)得像,于是被迫代替她去往敵國(guó)和親筒严。 傳聞我的和親對(duì)象是個(gè)殘疾皇子丹泉,可洞房花燭夜當(dāng)晚...
    茶點(diǎn)故事閱讀 45,870評(píng)論 2 361

推薦閱讀更多精彩內(nèi)容