簡介本資源是一套基于STM32F407微控制器的四軸飛行器飛控源碼工程面向嵌入式開發(fā)者、無人機(jī)愛好者及高校電賽/畢業(yè)設(shè)計(jì)實(shí)踐者聚焦氣壓計(jì)與超聲波雙模融合定高控制這一核心難點(diǎn)。壓縮包共590個(gè)文件含317個(gè)C源碼含傳感器驅(qū)動(dòng)、PID定高算法、電機(jī)控制邏輯、88個(gè)頭文件定義硬件接口與參數(shù)結(jié)構(gòu)、48個(gè)編譯中間文件.o/.d及Keil工程配置.uvproj/.uvopt、啟動(dòng)匯編cstart_thumb2.asm、固件鏡像.axf/.hex等完整覆蓋從底層驅(qū)動(dòng)到閉環(huán)控制的全鏈路實(shí)現(xiàn)包體大小為8.53MB。已有269人學(xué)習(xí)下載源碼中已集成氣壓計(jì)溫度補(bǔ)償、超聲波回波濾波、多傳感器數(shù)據(jù)融合策略及可調(diào)PID參數(shù)表配合ANO調(diào)試工具支持實(shí)時(shí)高度曲線觀測與參數(shù)在線調(diào)優(yōu)是深入理解飛控姿態(tài)與高度協(xié)同控制的高質(zhì)量參考工程。1. 為什么是F407氣壓計(jì)這套方案從姿態(tài)到高度的第一個(gè)臺(tái)階做四軸定高這件事看起來只是一個(gè)“飛控里加一個(gè)傳感器”的改動(dòng)但真正把它從Demo推進(jìn)到能穩(wěn)定懸停中間涉及的坑比想象中多得多。這篇東西我想把整套基于STM32F407飛控板、ANO飛控框架、氣壓計(jì)定高的實(shí)現(xiàn)思路和調(diào)試過程完整捋一遍適合已經(jīng)在跑姿態(tài)環(huán)、想往更上層加功能的新手也適合手頭正好有F407板子、想從零把一個(gè)定高四軸立起來的人。先交代一下背景。定高這個(gè)概念本身不難理解飛機(jī)在遙控器不給油門變化的時(shí)候高度維持在一個(gè)給定值附近。但為什么要用F407而不是更便宜的F103答案很直接F407主頻168MHz帶硬件浮點(diǎn)運(yùn)算單元FPU跑ADC采樣、I2C總線讀取氣壓計(jì)、姿態(tài)解算、串級(jí)PID控制四個(gè)任務(wù)疊加起來CPU占用率仍然能壓在40%以內(nèi)。F103做姿態(tài)解算還能湊合再加上高度融合、氣壓計(jì)濾波、速度估計(jì)這些任務(wù)就開始吃緊了尤其在中斷退出延遲上容易讓控制環(huán)出現(xiàn)抖動(dòng)表現(xiàn)出來就是油門響應(yīng)鈍、懸停掉高度。ANO這套框架我用了挺長時(shí)間匿名科創(chuàng)社區(qū)最初開源的ANO_FC就是基于F407做的。它的架構(gòu)清晰度在同類開源飛控里屬于比較友好的那種任務(wù)調(diào)度分成傳感器讀取任務(wù)、姿態(tài)解算任務(wù)、控制任務(wù)、通信任務(wù)。我加定高功能時(shí)主要是在傳感器任務(wù)里加入氣壓計(jì)采樣和外置濾波在控制任務(wù)里疊加高度/速度雙閉環(huán)思路就是外環(huán)高度環(huán)輸出期望垂直速度內(nèi)環(huán)速度環(huán)輸出期望油門。這一套結(jié)構(gòu)現(xiàn)在很多商業(yè)飛控也這么用只是參數(shù)和融合方式各家有差異。定高四軸的核心價(jià)值是讓操作者從“時(shí)刻都得盯著油門”里解放出來。飛姿態(tài)模式時(shí)哪怕油門打了同一個(gè)位置電池電壓從4.2V掉到3.7V電機(jī)轉(zhuǎn)速下降飛機(jī)就會(huì)緩慢下沉有了定高不用人手微調(diào)油門遙控器油門桿只需要管升降方向或者直接交給高度設(shè)定這對(duì)于航拍、室內(nèi)懸停、編隊(duì)飛行都是剛需。所以這篇的落點(diǎn)不只是“把代碼下載進(jìn)去能起飛”而是拆開講清楚氣壓計(jì)的原始數(shù)據(jù)怎么變成可信的高度、高度怎么變成一個(gè)穩(wěn)定的控制量、控制量怎么變成不抖動(dòng)的油門輸出。2. 硬件選型和飛控板布局氣壓計(jì)最容易被忽視的“命門”2.1 飛控板和傳感器選型邊界我用的是ANO系列經(jīng)典的F407飛控板主控為STM32F407VGT6板上集成了MPU6500六軸IMU和MS5611氣壓計(jì)。MS5611是個(gè)很老練的型號(hào)24位ADC輸出分辨率可以做到10cm級(jí)實(shí)際能穩(wěn)定撲捉到的高度變化約15-20cm量程從10mbar到1200mbar對(duì)應(yīng)海拔覆蓋從-1000米到20000米左右家用和航拍機(jī)完全夠用。相比之下BMP280雖然便宜但它主要是為手機(jī)氣象服務(wù)設(shè)計(jì)的氣壓噪聲和溫漂性能比MS5611差了一個(gè)檔次同樣是高度輸出BMP280在一米高度上數(shù)據(jù)能抖出±30cm的跳動(dòng)MS5611同樣環(huán)境下能壓到±10cm這個(gè)差距在控制環(huán)里會(huì)被放大導(dǎo)致油門來回抽動(dòng)。因此定高項(xiàng)目里氣壓計(jì)首選MS5611預(yù)算特別緊才考慮SPL06。選好氣壓計(jì)之外飛控板布局的判斷更重要。MS5611對(duì)氣流極其敏感但也怕熱、怕風(fēng)、怕光。在飛控板上它離電源模塊和主控芯片至少要保持5mm以上間距否則MOS管和穩(wěn)壓芯片的熱量會(huì)直接抬高氣壓計(jì)局部溫度造成漂移。我見過一款山寨飛控把MS5611貼著4A降壓芯片放開機(jī)兩分鐘高度數(shù)據(jù)就虛漂了3米。所以硬件到手先做靜態(tài)測試上電后把板子放桌上靜止不動(dòng)串口打印高度值觀察兩分鐘內(nèi)高度是否漂移超過50cm。如果漂移很大第一懷疑對(duì)象不是算法而是板子的熱源干擾。2.2 氣壓計(jì)安裝的“遮風(fēng)”與“透氣”矛盾螺旋槳的下洗氣流是定高最大的物理干擾源。電機(jī)一轉(zhuǎn)動(dòng)大量氣流直接沖刷飛控板頂部的氣壓計(jì)封裝。MS5611的芯片頂部有一個(gè)微小的通氣孔外部氣壓就是通過這個(gè)孔感知的。如果不做遮擋處理下洗氣流會(huì)形成局部壓強(qiáng)波動(dòng)高度數(shù)據(jù)會(huì)像喝醉了一樣上下亂跳。常規(guī)做法是給氣壓計(jì)蓋上一塊密度適中的泡棉泡棉厚度在5mm左右內(nèi)部要有一塊空腔。注意泡棉不是把孔堵死而是緩沖氣流的急變。我試過三種方案方案做法實(shí)測效果裸奔不遮氣壓計(jì)直接暴露波動(dòng)±50cm完全不可用薄海綿緊貼2mm薄海綿覆蓋封口波動(dòng)±20cm仍不夠穩(wěn)空腔泡棉蓋5mm泡棉挖出5x5mm空腔覆蓋氣孔波動(dòng)±8cm懸停穩(wěn)定空腔泡棉的原理是泡棉本身濾掉高頻氣流擾動(dòng)內(nèi)部空腔給氣壓變化一個(gè)緩沖時(shí)間相當(dāng)于給氣壓計(jì)加了一個(gè)機(jī)械低通濾波器。這個(gè)方法成本幾乎為零效果卻立竿見影。安裝時(shí)還要注意側(cè)面也要遮擋風(fēng)向室內(nèi)開風(fēng)扇測試時(shí)側(cè)風(fēng)也會(huì)從泡棉邊緣鉆進(jìn)去影響讀數(shù)。2.3 供電與I2C總線細(xì)節(jié)MS5611是I2C接口飛控板上通常作為I2C總線上的唯一或之一設(shè)備。F407的I2C外設(shè)歷史上飽受吐槽很多人直接用GPIO模擬I2C來規(guī)避硬件I2C的怪癖。我的經(jīng)驗(yàn)是在ANO框架里直接用硬件I2C其實(shí)問題不大但必須把I2C時(shí)鐘配置在200kHz左右太高的時(shí)鐘在長線纜上容易出ACK丟失。另外MS5611的VDD腳要并接100nF和10uF電容這一點(diǎn)很多自制PCB圖省事只放一個(gè)百納法電容上電瞬間電壓跌落會(huì)導(dǎo)致傳感器初始化失敗串口讀到固定值0x0000排查半天其實(shí)是供電不足。現(xiàn)在我需要強(qiáng)調(diào)一個(gè)布局順序飛控板供電一般由分電板或電源模塊通過杜邦線引出線長超過15cm時(shí)I2C數(shù)據(jù)線SDA/SCL建議和電源線分開走線避免電機(jī)大電流變化在電源線上產(chǎn)生壓降尖峰耦合到I2C線上干擾通信。成品飛控板一般已經(jīng)考慮好了但我見過不少自己改線導(dǎo)致I2C偶發(fā)卡死的情況卡死表現(xiàn)為氣壓計(jì)數(shù)據(jù)停止刷新飛控三秒后觸發(fā)失控保護(hù)。3. 氣壓計(jì)原始數(shù)據(jù)如何變成可信高度從I2C讀取到濾波融合3.1 MS5611讀取時(shí)序與代碼骨架先把氣壓計(jì)的底褲看清。MS5611內(nèi)部有兩個(gè)測量通道壓力通道和溫度通道。它的工作流程是發(fā)送轉(zhuǎn)換指令啟動(dòng)壓力測量等待轉(zhuǎn)換完成典型時(shí)間由OSR決定讀取24位壓力原始值再啟動(dòng)溫度測量讀取24位溫度原始值然后利用芯片出廠時(shí)燒錄的6組校準(zhǔn)系數(shù)C1-C6做補(bǔ)償計(jì)算得到真實(shí)氣壓值和溫度值。我的做法是定高循環(huán)里用讀取優(yōu)先級(jí)輪詢的方式每次調(diào)度周期只發(fā)起一種測量然后讀上一次的結(jié)果避免阻塞等待。偽代碼框架如下// 每5ms調(diào)用一次 void baro_poll(void) { static uint8_t state 0; uint32_t raw_press, raw_temp; switch (state) { case 0: ms5611_start_conv(MS5611_PRESS, OSR_2048); state 1; break; case 1: if (ms5611_read_adc(raw_press) 0) { pressure_raw raw_press; ms5611_start_conv(MS5611_TEMP, OSR_2048); state 2; } break; case 2: if (ms5611_read_adc(raw_temp) 0) { temp_raw raw_temp; pressure ms5611_compensate(pressure_raw, temp_raw); state 0; } break; } }OSR過采樣率是個(gè)關(guān)鍵參數(shù)。MS5611支持256/512/1024/2048/4096五檔過采樣率OSR越高內(nèi)部ADC積分時(shí)間越長輸出噪聲越低但同時(shí)單次轉(zhuǎn)換時(shí)間越長。用OSR_2048對(duì)應(yīng)轉(zhuǎn)換時(shí)間約5ms噪聲幅度約0.06mbar折算高度噪聲約0.5米R(shí)MS。這個(gè)值從原始噪聲看并不算小所以不能直接用瞬時(shí)值必須先經(jīng)過濾波。3.2 高度換算與溫度補(bǔ)償細(xì)節(jié)從氣壓值換算高度的公式是國際氣壓測高公式altitude (1.0 - pow(pressure / 101325.0, 0.190295)) * 44330.0;注意公式里的101325Pa是標(biāo)準(zhǔn)海平面氣壓。很多人都知道這個(gè)公式但忽略了一個(gè)重要問題天氣變化和飛行環(huán)境溫度差會(huì)導(dǎo)致基準(zhǔn)氣壓漂移。所以正確的做法是開機(jī)時(shí)鎖定當(dāng)前氣壓為H0之后所有高度輸出都相對(duì)于H0計(jì)算這樣就不需要知道絕對(duì)海拔基準(zhǔn)公式變成relative_altitude (1.0 - pow(pressure / pressure_takeoff, 0.190295)) * 44330.0;其中pressure_takeoff是解鎖時(shí)記錄的基準(zhǔn)氣壓。這個(gè)邏輯一定要在起飛瞬間鎖定基準(zhǔn)否則你在地面站里校準(zhǔn)完、通電后放了一小時(shí)氣壓變化5米再起飛時(shí)高度就偏了。溫度補(bǔ)償同樣不能省。MS5611的補(bǔ)償函數(shù)會(huì)利用C1-C6做二階補(bǔ)償保證從-40°C到85°C范圍內(nèi)精度在±1.5mbar以內(nèi)。忽略補(bǔ)償直接拿原始值用的后果是飛控板子發(fā)熱后高度會(huì)緩慢正漂移看起來像飛機(jī)自己在往上爬定高變成“緩慢爬升模式”。3.3 濾波方案從滑動(dòng)平均到限幅互補(bǔ)氣壓計(jì)原始高度數(shù)據(jù)直接用于控制是災(zāi)難性的。我在OSR_2048下實(shí)測原始高度數(shù)據(jù)每25ms跳動(dòng)大約±0.5米而這個(gè)跳動(dòng)頻率和電機(jī)轉(zhuǎn)速、結(jié)構(gòu)振動(dòng)、機(jī)架彈性都有關(guān)系。直接把這個(gè)值送進(jìn)PIDP項(xiàng)會(huì)讓油門瘋狂抖動(dòng)機(jī)身高頻率顫抖?；A(chǔ)處理是滑動(dòng)平均濾波窗口取20個(gè)點(diǎn)每10ms一個(gè)點(diǎn)窗口200ms能把高頻抖動(dòng)壓縮到±8cm。但滑動(dòng)平均有個(gè)天然的滯后問題窗口越長滯后越大飛機(jī)從2米快速爬升到3米時(shí)濾波后的高度值要拖后將近半秒才到2.8米這個(gè)滯后同樣會(huì)降低控制穩(wěn)定性。我最終用的是兩級(jí)組合先做5點(diǎn)中值濾波剔除離群點(diǎn)再做20點(diǎn)加權(quán)滑動(dòng)平均最后和加速度計(jì)積分高度做一次互補(bǔ)濾波?；パa(bǔ)濾波的思路是height_fused alpha * height_baro (1 - alpha) * (height_prev vel_vertical * dt);alpha取值在0.02到0.05之間。氣壓計(jì)負(fù)責(zé)長期穩(wěn)定加速度計(jì)積分負(fù)責(zé)短期響應(yīng)。氣壓計(jì)長期漂移但是短期噪聲大加速度計(jì)積分短期精準(zhǔn)但是長期會(huì)漂。兩者互補(bǔ)一個(gè)管長期一個(gè)管短期alpha就是信任權(quán)重。這套結(jié)構(gòu)跑下來高度數(shù)據(jù)的響應(yīng)延遲從直接濾波的200ms降到了80ms以內(nèi)懸停時(shí)高度波動(dòng)能控制在±15cm左右。關(guān)于加速度計(jì)積分這部分多說一句豎直方向的加速度是機(jī)體坐標(biāo)系下z軸加速度和姿態(tài)角的三角函數(shù)關(guān)系需要在姿態(tài)解算算出四元數(shù)后把機(jī)體系加速度投影到世界系取世界系z軸方向分量減去1g重力加速度然后再積分。這一步做錯(cuò)了會(huì)導(dǎo)致高度振動(dòng)最常見的錯(cuò)誤是忘了減重力。我剛開始調(diào)試時(shí)高度融合曲線像波浪一樣上下竄排查半天發(fā)現(xiàn)是加速度z軸沒補(bǔ)償重力項(xiàng)。3.4 氣壓計(jì)噪聲診斷先分清是“讀數(shù)噪聲”還是“飛控振動(dòng)”很多人在定高不穩(wěn)的時(shí)候一上來就調(diào)PID其實(shí)要先把噪聲源分清楚。排查方法很簡單飛機(jī)上鎖放在桌上螺旋槳不轉(zhuǎn)讀取濾波前氣壓計(jì)原始高度數(shù)據(jù)序列。如果此時(shí)波動(dòng)都超過±30cm那說明是傳感器自身干擾或硬件布局問題優(yōu)先做遮擋、濾波、電源去耦。接下來螺旋槳油門加到30%不推桿飛機(jī)還在地上壓著讀取同樣的數(shù)據(jù)。如果波動(dòng)顯著加劇那就是氣流沖擊和機(jī)體振動(dòng)通過PCB傳導(dǎo)給氣壓計(jì)造成的影響。這時(shí)候調(diào)泡棉位置、加橡膠減震柱比調(diào)PID更管用。最后才是飛起來看高度曲線的毛刺確認(rèn)濾波和融合參數(shù)。這個(gè)排查順序能省掉至少兩天的瞎調(diào)時(shí)間。4. 定高控制閉環(huán)外環(huán)高度、內(nèi)環(huán)速度與油門前饋的設(shè)計(jì)路徑4.1 串級(jí)PID結(jié)構(gòu)為什么外層只輸出速度而不是直接輸出油門定高控制最忌諱的就是高度誤差直接映射到油門。這種單環(huán)控制在小型四軸上幾乎必炸因?yàn)楦叨鹊接烷T之間的傳遞函數(shù)嚴(yán)重非線性同樣高度的偏差電池電壓高時(shí)油門修正一點(diǎn)點(diǎn)就夠電壓低時(shí)油門加2%都不夠四個(gè)電機(jī)轉(zhuǎn)速變化對(duì)升力的貢獻(xiàn)又受電池內(nèi)阻和環(huán)境溫度影響。所以這里使用內(nèi)外環(huán)串級(jí)控制這是工程上被反復(fù)驗(yàn)證的結(jié)構(gòu)外環(huán)高度環(huán)期望垂直速度 Kp_h * (目標(biāo)高度 - 當(dāng)前高度) Ki_h * 高度誤差積分當(dāng)高度低于目標(biāo)時(shí)這個(gè)值是正的向上高于目標(biāo)時(shí)是負(fù)的向下。這個(gè)速度指令作為內(nèi)環(huán)的輸入。內(nèi)環(huán)垂直速度/加速度環(huán)油門修正 Kp_v * (期望垂直速度 - 當(dāng)前垂直速度) Ki_v * 垂直速度誤差積分當(dāng)前垂直速度的來源可以用高度差分求導(dǎo)但我更推薦用融合后的垂直速度估計(jì)值就是互補(bǔ)濾波里那個(gè)積分量因?yàn)樗鼫p掉了很多氣壓計(jì)高頻噪聲讓內(nèi)環(huán)控制的微分項(xiàng)不那么暴躁。整個(gè)結(jié)構(gòu)看下來外環(huán)保高度穩(wěn)定內(nèi)環(huán)保高度變化速率這本質(zhì)上是“位置-速度”控制的嵌套抗擾能力強(qiáng)很多。4.2 垂直速度估計(jì)與高度微分的取舍通俗解釋高度環(huán)相當(dāng)于開車時(shí)看與前車的距離來控制車速速度環(huán)相當(dāng)于踩油門讓實(shí)際車速跟上期望車速。如果你只看距離去直接踩油門路一顛簸車就會(huì)一頓一頓但你先把距離換成期望車速再一腳一腳微調(diào)油門去匹配這個(gè)車速順暢得多。四軸定高也是同一個(gè)道理。垂直速度估計(jì)用互補(bǔ)濾波里得到的vel_vertical它融合了加速度積分和高度的差分修正。我實(shí)際測試時(shí)速度估計(jì)的延遲在60ms以內(nèi)這足以支撐內(nèi)環(huán)PID控制在120Hz頻率下運(yùn)行得比較平滑。4.3 油門前饋懸停油門的重要作用定高控制里PID只是修正量真正的力量來源是一個(gè)恒定懸停油門基線。這個(gè)值是飛機(jī)懸停時(shí)能穩(wěn)定維持高度所需的油門百分比取決于機(jī)架重量、電機(jī)效率、螺旋槳螺距、電池電壓每個(gè)機(jī)器都不一樣。我習(xí)慣的做法手動(dòng)模式下把飛機(jī)飛到1.5米左右基本穩(wěn)住高度讀出遙控器油門通道原始值換算成油門百分比例如遙控器1000-2000懸停油門在1450附近則懸停油門(1450-1000)/100045%。把這個(gè)值寫入?yún)?shù)throttle_hover。定高模式下總油門輸出 throttle_hover PID修正。沒有這個(gè)前饋PID只能從頭去“找”懸停油門的位置每次起飛懸停點(diǎn)都有偏移就會(huì)看到飛機(jī)升上去、掉下來、再升上去越晃越厲害。有了前饋PID只負(fù)責(zé)修正擾動(dòng)修正量從±20%降到±5%穩(wěn)定性一目了然。4.4 定高PID參數(shù)整定的實(shí)操順序參數(shù)整定必須按“從內(nèi)到外”的規(guī)則來。先只跑內(nèi)環(huán)速度環(huán)把外環(huán)去掉給定一個(gè)固定的期望垂直速度比如0.5m/s觀察飛機(jī)是否能勻速上升/下降。此時(shí)Kp_v從0.5開始加加到期望速度和實(shí)際速度的響應(yīng)快、超調(diào)小Kp_v大約在0.8-1.2之間Ki_v加0.1左右用于消除穩(wěn)態(tài)誤差。內(nèi)環(huán)調(diào)好后再啟用外環(huán)Kp_h從0.5開始給一個(gè)0.5米的高度階躍比如從1.5米目標(biāo)高度改成2.0米觀察高度響應(yīng)曲線。理想響應(yīng)是在1-2秒內(nèi)平滑到達(dá)目標(biāo)高度附近在一個(gè)窄帶內(nèi)穩(wěn)住不來回穿越。如果高度在目標(biāo)值附近以明顯的周期震蕩通常K作用不足或I作用過大如果高度到達(dá)目標(biāo)特別慢但最后也穩(wěn)了可以適當(dāng)加大Kp_h和I。我最后落地的參數(shù)組合是高度環(huán)Kp_h1.2Ki_h0.05速度環(huán)Kp_v0.9Ki_v0.1控制頻率外環(huán)50Hz內(nèi)環(huán)100Hz。這個(gè)組合在我這個(gè)400mm軸距、1080g起飛重量的機(jī)架上表現(xiàn)穩(wěn)定懸停高度波動(dòng)±15cm推力波動(dòng)小于±5%。換機(jī)架時(shí)參數(shù)不一定通用但整定順序和現(xiàn)象判斷方法是通用的。5. 實(shí)測中的坑與調(diào)參記錄起飛掉高、洗槳效應(yīng)與溫度漂移5.1 起飛瞬間掉高氣壓計(jì)被下洗氣流“壓住”了嗎第一次試飛定高時(shí)我遇到了一個(gè)很典型的坑接通定高模式后飛機(jī)先迅速向上躥了30cm然后又突然掉下來接近50cm最后才算晃晃悠悠穩(wěn)住。第一反應(yīng)是PID參數(shù)過激但串口打印數(shù)據(jù)顯示了一個(gè)更有趣的現(xiàn)象氣壓計(jì)高度在起飛瞬間上升了30cm而實(shí)際飛機(jī)高度并沒有動(dòng)。原因是起飛瞬間油門加大螺旋槳下洗氣流沖擊地面再反彈在飛控板附近形成了一股瞬時(shí)高壓氣流氣壓計(jì)感受到的壓強(qiáng)比環(huán)境高換算出的高度自然往上跳。飛控以為飛機(jī)高了就減小油門結(jié)果高度又往下掉。這就是“起飛掉高”的一個(gè)重要誘因。解決方法分三層在解鎖后到起飛前不急著把期望高度設(shè)置到當(dāng)前高度可以先記錄起飛瞬間的高度偏移作為基準(zhǔn)。油門切換瞬間速度環(huán)飽和限制要加限制期望垂直速度最大±1.5m/s避免高度計(jì)算錯(cuò)誤時(shí)輸出極端油門。起飛前在地面晚幾秒再切定高讓氣壓計(jì)完成“地面氣流擾動(dòng)”到“空中流動(dòng)場”的過渡。5.2 洗槳效應(yīng)低空懸停時(shí)的“地面效應(yīng)”非線性在距離地面30cm以內(nèi)低空懸停時(shí)定高非常容易抽風(fēng)表現(xiàn)為飛機(jī)上下來回竄。這是地面效應(yīng)在作祟近地時(shí)螺旋槳下洗氣流撞擊地面后向外擴(kuò)散相當(dāng)于在飛機(jī)下方墊了一個(gè)氣墊同樣油門提供的升力會(huì)突然變大。這個(gè)非線性環(huán)節(jié)和高度環(huán)的PID產(chǎn)生耦合如果PID響應(yīng)太快就會(huì)在氣墊效應(yīng)區(qū)域形成振蕩。處理經(jīng)驗(yàn)是定高高度的生效死區(qū)設(shè)置為±10cm。當(dāng)高度誤差小于死區(qū)時(shí)PID輸出保持上一值不變這樣避免微小抖動(dòng)讓執(zhí)行機(jī)構(gòu)來回動(dòng)作。另外可以降低低空段的外環(huán)P項(xiàng)這點(diǎn)我通過一個(gè)高度相關(guān)的增益衰減系數(shù)實(shí)現(xiàn)高度小于0.5米時(shí)Kp_h按(實(shí)際高度/0.5)的平方衰減。5.3 溫度漂移與“假高度”問題定高飛行到第三塊電池時(shí)我發(fā)現(xiàn)了一個(gè)隱蔽問題飛控在暖機(jī)狀態(tài)下高度自己慢慢從1.5米漂到了2.1米但是視覺上飛機(jī)并沒有明顯上升。這就是氣壓計(jì)溫漂帶來的“假高度”。排查過程很有意思我先是懷疑濾波算法把a(bǔ)lpha調(diào)小沒用換成雙濾波還是漂。最后用溫度數(shù)據(jù)一對(duì)比恍然大悟MS5611的溫度在開機(jī)三十秒內(nèi)從27°C升到42°C溫差15°C帶來的零點(diǎn)偏移足以換算成1米的高程差。雖然傳感器做了補(bǔ)償?shù)寮?jí)局部溫度梯度依然存在。解決方案在硬件層面是加大氣壓計(jì)泡棉和主控之間隔熱軟件層面有三個(gè)補(bǔ)充手段開機(jī)預(yù)熱飛控上電后至少等待40秒再解鎖觀察溫度變化速率小于0.5°C/3s時(shí)才算穩(wěn)定。溫漂校正建立溫度和高度漂移的查表每30秒記錄一次溫度值和高度值用線性擬合消除相關(guān)性部分。起飛前鎖基準(zhǔn)解鎖瞬間重新鎖定初始高度消除長時(shí)間靜態(tài)積累的漂移。這個(gè)方法結(jié)合之后我測試中連續(xù)飛行8分鐘定高漂移被壓在40cm以內(nèi)主要還是由于正常環(huán)境氣壓變化的部分在可接受范圍。5.4 油門響應(yīng)不平順的排查區(qū)分“傳感器噪聲”還是“控制極限環(huán)”調(diào)試后期遇到另一個(gè)怪問題定高時(shí)油門總是輕微周期性抽動(dòng)每0.5秒左右一次幅度大約油門值的3%肉眼可見螺旋槳轉(zhuǎn)速在細(xì)微地一變一變的。這個(gè)現(xiàn)象持續(xù)存在不是偶發(fā)。通過地面站回看數(shù)據(jù)發(fā)現(xiàn)高度曲線其實(shí)很平滑說明傳感器融合沒問題但速度內(nèi)環(huán)的目標(biāo)速度和控制輸出的差值在周期性波動(dòng)。進(jìn)一步分析是內(nèi)環(huán)速度控制頻率100Hz但高度環(huán)輸出頻率50Hz兩個(gè)環(huán)數(shù)據(jù)同步時(shí)序上差了半個(gè)周期導(dǎo)致速度目標(biāo)指令里面的更新帶有鋸齒狀的臺(tái)階內(nèi)環(huán)對(duì)臺(tái)階比較敏感就產(chǎn)生了0.5秒周期的極限環(huán)。解決辦法是在高度環(huán)和速度環(huán)之間加一級(jí)速度指令平滑緩沖用一個(gè)二階低通濾波器處理期望垂直速度截止頻率大約3Hz讓高度環(huán)輸出的速度階躍變成平滑斜坡速度內(nèi)環(huán)的抖動(dòng)徹底消失。如果遇到類似油門周期性抽動(dòng)不要第一時(shí)間懷疑氣壓計(jì)先檢查兩個(gè)控制環(huán)之間的信號(hào)同步和指令平滑這是我調(diào)過眾多飛控后最深刻的經(jīng)驗(yàn)之一。5.5 調(diào)試飛行過程的記錄方法整個(gè)調(diào)試過程中串口日志和地面站數(shù)據(jù)回放幫了大忙。我會(huì)在飛控代碼里把以下幾個(gè)變量以100Hz頻率輸出nudged高度濾波后融合高度、期望高度、期望垂直速度、實(shí)際垂直速度、油門輸出百分比、電機(jī)PWM原始值。每次飛完一個(gè)架次把日志拉下來用MATLAB離線畫圖圖和圖對(duì)照看幾乎每個(gè)異常現(xiàn)象都能定位到具體環(huán)節(jié)。記得有一次飛機(jī)定高時(shí)明顯前傾俯仰看日志才發(fā)現(xiàn)油門修正過程中姿態(tài)角被干擾了0.5度導(dǎo)致水平方向分力把飛機(jī)推走。這個(gè)現(xiàn)象如果只看高度曲線是完全看不出來的必須把姿態(tài)角、油門、高度三張圖疊在一起看。這也是為什么我一直建議不要只依賴機(jī)上飛行效果判斷而是要建立完整的遙測日志習(xí)慣。6. 最后的調(diào)參心得與可復(fù)用的參數(shù)模板調(diào)試了這么長時(shí)間我最大的體會(huì)是定高四軸的難度不在于某個(gè)單一環(huán)節(jié)而在于傳感器、融合、控制三個(gè)層面的耦合。氣壓計(jì)數(shù)據(jù)臟后面所有環(huán)節(jié)都會(huì)跟著遭殃融合滯后嚴(yán)重PID參數(shù)調(diào)到天上也壓不住振鈴前饋油門不準(zhǔn)一切修正都像是蒙著眼睛開車。所以永遠(yuǎn)記得自底向上排查先把數(shù)據(jù)變干凈再談控制。現(xiàn)在給出一份可直接上手的參數(shù)模板供參考具體機(jī)架還需微調(diào)參數(shù)取值說明氣壓計(jì)OSROSR_2048采樣時(shí)間約5ms噪聲與速度均衡氣壓計(jì)采樣周期10ms每次輪詢一種測量融合alpha0.035氣壓計(jì)權(quán)重響應(yīng)與穩(wěn)定性折中高度環(huán)頻率50Hz外環(huán)更新率速度環(huán)頻率100Hz內(nèi)環(huán)更新率高度環(huán)Kp_h1.2高度誤差到期望速度的比例高度環(huán)Ki_h0.05消除靜態(tài)誤差速度環(huán)Kp_v0.9速度誤差到油門修正速度環(huán)Ki_v0.10消除速度積差懸停油門throttle_hover45%左右依據(jù)實(shí)際機(jī)架測量填入期望垂直速度限幅±1.5m/s防止大誤差時(shí)輸出極端油門高度死區(qū)10cm避免微小抖動(dòng)觸發(fā)控制這份模板在400mm軸距穿越機(jī)機(jī)架、1080g起飛重量、MS5611 空腔泡棉、F407飛控板、ANO飛控框架下實(shí)現(xiàn)了懸停高度波動(dòng)±15cm、給一個(gè)0.5m高度階躍、1.5秒內(nèi)穩(wěn)定到目標(biāo)高度±5cm的實(shí)測效果。電池電壓從滿電4.2V一路降到3.6V定高無需重新調(diào)整懸停油門前饋已經(jīng)自動(dòng)把電量影響吸收掉。最后再分享一個(gè)小技巧定高代碼加好后不要急著飛在地面做一個(gè)“假裝起飛”測試。用一根細(xì)繩把機(jī)架吊在支架上讓四個(gè)電機(jī)離地3-5cm人不去推油門只把期望高度改成1米這時(shí)四個(gè)電機(jī)轉(zhuǎn)速同步升高繩子拉住機(jī)身不讓上升高度環(huán)感受到高度不變、速度為零應(yīng)該會(huì)把油門推到一個(gè)穩(wěn)定值附近。這個(gè)測試能驗(yàn)證整個(gè)定高控制鏈路的完整性和穩(wěn)定性又不會(huì)炸機(jī)非常值得在每次改參數(shù)后做一輪比直接上實(shí)飛安全得多。本文還有配套的精品資源點(diǎn)擊獲取