多旋翼飛行器設(shè)計(jì)與控制理論 -第11講-底層飛行控制V2

多旋翼飛行器設(shè)計(jì)與控制

第十一講底層飛行控制

2024/5/282東方智慧都江堰2024/5/283給定期望的位置，如何控制多旋翼的電機(jī)使其能夠飛到目標(biāo)位置？核心問(wèn)題大綱多旋翼底層飛行控制框架和模型化簡(jiǎn)位置控制姿態(tài)控制控制分配電機(jī)控制本講小結(jié)2024/5/284多旋翼的底層飛行控制可以分為四個(gè)層次，分別為位置控制、姿態(tài)控制、控制分配和電機(jī)控制位置控制：期望的三維位置->解算期望姿態(tài)角(滾轉(zhuǎn)、俯仰和偏航)以及期望總拉力姿態(tài)控制：期望姿態(tài)角->解算期望力矩控制分配：期望力矩和升力->解算n個(gè)電機(jī)的期望轉(zhuǎn)速電機(jī)控制：期望轉(zhuǎn)速->解算每個(gè)電機(jī)的期望油門指令1.多旋翼底層飛行控制框架和模型化簡(jiǎn)2024/5/285圖11.1多旋翼底層飛行控制框架多旋翼底層飛行控制框架2024/5/286圖11.2多旋翼全自主控制閉環(huán)框圖欠驅(qū)動(dòng)系統(tǒng)：4個(gè)輸入（總拉力和三軸力矩）控制6個(gè)輸出（位置和姿態(tài)角）。設(shè)計(jì)多旋翼飛行控制器時(shí)，可以采用內(nèi)外環(huán)的控制策略，其中內(nèi)環(huán)對(duì)多旋翼飛行器姿態(tài)角進(jìn)行控制，而外環(huán)對(duì)多旋翼飛行器的位置進(jìn)行控制。由內(nèi)外環(huán)控制實(shí)現(xiàn)多旋翼飛行器的升降、懸停、側(cè)飛等飛行模態(tài)。1.多旋翼底層飛行控制框架和模型化簡(jiǎn)多旋翼底層飛行控制框架2024/5/287

基于第六講的多旋翼非線性模型，忽略，得到簡(jiǎn)化的多旋翼模型如下：其中，為對(duì)地位置，為對(duì)地速度，為飛機(jī)機(jī)體歐拉角，為飛機(jī)機(jī)體角速度，為旋轉(zhuǎn)矩陣，為轉(zhuǎn)動(dòng)慣量矩陣，，其他符號(hào)定義詳見(jiàn)第六講。1.多旋翼底層飛行控制框架和模型化簡(jiǎn)線性模型化簡(jiǎn)2024/5/288

假設(shè)多旋翼處于懸停狀態(tài)，即標(biāo)稱輸入給定為。則此時(shí)有。通過(guò)在平衡點(diǎn)的小角度線性化，此時(shí)，其中可以認(rèn)為是輸入。進(jìn)一步，因?yàn)榭梢缘玫剑钥梢哉J(rèn)為輸入是。水平通道模型為線性的?？傻玫蕉嘈淼木€性模型：（1）水平通道模型1.多旋翼底層飛行控制框架和模型化簡(jiǎn)線性模型化簡(jiǎn)2024/5/289（3）姿態(tài)模型線性模型（2）高度通道模型線性模型小角度假設(shè)1.多旋翼底層飛行控制框架和模型化簡(jiǎn)線性模型化簡(jiǎn)2024/5/2810（3）姿態(tài)模型（2）高度通道模型每個(gè)通道都為線性模型！（1）水平通道模型原模型1.多旋翼底層飛行控制框架和模型化簡(jiǎn)線性模型化簡(jiǎn)2.位置控制2024/5/2811分類根據(jù)給定目標(biāo)軌跡

的不同，可將位置控制分為三類：定點(diǎn)控制、軌跡跟蹤和路徑跟隨。圖11.3位置控制三類形式2.位置控制2024/5/2812（1）定點(diǎn)控制（Set-PointControl）。期望目標(biāo)點(diǎn)

