第11講多旋翼底層控制

第十一講多旋翼的底層飛行全 自動(dòng)化科學(xué)與電航空航天大年 日北航主川省市都江堰市城西，坐落在平原西部的岷江上，始建于秦昭王(約公元前256～前251)，是蜀郡太守父子人鱉靈開(kāi)。。都江堰 來(lái)源 多旋翼底層控制框架和模型位置控姿態(tài)控控制分電機(jī)控綜合仿飛控原理與總作 多旋翼底層控制框架和模型化位置控制：期望的三維位pd算期望姿態(tài)角Θ(滾轉(zhuǎn)、俯仰和偏航d,d,d)以及期望總拉力fd姿態(tài)控制：期望姿態(tài)角Θd算期望力矩τd控制分配：期望力矩和升力τd,fdn個(gè)電機(jī)的期望轉(zhuǎn)速

,k1,2,,電機(jī)控制：期望轉(zhuǎn)速d,k算電機(jī)入油門(電 占空比)

,k1,2,,

圖.多旋翼底層控制層 1.多旋翼底層控制框架和模型化底層控fd τd控d姿態(tài)控位置控圖.多旋翼全自主控制閉環(huán)f角Θ），其中內(nèi)環(huán)。 1.多旋翼底層控制框架和模型化a基于第六章的多旋翼非線性模型，忽略bωJb ，apfsinsincossincos

fe

ge3

f

cossincossinsin pgfcos

其中 epp其中

, T 為對(duì)地位置，ev為對(duì)地速度bωyz eyz3機(jī)機(jī)體角速度R為旋轉(zhuǎn)矩陣，J為轉(zhuǎn)動(dòng)慣量矩陣 3 1.多旋翼底層控制框架和模型化假設(shè)多旋翼處于懸停狀態(tài)，即ppd0,dp0,0標(biāo)稱輸入給定fmgτ0。則此時(shí)有sincos1,sincos1。cossin sin

可以認(rèn)為Θh是輸入進(jìn)一步，因?yàn)間A以得到，所以可以可得到多旋翼的線性模水平通道

輸入是gAh。水平道模型為h。水平 ppx,

sin,

Rdiag

其中

p

cos

y

1.多旋翼底層控制框架和模型化高度通道

線性模 vgzm姿態(tài)WW

=ω=ω

線性模 多旋翼底層控制框架和模型化原模

水平通道模高度通vg 姿態(tài)模=ω 位置控分根據(jù)給定目標(biāo)軌跡pd的不同，可將位置控制分為三類：定點(diǎn) 2.位置控分dd3軌 33路徑跟隨（PathFollowing）[1]。期望軌pd是一條由參數(shù)3到0或者在0足夠小的鄰域modelinguncertainty.AutomaticControl,IEEETransactionson,2007,52(8):1362–1379. 2.位置控分根據(jù)解算出的姿態(tài)信息的不同，可將位置控制分為兩產(chǎn)生期望歐拉角的位置控制器d,d,d 。產(chǎn)生期望旋轉(zhuǎn)矩陣的位置控制位置控制器最終產(chǎn)生的期望值為旋轉(zhuǎn)矩陣和拉力Rd, 2.位置控（1）傳統(tǒng)的PID

gA

p

1hph當(dāng)考慮定點(diǎn)控制當(dāng)考慮定點(diǎn)控制 2.位置控產(chǎn)生期望歐拉角的位置控傳統(tǒng)的PID mg mg dzd zdp zpzdvg 當(dāng)考慮定點(diǎn)控制時(shí)，d

d

fdmgmkpzd

kpppzpz 2.位置控 k開(kāi)源自駕儀PID1） ept0,我們先針

limtkvhdKp vhdKp phdphh期望vz期望vz

limt

eωt limtevt

limt0ezt t

limt0eΘtlimt0tlimt0eΘthhlimtepth

t 其中h

vhdlimtlimt0ht2.位置控產(chǎn)生期望歐拉角的位置控制（2）開(kāi)源自駕儀PID

kk為了使limt t0,我們先針h

lim

limt

et zv

t gA

Kh

hK e

v

vp v

ezt

eΘt g1A1 vhi

t

limt0tdt如果

Θht

t

hhhlimt0epthh那

t

ev t2.位置控產(chǎn)生期望歐拉角的位置控制開(kāi)源自駕儀PID

kk

limtklimt limt zlimt0eztvzdz

pzpzd

eωt

hlimt0h

limt0eΘtlimt0eΘt

t

