多旋翼飛行器設(shè)計(jì)與控制實(shí)踐 第10講-定點(diǎn)位置控制器設(shè)計(jì)實(shí)驗(yàn)

第十講定點(diǎn)位置控制器設(shè)計(jì)實(shí)驗(yàn)1.實(shí)驗(yàn)原理2.基礎(chǔ)實(shí)驗(yàn)3.分析實(shí)驗(yàn)4.設(shè)計(jì)實(shí)驗(yàn)5.小結(jié)其中0<ξ<1，階躍響應(yīng)曲線如左圖所示1）超調(diào)量1）超調(diào)量2）調(diào)節(jié)時間在初步分析系統(tǒng)時，經(jīng)常采用下列公式計(jì)算調(diào)節(jié)時間ts。當(dāng)阻尼比<0.8時Bode圖也叫對數(shù)頻率特性曲線，它將開環(huán)幅相特性畫在對數(shù)坐標(biāo)上。對數(shù)穩(wěn)定判據(jù)根據(jù)開環(huán)對數(shù)幅頻與對數(shù)相頻曲線的相互關(guān)系來判別閉環(huán)系統(tǒng)穩(wěn)定性。)1G(jω1)H(jω1)1G(jω1)H(jω1)在閉環(huán)系統(tǒng)穩(wěn)定的條件下，系統(tǒng)的和越大，反應(yīng)系統(tǒng)的穩(wěn)定程度越高。穩(wěn)定裕度也間接反映了系統(tǒng)動態(tài)過程的平穩(wěn)性，裕度大意為著超調(diào)小，振蕩弱，“阻尼”大。一般要求：fd?d,kφθ,τd控制分配器p電機(jī)控制器多旋翼σd,kψdd位置控制器fd?d,kφθ,τd控制分配器p電機(jī)控制器多旋翼σd,kψdd位置控制器底層控制底層控制姿態(tài)控制器 欠驅(qū)動系統(tǒng)：4個輸入（總拉力f和三軸力矩τ)控制6個輸出（位置p和姿態(tài)角Θ)。設(shè)計(jì)多旋翼飛行控制器時，可以采用內(nèi)外環(huán)的控制策略，其中內(nèi)環(huán)對多旋翼飛行器姿態(tài)角進(jìn)行控制，而外環(huán)對多旋翼飛行器的位置進(jìn)行控制。由內(nèi)外環(huán)控制實(shí)現(xiàn)多旋翼飛行器的升降、懸停、側(cè)飛等飛行模態(tài)。.ph=vhψΘhdhdψΘhdhdphdhhd當(dāng)考慮定點(diǎn)控制時phdhdhhdphdhhd當(dāng)考慮定點(diǎn)控制時phdhdhdhd 其中K(.)表示參數(shù)。.pz=vz.fzzdpzdzzd.pz=vz.fzzdpzdzzdzdzd.ph=vhvhdσ=σ=σfdzz高度達(dá)到期望τd其中evhvh?vhd?v速度能達(dá)到期望，位速度能達(dá)到期望，位limevhlimevh水平位置達(dá)到期望σ=σσ=σkk,dfdfdτdτdt→∞vzt→∞vz度也就能達(dá)到期望度也就能達(dá)到期望zz高度達(dá)到期望t→0t→02）高度通道模型vzdpz(pz?pzd)zpevzvzivzdvz)豎直方向速度能達(dá)到期望，豎直方向速度能達(dá)到期望，高度也就能達(dá)到期望σ=σσ=σfdτfd水平位置達(dá)到期望控制器設(shè)計(jì)控制器設(shè)計(jì)Θ>2π同樣地，在開源自駕儀PID設(shè)計(jì)中，我們增加對evh和控制器右端的限幅hhevh+Kvhi≥0。保方向飽和函數(shù)satgd(u,a)的定義如下∞∞保方向飽和函數(shù)satgd(u,a)與傳統(tǒng)的飽和函數(shù)sat(u,a)的區(qū)別：飽和函數(shù)限制每個分量的絕對值不大于a，但它與x的方向可能不同；而保方向飽和函數(shù)，不僅可以限制最終分量每個分量的絕對值不大于a，還可以保證它的方向與x相同。y▲傳統(tǒng)飽和函數(shù)7y▲傳統(tǒng)飽和函數(shù)7TT導(dǎo)致方向偏離TT●..傳統(tǒng)飽和函數(shù)|kkkkx≤akkx>a保方向飽和函數(shù)satgd傳統(tǒng)飽和函數(shù)保方向的飽和函數(shù)可以保證多旋翼直線飛行，而傳統(tǒng)飽保方向的飽和函數(shù)可以保證多旋翼直線飛行，而傳統(tǒng)飽和函數(shù)則不行產(chǎn)生期望歐拉角的位置控制為了避免油門過大，同樣需要加入飽和。在傳統(tǒng)PID控制器設(shè)計(jì)中，我們需要對控制器的右端進(jìn)>0。同樣的，在開源自駕儀PID設(shè)計(jì)中，我們增加對evz和控制器右端的限幅dgdvzpvzvzivzvzdvz,5dgdvzpvzvzivzvzdvz,5>0。對于一維變量，保方向飽和函數(shù)satgd與傳統(tǒng)飽和函數(shù)sat的作用相同。特點(diǎn):提供正的相移。相位超前校正主要發(fā)生在頻段這一最大值發(fā)生在對數(shù)頻率特性曲線的幾何中心處，對應(yīng) 1 趙峙堯,戴訓(xùn)華,任錦瑞,鄧恒譯.《多旋翼飛行器設(shè)計(jì)與控制》,電子（2）軟件：MATALB2017b或以上版本，基于Simulink的控制器設(shè)計(jì)與仿真平臺，硬件在環(huán)仿真平臺，實(shí)驗(yàn)指導(dǎo)包“e6.1”（下載地址：/course）。（1）復(fù)現(xiàn)四旋翼Simulink仿真，分析控制作用在obxb軸和obyb軸的解耦。（2）對系統(tǒng)進(jìn)行掃頻以繪制bode圖，分析閉環(huán)位置控制系統(tǒng)穩(wěn)定裕度；（3）完成硬件在環(huán)仿真。PosEVelEAngEulerAngRateBMotorRPMSinPWMsQuadcopterDynamicsControlSystemPosEVelEAngEulerAngRateBMotorRPMSinPWMsQuadcopterDynamicsControlSystem1）參數(shù)初始化打開“e6\e6.1\sim\Init_control.m文件進(jìn)行參數(shù)初始化，“PosControl_Sim.slx”將會自動打開，如chch1ch2ch3ch4ch5pqrrollpitchyawxyzvxvyvzPWMLED2）開始仿真打開FlightGear-F450程序，然后點(diǎn)擊Simulink“Run”按鈕開始仿真。此時可以在FlightGear中觀察多旋翼空中，然后向oeye軸方向飛行，最3）控制效果高度狀態(tài)變化如圖所示。多旋翼先上升后保持懸停狀態(tài)。4）通道解耦分析四旋翼的水平位置如圖所示?？梢钥吹街粚byb軸位置進(jìn)行控制不會對obxb軸的狀態(tài)產(chǎn)生影響,說明obxb、obyb軸的控制已解耦。14exvxd714exvxd71）打開文件“e6\e6.1\tune\Init_control.m”文件初始化參數(shù),“PosControl_tune.slx”將會自vx_desiredx_desiredxxyxvx打開上述文件的“ControlSystem”子模塊中的“position_control”模塊，將期望x通道輸入線設(shè)為“Open-loopInput”，x通道的實(shí)際輸出設(shè)置為“Open-loopOutput”。相位(deg)相位(deg)幅值(dB)“ControlDesign”,選擇“LinearAnalysis”。3）在彈出的窗口中選擇“LINEARANALYSIS”,點(diǎn)擊Bode得到Bode圖。4）在曲線中點(diǎn)擊鼠標(biāo)右鍵，選擇“Characteristics”-“AllStabilityMargins”,可得到幅值裕度為15.3dB,此時的頻率為3.97rad/s;相位裕度為65.5，頻率為1.04rad/s。0-100-200-3000-180-360-540-720-241230-1-241230頻率(rad/s)'timestamp'uORBWriteTopic:'actuator_outputs''timestamp'uORBWriteTopic:'actuator_outputs'uORBWritephithetapsiqQuaternionvehicle_attitudequat2eulxyzvxvyvzRGB_MODERGBRGB_COLORRGB_Mode_SubsystemRGB_LEDlocalPosition1）打開硬件在環(huán)仿真文件運(yùn)行文件“e6\e6.1\HIL\Init_control.m”進(jìn)行參數(shù)初始化，“PosControl_HIL.slx”將會自pqryawxyzvxvyvzControlSystem2）硬件連接件在環(huán)仿真時機(jī)架類型應(yīng)該是“HILQuadcopterX”。數(shù)據(jù)線，另一數(shù)據(jù)線，另一端連電腦USB口樂迪R9DS接收機(jī)Pixhawk。最左側(cè)排針，順序白JR信號線：紅為電源正，黒為地線，白為信號線圖.硬件系統(tǒng)連接3）代碼編譯及下載將硬件在環(huán)仿真模型編譯并下載文件到Pixhawk自駕儀中。這樣就可以在Pixhawk自駕儀中運(yùn)行我們自己設(shè)計(jì)的位置控制程序。點(diǎn)擊編譯點(diǎn)擊下載下載完成點(diǎn)擊下載圖.代碼編譯及下載流程4）模型仿真器軟件配置。213雙擊CopterSim桌面快捷方式即可213打開CopterSim。讀者可以選擇不同的動力系統(tǒng)模型，步驟如下：單擊“模型參數(shù)”自定義參數(shù)，然后單擊“存儲并使用參數(shù)”存儲并使用參數(shù)。軟件會自動匹配串口號，單擊“開始仿真”按鈕就可以進(jìn)入硬件在環(huán)仿真模式。此時可以看到如所示的界面左下角收到自駕儀返回的相關(guān)消息。5）3DDisplay配置雙擊文件3DDisplay桌面快捷方式打開3DDisplay。6）仿真效果使用遙控器解鎖多旋翼，可以控制多旋翼懸停，以及以指定速度飛行。當(dāng)全部位置速度歐拉角實(shí)際飛行軌跡（1）調(diào)節(jié)PID控制器的相關(guān)參數(shù)改善系統(tǒng)控制性能，并記錄超調(diào)量和調(diào)節(jié)時間，得到一組滿意的分析其穩(wěn)定裕度。PID參數(shù)步驟與姿態(tài)控制的參數(shù)調(diào)試步驟相同。先調(diào)試內(nèi)環(huán)速度環(huán)，再調(diào)試外環(huán)的位置環(huán)，先調(diào)高度再調(diào)水平位置。調(diào)試文件在”e6\e6.2\tune”文件夾中。調(diào)節(jié)參數(shù)的初始狀態(tài)應(yīng)是飛行器處于高空懸停狀態(tài)，將初始高度設(shè)置為100m,電機(jī)的初始轉(zhuǎn)速設(shè)置為557.1420rad/s，這個初始條件對應(yīng)于飛行器在空中100m處懸停。修改“Init_control.m”文件中的對應(yīng)參數(shù)如右。ModelInit_PosE=[0,0,-100];ModelInit_VelB=[0,0,0];ModelInit_AngEuler=[0,0,0];ModelInit_RateB=[0,0,0];ModelInit_RPM=557.1420;4xyx_desiredvx7x4xyx_desiredvx7x首先調(diào)節(jié)內(nèi)環(huán)PID參數(shù)。打開“e6\e6.2\tune\PosControl_tune.slx”文件中的“ControlSystem”子模塊中的“position_control”模塊，即為位置控制系統(tǒng)模型。將其中x通道的速度期望部分換成階躍輸入，并將輸入輸出設(shè)置為“EnableDatalogging”。1Stepvx_desired1Stepexexvxdvx在“Init_control.m”文件中修改內(nèi)環(huán)PID參數(shù)的值。先設(shè)定比例項(xiàng)參數(shù)，積分和微分參數(shù)設(shè)為0，運(yùn)行“Init_control.m”文件。點(diǎn)擊Simulink的“Run”按鈕開始仿真，在“SimulationDataInspector”中查看輸入輸出波形。由小到大逐漸增大比例項(xiàng)系數(shù)值，得到階躍響應(yīng)曲線如圖。速度(m/s)54.543.532.5210.50速度(m/s)54.543.532.5210.50數(shù)，最終得到的響應(yīng)曲線如圖期望期望時間(s)vx_desiredexvxd4x_desiredvx_desiredexvxd4x_desired使用步驟二中得到的速度環(huán)參數(shù)，_輸入和“x”信號線設(shè)置為“EnableDataLogging”。11xxyvx77x位置(m)位置(m)在“Init_control.m”由小增大位置環(huán)比例項(xiàng)系數(shù)，即“Kpxp”的值，在“SimulationDataInspector”觀察階躍響應(yīng)。得到如下波形如圖，再進(jìn)一步微調(diào)比例項(xiàng)系數(shù)得到最終的響應(yīng)曲線。位置(m)時間(s)Kpxp=1.0;Kvxp=2.5;Kvxi=0.4;Kvxd=0.01;時間(s)設(shè)定信號輸入輸出點(diǎn)。將“x_desired”輸入線設(shè)為“Open-loopInput”，“x”設(shè)置為“Open-loopOutput”得到Bode圖如右圖。0-100-200-3000-180-360-540-720（2）理清系統(tǒng)的輸入輸出，正確選擇輸入輸出點(diǎn)，測試開環(huán)系統(tǒng)時選擇輸出點(diǎn)類型為“Open-loopoutput”，測試閉環(huán)時使用“OutputMeasurement”，具體介紹可以參考官方文檔/help/slcontrol/ug/spto-linearize-in-simul（2）軟件：MATLAB2017b或以上版本，基于Simulink的控制器設(shè)計(jì)與仿真平臺，硬件在環(huán)仿真平臺，實(shí)驗(yàn)指導(dǎo)包“e6.3”和“e6.4”（下載地址：htt1)建立位置控制通道的傳遞函數(shù)模型，使用MATLAB“ControlSystemDesigner”設(shè)計(jì)校正控制器,使得加入校正環(huán)節(jié)后系統(tǒng)速度控制環(huán)階躍響應(yīng)穩(wěn)態(tài)誤差erss≤0.01，相位裕度>75°,截止頻率>2.0rad/s。位置控制環(huán)截止頻率>1rad/s,相位裕度>60°。2)使用自己設(shè)計(jì)的控制器進(jìn)行軟件在環(huán)仿真實(shí)驗(yàn)和硬件在環(huán)仿真實(shí)驗(yàn)3)使用自己設(shè)計(jì)的控制器進(jìn)行實(shí)飛實(shí)驗(yàn)。PosEVelE11PosEVelE11首先只對一個通道進(jìn)行分析，這里選擇x通道。簡化后的模型如下圖。GcGc11K-Gc11K-輸入為期望速度，輸出為實(shí)際速度。步驟與生成Bode圖相同。設(shè)置輸入輸出點(diǎn)如圖所示。PosEPosEVelEAngEulerAngRateB生成Bode圖后，在左側(cè)“LinearAnalysisWorkspace”中會出現(xiàn)“l(fā)insys1”變量，按圖示操作即可得到傳遞函數(shù)模型。 化簡3331s4s8由傳遞函數(shù)建立如下m文件，運(yùn)行即可使用Matlab基于Bode圖的控制系12345num=[33315.039e052.563e074.486e085.371e08];%分子den=[12001.567e046.045e051.189e07...1.154e085.557e085.371e08280.1];%分母G=tf(num,den);%傳遞函數(shù)controlSystemDesigner('bode',G);響應(yīng)較慢，向上拖動Bode圖曲線增大開環(huán)增益。增益增大，從階躍響應(yīng)曲線上看，響應(yīng)時間變短，但是帶來了超調(diào)。從Bode圖上看，相位裕度為50.1°,相比設(shè)計(jì)目標(biāo)偏小。增加一個超前校正環(huán)節(jié)，不僅能提高相位裕度，還能進(jìn)一步提高截止頻率，增在Bode圖中右鍵->addPole/Zero->Lead零點(diǎn)極點(diǎn)直接拖動零極點(diǎn)觀測響應(yīng)曲線，得到合適相位(相位(deg)幅值(dB)加入步驟五得到的速度環(huán)校正器，得到位置環(huán)的Bode圖：0-100-200-300-90-180-270-360相位裕度68.5°,截止頻率0.95.基本滿足要求?？陕晕⒃黾釉鲆妫岣呓刂诡l率，如取位置環(huán)增截止頻率為1.12,滿足實(shí)驗(yàn)指標(biāo)。頻率(rad/s)前述設(shè)計(jì)的校正環(huán)節(jié)是時域連續(xù)的環(huán)節(jié)，加入該模型應(yīng)將其變?yōu)闀r域離散的環(huán)節(jié)。使用c2d函數(shù)將s域的傳遞函數(shù)變?yōu)閦域：H=tf([num],[den])Hd=c2d(H,Ts,‘zoh’)其中“num”為傳遞函數(shù)分子系數(shù)向量，“den”為傳遞函數(shù)分母系數(shù)向量，“Ts”為仿真步長，本例中為0.01s。替換后的傳遞函數(shù)為：xyvxexvxd3thetaeaxeay57xyvxexvxd3thetaeaxeay57將Simulink模型中的PID控制器替換成離散模塊，如右圖所示。1vx_desired thetaxvx多旋翼可以實(shí)現(xiàn)基本的直線飛行和懸停。故可認(rèn)為校正環(huán)節(jié)設(shè)計(jì)可實(shí)際飛行試驗(yàn)所采用的多旋翼為F450四旋翼，如圖所示，在實(shí)際飛行時需要在QGC中將Pixhawk的機(jī)架類型從“HILQuadcopterX”修改為“DJIFlameWheelF450”，并完成傳感器校相比設(shè)計(jì)實(shí)驗(yàn)中的硬件在環(huán)仿真模型，這里只是將其PWM輸出部分替換了。為了記錄飛行中的數(shù)據(jù)，我們在模型中加入了