多旋翼飛行器設(shè)計(jì)與控制理論 -第13講-任務(wù)決策V2

2024/5/2832024/5/2842024/5/285（1）任務(wù)規(guī)劃通常由操作人員離線完成，主要（1）任務(wù)規(guī)劃通常由操作人員離線完成，主要（2）路徑規(guī)劃根據(jù)飛行任務(wù)點(diǎn)給定期望的可飛路徑，即期望的飛行器空間三維坐標(biāo)?？梢匀蝿?wù)決策層任務(wù)規(guī)劃路徑規(guī)劃期望路徑期望偏航角期望路徑是連續(xù)的路徑，也可以是離散的目標(biāo)點(diǎn)。底層控制層（3）底層控制控制電機(jī)轉(zhuǎn)動，實(shí)現(xiàn)對可飛路徑的底層控制層2024/5/2862024/5/287視頻：多無人機(jī)協(xié)同農(nóng)田噴灑協(xié)議設(shè)計(jì),https://youtu.be/fkp043jE5fQ2024/5/2881）給定條件①已知期望航路點(diǎn);②已知多旋翼實(shí)時位置p和電量。pwp,nwppwp,nwpwpwpwp2024/5/2892）任務(wù)需求①多旋翼從充電站出發(fā)，按S型遍歷所有期望航路點(diǎn)；②期望偏航角指向下一個航路點(diǎn)；③當(dāng)電量低于門限值時，多旋翼保存當(dāng)前位置并返回充電站充電，充電pwp,nwppwp,nwpwpwpwp完成后飛回原地繼續(xù)完成任務(wù)；④遍歷完所有航路點(diǎn)后多旋翼飛回充電站。2024/5/28輸入：多旋翼實(shí)時位置信息p，實(shí)時電量信息b。輸出：實(shí)時目標(biāo)位置pd（作為位置控制器期望輸入）。1）確定矩形區(qū)域坐標(biāo)系；2）根據(jù)任務(wù)需求生成需要遍歷的航路點(diǎn),pwp,nwp坐標(biāo)及充電站坐標(biāo)pwp,0，并將所有航路點(diǎn)坐標(biāo)依次壓入棧中；3）令k=nwp，并令當(dāng)前目標(biāo)航路點(diǎn)為pd=pwp,k，設(shè) 置ε>0,bth>0，其中ε用于判斷多旋翼th是電量門限值；ppwp,0pwp,nwppwp,1pwp,02024/5/284）如果pd?p≤ε,說明多旋翼已進(jìn)入目標(biāo)航路點(diǎn)，轉(zhuǎn)至5）否則轉(zhuǎn)至65）如果pd=pwp,0&k=0，說明多旋翼已遍歷完航路點(diǎn)，結(jié)束任務(wù)；如果pd=pwp,0&k≠0，說明任務(wù)還沒完成，只是電池電量過低返回充電。當(dāng)多旋翼充電完成后，停在原地等待起飛指令。一旦接收到起飛dwp,k并跳轉(zhuǎn)至4）繼續(xù)運(yùn)行；如果pd≠pwp,0，說明多旋翼任務(wù)還未完成且電量d2024/5/28說明多旋翼在飛向航路點(diǎn)的過程中電量多旋翼先飛回充電站充電再返回當(dāng)前位置繼續(xù)完成任務(wù)，該過程通過執(zhí)行以下指令實(shí)現(xiàn)：wp,kwp,kpd=pwp,k.否則跳轉(zhuǎn)至4）繼續(xù)運(yùn)行。否則跳轉(zhuǎn)至wp,k=pwp,0pwp,kpwp,1pwp,1pwppwp,02024/5/281）路徑規(guī)劃：多旋翼在飛行中不僅受到自身約束的限制，還會受到環(huán)境約束的影響。路徑規(guī)劃是為了給多旋翼提pwp,kpwp,k+1=pd,k+3供既滿足自身約束又滿足環(huán)境約束的可飛路徑。2）全局路徑規(guī)劃和局部路徑規(guī)劃[1]：根據(jù)任務(wù)生成航路點(diǎn)，并用直線將航路點(diǎn)連接起來，這一過程稱為全局路徑規(guī)劃；根據(jù)局部環(huán)境條件和多旋翼的動力學(xué)運(yùn)動學(xué)約束將直線路徑提煉為可飛路徑，這一過程稱為局部路徑規(guī)劃。2024/5/283）人工勢場法：給目標(biāo)航路點(diǎn)和直線路徑分配吸引勢場，給障礙物分配排斥勢場，使三維飛行區(qū)域處于目標(biāo)航路點(diǎn)、直線路徑和障礙物的疊加勢場中，飛行在該區(qū)域中的多旋翼將會被目標(biāo)航路點(diǎn)和直線路徑同時吸引，而被障礙物排斥，使得多旋翼在飛向目標(biāo)航路點(diǎn)的同時沿著直線飛行，且能避開障礙物[2]。[2]KhatibO.Real-timeobstacleavoidanceformanipulatorsandmobilerobots.Theinternationaljournalofroboticsresearch,1986,5(1):90-982024/5/28(航路點(diǎn)，pwp,last∈R3表示上一個航路點(diǎn)。設(shè)計(jì)局部路徑規(guī)劃器實(shí)時地生路徑飛行直到到達(dá)目標(biāo)航路點(diǎn)pwp。2）算法設(shè)計(jì)將多旋翼看作質(zhì)點(diǎn)模型，其滿足牛頓第二定律..v=u2024/5/28pwpwp,lastpwppwpwp,lastpwp那么pwppwp,perpwp)其中Tpwp,last?pwppwp,last?pwp32pd垂足可以表示為TTwppwp,last?pwp′wp,lastwp2pwpwp,lastpwp,lastp wpp2024/5/282）算法設(shè)計(jì)若定義李雅普諾夫函數(shù)為0wp靠近目標(biāo)航路點(diǎn)靠近期望路徑kk 2+意味著多旋翼能同時靠近目標(biāo)航路點(diǎn)和期望路徑。2024/5/282）算法設(shè)計(jì)然而，這里需要考慮控制量的飽和，因而需要對李雅普諾夫函數(shù)進(jìn)行示由wp參數(shù)化的平滑曲線。李雅普諾夫函數(shù)可以設(shè)計(jì)為Cpgdk0wpC靠近目標(biāo)航路點(diǎn)并靠近直線航路.T~~T.T~~T2024/5/282）算法設(shè)計(jì)k0wpk0wp根據(jù)不變集理論可知2系統(tǒng)將全局收斂到(p,v)，其中p是方程（1）的3×1。調(diào)節(jié)參數(shù)k0,k1可以調(diào)整多旋翼靠近到直線航路和靠近到目標(biāo)航路點(diǎn)的相對快慢2024/5/2820pdpdpd2）算法設(shè)計(jì)將虛擬控制量寫成PD控制器的形式其中pdpdpdk0pwpd的物理意義如右圖所示。2024/5/2821Y(m)Y(m)50-5-10111pwp,pY(m)Y(m)50-5-10111pwp,p03）算法仿真pdpdk1k1越大，多旋翼靠近到直線航路的速度越快!wppwp,lastpwppwp,last↑-1005-5-1005X(m)50-5-10-10-50X(m)(a)人工勢場(b)從p0出發(fā)在pwp結(jié)束的三條飛行路徑2024/5/2822pwpropwpropopwp,last3pp航路點(diǎn)。設(shè)計(jì)局部路徑規(guī)劃器實(shí)時地生成期望位置pd∈3，引導(dǎo)多旋翼飛向目標(biāo)航路點(diǎn)1.飛行器剛開始不在障礙區(qū)域內(nèi)；2.目標(biāo)位置離障礙足夠遠(yuǎn)。翼飛向目標(biāo)航路點(diǎn)1.飛行器剛開始不在障礙區(qū)域內(nèi)；2.目標(biāo)位置離障礙足夠遠(yuǎn)。2）兩個假設(shè)假設(shè)2：目標(biāo)航路點(diǎn)pwp滿足≈0。2024/5/28233）算法設(shè)計(jì)定義oo，考慮到飽和處理，定義函數(shù)Ck0C靠近目標(biāo)航路點(diǎn)k1避障kk vv2>，即多旋翼不會與障礙物發(fā)生碰撞。2024/5/28243）算法設(shè)計(jì)如果虛擬控制量wp,a0Tv2v2vTudogd(v,a1)o2024/5/28253）算法設(shè)計(jì)3）算法設(shè)計(jì) gd2o進(jìn)一步進(jìn)一步u=0。2其中awpo2,κax,∞,∞,∞,∞的解。參數(shù)k0,k1可以調(diào)整多旋翼靠近到目標(biāo)航路點(diǎn)和避障的相對快慢。2024/5/2826BACCpwpopoBACCpwpopoor ′p1 ′p2porp是不穩(wěn)定的的平衡點(diǎn)是不穩(wěn)定的的平衡點(diǎn)pwp是唯一穩(wěn)定的平衡點(diǎn)awpo排斥力 ′p2024/5/2827pdp0r0p0ppwppwppwpp(a)人工勢場(b)避障算法輸出的期望位置qpodpdp0r0p0ppwppwppwpp(a)人工勢場(b)避障算法輸出的期望位置qpod3）算法設(shè)計(jì)3）算法設(shè)計(jì)把虛擬控制量寫成PD控制器的形式d其中pdod的物理意義如圖（b）所示。2024/5/2828Y(m)Y(m)5050Y(m)Y(m)5050k1越大，多旋翼越早做出避障行為！k1越大，多旋翼越早做出避障行為！4）算法仿真pwp > >0505X(m)pwp物1p0k=1001k=15015050X(m)(a)人工勢場(b)從p0出發(fā)在pwp結(jié)束的3條壁障路徑2024/5/2829opoopo通過疊加直線路徑、目標(biāo)位置具體可以參考本門課程給定的pwpr▲pwp,lastp綜合算法示意圖2024/5/2830通常，根據(jù)自駕儀自主控制的程度，把處于半自主控制下的多旋翼分?自穩(wěn)定模態(tài)（StabilizeMode）?高度保持模態(tài)（AltitudeHoldMode）?定點(diǎn)模態(tài)（LoiterMode）2024/5/28通常，根據(jù)自駕儀自主控制的程度，把處于半自主控制下的多旋翼分?在遙控（RC?在遙控（RC）模式下飛控手可以利用遙控器的滾轉(zhuǎn)/俯仰搖桿控制多旋翼的滾轉(zhuǎn)/俯仰角，從整升降搖桿，多旋翼的高度會改變，若想使多旋翼保持定點(diǎn)懸停，飛控手需要不斷調(diào)整遙控?當(dāng)飛控手釋放搖桿時，多旋翼自動進(jìn)入自動（AC）模式，多旋翼會自動保持自身水平，但是位置會漂移。此外，飛控手可以利用遙控器的偏航搖桿控制多旋翼的偏航角。當(dāng)飛控手釋放偏航搖桿時，多旋翼會保持當(dāng)前機(jī)頭方向。?自穩(wěn)定模態(tài)（StabilizeMode）/?高度保持模態(tài)（AltitudeHoldMode）?定點(diǎn)模態(tài)（LoiterMode）2024/5/28滾轉(zhuǎn)滾轉(zhuǎn)●俯仰油門死區(qū)●俯仰油門死區(qū)偏航遙控器（RC）通常，根據(jù)自駕儀自主控制的程度，把處于半自主控制下的多旋翼分為3種模態(tài)：?自穩(wěn)定模態(tài)（StabilizeMode）?高度保持模態(tài)（AltitudeHoldMode）?定點(diǎn)模態(tài)（LoiterMode）圖13.17遙控器搖桿示意圖?如圖所示，當(dāng)油門搖桿位于死區(qū)范圍時，多旋翼自動進(jìn)入自動模式，然后油門被自動地調(diào)整來保持當(dāng)前的高度，飛控手需要不斷地調(diào)整遙控器的滾轉(zhuǎn)/俯仰搖桿保持懸入遙控模式，也就是說此時多旋翼是上升高度保持模態(tài)需要高度傳感器的支持才能實(shí)現(xiàn)，例如氣壓計(jì)、超聲波測距儀等。2024/5/28?定點(diǎn)模態(tài)下多旋翼能自動保持當(dāng)前的水平位置、航向和高度。當(dāng)飛控手釋放滾轉(zhuǎn)/俯?定點(diǎn)模態(tài)下多旋翼能自動保持當(dāng)前的水平位置、航向和高度。當(dāng)飛控手釋放滾轉(zhuǎn)/俯仰搖桿，偏航搖桿，并將油門搖桿推到死區(qū)范圍時，多旋翼自動進(jìn)入自動模式，并保持當(dāng)前的水平位置、航向和高度。高精度的GPS位置信息、磁羅盤信息以及機(jī)身低振動對獲得良好的定點(diǎn)效果非常重要。通常，根據(jù)自駕儀自主控制的程度，把處于半自主控制下的多旋翼分為3種模態(tài)：?自穩(wěn)定模態(tài)（StabilizeMode）?高度保持模態(tài)（AltitudeHoldMode）?控模式來實(shí)現(xiàn)對多旋翼水平位置、航向和高度的控制。定點(diǎn)模態(tài)需要測高儀器和位定點(diǎn)模態(tài)（LoiterMode）?飛控手可以通過推動搖桿使多旋翼進(jìn)入遙?控模式來實(shí)現(xiàn)對多旋翼水平位置、航向和高度的控制。定點(diǎn)模態(tài)需要測高儀器和位置傳感器的支持才能實(shí)現(xiàn)，例如GPS和攝像2024/5/28ddrc分別表示期望值和遙控器輸出值。在半自主控制下，油門/偏航搖桿和滾轉(zhuǎn)/俯仰搖桿分別用來輸出期望總拉力fdrc=uT，期望偏航角速率drc=uωz，期望滾轉(zhuǎn)角φdrc=uφ和期望俯仰角θdrc=uθ。在直接式油門中，遙控器輸出的總拉力指令與油門搖桿的偏移量成正比。此外，一些多旋翼產(chǎn)品，例如大疆的小精靈，采用增量式油門，這種油門有兩個特點(diǎn)：①飛控手釋放油門搖桿時油門搖桿能自動彈回死區(qū)位置；②油門搖桿的偏移量與期望的垂直方向速率或者拉力變化2024/5/28f死區(qū)0σdrc死區(qū)0.40.50.61死區(qū)0圖13.18油門搖桿偏移量與油門控制量的關(guān)系drc中σdrc=0.5表示油門搖桿的中間位置。如fdrc(σdrc)=0，然后自動控制器開始引入高度反饋進(jìn)行高度保持。在這里，前饋控制量mg由自動控制器給出，所以，如圖所示，fdrc從-mg開始。死區(qū)用來消除油門搖桿微弱變化2024/5/28接管多旋翼的控制。事實(shí)上，無論多旋翼處于自穩(wěn)定模態(tài)、高度保持模態(tài)還是定點(diǎn)模態(tài)，任務(wù)決策模塊都會產(chǎn)生期望姿態(tài)或者期望位置pdac和偏航角ψdac，狀態(tài)估計(jì)模塊都會產(chǎn)生位置估計(jì)量=xyzT和姿態(tài)估計(jì)量=T，自動模式的任務(wù)都是控制多旋翼飛行使得導(dǎo)航信息的估計(jì)精度會有所不同。2024/5/28偏航通道油門通道水平通道py偏航通道油門通道水平通道pyd=yΨd遙控器指令pzd=zΨdpzdpxd,pyd拉力和力矩。如圖所示，自駕儀根據(jù)遙控器指令生成期望位置pd2024/5/28phdhphphhd))zA1Kphdh fd=mg+mkpzdz2024/5/2839這意味著自動控制器能使多旋翼保持水平。此外，自動控制器并由于缺少位置信息反饋，自穩(wěn)定模態(tài)不能使多旋翼個模態(tài)通常在沒有GPS和高度測量傳感器或者這些傳感器失效的時2024/5/2840偏航通道油門通道pxd=xpyd=yΨd偏航通道油門通道pxd=xpyd=yΨdpzd=pzdold是否期望水平位置phd=h和期望偏航角Ψdhdpzdold=zpz=Ψdpzdpxd,pyd圖13.20高度保持模態(tài)下期望位置和偏航角的生成原理2024/5/2841水平通道pyd=yΨdpzd水平通道pyd=yΨdpzd=pzdold偏航通道是否在是油門通道否遙控器指令pzdold=zddoldpz=ddoldpzdΨdpxd,pzdΨd圖13.21高度保持模態(tài)下期望位置和偏航角的生成原理如圖所示，將油門搖桿回到中間位置的時刻記為tzd，將該時刻的高度估計(jì)量記為pzdold=z(tzd)。與此同時，高度保持模態(tài)將多旋翼的高度保持在pzd=pzdold。信息反饋，高度保持模態(tài)也不能使多旋翼穩(wěn)定懸停。高度保持模態(tài)通常在高度測量傳感器可用，而缺少位置傳感器或電子羅盤不可用的時候使用。2024/5/2842遙控器指令偏航控制搖桿油門控制搖桿俯仰控制搖桿是否在死區(qū)內(nèi)？是ppp滾轉(zhuǎn)控制搖桿否是p=pp=pp=遙控器指令偏航控制搖桿油門控制搖桿俯仰控制搖桿是否在死區(qū)內(nèi)？是ppp滾轉(zhuǎn)控制搖桿否是p=pp=pp=ppΨΨΨΨpp=pppΨ是否在是否在是否否否是否在是圖13.22定點(diǎn)模態(tài)下期望位置和期望偏航角的生成原理如圖所示，定點(diǎn)模態(tài)根據(jù)期望的位置pd=pdold和偏航角Ψd=Ψdold生成期望總拉力和力矩。將4個搖桿回到中間位置的時刻分別記為tΨd,tzd,txd,tyd，將xdoldxxd這些時刻的估計(jì)量分別記為Ψdold=xdoldxxdtzd)tyd)自主控制器將控制多旋翼懸停在pd=pdold并保持偏航角為Ψd=Ψdold。定點(diǎn)模態(tài)通常在高度測量傳感器和位置傳感器都可用的時候使用。2024/5/2843滾轉(zhuǎn)控制搖桿是否在死區(qū)內(nèi)？是否在是否在是否否否是否是pyd=pydoldpxdold=xpxd=pxdoldddoldpzdold滾轉(zhuǎn)控制搖桿是否在死區(qū)內(nèi)？是否在是否在是否否否是否是pyd=pydoldpxdold=xpxd=pxdoldddoldpzdold=zpz=pzΨd=Ψdoldpxd=pxdoldΨd=Ψdoldpydold=ypy