t豎直方向速度能達(dá)到豎直方向速度能達(dá)到期望，高度也就能達(dá)到期h

ept 2.位置控產(chǎn)生期望歐拉角的位置控加飽和的PID傳統(tǒng)自駕儀

就沒(méi)意義位置誤差很 角度誤差很 小角度 設(shè)被破壞加加飽和非常 2.位置控產(chǎn)生期望歐拉角的位置控（3）加飽和的PID同樣地，在開(kāi)源自駕儀PID設(shè)計(jì)中，我們?cè)黾訉?duì)h

和控制器右端的限hsatgdvhvhd,a1 11vhpe vhd,2其中a1a2。保方向飽和函數(shù)satgdxa的定義見(jiàn)10下面討論保方向飽和函數(shù)satgdxa與傳統(tǒng)的飽和函satxa的區(qū)別。雖然飽和函數(shù)限satxa的每個(gè)分量的絕對(duì)值不大于a，但它與x的方向可能不同。而保方向飽和函數(shù)satgdx不僅可以限制最終向量每個(gè)分量的絕對(duì)值不大于a，還可以保證satgdxa的方向與x相同 2.位置控產(chǎn)生期望歐拉角的位置控（3）加飽和的PIDTsatphdph,20 Ty

phd T傳統(tǒng)飽和函T

保方向飽和函satgdx,ahp h

satgdphdph,20

導(dǎo)致方向偏圖.兩種飽和函數(shù)的作用效果對(duì)satx,a

傳統(tǒng)飽和函

, ,satx,a kk

satx,a

asignxk,xk

水平面運(yùn)動(dòng)軌 2.位置控產(chǎn)生期望歐拉角的位置控（3）加飽和的PID其中a3。同樣地，在開(kāi)源自PID設(shè)計(jì)中，我們?cè)黾訉?duì)ev和控制器的右端zz限 satgdvzvzd,a4z限其中a4a5對(duì)于一維變量，保方向飽和函數(shù)satgd和與傳統(tǒng)飽和函數(shù)sat的作用相同 2.位置控內(nèi)外環(huán)控制時(shí)，我們自然期望位置控制器可以直接給出期望的旋轉(zhuǎn)矩陣Rd。因?yàn)樾D(zhuǎn)矩陣Rd需要直接通過(guò)向量進(jìn)行設(shè)計(jì)，所以外環(huán)的位置控制可直接針對(duì)圖.位置控制器給出Rd情況下的多旋翼總體控制 2.位置控多旋翼多旋翼位置模針對(duì)非線性耦合模型進(jìn)行如下控制器設(shè)gefd 期望的加速，需要加飽其中期望的加速，需要加飽

,a vhpv vhi

vhdv 1 d d

kvzi

kv

,a2 先求解期望的旋轉(zhuǎn)矩陣Rd。由上式可以得

mge3adfd 2.位置控d,3但是為了滿足rT 1，我們只需要得到d,3ge3ge3

ge3然而Rd

rd,3，因此還要確定rd,1rd,2中的一個(gè)，我們就可以恢出 了。根據(jù)旋轉(zhuǎn)矩陣的定義，r3 coscosrcossin 2.位置控因此，我們定義向cosd

d 在姿態(tài)角變化不大的情況下，可認(rèn)為rd,1rd,1。這樣rd,1和rd,3可以將rd,2定義

rd,3rd,3根據(jù)叉乘的定義，可以知道rd,2rd,3。進(jìn)一步定義rd,1rd,2rd,3rd,1rd,3rd,1rd,2 2.位置控至此，可得期望的姿態(tài)矩陣Rd rd,3其滿足RT I。下面進(jìn)一步考慮期望升力f的計(jì)算，因?yàn)樽兞縡和

耦合在一起，因此進(jìn)行如下處理。首gefd

a兩邊同乘rT，利用rT

1可以得

d,3 mrTgea 如果限定fdfminfmax，那么姿態(tài)控制器可以寫

ge

fminfmax,fmaxfminfmin gd

3.姿態(tài)控多旋翼采用分層控制外層控制器為內(nèi)層控

kkΘhd或者RdtΘhtΘhdt0

或者

zz

0。不僅如此，一般要

limt

limt0eωt 可以認(rèn)為姿態(tài)控制目標(biāo) 或RtRt被limt0ezt “瞬間”實(shí)現(xiàn)。因此，目標(biāo)像擊鼓傳花一樣，傳給

limt0eΘt態(tài)控制了。只要姿態(tài)控制被很好地實(shí)現(xiàn)水平位

evth

