多旋翼飛行器設計與控制理論 -第11講-底層飛行控制V2

多旋翼飛行器設計與控制

第十一講底層飛行控制

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

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

假設多旋翼處于懸停狀態(tài)，即標稱輸入給定為。則此時有。通過在平衡點的小角度線性化，此時，其中可以認為是輸入。進一步，因為可以得到，所以可以認為輸入是。水平通道模型為線性的?？傻玫蕉嘈淼木€性模型：（1）水平通道模型1.多旋翼底層飛行控制框架和模型化簡線性模型化簡2024/5/289（3）姿態(tài)模型線性模型（2）高度通道模型線性模型小角度假設1.多旋翼底層飛行控制框架和模型化簡線性模型化簡2024/5/2810（3）姿態(tài)模型（2）高度通道模型每個通道都為線性模型！（1）水平通道模型原模型1.多旋翼底層飛行控制框架和模型化簡線性模型化簡2.位置控制2024/5/2811分類根據(jù)給定目標軌跡

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

是一個常值。設計控制器使得，當

時，

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

是一條與時間相關的軌跡。設計控制器使得，當

時，

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

是一條由參數(shù)

直接決定的路徑，而不是時間。設計控制器使得，當

時，

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

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

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

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

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

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

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

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

和控制器右端項的限幅其中。

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

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

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

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

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

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

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

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

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

它滿足

其中。因此，有如下關系以六旋翼為例，

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