多旋翼飛行器設(shè)計與控制實踐 第03講-實驗平臺使用

第三講實驗平臺使用2.控制器設(shè)計與仿真平臺3.PSP工具箱5.硬件在環(huán)多旋翼飛行器仿真器Pixhawk自駕儀系統(tǒng)Pixhawk自駕儀系統(tǒng)(1)(1)基于Simulink的控制器設(shè)計與仿真平臺(2)硬件在環(huán)仿真平臺(1)多旋翼模型配置與運(yùn)動模擬器(2)多旋翼三維飛行顯示程序模擬測試，排除試飛實驗可能存在的各種問題數(shù)，通過實驗驗證來評估設(shè)計控制算法的性能主文件見“e0\1.SoftwareSi圖.提供的環(huán)境展示例程FlightGearFlightGear接口圖.Simulink仿真環(huán)境例程圖.控制器模塊內(nèi)部示意圖左圖所示例程：簡單的俯仰和滾轉(zhuǎn)姿態(tài)的保持與控制，控制器響應(yīng)遙控器的控制輸入，將多旋翼控制2.遙控信號處理：將通道信號映射為期望姿態(tài)角3.姿態(tài)控制器：計算期望輸出力和力矩大小4.電機(jī)輸出分配：將力和力矩的控制量映射為四個5.輸出接口：將控制量補(bǔ)齊并映射為電機(jī)PWM信號1）模型輸入：電機(jī)的PWM控制量2）模型輸出:多旋翼的狀態(tài)和傳感器信息圖.多旋翼模型內(nèi)部示意圖圖.多旋翼模型內(nèi)部示意圖(a)Simulink“運(yùn)行”按鈕（MATLAB2017b-2019a）(b)Simulink“運(yùn)行”按鈕（MATLAB2019b及更高版本）有三個輸入量：多旋翼位置，多旋翼姿態(tài)歐拉角和電機(jī)的PWMFlightGear三維視景軟件在FlightGear界面中看到多旋翼從地面緩慢起飛。你可圖.FlightGear界面圖.FlightGear界面工具箱是Mathworks公司官方為Pixhawk推出的一Pixhawk硬件系統(tǒng)之間的關(guān)系。 個工具箱。該工具箱能在工具箱是Mathworks公司官方為Pixhawk推出的一Pixhawk硬件系統(tǒng)之間的關(guān)系。 個工具箱。該工具箱能在Simulink中利用嵌入式代碼產(chǎn)生器（EmbeddedCoder）將Simulink模型自 1.能在Simulink中對不同的飛機(jī)模型和自駕儀算法進(jìn)行仿真和測試，并能自動將算法部2.工具箱提供了一些實用實例，包括燈光控制、遙控器數(shù)據(jù)處理和姿態(tài)控制器等；3.工具箱中提供了很多接口模塊，用于訪問Pixhawk的軟硬件組件；4.能自動記錄傳感器、執(zhí)行機(jī)構(gòu)以及自己部署進(jìn)去的控制器的飛行數(shù)據(jù)；5.能訂閱和發(fā)布uORB話題消息。PX4自駕儀軟件的所有數(shù)據(jù)都暫存在一個uORB消息池中，通過uORB訂閱功能可以從消息池中讀取感興趣的話題，通過uORB發(fā)布功能可以1.Pixhawk自駕儀系統(tǒng)的軟硬件結(jié)構(gòu)包括：硬件系統(tǒng)（類似于電腦主機(jī)）+PX4自駕儀軟2.整個PX4自駕儀軟件系統(tǒng)可以分為若干個小模塊，每個模塊獨立運(yùn)行（多線程并行各個模塊通過uORB消息模塊的訂閱與發(fā)布功能實現(xiàn)數(shù)據(jù)的傳輸與交互。3.Simulink生成的代碼部署到PX4自駕儀軟件之后，不會影響原生PX4自駕儀軟件的運(yùn)行，4.由于原生PX4控制算法可能需要訪問和“px4_simulink_app”同樣的硬件輸出資源，這會產(chǎn)生讀寫沖突。因此，平臺一鍵部署腳本提供了自動屏蔽PX4原生固件對執(zhí)行器的選項，以確保只有“px4_simulink_app”模塊能夠輸出電機(jī)控制量。2.將該代碼導(dǎo)入到PX4自駕儀的源代碼中，生成一個“px4_simulink_app”獨立運(yùn)行的程序；3.PSP工具箱調(diào)用編譯工具將所有代碼編譯為“.px4”的PX4自駕儀軟件固件文件；4.將得到的固件下載到Pixhawk自駕儀中并燒錄，由Pixhawk自駕儀執(zhí)行帶有生成的算法代碼的PX4軟件PX4軟件 SensorsPosition&AttitudeEstimatorPositionContollerNavigatorMixerActuatorPosition&RateControllerRC注：另一種可行方法是修改PX4模塊啟動腳本文件"Firmware\ROMFS\px4fmu_common\init.d\rcS"，并注釋掉想要屏蔽的模塊SensorsPosition&AttitudeEstimatorPositionContollerNavigatorMixerActuatorPosition&RateControllerRC注：另一種可行方法是修改PX4模塊啟動腳本文件"Firmware\ROMFS\px4fmu_common\init.d\rcS"，并注釋掉想要屏蔽的模塊生成的Simulink代碼也可以用于替換如右圖所示PX4控制軟件的某些原生模塊（傳感器、濾波器、姿態(tài)控制器等），但是需要手動修改PX4固件代碼來屏蔽原始模塊的輸出接口。例如，想用Simulink實現(xiàn)一個濾波器模塊（輸入傳感器數(shù)據(jù)，輸出狀態(tài)濾波數(shù)據(jù)）來替換PX4原始的濾波器，需要手動屏蔽右圖中的“Position&AttitudeEstimator”濾波器模塊向uORB消息池發(fā)布濾波后姿MessageBusuORBPx4_Simulink_appMessageBusuORBPx4_Simulink_app2.在其中屏蔽掉“ORB_ID(vehi將其替換為“UNUSED(att);”。這里UNUSED用于防止3.在Simulink中編寫姿態(tài)濾波器，并用uORBWrite模塊發(fā)圖.SimulinkPSP工具箱展示圖.PSP工具箱傳感器和執(zhí)行器接口庫示意圖PSP工具箱官方還提供了大量的例程（詳見文件夾“e0\2.PSPOfficialExps”）以及官方使用手冊（詳見文檔“e0\2.PSPOfficia\Pixhawk_Pilot_Support_Package.pdf”）。通圖圖.PSP工具箱官方例程及使用手冊送PWM信號到PX4IO的輸出端口以控圖.電機(jī)PWM輸出模塊及其參數(shù)設(shè)置框器的通道數(shù)量，以及其他的一些信息。每個選項的含義可點擊對話框的“help”按鈕查看或者查閱官方“e0\2.PSPOfficialExps\px4demo_圖.遙控器輸入模塊及其參數(shù)設(shè)置框蜂鳴器模塊：通過這個模塊可以控制蜂鳴器在特定的事件發(fā)出特定的音調(diào)。（PSP工具箱提供的該模塊學(xué)圖.蜂鳴器模塊及其參數(shù)設(shè)置框RGB_LED模塊：通過這個模塊可以控制LED燈閃爍一個是模式（Mode另一個是顏色（Color可用模式和可用顏色可以點擊對話框的“help”按鈕“e0\2.PSPOfficialExps\px4demo圖.多彩LED燈模塊及其參數(shù)設(shè)置框傳感器組合模塊：通過這個模塊可以獲取Pixhawk中可用的傳感器數(shù)據(jù)，然后這些數(shù)據(jù)可以用于控制模型儀、氣壓計和時間戳。(PSP工具箱提供學(xué)習(xí)例程見“e0\2.PSPOfficialExps\px4demo_attitude_system.slx”)圖.傳感器數(shù)據(jù)組合模塊及其參數(shù)設(shè)置框姿態(tài)數(shù)據(jù)模塊：該模塊提供了經(jīng)過濾波的姿態(tài)數(shù)據(jù)的姿態(tài)數(shù)據(jù)。(PSP工具箱提供的該模塊學(xué)習(xí)例程見“e0\2.PSPOfficialExps\px4demo_attitude_system.slx”)圖.姿態(tài)模塊及其參數(shù)設(shè)置框GPS數(shù)據(jù)模塊：通過這個模塊可以獲取Pixhawk圖.GPS數(shù)據(jù)模塊及其參數(shù)設(shè)置框“battery_status”實現(xiàn)的，所以在實際運(yùn)行時需圖.電池數(shù)據(jù)模塊及其參數(shù)設(shè)置框理論上通過理論上通過uORB消息的讀寫，我們可以獲取并改變飛控中的所有數(shù)uORB模塊。通過左下圖所示的uORB模塊可以從PX4自駕儀軟件中讀取或發(fā)送uORB消息，其中PX4自駕儀支持的所有uORB消息類型可以在軟件包安裝目錄（默認(rèn)路徑是“C:\PX4PSP”）下的“Firmware\msg”目錄中查看。雙擊左下圖中的“uORBwrite”模塊可以打開右下圖所示的uORB消于消息讀寫的uORB寫接口模塊參uORB模塊數(shù)設(shè)置模塊框點擊右上圖“uORBwrite”模塊配置界面中的“Open.msgfile”按鈕可以打開選定的uORB消息文件（見圖左下圖點擊按鈕“Open.msgfolder”打開消息目錄（見右下圖）。圖.uORB消息文件 圖.uORB模塊參數(shù)設(shè)置框“Open.msgfolder”按鈕彈出框后綴“_dai”的模塊允許發(fā)送基于某uORB消實際上，本小節(jié)前面提到的所有模塊在底層均是通過uORB消息的讀寫來實現(xiàn)的。理論上通過uORB消息的讀寫，我們可以獲取并改變自駕儀內(nèi)的所有消息和中間變量，實現(xiàn)更高級的控制器功能。因此掌握該模塊的使用，我們擁有更大的權(quán)限與自由度，使得實現(xiàn)更為復(fù)雜的功能稱為“e0\2.PSPOfficialExps\px4demo_fcn_call_uorb_example.slx”和“e0\2.PSPOfficialExps\px4demo._._為了方便調(diào)參與測試，PSP工具箱還提供了訪問Pixhawk內(nèi)部參數(shù)的方法，這樣可以在飛行測試實驗中，通過地面站軟件來修改Simulink生成控制器參數(shù)，而不需要在Simulink中修改后再生成代碼。官方例子見“e0\2.PSPOfficialExps\px4demo_Parameter_CSC_example.slx”文件。使用時需要將參數(shù)聲圖.Pixhawk參數(shù)讀取例程圖.Pixhawk參數(shù)讀取的Simulink初始腳本設(shè)置PX4PSPDemo-AttitudeControlSystemQuaternionARMOutputpwm1pwm2pwm3pwm4pwm5pwm6PX4PSPDemo-AttitudeControlSystemQuaternionARMOutputpwm1pwm2pwm3pwm4pwm5pwm6quat2eulCh1Ch2Ch3Ch4Ch5TuneIDIsTuneOverrideTriggerGyroX(p)GyroY(q)GyroZ(r)Tune_Mode_SubsystemModeColor1)控制器代碼生成前的準(zhǔn)備工作rollpx4demo_attitude_controlpitchch1ch2ch3ch4ch5PWMpARM_ControlqrControlSubsystemCopyright2015-2016TheMathWorks,Inc.圖.控制器連接PSP接口模塊箱模塊庫中的輸入（傳感器、遙控“e0\2.PSPOfficialExps\px4demo_a2)選擇目標(biāo)硬件為了讓Simulink根據(jù)目標(biāo)硬件編譯和組建模型，需要點擊Simulink的“設(shè)置”按鈕進(jìn)行相Board”下拉框中設(shè)置硬件板卡為“PixhawkPX4”。3)編譯與固件生成右圖點擊Simulink的“診斷”按鈕(a)Simulink“編譯”按鈕（MATLAB2017b~2019a）(b)Simulink“編譯”按鈕（MATLAB2019b及更高版本）“診斷”按鈕(a)(a)Simulink“診斷”按鈕（MATLAB2017b~2019a）(b)Simulink“診斷”按鈕（MATLAB2019b及更高版本）4)編譯過程與結(jié)果一次成功的編譯過程在“DiagnosticViewer”窗口中到編譯進(jìn)度條完成，同時顯所示的“CodeGeneration欄【Code】-【PX4PSP:UploadcodetoPx4FMU】(a)Simulink的“PX4Upload”按鈕(b)MATLAB的“PX4Upload”命令（MATLAB2017b~2019a）（MATLAB2019b及更高版本）將代碼上傳進(jìn)入飛控；對于MATLAB2019b及更“PX4Upload”命令來下載固件。注意看窗口提示，有時需要重新插拔Pixhawk才MicroUSBPixhawk自駕儀MicroUSBPixhawk自駕儀(CH4)(CH1)開關(guān)遙控方式(CH6)本課程使用的遙控器被設(shè)置成了“美國手”的操縱方式搖桿對應(yīng)的油門與偏航控制量，而右側(cè)搖桿對應(yīng)滾轉(zhuǎn)與俯仰。遙控器中滾轉(zhuǎn)、俯仰、油門和偏航分別對應(yīng)了接收機(jī)的CH1~CH4通道，左右上側(cè)撥桿對應(yīng)了CH5/CH6號通道，用于觸油門：控制上下運(yùn)動，對應(yīng)固定翼油門桿偏航：控制機(jī)頭轉(zhuǎn)向，對應(yīng)固定翼方向舵俯仰：控制前后運(yùn)動，對應(yīng)固定翼升降舵滾轉(zhuǎn)：控制左右運(yùn)動，對應(yīng)固定翼副翼圖.油門：控制上下運(yùn)動，對應(yīng)固定翼油門桿偏航：控制機(jī)頭轉(zhuǎn)向，對應(yīng)固定翼方向舵俯仰：控制前后運(yùn)動，對應(yīng)固定翼升降舵滾轉(zhuǎn)：控制左右運(yùn)動，對應(yīng)固定翼副翼圖.遙控器通道示意圖端到最右端對應(yīng)PWM信號從1100到1900；俯仰（CH2通道）搖桿從最下端到最上端對應(yīng)PWM信號從1900到1100；CH5/6為三段開關(guān)，從頂部（最遠(yuǎn)離使用者的檔位）到底部（最靠近使用者的612345612345打開“打開“QGroundControl”軟件1）點擊設(shè)置按鈕（右圖齒輪圖標(biāo)2）點擊“Firmware”標(biāo)簽，此時用USB動檢測自駕儀（注意：后續(xù)操作僅針對Pixhawk1，這里直接點擊“OK”使用官方最新固件即可，不需要進(jìn)行后續(xù)步驟）QGroundControlv3.4.4QGroundControlv3.4.4SelectFirmwareFileC>e0>2.PSPOfficialExpsNewFolder QuickaccessOneDriveThisPCpx4fmu-v3_defNetworkpx4px4fmu-v3_default1.7.3Stable.px4SerialCommProtocolExampleExternalMode_Examplesqgc_tune_parameter pixhawk_A.bindatalog_A.binOrganize55）在彈出標(biāo)簽頁中選擇7）此時會彈出Windows的文件選擇界面，選擇“\e0\2.PSPOfficial\px4fmu-并點擊“打開”按鈕，此時地面站會將固件上傳并燒錄到里的.px4固件問題只針對Pixhawk1或Cube，對于其他Pixhawk硬件產(chǎn)品請從如下網(wǎng)址下載合適固件數(shù)據(jù)線連接到Pixhawk自接自駕儀時，可見到右架模式。這一項設(shè)置對后續(xù)的硬件在環(huán)仿真至關(guān)重要，如果不處于該模式設(shè)置方法：選中右下圖中的“HIL偏航通道滾轉(zhuǎn)通道偏航通道滾轉(zhuǎn)通道開關(guān)1、正確連接自駕儀與接收機(jī)，用USB數(shù)據(jù)線連接自遙控方式俯仰通道油門：控制上下運(yùn)動，對應(yīng)固定翼油門桿油門：控制上下運(yùn)動，對應(yīng)固定翼油門桿偏航：控制機(jī)頭轉(zhuǎn)向，對應(yīng)固定翼方向舵俯仰：控制前后運(yùn)動，對應(yīng)固定翼升降舵滾轉(zhuǎn)：控制左右運(yùn)動，對應(yīng)固定翼副翼俯仰：控制前后運(yùn)動，對應(yīng)固定翼升降舵滾轉(zhuǎn)：控制左右運(yùn)動，對應(yīng)固定翼副翼3、點擊右下圖的“Calibrate”按鈕，按提示可以校4、點擊QGC地面站上的“Calibrate”–“Next”按鈕，然后依次將搖桿置于右圖所示位置（根據(jù)QGC頁面的實時提示）即可俯仰通道俯仰通道滾轉(zhuǎn)通道開關(guān)遙控方式偏航通道油門：控制上下運(yùn)動，對應(yīng)固定翼油門桿偏航：控制機(jī)頭轉(zhuǎn)向，對應(yīng)固定翼方向舵俯仰：控制前后運(yùn)動，對應(yīng)固定翼升降舵滾轉(zhuǎn)：控制左右運(yùn)動，對應(yīng)固定翼副翼1、經(jīng)過上面的遙控器校準(zhǔn)步驟后，點擊地面站進(jìn)入“FlightModes”（飛行模式）設(shè)置頁面，選擇“ModeChannel”（模式通道）為前面測試過的CH6通道。由于CH6通道是一個三段開關(guān)，開關(guān)的頂部、中部、下部檔位分別對應(yīng)了2、如右圖所示，將這三個標(biāo)簽分別設(shè)置為“Stabilized”（自穩(wěn)模式，只有姿態(tài)控制）、“Altitude”（定高模式，姿態(tài)和高度控制）和“Position”（定點模式，有姿態(tài)、定高和水平位置控制）。在后續(xù)的硬件在環(huán)仿真中，可以通過切換不同的模式來體驗不同的控制效果。硬件在環(huán)多旋翼飛行器仿真器包括一個實時運(yùn)動仿真軟件——CopterSim和一個三維可視化視景軟件——3DDisplay。下面，我們分別對這兩個軟件進(jìn)行介紹。1)雙擊桌面的CopterSim1)雙擊桌面的CopterSim快捷方式打軟件，其界面如左圖所示。CopterSim的仿真模型和參數(shù)與前文提到的Simulink多旋翼軟件在環(huán)模型相同，因為CopterSim是通過Simulink多旋翼模型代碼生成整合而成的。模擬器軟件運(yùn)行在Pixhawk自駕儀進(jìn)行串口通信。2)該界面分為兩大部分，上半部分是讀者自定義選擇多旋翼模型的輸入界面，下半部分是連接自駕儀進(jìn)行仿真的界面。注意：本軟件僅開放了本書所有實驗需要的功能，還有許多實用的功能（多機(jī)仿真、UE4高逼真場景、固定翼等其他機(jī)型仿真等）需要注冊之后才能使用。CopterSim+3CopterSim+3DDisplay Pixhawk自駕儀系統(tǒng)?CopterSim軟件會發(fā)送傳感器數(shù)據(jù)給Pixhawk自駕儀，然后自駕儀會解算得到的電機(jī)PWM控制信號并?因此Pixhawk自駕儀可以實時控制CopterSim中的模擬多旋翼，實現(xiàn)和真實飛行同樣的效果。?與此同時，飛機(jī)的姿態(tài)與位置信息會以UDP協(xié)議向本地網(wǎng)絡(luò)中發(fā)送，而3DDisplay軟件會接收這些信息來完成實時三維視景顯示。圖.模型參數(shù)頁面模型參數(shù)頁面，默認(rèn)顯示的是上一次仿真存儲的模型參數(shù)。2）該界面主要包含了懸停信息（懸停時間、油門、輸出功率、電機(jī)轉(zhuǎn)速等）和飛機(jī)模型的基本參數(shù)（總質(zhì)量、轉(zhuǎn)動慣量、半徑、拉力系數(shù)和阻力系數(shù)等）。3）點擊該頁面的“還原默認(rèn)值”可以將模型參數(shù)還前參數(shù)存儲到數(shù)據(jù)庫中，作為本次和后續(xù)仿真的默認(rèn)參數(shù)（重啟軟件參數(shù)也會保留，下次直接點擊“開始4）“噪聲水平（0~1）”按鈕是控制模型噪聲的系數(shù)，“0”是啟用最低噪聲，“1”是噪聲水平與實際Pixhawk自駕儀傳感器噪聲一致。特別注意1：如果使用硬件在環(huán)仿真，飛控連上電腦后需要等待10特別注意1：如果使用硬件在環(huán)仿真，飛控連上電腦后需要等待10秒鐘左右，等Pixhawk啟動完畢后才能在CopterSim點擊“開始仿真”按鈕特別注意2：CopterSim開始仿真后，需要等待消息欄顯示“**EKFinitializationfinished”后，才能使用使用遙控器解鎖并控制飛機(jī)拉菜單中就會出現(xiàn)可連接的Pixhawk自駕儀串口。選中需要的Pixhawk自駕儀串口（通常名字中包含字符串“FMU”），點擊“開始仿真”按鈕，就可以開始硬件在環(huán)仿真。如上圖所示，CopterSim可以接收到Pixhawk回傳的消息，說明硬件仿真正常運(yùn)行。在仿真過程中也可以點雙擊桌面的3DDisplay快捷方式即可打開雙擊桌面的3DDisplay快捷方式即可打開3DDisplay主界面界面窗口左側(cè)以3D圖形的方式展示多旋翼當(dāng)前的飛行狀態(tài)；3DDisplay主界面右上角窗口展示了基本的飛行數(shù)據(jù)，包括電機(jī)轉(zhuǎn)速，位置信息，顯示多旋翼的飛行軌跡。圖.三維顯示器程序主界面注意1：由于性能和兼容性等問題，RflySim高級版中已經(jīng)取消3DDisplay軟件，全面使用RflySim3D來進(jìn)行三維場景的觀察圖.三維顯示器程序主界面雙擊桌面的Rf