是一個(gè)常值。設(shè)計(jì)控制器使得，當(dāng)

時(shí)，

收斂到0或者在0足夠小的鄰域內(nèi)。（2）軌跡跟蹤（TrajectoryTracking）。期望軌跡

是一條與時(shí)間相關(guān)的軌跡。設(shè)計(jì)控制器使得，當(dāng)

時(shí)，

收斂到0或者在0足夠小的鄰域內(nèi)。（3）路徑跟隨（PathFollowing）。期望軌跡

是一條由參數(shù)

直接決定的路徑，而不是時(shí)間。設(shè)計(jì)控制器使得，當(dāng)

時(shí)，

收斂到0或者在0足夠小的鄰域內(nèi)。分類[1]HespanhaJP.Trajectory-trackingandpath-followingofunderactuatedautonomousvehicleswithparametricmodelinguncertainty.AutomaticControl,IEEETransactionson,2007,52(8):1362–1379.2.位置控制2024/5/2813根據(jù)解算出的姿態(tài)信息的不同，可將位置控制分為兩類：（1）產(chǎn)生期望歐拉角的位置控制。

針對(duì)線性系統(tǒng)設(shè)計(jì)控制器。這種類型的位置控制器最終產(chǎn)生的期望值為歐拉角和拉力

。（2）產(chǎn)生期望旋轉(zhuǎn)矩陣的位置控制。

直接針對(duì)非線性耦合系統(tǒng)設(shè)計(jì)控制器。這種類型的位置控制器最終產(chǎn)生的期望值為旋轉(zhuǎn)矩陣和拉力

。分類2.位置控制2024/5/2814產(chǎn)生期望歐拉角的位置控制水平通道模型期望水平位置動(dòng)態(tài)當(dāng)考慮定點(diǎn)控制時(shí)

（1）傳統(tǒng)的PID設(shè)計(jì)其中表示參數(shù)。2.位置控制2024/5/2815產(chǎn)生期望歐拉角的位置控制高度通道模型期望高度動(dòng)態(tài)當(dāng)考慮定點(diǎn)控制時(shí)，上式就變?yōu)椋?）傳統(tǒng)的PID設(shè)計(jì)在的前提下如果2.位置控制2024/5/2816產(chǎn)生期望歐拉角的位置控制（2）開(kāi)源自駕儀PID設(shè)計(jì)1）水平位置通道

為了使,我們先針對(duì)那么其中。速度能達(dá)到期望，位置也就能達(dá)到期望期望速度圖11.4多旋翼控制目標(biāo)關(guān)系圖水平達(dá)到期望位置高度達(dá)到期望2.位置控制2024/5/2817產(chǎn)生期望歐拉角的位置控制（2）開(kāi)源自駕儀PID設(shè)計(jì)1）水平位置通道為了使,我們先針對(duì)如果那么角度能達(dá)到期望，速度也就能達(dá)到期望圖11.4多旋翼控制目標(biāo)關(guān)系圖水平達(dá)到期望位置高度達(dá)到期望2.位置控制2024/5/2818產(chǎn)生期望歐拉角的位置控制（2）開(kāi)源自駕儀PID設(shè)計(jì)2）高度通道豎直方向速度能達(dá)到期望，高度也就能達(dá)到期望

類似于水平通道設(shè)計(jì)，高度通道控制器設(shè)計(jì)如下圖11.4多旋翼控制目標(biāo)關(guān)系圖水平達(dá)到期望位置高度達(dá)到期望2.位置控制2024/5/2819產(chǎn)生期望歐拉角的位置控制（3）加飽和的PID控制傳統(tǒng)PID自駕儀PID位置誤差很大角度誤差很大小角度假設(shè)被破壞！加飽和非常必要控制器設(shè)計(jì)就沒(méi)意義了2.位置控制2024/5/2820產(chǎn)生期望歐拉角的位置控制（3）加飽和的PID控制考慮飽和之后，傳統(tǒng)PID控制器變?yōu)槠渲小１７较蝻柡秃瘮?shù)的定義見(jiàn)第十講。同樣地，在開(kāi)源自駕儀PID設(shè)計(jì)中，我們?cè)黾訉?duì)

