做過(guò)多旋翼飛控開(kāi)發(fā)的朋友大概率都有過(guò)這種經(jīng)歷在Gazebo里把PID參數(shù)調(diào)得漂漂亮亮飛機(jī)懸停穩(wěn)得像釘在地上信心滿滿地?fù)Q到真機(jī)結(jié)果剛解鎖推油門機(jī)身就開(kāi)始高頻哆嗦嚴(yán)重的時(shí)候直接原地翻跟頭。問(wèn)題出在哪里SITL軟件在環(huán)仿真里PX4固件跑在x86處理器上傳感器數(shù)據(jù)由仿真器以近乎理想的方式喂進(jìn)來(lái)調(diào)度時(shí)序、總線讀取、中斷優(yōu)先級(jí)統(tǒng)統(tǒng)和真實(shí)飛控?zé)o關(guān)。于是你根本分不清是控制器參數(shù)不對(duì)、傳感器噪聲太大還是電調(diào)響應(yīng)跟不上。硬件在環(huán)仿真HIL就是為這個(gè)場(chǎng)景準(zhǔn)備的——把真實(shí)飛控板接進(jìn)仿真回路用仿真世界的傳感器數(shù)據(jù)喂給它再把它的控制輸出送回仿真世界讓整條鏈路的實(shí)時(shí)性、時(shí)序和采集噪聲暴露在正式試飛之前。這篇文章我完整梳理一遍Simulink與PX4硬件在環(huán)仿真的實(shí)現(xiàn)路徑覆蓋PX4端HITL模式配置、Simulink端動(dòng)力學(xué)模型與MAVLink消息收發(fā)搭建、聯(lián)調(diào)排錯(cuò)要點(diǎn)以及這套平臺(tái)能擴(kuò)展的故障注入和自動(dòng)化測(cè)試方法給正在從純仿真邁向真機(jī)驗(yàn)證的開(kāi)發(fā)者一條可以直接走通的路線。1. 為什么SITL出不了問(wèn)題卻在真機(jī)上翻車HIL到底補(bǔ)上了哪一環(huán)1.1 純軟件仿真與硬件在環(huán)的本質(zhì)差異先把這個(gè)概念掰開(kāi)。SITLSoftware-in-the-Loop里面PX4是作為L(zhǎng)inux/x86原生的進(jìn)程跑在電腦上的物理飛控板完全不存在。傳感器驅(qū)動(dòng)、I2C/SPI總線、中斷處理都被模擬器數(shù)據(jù)替代或者干脆繞過(guò)了。這意味著飛控自身對(duì)時(shí)間的感知是理想化的——任務(wù)調(diào)度器不會(huì)因?yàn)榭偩€等待而阻塞EKF拿到的傳感器數(shù)據(jù)沒(méi)有真實(shí)采樣過(guò)程中的時(shí)序抖動(dòng)控制輸出經(jīng)過(guò)網(wǎng)絡(luò)回到仿真器時(shí)也沒(méi)有額外延遲。HITLHardware-in-the-Loop則把PX4原樣編譯進(jìn)真實(shí)飛控板比如Pixhawk 4或者Holybro Pixhawk 6X。板子上的ARM處理器、任務(wù)調(diào)度器、中斷機(jī)制全部真實(shí)運(yùn)行只是傳感器數(shù)據(jù)來(lái)源從真實(shí)IMU/GPS換成了MAVLink注入的仿真數(shù)據(jù)。這個(gè)區(qū)別非常關(guān)鍵你驗(yàn)證的不只是控制算法還包括固件在真實(shí)硬件上的調(diào)度行為、傳感器數(shù)據(jù)被EKF接收后的實(shí)際表現(xiàn)、MAVLink鏈路的通信質(zhì)量。我見(jiàn)過(guò)不少開(kāi)發(fā)者在SITL里完成了整套姿態(tài)控制驗(yàn)證上真機(jī)后第一次起飛就碰到EKF告警、position漂移甚至電機(jī)振蕩。這些問(wèn)題的根子大多不是控制律算錯(cuò)了而是SITL環(huán)境下傳感器數(shù)據(jù)的理想程度遠(yuǎn)高于真機(jī)飛控里很多針對(duì)異常數(shù)據(jù)的安全邏輯根本沒(méi)有觸發(fā)。HIL的價(jià)值就是把這些藏在時(shí)序和噪聲里的問(wèn)題提前暴露出來(lái)而且不需要承擔(dān)炸機(jī)風(fēng)險(xiǎn)。1.2 在HIL閉環(huán)里Simulink到底扮演什么角色很多人第一次接觸HIL時(shí)會(huì)搞混Simulink和PX4的職責(zé)。在SimulinkPX4的硬件在環(huán)架構(gòu)里PX4是主飛控跑在真實(shí)硬件上負(fù)責(zé)姿態(tài)估計(jì)、控制解算、混控輸出。Simulink不是用來(lái)當(dāng)控制器的而是扮演虛擬環(huán)境被控對(duì)象傳感器這三個(gè)角色接收PX4發(fā)出的執(zhí)行器指令解算飛行器剛體動(dòng)力學(xué)再把計(jì)算出的姿態(tài)、角速度、位置、氣壓、GPS信號(hào)編碼成MAVLink消息重新喂給PX4。這個(gè)回路和真機(jī)飛行的數(shù)據(jù)流是嚴(yán)格對(duì)應(yīng)的。真機(jī)上IMU和GPS提供測(cè)量PX4內(nèi)部EKF融合后得到狀態(tài)估計(jì)控制器輸出電機(jī)轉(zhuǎn)速指令在HIL里Simulink模型相當(dāng)于一個(gè)可編程的物理世界PX4感覺(jué)自己在飛實(shí)際上是在飛一個(gè)數(shù)學(xué)方程描述的虛擬機(jī)體。如果你想讓Simulink做外環(huán)控制器、PX4只做底層的姿態(tài)穩(wěn)定和電機(jī)輸出那架構(gòu)上就是另一回事了——那叫控制器快速原型數(shù)據(jù)流方向和控制權(quán)限分配都不同。為了不混淆這篇文章全程假設(shè)PX4是主控Simulink是虛擬被控對(duì)象這也是HIL仿真的經(jīng)典定義。2. 三種落地路線對(duì)比直連HIL、FMU模型交換還是嵌入式代碼集成2.1 路線AMAVLink HIL消息直連最正統(tǒng)的硬件在環(huán)PX4固件原生支持HITL模式。開(kāi)啟后飛控內(nèi)部的傳感器模塊不再?gòu)腎2C/SPI總線讀取真實(shí)IMU而是等待MAVLink的HIL_SENSOR消息注入imu、氣壓計(jì)、GPS等uORB主題的數(shù)據(jù)源全部切到虛擬通道。PX4解算出的控制量也不會(huì)輸出到電調(diào)而是通過(guò)HIL_ACTUATOR_CONTROLS消息發(fā)回仿真器。這條路線對(duì)固件的侵入最小不需要改一行PX4代碼只需要Simulink端按照MAVLink協(xié)議編解碼消息完成執(zhí)行器指令到傳感器數(shù)據(jù)的換算。這也是我最終選定的方案因?yàn)樗Ａ袅送暾腜X4原生安全邏輯、Mag校準(zhǔn)流程和EKF故障保護(hù)機(jī)制跟你將來(lái)跑真機(jī)時(shí)使用的固件完全一致。2.2 路線BFMU導(dǎo)出與聯(lián)合仿真更適合算法階段驗(yàn)證FMI/FMU標(biāo)準(zhǔn)最近幾年在飛控圈子越來(lái)越常見(jiàn)?？梢园裇imulink控制模型導(dǎo)出成FMU也可以把PX4的SITL固件封裝成FMU供Simulink調(diào)用。這種方式的優(yōu)點(diǎn)是省去手動(dòng)編寫MAVLink收發(fā)邏輯Simulink和PX4通過(guò)標(biāo)準(zhǔn)接口交互變量開(kāi)發(fā)速度快很多。但FMU方式有個(gè)繞不開(kāi)的缺陷導(dǎo)出的PX4模型本質(zhì)上還是跑在x86處理器上硬件時(shí)序、中斷優(yōu)先級(jí)這些真實(shí)感全丟了。它更適合在SITL階段快速嘗試控制算法、做早期參數(shù)掃掠不適合做上真機(jī)前最后一道驗(yàn)證。如果你的目標(biāo)是驗(yàn)證控制律本身而不是驗(yàn)證飛控硬件和軟件鏈路選這條路線效率更高。我自己會(huì)在HIL之前先通過(guò)FMU把外環(huán)控制器的結(jié)構(gòu)和參數(shù)空間縮小避免一上來(lái)就在HIL環(huán)境下漫無(wú)目的地調(diào)參。2.3 路線C模型代碼生成后嵌入PX4嚴(yán)格說(shuō)不是HIL但常見(jiàn)另一種常見(jiàn)做法是用Simulink設(shè)計(jì)控制器通過(guò)Embedded Coder生成C代碼然后以自定義模塊的形式集成到PX4固件中替換掉原有的控制律模塊。這種方式的最終形態(tài)是模型驅(qū)動(dòng)的嵌入式開(kāi)發(fā)在產(chǎn)品階段價(jià)值很大因?yàn)樗验_(kāi)發(fā)和部署統(tǒng)一到了同一條工具鏈上。但在HIL仿真的語(yǔ)境里這條路線的定位有些尷尬。代碼集成后你測(cè)試的是融合了自定義控制器的PX4在硬件上的表現(xiàn)沒(méi)法把固件原版控制器當(dāng)作基準(zhǔn)來(lái)對(duì)照排查問(wèn)題時(shí)固件、模型、硬件之間的問(wèn)題會(huì)互相糾纏。如果你要做HIL是為了驗(yàn)證PX4整體的魯棒性建議不要選路線C如果你本來(lái)就要交付一個(gè)產(chǎn)品化控制器那路線C值得認(rèn)真考慮。2.4 選型建議對(duì)比表對(duì)比維度路線AMAVLink直連HIL路線BFMU聯(lián)合仿真路線C代碼集成對(duì)PX4固件的修改無(wú)需修改無(wú)但模型非真機(jī)態(tài)需集成自定義控制器硬件時(shí)序真實(shí)性完整保留基本丟失完整保留傳感器鏈路驗(yàn)證精準(zhǔn)弱取決于集成方式開(kāi)發(fā)工作量中高需碼MAVLink低高適用場(chǎng)景上真機(jī)前最關(guān)鍵驗(yàn)證控制算法探索產(chǎn)品化控制器落地我的建議很明確如果你目標(biāo)是驗(yàn)證PX4與真實(shí)硬件的可靠性選A如果你還在早期摸索控制架構(gòu)選B如果你本來(lái)就要做產(chǎn)品級(jí)嵌入式控制器交付選C。3. PX4一側(cè)的HITL模式準(zhǔn)備固件、參數(shù)和傳感器靜默3.1 固件版本與SYS_HITL參數(shù)開(kāi)啟PX4官方對(duì)HITL的支持在1.13及之后的版本都比較穩(wěn)定推薦直接選當(dāng)前官方主線的穩(wěn)定release版本避免用太老的1.9、1.10。固件用標(biāo)準(zhǔn)版即可不需要做任何編譯期的特殊配置HITL是一個(gè)運(yùn)行時(shí)功能。開(kāi)啟方法很簡(jiǎn)單用Micro USB連上飛控和QGroundControl在參數(shù)列表里搜索 SYS_HITL把值從0改成1然后重啟飛控。QGC新版界面里右上角也有模擬模式入口選擇Hardware In The Loop后會(huì)自動(dòng)引導(dǎo)你設(shè)置對(duì)應(yīng)參數(shù)。設(shè)置完重啟后通過(guò)MAVLink Console輸入param show SYS_HITL如果輸出是1說(shuō)明HITL模式已經(jīng)啟用。這時(shí)飛控會(huì)打印類似waiting for HIL sensor data的提示說(shuō)明它已經(jīng)準(zhǔn)備好接收仿真器數(shù)據(jù)。3.2 HITL模式下EKF的準(zhǔn)備與傳感器靜默這里有個(gè)很多人忽視的細(xì)節(jié)HITL模式下真實(shí)傳感器數(shù)據(jù)源雖然被切走了但EKF的初始化和校準(zhǔn)流程并沒(méi)有被豁免。第一次接上QGC進(jìn)入HITL后我強(qiáng)烈建議老老實(shí)實(shí)做一遍加速度計(jì)、陀螺儀、磁力計(jì)的標(biāo)準(zhǔn)校準(zhǔn)以及水平校準(zhǔn)。雖然這些校準(zhǔn)在HITL中不會(huì)真正影響傳感器讀數(shù)但它會(huì)初始化EKF里的初始偏置估計(jì)跳過(guò)這一步EKF狀態(tài)在啟動(dòng)后很容易因?yàn)槌跏计卯惓６L(zhǎng)時(shí)間處于不健康狀態(tài)。磁力計(jì)校準(zhǔn)要特別小心。電腦、調(diào)試器、電源線在地面測(cè)試時(shí)都會(huì)產(chǎn)生磁場(chǎng)干擾如果仿真?zhèn)鞲衅髂Ｐ徒o的是一個(gè)固定磁場(chǎng)方向的簡(jiǎn)單向量校準(zhǔn)過(guò)程會(huì)變得很奇怪——讀數(shù)一直不變或者緩慢漂移。我的做法是先不啟用磁力計(jì)主導(dǎo)的航向估計(jì)把EKF2_AID_MASK里的磁力計(jì)輔助項(xiàng)先關(guān)掉只保留IMU主導(dǎo)的姿態(tài)估計(jì)等HIL鏈路完全穩(wěn)定后再打開(kāi)磁力計(jì)項(xiàng)。EKF2_AID_MASK的值按PX4文檔配置即可。如果只做姿態(tài)控制可以暫時(shí)去掉GPS輔助減少對(duì)HIL_GPS消息的依賴如果要做位置控制GPS的輔助功能必須保留并且要確保HIL_GPS消息質(zhì)量足夠高。3.3 通信鏈路設(shè)計(jì)串口波特率與消息通道HIL模式下Simulink和飛控之間的數(shù)據(jù)交換鏈路是整個(gè)系統(tǒng)最容易出問(wèn)題的地方。常見(jiàn)方案是串口直連和UDP網(wǎng)絡(luò)連接兩種。如果走串口TELEM2口是最常用的HIL串口波特率必須設(shè)成921600不能沿用默認(rèn)的115200。為什么HIL_SENSOR消息大約70到80字節(jié)以250Hz發(fā)送每秒的數(shù)據(jù)量接近20KB。115200波特率的有效數(shù)據(jù)吞吐率只有約11.5KB每秒連理論值都喂不滿921600才有足夠余量。我第一次跑HIL時(shí)就因?yàn)槭掷镏挥?15200的USB-TTL模塊眼睜睜看著PX4報(bào)傳感器超時(shí)換了高波特率模塊后立刻恢復(fù)正常。如果走UDP飛控需要掛一個(gè)能接入網(wǎng)絡(luò)的模塊比如通過(guò)UART連接的WiFi模塊或者直接把飛控USB連到電腦通過(guò)MAVLink over UDP轉(zhuǎn)發(fā)。UDP的好處是帶寬大、不容易因?yàn)樽止?jié)粒度的阻塞丟幀而且Simulink天然支持UDP收發(fā)代碼寫起來(lái)比串口簡(jiǎn)單。缺點(diǎn)是網(wǎng)絡(luò)棧本身會(huì)引入抖動(dòng)同時(shí)你必須先確定防火墻沒(méi)有攔截飛控與電腦之間的UDP包——這個(gè)坑我踩過(guò)排查了半天才發(fā)現(xiàn)是Windows防火墻把MAVLink端口擋了Simulink的發(fā)送模塊壓根沒(méi)收到飛控的任何回包。4. Simulink端動(dòng)力學(xué)模型與MAVLink收發(fā)實(shí)現(xiàn)4.1 模型頂層結(jié)構(gòu)從執(zhí)行器輸出到傳感器數(shù)據(jù)Simulink模型的頂層可以做得很規(guī)整核心鏈路就一條UDP/串口接收MAVLink消息解出HIL_ACTUATOR_CONTROLS里的執(zhí)行器指令送入飛行器動(dòng)力學(xué)模型算完?duì)顟B(tài)后再經(jīng)過(guò)傳感器模型生成陀螺、加速度計(jì)、磁力計(jì)、氣壓、GPS數(shù)據(jù)最后通過(guò)MAVLink編碼發(fā)回PX4。我推薦用Aerospace Blockset里的6DoFQuaternion剛體動(dòng)力學(xué)模塊作為機(jī)體模型。它的輸入是三軸力矩和總拉力輸出是位置、速度、姿態(tài)四元數(shù)和角速度。關(guān)鍵在于怎么把PX4輸出的HIL_ACTUATOR_CONTROLS換算成力和力矩。HIL_ACTUATOR_CONTROLS里前四個(gè)控制值分別對(duì)應(yīng)橫滾、俯仰、偏航、油門取值范圍在-1到1附近。對(duì)多旋翼而言這四個(gè)值經(jīng)過(guò)PX4的混控器語(yǔ)義對(duì)應(yīng)到執(zhí)行機(jī)構(gòu)后需要等比例映射到推力系數(shù)和力矩系數(shù)上。我通常的做法是油門通道映射到總拉力基值注意要把重力配平的靜推力考慮進(jìn)去懸停時(shí)的油門通常對(duì)應(yīng)約1g的重力加速度補(bǔ)償橫滾和俯仰通道映射到繞機(jī)體X/Y軸的力矩縮放系數(shù)參考電機(jī)的力臂長(zhǎng)度和推力系數(shù)估算偏航通道映射到繞機(jī)體Z軸的反扭矩差。這個(gè)映射不需要一開(kāi)始就特別精確因?yàn)槟闶情]環(huán)仿真PX4的姿態(tài)控制器會(huì)主動(dòng)修正靜差。但如果模型偏得太離譜——比如把拉力方向搞反了或者把橫滾和俯仰弄混了——飛控再怎么調(diào)也無(wú)法收斂這類低級(jí)錯(cuò)誤往往也是新手最容易踩的坑。4.2 必須打通的六條核心MAVLink消息HIL鏈路的核心是MAVLink消息以下是必須打通的六類消息以及我實(shí)際使用的頻率和字段要點(diǎn)消息名消息ID方向頻率建議關(guān)鍵字段說(shuō)明HIL_SENSOR107Simulink - PX4250Hz陀螺、加速度、磁力、氣壓、空速、temperature、fields_updatedHIL_GPS113Simulink - PX450Hz經(jīng)緯度、高度、速度分量、fix_type、satellites_visibleHIL_RC_INPUTS_RAW92Simulink - PX450Hz遠(yuǎn)程遙控通道值PWM數(shù)值解鎖時(shí)需要有效RC信號(hào)SYSTEM_TIME2Simulink - PX41Hz對(duì)齊PX4的time_boot_ms確保時(shí)間基準(zhǔn)一致HIL_ACTUATOR_CONTROLS93PX4 - Simulink250Hz執(zhí)行機(jī)構(gòu)控制量前4個(gè)是橫滾/俯仰/偏航/油門HIL_STATE_QUATERNION115PX4 - Simulink50HzPX4內(nèi)部狀態(tài)估計(jì)回傳用于監(jiān)控和對(duì)比編解碼MAVLink最省力的方式是用現(xiàn)成的mavlink C庫(kù)通過(guò)Simulink的Legacy Code Tool包裝成S-Function調(diào)用。如果為了快速驗(yàn)證也可以在MATLAB Function里手寫字節(jié)拼包但一定要小心MAVLink的CRC校驗(yàn)和字節(jié)序——MAVLink 1使用X.25 CRCMAVLink 2還有額外的校驗(yàn)字節(jié)一旦CRC對(duì)不上PX4會(huì)靜默丟棄不會(huì)給你任何報(bào)錯(cuò)。第一次聯(lián)調(diào)時(shí)我建議先用Wireshark或者tcpdump抓包確認(rèn)Simulink發(fā)出去的包確實(shí)是通過(guò)MAVLink協(xié)議編碼的合法幀。4.3 實(shí)時(shí)性討論外部模式、UDP與調(diào)度抖動(dòng)Simulink模型在普通Windows/Linux系統(tǒng)上跑本質(zhì)上不是一個(gè)硬實(shí)時(shí)系統(tǒng)。Windows的定時(shí)器分辨率在默認(rèn)情況下大約是15.6ms這意味著如果你直接讓Simulink以250Hz的速率發(fā)送HIL_SENSOR發(fā)送間隔會(huì)出現(xiàn)比較大的抖動(dòng)。PX4的傳感器模塊對(duì)數(shù)據(jù)到達(dá)間隔有一定容忍度但如果抖動(dòng)過(guò)大或者某段時(shí)間內(nèi)完全沒(méi)數(shù)據(jù)就會(huì)觸發(fā)傳感器超時(shí)。幾種緩解方案按效果排序Simulink模型使用定步長(zhǎng)離散求解器步長(zhǎng)建議1ms然后用Rate Transition塊把傳感器數(shù)據(jù)降采樣到250Hz發(fā)送使用UDP發(fā)送而非串口UDP自帶緩沖允許短時(shí)間內(nèi)的小抖動(dòng)不丟包不要在這個(gè)場(chǎng)景里依賴Simulink外部模式來(lái)實(shí)時(shí)調(diào)參。外部模式本質(zhì)上是宿主機(jī)和目標(biāo)機(jī)的通信鏈路它本身會(huì)增加額外的調(diào)度開(kāi)銷和延遲。如果模型就是HIL的數(shù)據(jù)源外部模式會(huì)讓抖動(dòng)問(wèn)題雪上加霜。更好的做法是讓模型通過(guò)UDP周期輸出狀態(tài)日志用Simulink Dashboard或MATLAB Analysis離線分析如果要做嚴(yán)格的真實(shí)時(shí)HIL應(yīng)該把Simulink模型生成C代碼后部署到帶實(shí)時(shí)擴(kuò)展的工控機(jī)比如Speedgoat或者裝PREEMPT_RT補(bǔ)丁的Linux機(jī)器。我自己在非實(shí)時(shí)系統(tǒng)上跑HIL的經(jīng)驗(yàn)是250Hz的HIL_SENSOR配合1ms的模型步長(zhǎng)在普通Ubuntu 22.04主機(jī)上發(fā)送間隔的標(biāo)準(zhǔn)差可以控制在0.5ms以內(nèi)這對(duì)PX4驗(yàn)證來(lái)說(shuō)已經(jīng)足夠。再低頻率——比如降到200Hz——就需要調(diào)低PX4的IMU預(yù)測(cè)頻率否則EKF會(huì)因?yàn)閿?shù)據(jù)稀疏而表現(xiàn)變差。5. 聯(lián)調(diào)實(shí)錄從連不上到解鎖起飛遇到的四個(gè)坎5.1 坎一PX4一直在報(bào)傳感器超時(shí)無(wú)法進(jìn)入預(yù)飛行現(xiàn)象QGC面板一片紅報(bào)PREFLIGHT FAIL: SENSORMAVLink Console里連續(xù)輸出sensor missing。這次排錯(cuò)花了大半天。排查鏈路是這樣的先確認(rèn)SYS_HITL1并重啟沒(méi)問(wèn)題然后確認(rèn)Simulink確實(shí)在發(fā)HIL_SENSOR用tcpdump抓包看UDP端口能抓到107號(hào)消息但內(nèi)容是否合法不確定再檢查飛控側(cè)發(fā)現(xiàn)飛控壓根沒(méi)收到任何消息。最終定位到兩個(gè)疊加的問(wèn)題。第一個(gè)是Windows防火墻攔截了同一網(wǎng)段下的UDP通信Simulink發(fā)包沒(méi)有報(bào)錯(cuò)但數(shù)據(jù)根本沒(méi)離開(kāi)本機(jī)網(wǎng)卡第二個(gè)是MAVLink的sysid/compid沒(méi)配對(duì)Simulink發(fā)出去的包系統(tǒng)ID是255而飛控配置里MAV_SYS_ID是1PX4對(duì)這種來(lái)源的消息直接不予理睬。經(jīng)驗(yàn)聯(lián)調(diào)的第一步永遠(yuǎn)是讓兩個(gè)端點(diǎn)之間有一個(gè)能互相確認(rèn)消息的中間視圖。先用QGC的MAVLink Console確認(rèn)飛控能收到HEARTBEAT以外的任何MAVLink消息再用抓包軟件確認(rèn)Simulink發(fā)出去的消息內(nèi)容兩端對(duì)上了再談控制。5.2 坎二EKF狀態(tài)不健康解鎖被拒現(xiàn)象能連上飛控消息也在跑但QGC姿態(tài)儀表盤總有一半是紅的解鎖鍵點(diǎn)了沒(méi)反應(yīng)或者解鎖后幾秒鐘自動(dòng)跳回HOLD。查了EKF2的狀態(tài)日志發(fā)現(xiàn)加速度計(jì)和磁力計(jì)的innovation值特別大。原因是仿真器發(fā)過(guò)來(lái)的傳感器數(shù)據(jù)沒(méi)有加噪聲也沒(méi)有合理的初始偏置——EKF拿到這種過(guò)于干凈的數(shù)據(jù)反而不容易收斂因?yàn)檎鎸?shí)世界里任何傳感器都有噪聲和偏置EKF依賴這些統(tǒng)計(jì)特性去估計(jì)狀態(tài)。解決方法是給傳感器模型加入合適的噪聲模型。我在模型里為陀螺加了約0.003度每秒每根號(hào)赫茲的白噪聲和常值偏置加速度計(jì)加了約0.05米每二次方秒的噪聲磁力計(jì)加了中等強(qiáng)度的白噪聲。加完噪聲后EKF立刻進(jìn)入normal狀態(tài)。另外如果一直不執(zhí)行QGC的校準(zhǔn)向?qū)Р襟EEKF在啟動(dòng)后長(zhǎng)時(shí)間停留在unhealthy狀態(tài)。即使傳感器數(shù)據(jù)來(lái)自仿真器也不要跳過(guò)這個(gè)初始化步驟。5.3 坎三時(shí)間戳不同步導(dǎo)致數(shù)據(jù)被丟棄現(xiàn)象MAVLink消息計(jì)數(shù)在漲狀態(tài)估計(jì)器卻一動(dòng)不動(dòng)日志里頻繁出現(xiàn)timestamp error。原因很隱蔽。HIL_SENSOR消息里的time_usec字段是微秒級(jí)時(shí)間戳PX4會(huì)用這個(gè)時(shí)間戳做數(shù)據(jù)時(shí)序處理。我的Simulink模型一開(kāi)始直接用電腦的系統(tǒng)時(shí)間填入但這個(gè)時(shí)間和飛控上電后的time_boot_ms基準(zhǔn)差了很遠(yuǎn)PX4判定時(shí)間戳不連續(xù)把大多數(shù)消息當(dāng)成無(wú)效數(shù)據(jù)丟掉。解決方法是Simulink端發(fā)一條SYSTEM_TIME消息給PX4把基準(zhǔn)時(shí)間對(duì)齊。更穩(wěn)妥的做法是模型啟動(dòng)時(shí)先讀取飛控返回的HEARTBEAT消息里的time_boot_ms字段以此為基準(zhǔn)計(jì)算自己的相對(duì)時(shí)間再填入HIL_SENSOR。這樣一來(lái)Simulink和PX4的時(shí)鐘在同一個(gè)參考系下PX4不會(huì)再因?yàn)闀r(shí)間戳跳變丟棄數(shù)據(jù)。5.4 坎四控制頻率和模型步長(zhǎng)的匹配現(xiàn)象解鎖成功后飛機(jī)懸停出現(xiàn)明顯的高頻抖動(dòng)頻率大概十幾赫茲和電機(jī)轉(zhuǎn)速并不相同。排查后發(fā)現(xiàn)是模型步長(zhǎng)和HIL_SENSOR發(fā)送頻率不匹配。我最初把Simulink主步長(zhǎng)設(shè)成了5ms200Hz然后以200Hz發(fā)送HIL_SENSORPX4的IMU控制循環(huán)默認(rèn)是250Hz拿到的傳感器數(shù)據(jù)更新率低于自身控制頻率EKF和控制器的相位裕度受到嚴(yán)重影響。把模型主步長(zhǎng)降到1ms1000Hz用Rate Transition塊把HIL_SENSOR的發(fā)送頻率鎖定在250Hz后抖動(dòng)基本消失。之后我又嘗試把發(fā)送頻率提到400Hz效果更平滑但串口鏈路已經(jīng)開(kāi)始接近帶寬上限所以如果沒(méi)有特別需要250Hz是一個(gè)性價(jià)比最高的選擇。6. 在HIL平臺(tái)上還能做什么故障注入、測(cè)試自動(dòng)化與跨領(lǐng)域延伸6.1 常見(jiàn)故障注入HIL平臺(tái)最大的優(yōu)勢(shì)是你可以隨意制造事端這是真機(jī)測(cè)試沒(méi)法做到的。實(shí)操中值得做的故障注入類型包括GPS斷鏈突然停止發(fā)送HIL_GPS消息觀察PX4從定位模式回退到姿態(tài)模式的邏輯是否正常位置估計(jì)是否漂移切換時(shí)間是否符合預(yù)期磁力計(jì)干擾在Simulink里給磁力計(jì)數(shù)據(jù)加上一個(gè)階躍性偏置模擬飛控附近突然出現(xiàn)強(qiáng)磁場(chǎng)源觀察EKF航向估計(jì)的漂移和恢復(fù)過(guò)程單電機(jī)飽和把某個(gè)執(zhí)行器輸出手動(dòng)限幅模擬真實(shí)情況下電機(jī)堵轉(zhuǎn)或電調(diào)損壞看PX4的混控補(bǔ)償機(jī)制如何處理傳感器噪聲放大把陀螺儀白噪聲方差臨時(shí)放大5倍模擬高溫振動(dòng)環(huán)境下IMU性能下降驗(yàn)證姿態(tài)控制在傳感器質(zhì)量惡化時(shí)的魯棒性。每一類故障注入都應(yīng)該和真機(jī)試飛的測(cè)試場(chǎng)景對(duì)應(yīng)起來(lái)跑HIL不是圖新鮮而是為了在零風(fēng)險(xiǎn)條件下積累飛控對(duì)不同故障的響應(yīng)數(shù)據(jù)。把故障注入的時(shí)機(jī)和結(jié)果記錄成時(shí)間線后續(xù)對(duì)比真機(jī)表現(xiàn)時(shí)會(huì)很有價(jià)值。6.2 從手調(diào)到自動(dòng)掃參HIL環(huán)境沒(méi)有炸機(jī)風(fēng)險(xiǎn)非常適合自動(dòng)化參數(shù)掃掠。我的做法是用一個(gè)MATLAB腳本驅(qū)動(dòng)Simulink模型上一輪跑完后通過(guò)MAVLink PARAM_SET命令寫一組新的PID參數(shù)到PX4等EKF重新健康后再次解鎖并執(zhí)行標(biāo)準(zhǔn)試飛動(dòng)作。重復(fù)這個(gè)循環(huán)就能在一個(gè)下午內(nèi)測(cè)試十幾組參數(shù)組合。要注意自動(dòng)掃參時(shí)必須把起飛位置、起飛準(zhǔn)備時(shí)間、任務(wù)動(dòng)作序列完全固定否則仿真結(jié)果之間的差異無(wú)法歸因于參數(shù)變化。日志方面Simulink記錄的時(shí)間序列需要和PX4的ULog按時(shí)間戳對(duì)齊我一般用MAVLink的SYSTEM_TIME和HIL_STATE_QUATERNION作為對(duì)齊錨點(diǎn)兩邊時(shí)間戳完全一致時(shí)才認(rèn)為該輪數(shù)據(jù)有效。Simulink的UDP收發(fā)天然適合這種自動(dòng)化模式。腳本控制Simulink模型的啟動(dòng)和停止通過(guò)MATLAB API讀取模型里的記錄模塊數(shù)據(jù)再結(jié)合PX4的ULog做對(duì)比分析。整個(gè)過(guò)程無(wú)需人工干預(yù)效率比手動(dòng)操作QGC高一個(gè)數(shù)量級(jí)。6.3 車輛HIL、轉(zhuǎn)向臺(tái)架與Simulink生態(tài)的共性這套HIL模式Simulink模型MAVLink/總線消息注入的方法論在很多其他行業(yè)都能看到同構(gòu)實(shí)現(xiàn)。汽車轉(zhuǎn)向臺(tái)架HIL就是一個(gè)典型例子真實(shí)ECU或域控制器通過(guò)CAN總線連接到負(fù)載模擬器和轉(zhuǎn)向機(jī)器人Simulink配合CarSim或AMEsim解算整車動(dòng)力學(xué)再把方向盤扭矩、車速、輪速等信息轉(zhuǎn)換成傳感器信號(hào)喂給控制器。它的架構(gòu)和PX4的HIL完全是一個(gè)套路只是MAVLink換成了CAN/以太網(wǎng)無(wú)人機(jī)動(dòng)力學(xué)換成了整車模型。如果你之后從飛控轉(zhuǎn)向域控制器、VCU或者底盤HIL方法論是高度可遷移的Simulink既可以是被控對(duì)象仿真器也可以作為測(cè)試管理器負(fù)責(zé)場(chǎng)景編排、數(shù)據(jù)回灌和自動(dòng)判據(jù)。搜到的很多關(guān)于VCU控制策略Simulink建模、CAN報(bào)文故障診斷Simulink、CANoe聯(lián)合仿真的工作本質(zhì)上都在復(fù)用這一套信號(hào)級(jí)仿真與故障注入的思想。我現(xiàn)在養(yǎng)成的習(xí)慣是任何一次在SITL里優(yōu)化過(guò)的控制器參數(shù)在提交真機(jī)試飛前都會(huì)先跑一遍HIL至少讓它完成起飛、懸停、位置控制、降落整套動(dòng)作并且刻意在中間注入一兩個(gè)故障看飛控怎么處理。很多問(wèn)題不是算法本身而是隱藏在鏈路里的時(shí)序和數(shù)據(jù)質(zhì)量問(wèn)題只有把真實(shí)硬件挪進(jìn)回路這些問(wèn)題才會(huì)現(xiàn)形。這套SimulinkPX4的HIL鏈路給了我極大的安全感也希望這篇文章能幫你少走幾個(gè)我走過(guò)的彎路。