多旋翼飛行器設(shè)計(jì)與控制實(shí)踐 第11講-半自主控制模式設(shè)計(jì)實(shí)驗(yàn)

第十一講半自主控制模式設(shè)計(jì)實(shí)驗(yàn)自動(dòng)化科學(xué)與電氣工程學(xué)院北京航空航天大學(xué)1.實(shí)驗(yàn)原理2.基礎(chǔ)實(shí)驗(yàn)3.分析實(shí)驗(yàn)4.設(shè)計(jì)實(shí)驗(yàn)5.小結(jié)通常，根據(jù)自駕儀自主控制的程度，把處于半自主控制下的多旋翼分為3種模??在自穩(wěn)模式下飛控手可以利用遙控器的滾轉(zhuǎn)/俯仰搖桿控制多旋翼的滾轉(zhuǎn)/俯仰角，從而控制其前后左右飛行，此時(shí)，如果不同時(shí)調(diào)整升降搖桿，多旋翼的高度會(huì)改變，若想使多旋翼保持定點(diǎn)懸停，飛控手但是位置會(huì)漂移。此外，飛控手可以利用遙滾轉(zhuǎn)俯仰油門死區(qū)偏航遙控器（RC）滾轉(zhuǎn)俯仰油門死區(qū)偏航遙控器（RC）通常，根據(jù)自駕儀自主控制的程度，把處于半自主飛控手需要不斷地調(diào)整遙控器的滾轉(zhuǎn)/俯仰搖桿保持?一旦油門搖桿離開死區(qū)范圍，多旋翼將以期望的速度上升或下降。高度控制模式需要高度傳感器的支通常，根據(jù)自駕儀自主控制的程度，把處于半自主控制下高精度的GPS位置信息、磁羅盤信息以及機(jī)身低振?飛控手可以通過推動(dòng)搖桿使多旋翼以指定的速度在三維空間中運(yùn)動(dòng)。位置控制模式需要測(cè)高儀器和位置傳感器的支持才能實(shí)現(xiàn)，例如GPS和攝像機(jī)等。輸出三種模式（自穩(wěn)模式、定高速率,對(duì)角速率積分得到期望偏θ,φ,ψdrc產(chǎn)生期望扭矩，期望油門即為fdrc，相當(dāng)于純姿態(tài)控制。已知φψdrc]T，設(shè)計(jì)控制器使得eΘ(t)我們先針對(duì).設(shè)計(jì)角速度的期望ωd為ωd=?KΘeΘ其中KΘ3×3所有元素都大于0。以上兩式構(gòu)成了角度控制環(huán)。針對(duì)設(shè)計(jì)轉(zhuǎn)矩的期望τd:τdωpeωωiωωdω3×3。以上兩式構(gòu)成了角速度控制環(huán)。定高模式下，期望的油門不再由遙控器直接指定，而是通過位置控制器中高度通道的輸出給定。期望的力矩仍然由遙控器輸入的期望姿態(tài)角經(jīng)姿態(tài)控制器產(chǎn)生，這一點(diǎn)與自穩(wěn)模式相同。.pz=vz設(shè)計(jì)期望的速度為vzd針對(duì)設(shè)計(jì)期望的加速度為zd進(jìn)一步得到期望油門值為.fvzmzdvzi油門搖桿進(jìn)入死區(qū)范圍的時(shí)刻記為油門搖桿進(jìn)入死區(qū)范圍的時(shí)刻記為pzdpzd=pzdold多旋翼高度被自動(dòng)調(diào)節(jié)在z(td)。zdrc此時(shí)油門通道控制的是Z軸方向的速度。與自穩(wěn)模式相似，由于沒有水平位置反饋，多旋翼不能保持懸停。高度保持模式通常用在測(cè)高儀器可以正常使用而位置傳感器不可使用的場(chǎng)合。定點(diǎn)模式下，期望的油門由位置控制器中高度通道的輸出給定，這點(diǎn)與高度控制模式相同。期望力矩由位置控制器中水平位置通道產(chǎn)生的期望姿態(tài)角再經(jīng)姿態(tài)控制器得出。給定期望水平位置，對(duì).ph=vh設(shè)計(jì)期望的速度為vhd針對(duì)設(shè)計(jì)期望的加速度為hhdvhpevhvhievhvhdvh其中hev=vh?vhdh進(jìn)一步得到期望姿態(tài)角為vhpevh?Kvhievhvhdvh)水平通道的實(shí)現(xiàn)與定高模式完全相同。俯仰/滾轉(zhuǎn)搖桿進(jìn)入死區(qū)范圍的時(shí)刻記為td，此時(shí)水平位置估計(jì)量h(td)被保存為phdold=h(td)，vhdrc=0。此時(shí)多旋翼將進(jìn)入位置保持狀態(tài)phd=phdold多旋翼水平位置被自動(dòng)調(diào)節(jié)在h(td)。當(dāng)俯仰/滾轉(zhuǎn)搖桿不在死區(qū)范圍內(nèi)時(shí)，多旋翼進(jìn)入手動(dòng)控制模式phd此時(shí)俯仰/滾轉(zhuǎn)通道控制的是水平方向的速度。當(dāng)多旋翼進(jìn)入位置保持狀態(tài)和高度保持狀態(tài)時(shí)，即此時(shí)搖桿全部回中，多旋翼將保持定點(diǎn)懸趙峙堯,戴訓(xùn)華,任錦瑞,鄧恒譯.《多旋翼飛行器設(shè)計(jì)與控制》,電子（1）在基于Simulink的控制器設(shè)計(jì)與仿真平臺(tái)上，復(fù)現(xiàn)仿真實(shí)驗(yàn)分析四旋翼姿態(tài)和位置響應(yīng)的特點(diǎn)；記錄當(dāng)期望姿態(tài)為零時(shí)的水平位置響應(yīng)；記錄當(dāng)油門回中時(shí)1XY11zEulerAngle21XY11zEulerAngle21）打開Simulink仿真模型打開“e7\e7.1\sim\Init_control.m”，文“StabilizeControl_Sim.slx”將會(huì)自動(dòng)打開如右圖。可以看到模型在姿態(tài)角輸出部分加了常值擾動(dòng)，以模擬實(shí)際飛行過程中的偏差。為觀測(cè)自穩(wěn)模式下姿態(tài)和位置的輸出，設(shè)置如圖所示示波器觀測(cè)姿態(tài)和位置的輸出狀態(tài)。2）開始仿真并分析記錄實(shí)驗(yàn)結(jié)果。點(diǎn)擊Simulink開始仿真按鈕，開始仿真，觀察并記錄波形，分析實(shí)驗(yàn)結(jié)果。期望俯仰角和橫滾角為0，實(shí)際滾轉(zhuǎn)角和俯仰始并不為0，然后在姿態(tài)控制器的調(diào)節(jié)下滾轉(zhuǎn)角和俯仰角趨近期望滾轉(zhuǎn)角，最終達(dá)到穩(wěn)態(tài)，逼近期在這個(gè)過程中，產(chǎn)生一個(gè)非零速度，導(dǎo)致水平位置輸出如圖所示，可以看到，因?yàn)楦蓴_的存在，水平位置又缺乏反饋控制，最終水平位置產(chǎn)生了漂移。所示。所以在自穩(wěn)模式下，四旋翼只能姿態(tài)而不能保持位置穩(wěn)定，懸停油門盡管是經(jīng)過uORBWriteTopic:'actuator_outputs'qxyzvxvyvzRGB_MODERGB_COLORuORBWriteTopic:'actuator_outputs'qxyzvxvyvzRGB_MODERGB_COLOR1）打開Simulink仿真模型打開“e7\e7.1\HIL\StabilizeControl_HIL.slx”Simulink文件，如右圖。值得注意的是，“ControlSystem”模塊和軟件在環(huán)仿真的相chch1ch2ch3ch4ch5pqrrollpitchyawxyzvxvyvzRC_Armed/LEDPWMControlSystem2）硬件連接需要注意的是，在進(jìn)行硬件在環(huán)仿真時(shí)機(jī)架類型應(yīng)該是“HILQuadcopterX”。數(shù)據(jù)線，另一數(shù)據(jù)線，另一端連電腦USB口樂迪R9DS接收機(jī)Pixhawk。最左側(cè)排針，順序白JR信號(hào)線：紅為電源正，黒為地線，白為信號(hào)線圖.硬件系統(tǒng)連接3）代碼編譯及下載將硬件在環(huán)仿真模型編譯并下載文件到Pixhawk自駕儀中。這樣就可以在Pixhawk自駕儀中運(yùn)行我們自己設(shè)計(jì)的半自主控制程序。點(diǎn)擊編譯點(diǎn)擊下載下載完成點(diǎn)擊下載圖.代碼編譯及下載流程4）模型仿真器軟件配置。213雙擊CopterSim桌面快捷方式即213可打開CopterSim。讀者可以選擇不同的動(dòng)力系統(tǒng)模型，步驟如下：單擊“模型參數(shù)”自定義參數(shù)，然后單擊“存儲(chǔ)并使用參數(shù)”存儲(chǔ)并使用參數(shù)。軟件會(huì)自動(dòng)匹配串口號(hào)，單擊“開始仿真”按鈕就可以進(jìn)入硬件在環(huán)仿真模式。此時(shí)可以看到如所示的界面左下角收到自駕儀返回的相關(guān)消息。位置速度5）3位置速度雙擊3DDisplay桌面快捷方式打開3DDisplay。歐拉角6）用遙控器解鎖多旋翼，實(shí)現(xiàn)手動(dòng)控制。歐拉角通過遙控器給定四旋翼一個(gè)期望的姿態(tài)，當(dāng)遙控器搖桿全部回中時(shí)，四旋翼的姿態(tài)基實(shí)際飛行軌跡（2）軟件：MATLAB2017b或以上的版本，基于Simul（1）在自穩(wěn)模式的基礎(chǔ)上改成定高模式。根據(jù)實(shí)驗(yàn)分析，與自穩(wěn)模式相比，多旋翼在定高模式下姿態(tài)和位置輸出值的變化。自穩(wěn)模式閉環(huán)控制框圖如所示，Switch1選擇θdrc和φdrc作為期望姿態(tài)角（期望偏航角在三種模式下的來源相同而Switch2選擇fdrc作為期望高度控制模式下，Switch1選擇θdrc和φdrc作為高度控制閉環(huán)控制框圖如圖所示?；癁関zdrc，它經(jīng)過高度控制器之后會(huì)產(chǎn)生期望油門fdap。1）設(shè)置遙控器輸入的定高死區(qū)若給u輸入1）設(shè)置遙控器輸入的定高死區(qū)若給u輸入[1000,2000]的斜坡信號(hào)，則對(duì)應(yīng)的輸出如右圖，同時(shí)這里對(duì)輸入信號(hào)進(jìn)行了歸一化處理所以輸出的信號(hào)幅值在[-1,1]。123456789deadZone=deadZoneRate*y=(u-RCMid-deadZone)*y=(u-RCMid+deadZone)ooz2）設(shè)置期望位置當(dāng)油門桿偏離中間時(shí)，期望高度即為當(dāng)前高度，在這樣的期望高度下，高度誤差為零所以位置反饋中的高度控制部分不工作，只存在方向的速度反饋。當(dāng)油門桿在中間位置時(shí)，期望高度為搖桿回到中間時(shí)刻的當(dāng)前高度值pzold，若 遙感一直在中間位置，期望高度一直保持pzold不變，設(shè)計(jì)如右圖所示。123456789ifisempty(z1)ifisempty(holdzflag)ifabs(vzd)<0.001&&abif(hold_z>0.5)&&(holdzfla%油門不在中間位置，期望高度即為當(dāng)前高度-T-01ch12ch234ch4ch3InputConditioning1xPWM-T-定高模式1ch112ch23ch34ch4InputConditioningInputConditioning1死區(qū)模塊期望位置模塊1xPWM-T-01ch12ch234ch4ch3InputConditioning1xPWM-T-定高模式1ch112ch23ch34ch4InputConditioningInputConditioning1死區(qū)模塊期望位置模塊1xPWM3）建立高度控制模型將前面設(shè)計(jì)的死區(qū)和期望位置模塊加入自穩(wěn)模式模型中，并將“mode”的輸入由0改為1，表示使用高度控制模式，即得到高度控制模型。4）開始仿真并進(jìn)行試驗(yàn)結(jié)果分析姿態(tài)和水平位置輸出與自穩(wěn)模式下相同，即姿態(tài)能保持穩(wěn)定，水平位置不能保持穩(wěn)定。4）開始仿真并進(jìn)行試驗(yàn)結(jié)果分析當(dāng)高度輸入在1460~1540之間時(shí)，即油門遙感在中間死區(qū)時(shí)，如右圖?？梢钥吹礁叨炔▌?dòng)很小誤差在±0.002m之間，可以認(rèn)為高度保持穩(wěn)定成立。4）開始仿真并進(jìn)行試驗(yàn)結(jié)果分析以看到oeze軸實(shí)際速度跟隨期望速度并保持穩(wěn)定，pqryawxyzControlSystemlocalPosition'timestamp'uORBWriteTopic:'actuator_outputs''output'Ch1Ch2Ch3Ch4Ch5GyroX(p)GyroY(q)GyroZ(r)ModeColorQuaternionRGB_Mode_Subsystem12:34theta-C-quat2eulqyxz1）搭建硬件在環(huán)仿真模型將“StabilizeControl_HIL.slx”模型中的“ControlSystem”模塊替換成高度控制模式的Simulink模型中的ControlSystem模塊即可得到高度控制模式的硬件在環(huán)仿真模型“HeightControl_HIL.slx”.2）硬件連接需要注意的是，在進(jìn)行硬件在環(huán)仿真時(shí)機(jī)架類型應(yīng)該是“HILQuadcopterX”。數(shù)據(jù)線，另一數(shù)據(jù)線，另一端連電腦USB口樂迪R9DS接收機(jī)Pixhawk。最左側(cè)排針，順序白JR信號(hào)線：紅為電源正，黒為地線，白為信號(hào)線圖.硬件系統(tǒng)連接3）代碼編譯及下載將硬件在環(huán)仿真模型編譯并下載文件到Pixhawk自駕儀中。這樣就可以在Pixhawk自駕儀中運(yùn)行我們自己設(shè)計(jì)的半自主控制程序。點(diǎn)擊編譯點(diǎn)擊下載下載完成點(diǎn)擊下載圖.代碼編譯及下載流程4）模型仿真器軟件配置。213雙擊CopterSim桌面快捷方式即213可打開CopterSim。讀者可以選擇不同的動(dòng)力系統(tǒng)模型，步驟如下：單擊“模型參數(shù)”自定義參數(shù)，然后單擊“存儲(chǔ)并使用參數(shù)”存儲(chǔ)并使用參數(shù)。軟件會(huì)自動(dòng)匹配串口號(hào)，單擊“開始仿真”按鈕就可以進(jìn)入硬件在環(huán)仿真模式。此時(shí)可以看到如所示的界面左下角收到自駕儀返回的相關(guān)消息。5）3DDisplay配置雙擊3DDisplay桌面快捷方式打開3DDisplay。6）用遙控器解鎖多旋翼，實(shí)現(xiàn)手動(dòng)控制。使用遙控器控制四旋翼時(shí)，四旋翼的姿態(tài)與水平位置的表現(xiàn)與自穩(wěn)模式相同，當(dāng)友們搖桿回中時(shí)，四旋翼高度能保持穩(wěn)定。歐拉角實(shí)際飛行軌跡位置速度（2）軟件：MATLAB2017b或以上的版本，基于Simulink的控制器設(shè)計(jì)與仿真平（1）在自穩(wěn)模式的基礎(chǔ)上改成定點(diǎn)模式。根據(jù)實(shí)驗(yàn)分析，與自穩(wěn)模式相比，多旋翼在定點(diǎn)模式下姿態(tài)和位置輸出值的變化。（2）利用三段撥碼開關(guān)實(shí)現(xiàn)三種模式的自由切換，完成硬件在環(huán)仿真實(shí)驗(yàn)和實(shí)飛實(shí)1）位置控制模式設(shè)計(jì)期望油門fdrc，另一個(gè)是給位置控制器的期望的z軸速度vzdrc。期望俯仰角和滾轉(zhuǎn)角θdrc,φdrc，另一個(gè)是給位置控制器的期望x、y軸速度vhdrc。1）位置控制模式設(shè)計(jì)位置控制模式：switch1選擇θdap,φdap作為期望姿態(tài)角，switch2選擇fdap作為期望油門。2）設(shè)置位置保持死區(qū)位置保持死區(qū)設(shè)置與定高的死區(qū)設(shè)置相對(duì)應(yīng)的輸出如圖，這里同時(shí)對(duì)輸入信號(hào)進(jìn)行了歸一化處理所以輸出的信號(hào)幅值在[-1,1]。 3）設(shè)置期望水平位置。期望位置即為當(dāng)前位置，此時(shí)位置反饋無效，只存在速度反饋。當(dāng)搖桿在中間位置時(shí)，期望位置為搖桿回到中間時(shí)刻的位置pdold，若搖桿一直在中間位置，期望位置一直保持pdold不變。123456789ifisempty(x1)…ifisempty(holdxflag)…ifabs(vxd)<0.001&&abs(vx)<8%搖桿進(jìn)入死區(qū)，且速度較小，啟用if(hold_x>0.5)&&(holdxflag<0.5)%啟用了位置控制，進(jìn)入定點(diǎn)ifhold_x<0.5%沒有啟用定高，期望位置即為當(dāng)前4）設(shè)計(jì)模式轉(zhuǎn)換控制器設(shè)置遙控器的一個(gè)三段開關(guān)為模式切換開關(guān)，這里將遙控器CH5設(shè)置為模式切換開關(guān)。對(duì)于ch5，將其設(shè)置為值在1000-1400時(shí)，多旋翼為自穩(wěn)模式；值在1400-16002000時(shí)，多旋翼為位置控制模式。123456789定點(diǎn)模式死區(qū)模塊期望位置模塊定點(diǎn)模式死區(qū)模塊期望位置模塊1）建立位置控制模型將前面設(shè)計(jì)的死區(qū)和期望位置模塊加入自穩(wěn)模式模型中，并將mode的輸入由0改為2，表示使用位置控制模式，即得到定點(diǎn)控制模型。2）開始仿真并進(jìn)行試驗(yàn)結(jié)果分析高度能保持穩(wěn)定。2）開始仿真并進(jìn)行試驗(yàn)結(jié)果分析當(dāng)滾轉(zhuǎn)、俯仰搖桿輸入在1460~1540之間時(shí)，水平位置輸出如圖?？梢钥吹?，盡管滾轉(zhuǎn)、俯仰通道上固定干擾，但是在定點(diǎn)模式下，這種干擾被很好地抑制了。當(dāng)俯仰輸入為1600時(shí)，觀測(cè)到X軸的速度如下圖所示。速度輸出能夠比較好的跟隨期望速度。Ch1Ch2Ch3Ch4Ch5Ch6-C-GyroX(p)GyroY(q)GyroZ(r)Quaternionqquat2Ch1Ch2Ch3Ch4Ch5Ch6-C-GyroX(p)GyroY(q)GyroZ(r)Quaternionqquat2eulxyzModeColorRGB_Mode_SubsystemlocalPosition1）搭建硬件在環(huán)仿真模型將“StabilizeControl_HIL.slx”模型中的“ControlSystem”模塊替換成位置控制模式的Simulink模型中的“ControlSystem”模塊，并加入遙控器的ch5通道進(jìn)行模式切換，即可得到三種模式切換“ModeSwitch_HIL.slx”.pqrxyzControlSystem2）硬件連接需要注意的是，在進(jìn)行硬件在環(huán)仿真時(shí)機(jī)架類型應(yīng)該是“HILQuadcopterX”。數(shù)據(jù)線，另一數(shù)據(jù)線，另一端連電腦USB口樂迪R9DS接收機(jī)Pixhawk。最左側(cè)排針，順序白JR信號(hào)線：紅為電源正，黒為地線，白為信號(hào)線圖.硬件系統(tǒng)連接仿真實(shí)驗(yàn)步驟3）代碼編譯及下載將硬件在環(huán)仿真模型編譯并下載文件到Pixhawk自駕儀中。這樣就可以在Pixhawk自駕儀中運(yùn)行我們自己設(shè)計(jì)的半自主控制程序。點(diǎn)擊編譯點(diǎn)擊下載下載完成點(diǎn)擊下載圖.代碼編譯及下載流程4）模型仿真器軟件配置。213雙擊CopterSim桌面快捷方式即213可打開CopterSim。讀者可以選擇不同的動(dòng)力系統(tǒng)模型，步驟如下：單擊“模型參數(shù)”自定義參數(shù)，然后單擊“存儲(chǔ)并使用參數(shù)”存儲(chǔ)并使用參數(shù)。軟件會(huì)自動(dòng)匹配串口號(hào)，單擊“開始仿真”按鈕就可以進(jìn)入硬件在環(huán)仿真模式。此時(shí)可以看到如所示的界面左下角收到自駕儀返回的相關(guān)消息。仿真實(shí)驗(yàn)步驟5）3DDisplay配置雙擊3DDisplay桌面快捷方式打開3DDisplay。CH5撥碼開關(guān)切換到自穩(wěn)模式時(shí)，四旋翼的響應(yīng)與基礎(chǔ)實(shí)驗(yàn)中的一致；當(dāng)切換到定高模式時(shí)，四旋翼的表現(xiàn)與設(shè)計(jì)實(shí)驗(yàn)時(shí)一致；當(dāng)經(jīng)過調(diào)整后在空中懸停。位置速度歐拉角實(shí)際飛行軌跡實(shí)際飛行試驗(yàn)所采用的多旋翼為F450四旋翼，如圖所示，在實(shí)際飛行時(shí)需要在QGC中將Pixhawk的機(jī)架類型從“HILQuadcopterX”修改為“DJIFlameWheelF450”，并完成傳感器校準(zhǔn)。HIL模型HIL模型實(shí)飛實(shí)驗(yàn)步驟相對(duì)硬件在環(huán)仿真模型，這里將其PWM輸出部分進(jìn)行了替換。實(shí)飛模型實(shí)飛模型實(shí)飛實(shí)驗(yàn)步驟與硬件在環(huán)仿真過程中編譯下載代碼過程完全一致。為確保安全，可在飛機(jī)上系上安全繩，并將安全繩的另一端固定在重物上，如右圖所示。另外應(yīng)在空曠場(chǎng)圖.室外實(shí)飛position(m)43210-1-2-3-4-5posit