和控制器右端項(xiàng)的限幅其中。

2.位置控制2024/5/2821產(chǎn)生期望歐拉角的位置控制（3）加飽和的PID控制下面討論保方向飽和函數(shù)和傳統(tǒng)飽和函數(shù)的區(qū)別。雖然傳統(tǒng)飽和函數(shù)限制的每個(gè)分量的絕對(duì)值不大于，但它與的方向可能不同。而保方向飽和函數(shù)不僅可以限制最終向量每個(gè)分量的絕對(duì)值不大于，還可以保證的方向與相同。傳統(tǒng)飽和函數(shù)保方向飽和函數(shù)傳統(tǒng)飽和函數(shù)導(dǎo)致方向偏離2.位置控制2024/5/2822產(chǎn)生期望歐拉角的位置控制（3）加飽和的PID控制

圖11.4兩種飽和函數(shù)的作用效果對(duì)比圖圖11.5飽和情況下多旋翼定點(diǎn)跟蹤水平面運(yùn)動(dòng)軌跡保方向的飽和函數(shù)可以保證多旋翼直線飛行，而傳統(tǒng)飽和函數(shù)則不行2.位置控制2024/5/2823產(chǎn)生期望歐拉角的位置控制（3）加飽和的PID控制為了避免油門過(guò)大，同樣需要加入飽和。在傳統(tǒng)PID控制器設(shè)計(jì)中，我們需要對(duì)控制器的右端進(jìn)行如下限幅其中。同樣地，在開(kāi)源自駕儀PID設(shè)計(jì)中，我們?cè)黾訉?duì)和控制器的右端的限幅其中。對(duì)于一維變量，保方向飽和函數(shù)和與傳統(tǒng)飽和函數(shù)的作用相同。2.位置控制2024/5/2824產(chǎn)生期望旋轉(zhuǎn)矩陣的位置控制圖11.6基于旋轉(zhuǎn)矩陣的的多旋翼閉環(huán)控制框圖基于旋轉(zhuǎn)矩陣描述的姿態(tài)運(yùn)動(dòng)學(xué)方程為其中是旋轉(zhuǎn)矩陣。系統(tǒng)期望的姿態(tài)控制指令為。因此位置控制器最好可以直接給出期望的旋轉(zhuǎn)矩陣

。2.位置控制2024/5/2825多旋翼位置模型期望的加速度，需要加飽和產(chǎn)生期望旋轉(zhuǎn)矩陣的位置控制[2]針對(duì)非線性耦合模型進(jìn)行如下控制器設(shè)計(jì)其中以及先求解期望的旋轉(zhuǎn)矩陣。由上式可以得到[2]Lee,T,LeokyM,andMcClamrochNH.GeometrictrackingcontrolofaquadrotorUAVonSE(3).In:Proceedingsofthe49thIEEEConferenceonDecisionandControl.Atlanta,Georgia,USA:IEEE,2010.5420–5425.2.位置控制2024/5/2826產(chǎn)生期望旋轉(zhuǎn)矩陣的位置控制但是為了滿足，我們只需要得到方向至此，向量已經(jīng)確定。下面介紹兩種獲得的方法。一種基于小角度假設(shè)，另一種同樣適用于大角度飛行。（1）小角度情況

然而，因此還要確定中的一個(gè)，我們就可以恢復(fù)出了。根據(jù)旋轉(zhuǎn)矩陣的定義，為2.位置控制2024/5/2827產(chǎn)生期望旋轉(zhuǎn)矩陣的位置控制因此，我們定義向量在姿態(tài)角變化不大的情況下，可認(rèn)為。這樣通過(guò)和可以將定義為根據(jù)叉乘的定義，可以知道。進(jìn)一步，定義。這樣可得。至此，可得期望的姿態(tài)矩陣為其滿足。2.位置控制2024/5/2828產(chǎn)生期望旋轉(zhuǎn)矩陣的位置控制(*)（2）大角度情況