limt0tdt

epth 3.姿態(tài)控基本概多旋翼姿態(tài)控制是飛行器位置控制的基礎(chǔ)。常見(jiàn)的剛體姿態(tài)描述方法拉角描述方法和旋轉(zhuǎn)矩陣描表.姿態(tài)參數(shù)性能對(duì)姿態(tài)參優(yōu)缺歐拉旋轉(zhuǎn) 3.姿態(tài)控姿態(tài)控制的目標(biāo)是已知參考姿態(tài)角Θ T，設(shè)計(jì)控制器τ使得

et0 d

t 中eΘΘΘd。這里Θhd是由位置控制器給定的，而d是任務(wù)規(guī)劃給定的。為了達(dá)到這個(gè)目的我們先針設(shè)計(jì)角速度的期望ωd

ωdωd其中K 是正定的常值對(duì)角矩陣，所有元素都大于0。以上兩式構(gòu)成了角度控制環(huán)。接來(lái)的任務(wù)設(shè)計(jì)轉(zhuǎn)矩的期望τd其中eωωωdKωpKωiKωd

τdτd=KωpeωKωieω。以上兩式構(gòu)成了角速度 3.姿態(tài)控基于旋轉(zhuǎn)矩陣的控制器設(shè)計(jì)望坐標(biāo)旋轉(zhuǎn)矩陣與實(shí)際坐標(biāo)旋轉(zhuǎn)矩陣定義姿態(tài) 誤差，并通過(guò) 誤差設(shè)計(jì)姿態(tài)控制器。d多旋翼姿 控制的控制目標(biāo)總結(jié)為limt

0定義姿 誤差

1vexRTRRTR x2

xT

x,vexx

1 0 3.姿態(tài)控姿態(tài)角速 誤差定義eω=ωR小角度假設(shè)下ωdΘd為期望的角速度eωω通過(guò)計(jì)算可得eRd?？梢栽O(shè)計(jì)如下的PD控制器ττdKReR其中K, 33為正定增益矩陣上面設(shè)計(jì)的PD控制器只可以在懸停位置的小范圍內(nèi)保證系統(tǒng) 定。為了獲得更大范圍的穩(wěn)定性，通過(guò)引入誤差校正項(xiàng)，設(shè)計(jì)非線性控制器為τK KeJωRTRωRTRω R ω

略也可以獲得滿意的性能。但對(duì)于大機(jī)動(dòng)飛行中后兩項(xiàng)的值很大，對(duì)系統(tǒng)性能的影響很大，則必須予以 4.控制分基本概簡(jiǎn)單來(lái)說(shuō)，控制分配問(wèn)題可描述為：給定uvt，尋找δt使得uvtg其中g(shù):mn為被控系統(tǒng)中執(zhí)行機(jī)構(gòu)控制輸入到 分配問(wèn)

uvt

gg上層控制和底層控制分uvtBδt其中Bmn為已知控制效率矩陣

有效分配，防止飽通過(guò)分配，提高對(duì)故障損傷的魯棒 4.控制分+字形四旋翼的控制分配和多旋翼的控制分配模型見(jiàn)第6講f

f

1

1

dcTsin

dcTsin

dcTsin

T 2 x

r 2

02

dc

dcT dcT McM n 3 McM n

r

c

c

4

nr對(duì)于四旋翼來(lái)說(shuō)M44可逆，可直接求逆得到控制分配矩陣PM1，其中P

種分配是唯一的。然而，對(duì)于旋翼數(shù)大于4的多旋翼，分配可以有無(wú)窮多種。分配過(guò)程中能讓某個(gè)旋翼的轉(zhuǎn)速達(dá)到飽和。因此，好的控制分配算法就十分重要。在開(kāi)源的自駕儀中，一般的算法是通過(guò)求偽逆得到控制分配矩陣，即 其中Pnr4

4nr4.控制分在實(shí)際中，通過(guò)控制器得到期望拉力fd和期望力矩τd后，進(jìn)一步再通 d,1Pnr Pnr fdτd d,nr可得到每個(gè)螺旋槳的期望轉(zhuǎn)速d,ii12,nr

與參數(shù)相在工程實(shí)際中，Mn中的參數(shù)未知，即cT,cM,d（具體定義參見(jiàn)第六講未知時(shí)r？那這又是如何進(jìn)行控制分配？ 4.控制分要說(shuō)明這個(gè)問(wèn)題，首先定義映射矩

sin dcsin dcsin c, ,d

nr

dc dc

dc

cM