向量也可以表示為上式的解為其中2.位置控制2024/5/2829產(chǎn)生期望旋轉(zhuǎn)矩陣的位置控制盡管每個(gè)歐拉角有兩個(gè)可能的取值，大多數(shù)情況下其真值可由式（*）唯一確定。進(jìn)一步，可由期望的歐拉角計(jì)算得到。

下面進(jìn)一步考慮期望拉力的計(jì)算，因?yàn)楹婉詈显谝黄?，因此我們進(jìn)行如下處理。首先在兩邊同乘，利用可以得到如果限定，那么姿態(tài)控制器可以寫(xiě)為3.姿態(tài)控制2024/5/2830多旋翼采用內(nèi)外層控制，外層控制器為內(nèi)層控制器提供指令，即把位置通道控制器的輸出（或）作為姿態(tài)控制系統(tǒng)的參考值。后續(xù)的姿態(tài)控制的目標(biāo)是實(shí)現(xiàn)或者。不僅如此，一般要求姿態(tài)環(huán)的收斂速度比位置通道動(dòng)態(tài)快4～10倍。從位置通道看來(lái)，可以認(rèn)為姿態(tài)控制目標(biāo)或已經(jīng)被實(shí)現(xiàn)了。因此，剩余的控制目標(biāo)就傳給了姿態(tài)控制。只要姿態(tài)控制被很好地實(shí)現(xiàn)，水平位置跟蹤的問(wèn)題就可以被解決。下面介紹姿態(tài)控制。水平達(dá)到期望位置高度達(dá)到期望多旋翼控制目標(biāo)關(guān)系圖3.姿態(tài)控制2024/5/2831姿態(tài)參數(shù)優(yōu)點(diǎn)缺點(diǎn)歐拉角無(wú)冗余參數(shù)，物理意義明確俯仰角為90度時(shí)存在奇異，存在大量超越函數(shù)運(yùn)算，死鎖；偏航角從2π到0的過(guò)渡問(wèn)題。旋轉(zhuǎn)矩陣無(wú)奇異，無(wú)超越函數(shù)運(yùn)算，可用于連續(xù)旋轉(zhuǎn)表示，全局且唯一，便于插值六個(gè)冗余參數(shù)表11.1姿態(tài)參數(shù)性能對(duì)比基本概念多旋翼姿態(tài)控制是位置控制的基礎(chǔ)。常見(jiàn)的剛體姿態(tài)描述方法為歐拉角描述方法和旋轉(zhuǎn)矩陣描述方法。姿態(tài)控制的目標(biāo)是已知參考姿態(tài)角，設(shè)計(jì)控制器使得，其中。這里是由位置控制器給定的，而是任務(wù)規(guī)劃給定的。為了達(dá)到這個(gè)目的，我們先針對(duì)設(shè)計(jì)角速度的期望為其中是正定的常值對(duì)角矩陣，所有元素都大于0。以上兩式構(gòu)成了角度控制環(huán)。在的前提下，當(dāng)可以使得其中。接下來(lái)的任務(wù)是針對(duì)設(shè)計(jì)期望的轉(zhuǎn)矩其中是正定常值對(duì)角矩陣。以上兩式構(gòu)成了角速度控制環(huán)。3.姿態(tài)控制2024/5/2832如何加入飽和？基于歐拉角的控制器設(shè)計(jì)3.姿態(tài)控制2024/5/2833