r它滿其中Padiag

MncT,cM,dPaMn cM。因此，有如下關(guān)M?c, ,dM? 已知參

未知參 4.控制分03以六旋翼為例M6cTcMd可以03

1

Mc,c,d

M?

1

13 3

dc dc

6

T T

13 3

cM

于圖.常規(guī)布局的六旋圖.常規(guī)布局的六旋

來(lái)補(bǔ)償未知量。

fd

M?1,1,1P-1

aτ aτ

τd

dcT

d,6

5.電機(jī)控閉環(huán)控來(lái)感知轉(zhuǎn)子的整流，利用高頻脈寬調(diào)制模塊 ）來(lái)控制電機(jī)的電壓。下面給出利用電機(jī)油門實(shí)現(xiàn)電機(jī)控制的控制器設(shè)計(jì)?？刂颇繕?biāo)為：設(shè)計(jì)每個(gè)電機(jī)的輸入油門d,k使limtkd,k0。以油作為輸入的電機(jī)模型見(jiàn)第六章，對(duì)應(yīng)到多旋翼上每個(gè)電機(jī)的動(dòng)態(tài)模型k b Ck bTms1d,k其中kd,k 5.電機(jī)控開(kāi)環(huán)控d,kad,k在開(kāi)源自駕儀中，電機(jī)為開(kāi)環(huán)控制fd,τd之后，期望的每個(gè)轉(zhuǎn)子的轉(zhuǎn)速為d,kk12,nr可通過(guò)控制分配直接得到，d,kad,k參數(shù)ab可以通過(guò)位置和姿態(tài)控制器中的PID參數(shù)來(lái)補(bǔ)d圖.多旋翼動(dòng)力系統(tǒng)的開(kāi)環(huán)控制及工作 6.綜合仿為了驗(yàn)證算法的有效性，我們利用進(jìn)行了相關(guān)仿真。仿真中采用的四旋翼構(gòu)型是十字形。它對(duì)于X字形構(gòu)型的四旋翼，除了控制矩陣M4不同表.四旋翼主要模型參g9.8100ms2J0.0095kgm2d0.2223J0.0186kgm2m1.02303.7882e-06kgm21.4865e-07N/rpm280.58402.9250e-09Nm/rpm2b976.20000.0095kgm20.0760 6.綜合仿控制目標(biāo)為：以初始位置作為中心點(diǎn)，從中心點(diǎn)出發(fā)在個(gè)

4000Trpm,p0

3Tm,v

0TΘ

0T ω

0Tdeg/s.期望的航跡指令由時(shí)間序列給表.期望指令時(shí)間序時(shí)間zd（md（deg000050000000000000 6.綜合仿姿態(tài)模指令模位置模圖.綜合控制仿真搭建框下面將給出基于此仿真搭建的兩種控制方案的仿真結(jié)果基于歐拉角的姿態(tài)控制結(jié)合基于歐拉角的位置基于旋轉(zhuǎn)矩陣的姿態(tài)控制結(jié)合基于旋轉(zhuǎn)矩陣的位置 6.綜合仿基于歐拉角的姿態(tài)控制結(jié)合基于歐拉角的位置 PQPQR

0

0ThetaTheta

0PsiPsi

PhiPhi04

004

00

圖.航 效果

MotorMotorSpeedVxVyVxVy

VzVz

Motor

0

38.5

55Time55

5XX0

5YY0

ZZ0

3838.5

Motor Time

圖.電機(jī)油門指令和電機(jī)轉(zhuǎn)速響應(yīng) 6.綜合仿基于旋轉(zhuǎn)矩陣的姿態(tài)控制結(jié)合基于旋轉(zhuǎn)矩陣的位置

PP00PhiPhi0

QQ0ThetaTheta0

0RR0PsiPsi

4

4

圖.航 效果MotorThrottleMotorThrottleCommand

MotorMotorSpeedVxVyVxVy

VzVz

Motor

5

5

0

0

Motor

XX

YY

ZZ

Motor

Time

圖.電機(jī)油門指令和電機(jī)轉(zhuǎn)速響 6.綜合仿大機(jī)動(dòng)控基于旋轉(zhuǎn)矩陣的控制器相比于基于歐拉角控制器的一大優(yōu)勢(shì)就是其可以避免奇異問(wèn)題，實(shí)現(xiàn)全姿態(tài)飛行，因此可以實(shí)現(xiàn)類似飛行表演中的大機(jī)動(dòng)動(dòng)基于旋轉(zhuǎn)矩陣實(shí)現(xiàn)四旋翼的倒飛翻轉(zhuǎn)。由下圖可見(jiàn)，滾轉(zhuǎn)角經(jīng)歷了一個(gè)由初始正180度（倒飛）到最終的0度（正飛）的過(guò)程。位置經(jīng)過(guò)小幅波動(dòng)后最終穩(wěn)定在初始 1