基于旋轉(zhuǎn)矩陣的控制器設(shè)計(jì)控制器設(shè)計(jì)的思路為：根據(jù)旋轉(zhuǎn)矩陣和期望旋轉(zhuǎn)矩陣定義姿態(tài)誤差矩陣為。由上面的定義可知：當(dāng)且僅當(dāng)時(shí)，?；谝陨戏治隹梢詫⒒谛D(zhuǎn)矩陣的多旋翼姿態(tài)跟蹤控制的控制目標(biāo)總結(jié)為。定義姿態(tài)跟蹤誤差為其中vex運(yùn)算定位為角速度跟蹤誤差定義為小角度假設(shè)下為期望的角速度，一般情況下可忽略，則。可以設(shè)計(jì)如下的PD控制器其中為正定增益矩陣。上面設(shè)計(jì)的PD控制器只可以在懸停位置的小范圍內(nèi)保證系統(tǒng)穩(wěn)定。為了獲得更大范圍的穩(wěn)定性，通過(guò)引入誤差校正項(xiàng)，設(shè)計(jì)非線性控制器為：此控制器可以保證在幾乎任意旋轉(zhuǎn)情況下系統(tǒng)指數(shù)穩(wěn)定。實(shí)際中控制器后兩項(xiàng)的值很小，直接忽略也可以獲得滿意的性能。但對(duì)于大機(jī)動(dòng)飛行中后兩項(xiàng)的值很大，對(duì)系統(tǒng)性能的影響很大，則必須予以考慮。具體可參考如下文獻(xiàn)。3.姿態(tài)控制2024/5/2834

基于旋轉(zhuǎn)矩陣的控制器設(shè)計(jì)[3]Lee,T,LeokyM,andMcClamrochNH.GeometrictrackingcontrolofaquadrotorUAVonSE(3).In:Proceedingsofthe49thIEEEConferenceonDecisionandControl.Atlanta,Georgia,USA:IEEE,2010.5420–5425.4.控制分配2024/5/2835基本概念上層控制和底層控制分離有效分配，防止飽和通過(guò)分配，提高對(duì)故障及損傷的魯棒性….簡(jiǎn)單來(lái)說(shuō)，控制分配問(wèn)題可描述為：給定，尋找使得其中為被控系統(tǒng)中執(zhí)行機(jī)構(gòu)控制輸入到偽控制輸入的映射。通常假設(shè)執(zhí)行機(jī)構(gòu)偏轉(zhuǎn)量與其產(chǎn)生的控制力矩之間為線性函數(shù)關(guān)系，則可得到線性控制分配問(wèn)題其中為控制效率矩陣?？刂品峙涫疽鈭D

+字形四旋翼和多旋翼的控制效率模型見(jiàn)第六講：對(duì)于四旋翼來(lái)說(shuō)，可逆，可直接求逆得到控制分配矩陣。這種分配是唯一的。然而，對(duì)于螺旋槳數(shù)大于4的多旋翼，分配可以有無(wú)窮多種。分配過(guò)程中，可能讓某個(gè)螺旋槳的轉(zhuǎn)速達(dá)到飽和。因此，好的控制分配算法就十分重要。在開(kāi)源的自駕儀中，一般的算法是通過(guò)求偽逆得到控制分配矩陣，即其中，。4.控制分配2024/5/2836多旋翼控制分配的自駕儀實(shí)現(xiàn)在實(shí)際中，通過(guò)控制器得到期望拉力和期望力矩后，進(jìn)一步再通可得到每個(gè)螺旋槳的期望轉(zhuǎn)速。在工程實(shí)際中，中的參數(shù)未知，即（具體定義參見(jiàn)第六講）未知時(shí)。那這又是如何進(jìn)行控制分配的呢？4.控制分配2024/5/2837多旋翼控制分配的自駕儀實(shí)現(xiàn)與參數(shù)相關(guān)？4.控制分配2024/5/2838多旋翼控制分配的自駕儀實(shí)現(xiàn)未知參數(shù)已知參數(shù)要說(shuō)明這個(gè)問(wèn)題，首先定義控制效率矩陣為

它滿足

其中。因此，有如下關(guān)系以六旋翼為例，

可以表示為4.控制分配2024/5/2839多旋翼控制分配的自駕儀實(shí)現(xiàn)圖11.7常規(guī)布局的六旋翼于是自駕儀中，控制器