1

2

1

0505 7.飛控原理與調(diào)滾轉(zhuǎn)俯仰偏航PID 7.飛控原理與調(diào)高度控 7.飛控原理與調(diào)MissionPlanner調(diào)參 7.飛控原理與調(diào)PID控制簡(jiǎn)PIDProportionalIntegral控制是最早發(fā)展起來(lái)的控制策略之一，由于其算法簡(jiǎn)單、魯棒性好和可靠性高，被廣泛應(yīng)用于工業(yè)過(guò)程控制，尤其適用于可建立精確數(shù)學(xué)模型的確定性控制系統(tǒng)。正因?yàn)樗暮?jiǎn)單易用性，因而成為了最早實(shí)用化的控制器，應(yīng)用近百年仍然處于無(wú)法取代的位置。連續(xù)形式離散形式

utKPetKItkt

ukKPekKIeiKDekek 7.飛控原理與調(diào)yr 穩(wěn)態(tài)誤yr 穩(wěn)態(tài)誤Ot調(diào)節(jié)時(shí)上升時(shí)超調(diào)eer++y+uy+DKKI圖.PID控制框效果指標(biāo)

圖.系統(tǒng)階躍響應(yīng)曲)輸入y從10%增加到90%所消耗時(shí)間)。超調(diào)量(Overshoot)：在響應(yīng)過(guò)程中，超出穩(wěn)態(tài)值的最大偏離量與穩(wěn)態(tài)值之比%ymaxy調(diào)節(jié)時(shí)間(SettlingTime)：輸出曲線最終收斂于穩(wěn)態(tài)值(5%以內(nèi))所用的時(shí)間 7.飛控原理與調(diào)PID穩(wěn)定性要求超調(diào)量小無(wú)震蕩，準(zhǔn)確性要求穩(wěn)態(tài)誤差盡量小。PID調(diào)整的主要工作就是如何 I 現(xiàn) I utKet ted0

KP比例增益。優(yōu)點(diǎn)：調(diào)整系統(tǒng)的開(kāi)環(huán)比例系數(shù)，提高系統(tǒng)的穩(wěn)態(tài)精度，加快響應(yīng)速度點(diǎn)：過(guò)大會(huì)使系統(tǒng)的超調(diào)量增大，而且會(huì)使系統(tǒng)穩(wěn)定裕度變小，甚至不穩(wěn)定KI積分增益。優(yōu)點(diǎn)：消除穩(wěn)態(tài)誤差；缺點(diǎn)：影響系統(tǒng)的穩(wěn)定性，使系統(tǒng)的穩(wěn)定裕度減小。KD微分增益。優(yōu)點(diǎn)：使系統(tǒng)的響應(yīng)速度變快，超調(diào)減小，振蕩減輕，對(duì)動(dòng)態(tài)過(guò)程有“預(yù)測(cè)” 7.飛控原理與調(diào)PID調(diào)節(jié)準(zhǔn)上升時(shí)超調(diào)調(diào)節(jié)時(shí)穩(wěn)態(tài)誤系統(tǒng)穩(wěn)定PIKD更形象的說(shuō)，P—比例控制系統(tǒng)的響應(yīng)快速性，快速作用于輸出，好比“現(xiàn)在”（現(xiàn)在就起用，快），I—積分控制系統(tǒng)的準(zhǔn)確性，消除過(guò)去的累積誤差，好比“過(guò)去”（清除過(guò)去積，回到準(zhǔn)確軌道），微分控制系統(tǒng)的穩(wěn)定性，具有超前控制作用，好比“（的理解PID的作用。在調(diào)整的時(shí)候，你所要做的任務(wù)就是在系統(tǒng)結(jié)構(gòu)允許的情況下，在這三個(gè)參數(shù)之間權(quán)衡調(diào)整，達(dá)到最佳控制效果，實(shí)現(xiàn)穩(wěn)快準(zhǔn)的控制特點(diǎn)。 7.飛控原理與調(diào)PID中積分飽積分飽和(Integral ：如果執(zhí)行機(jī)構(gòu)已經(jīng)到極限位置，仍然不能消除偏差時(shí)，盡小相。 ar0

r tab

圖