控制實戰(zhàn))
簡介這是一套面向電子愛好者與嵌入式初學者的二輪平衡車DIY完整資料包涵蓋從硬件搭建到軟件調(diào)試的全鏈路內(nèi)容。資源定位為STM32平臺平衡車的開源學習套件包含互補濾波、卡爾曼濾波、DMP三種姿態(tài)解算方案的源碼三套工程均編譯通過、可直接燒錄驗證適合希望動手制作平衡小車或深入理解濾波算法差異的讀者。壓縮包大小148.18MB內(nèi)置全部平衡小車相關(guān)資料類型上以Keil工程源碼、電路原理圖、PCB文件、連線說明文檔為主硬件與軟件配套呈現(xiàn)可以大幅降低DIY門檻。已有3129人學習瀏覽整體經(jīng)過本科階段完整調(diào)試驗證是一份少有的“從零到跑通”的參考資源。讀者可以獲得實物接線指引、電路設(shè)計源文件以及三種濾波方案的對比實現(xiàn)既能直接復刻樣車也能基于源碼二次開發(fā)用于課程設(shè)計、畢業(yè)設(shè)計或興趣項目。1. 平衡車全套源碼從零開始搭一輛能站穩(wěn)的兩輪車第一次接觸二輪平衡車的人往往以為難點在機械組裝等真把套件拼好、電機轉(zhuǎn)起來才發(fā)現(xiàn)真正讓人頭疼的是讓它站穩(wěn)。一個常見現(xiàn)象是車能啟動但兩三秒內(nèi)就朝一個方向沖出去或者原地高頻抖動這就是典型的角度環(huán)與速度環(huán)沒配合好。市面上能跑的平衡車源碼不少但大多數(shù)要么只給姿態(tài)解算片段要么把 PID 調(diào)參寫成玄學讓新手連入口都找不到。這套二輪平衡車全套源碼的價值在于它把從傳感器讀取、姿態(tài)解算到雙環(huán)/三環(huán)控制、PWM 輸出的完整鏈路都串起來了適合正在做課設(shè)、電賽備賽或想入門嵌入式控制的人直接照抄再改。2. 姿態(tài)解算選型卡爾曼濾波、DMP 與互補濾波的取舍和落地代碼2.1 三種解算方案的適用邊界為什么不是越復雜越好平衡車要解決的核心問題是MPU6050 輸出的加速度計和陀螺儀數(shù)據(jù)各自有硬傷。加速度計在靜止時能準確給出重力方向但車體一震動高頻噪聲會直接把角度信號淹沒陀螺儀積分得到的角度在短時間內(nèi)很平滑但積分漂移會隨時間累積幾秒后角度就開始“飄”。姿態(tài)解算的全部工作就是把這兩個都不完美的信號融合成一個相對可信的角度?？柭鼮V波的思路是把角度和陀螺儀零偏作為狀態(tài)量每次拿加速度計的角度做測量更新拿陀螺儀的角速度做預測更新。它收斂快、動態(tài)響應好但需要反復調(diào)協(xié)方差矩陣的過程噪聲和測量噪聲調(diào)不好就會出現(xiàn)角度滯后或者過沖。DMP 是 MPU6050 內(nèi)部的硬件解算引擎DMP 庫初始化后直接讀四元數(shù)再轉(zhuǎn)歐拉角CPU 占用極小但它把融合過程當黑匣子輸出是相對姿態(tài)想改濾波特性基本無從下手?；パa濾波則用一個系數(shù) alpha 把兩個信號按頻率高低切開陀螺儀負責高頻段加速度計負責低頻段。代碼只有幾行在平衡車這種垂直安裝、運行范圍不到 30 度的場景里完全夠用而且調(diào)試只要調(diào)一個 alpha。我的建議很簡單做課設(shè)或功能演示優(yōu)先互補濾波代碼短、好解釋、調(diào)參直觀做競賽或追求極限動態(tài)響應用卡爾曼濾波DMP 庫適合不想碰矩陣運算、把精力全放在 PID 上的人。這套源碼把三種方案都放了切換時只需要改融合函數(shù)入口不用動上層控制邏輯。2.2 卡爾曼濾波實現(xiàn)一維模型與核心代碼卡爾曼濾波在平衡車場景里可以簡化為一維模型——只對俯仰角pitch做融合橫滾角同理但角度變化幅度遠小于俯仰可以先不處理。狀態(tài)量有兩個角度 angle 和陀螺儀零偏 bias。預測方程用的是陀螺儀角速度減掉零偏后的值測量方程用的是加速度計算出的角度。下面是一份可直接嵌入單片機的卡爾曼濾波 C 代碼typedef struct { float angle; // 融合后的角度輸出 float bias; // 陀螺儀零偏 float P[2][2]; // 協(xié)方差矩陣 float Q_angle; // 角度過程噪聲 float Q_bias; // 零偏過程噪聲 float R_measure; // 測量噪聲 } Kalman_t; void Kalman_Init(Kalman_t *kf) { kf-angle 0.0f; kf-bias 0.0f; kf-P[0][0] 0.0f; kf-P[0][1] 0.0f; kf-P[1][0] 0.0f; kf-P[1][1] 0.0f; kf-Q_angle 0.001f; kf-Q_bias 0.003f; kf-R_measure 0.03f; } float Kalman_Update(Kalman_t *kf, float new_angle, float new_rate, float dt) { float rate new_rate - kf-bias; kf-angle dt * rate; kf-P[0][0] dt * (dt * kf-P[1][1] - kf-P[0][1] - kf-P[1][0] kf-Q_angle); kf-P[0][1] - dt * kf-P[1][1]; kf-P[1][0] - dt * kf-P[1][1]; kf-P[1][1] kf-Q_bias * dt; float S kf-P[0][0] kf-R_measure; float K[2]; K[0] kf-P[0][0] / S; K[1] kf-P[1][0] / S; float y new_angle - kf-angle; kf-angle K[0] * y; kf-bias K[1] * y; float P00_temp kf-P[0][0]; float P01_temp kf-P[0][1]; kf-P[0][0] - K[0] * P00_temp; kf-P[0][1] - K[0] * P01_temp; kf-P[1][0] - K[1] * P00_temp; kf-P[1][1] - K[1] * P01_temp; return kf-angle; }這套代碼的邏輯是每毫秒調(diào)用一次 Update傳入加速度計解算出的角度 new_angle 和陀螺儀的原始角速度 new_rate。注意代碼里 P 矩陣的更新方式和網(wǎng)絡上流傳的簡化版不太一樣這里是完整保留了協(xié)方差矩陣的遞推關(guān)系在車體低轉(zhuǎn)速時能明顯減少角度輸出的抖動。參數(shù)調(diào)整的經(jīng)驗是如果車體反應遲鈍、倒下前角度還沒跟上就把 Q_angle 調(diào)大一點如果角度噪聲大、車體原地微抖就把 R_measure 調(diào)大。 dt 必須和實際調(diào)用周期一致如果主循環(huán)是 2ms 調(diào)用一次dt 就傳 0.002寫死 0.001 會讓濾波輸出失真這也是很多人移植濾波代碼后發(fā)現(xiàn)角度“慢半拍”的直接原因。2.3 互補濾波與 DMP更輕量的替代方案互補濾波的實現(xiàn)思路非常直觀陀螺儀積分后的角度在短時間內(nèi)可信加速度計直接解算的角度在長時間內(nèi)可信所以每次取 0.98 倍的陀螺儀角度加上 0.02 倍的加速度計角度一秒內(nèi)完成約 50 次這樣的遞推后陀螺儀漂移就被不斷“拉回”到加速度計參考軸上。典型代碼如下#define ALPHA 0.98f float Complementary_Filter(float acc_angle, float gyro_rate, float dt) { static float angle 0.0f; angle ALPHA * (angle gyro_rate * dt) (1.0f - ALPHA) * acc_angle; return angle; }ALPHA 越大濾波結(jié)果越信任陀螺儀角度平滑但滯后明顯ALPHA 越小角度越容易受振動影響但跟隨加速度計的速度更快。平衡車上一般從 0.95 起步試車體前后傾斜時角度響應遲鈍就減小到 0.90高頻抖動就回調(diào)到 0.98。它雖然沒有卡爾曼濾波那種嚴格的統(tǒng)計最優(yōu)性但在平衡車這種控制周期只有 5ms、角度變化率有限的場景里性能差異幾乎感知不到。DMP 的接入路徑是官方庫配合 I2C 讀取。初始化時調(diào)用 dmp_init 并配置采樣率主循環(huán)里通過 dmp_read_fifo 獲取四元數(shù)再用四元數(shù)轉(zhuǎn)歐拉角的公式得到 pitch。需要注意三點一是 DMP 初始化后必須先調(diào)用 dmp_set_fifo_rate 把輸出頻率設(shè)到 100Hz 左右太高的頻率會導致 FIFO 頻繁溢出二是讀取時要用 dmp_get_pitch 這類接口而不是直接拿原始四元數(shù)算避免單位轉(zhuǎn)換錯誤三是 DMP 的 yaw 會隨機漂移但平衡車控制根本不依賴 yaw只取 pitch 和 roll 就夠了。3. 源碼架構(gòu)拆解數(shù)據(jù)流、工程結(jié)構(gòu)與啟動順序3.1 從傳感器到 PWM 輸出的完整數(shù)據(jù)鏈路這套源碼的工程結(jié)構(gòu)并不復雜核心可以分成四層傳感器層負責初始化 MPU6050 并讀取原始數(shù)據(jù)解算層負責把原始數(shù)據(jù)變成角度和角速度控制層跑 PID 三環(huán)輸出層把控制量換算成 PWM 占空比。數(shù)據(jù)從傳感器到電機輸出完整鏈路是MPU6050 原始數(shù)據(jù) → 加速度計/陀螺儀分量提取 → 姿態(tài)解算 → 機械角度與角速度 → 直立環(huán) PD → 速度環(huán) PI → 轉(zhuǎn)向環(huán) PD → 左右電機 PWM。這條鏈路里最容易斷的位置在解算層和控制層之間。解算層輸出的是“物理角度”單位是度控制層期望的輸入是“相對于機械中位”的角度偏差。如果車體安裝時傳感器不是絕對水平解算出的角度就帶了固定偏置這時必須做機械零點校準把這組偏置值記錄在結(jié)構(gòu)體里否則直立環(huán)會一直朝著一個方向推電機車一上電就沖出去。源碼里用了 Angle_Offset 結(jié)構(gòu)體來存三軸零偏這個設(shè)計很實用換車架時只需要重新校準不需要改控制代碼。3.2 主循環(huán)與控制周期設(shè)計整個控制實時性依賴定時器中斷而不是主循環(huán) while(1) 里裸跑。源碼用定時器產(chǎn)生 5ms 中斷中斷里依次執(zhí)行讀取 MPU6050 原始數(shù)據(jù)、姿態(tài)解算更新、直立環(huán)計算、速度環(huán)計算、轉(zhuǎn)向環(huán)計算、最終 PWM 輸出。這段代碼是核心中的核心按順序?qū)懬宄笃谡{(diào)參非常關(guān)鍵。主循環(huán)部分通常長這樣int main(void) { System_Init(); // 時鐘、GPIO、定時器初始化 MPU6050_Init(); // 傳感器初始化含自檢 Kalman_Init(kf); // 選擇卡爾曼濾波時啟用 PID_Init(); // 三環(huán) PID 參數(shù)初始化 Encoder_Init(); // 編碼器捕獲初始化 Motor_Init(); // PWM 定時器初始化 MPU6050_Calibrate(); // 靜止校準獲取零點偏置 while (1) { // 主循環(huán)只做低優(yōu)先級任務 // 串口打印調(diào)試數(shù)據(jù)、按鍵檢測、OLED 顯示 Debug_Print_Angle(); Delay(10); } }這里有一個值得注意的設(shè)計控制部分全部塞進中斷回調(diào)主循環(huán)只做調(diào)試信息輸出和按鍵掃描這樣 CPU 不會被耗時的浮點運算卡住控制節(jié)拍穩(wěn)定。代碼里涉及到一個關(guān)鍵的結(jié)構(gòu)體 Balance_Control_t里面保存了角度目標值、速度累加值、上一次誤差等狀態(tài)每次中斷更新它主循環(huán)讀它避免數(shù)據(jù)競爭。3.3 模塊劃分與關(guān)鍵參數(shù)速查表源碼把功能拆成了獨立的模塊文件命名和職責對應得非常清楚。下表列出主要文件、核心接口和修改參數(shù)時的關(guān)注點方便拿到源碼后快速定位文件模塊核心接口參數(shù)修改點mpu6050.cMPU6050_Init / MPU6050_Read低通濾波帶寬、采樣率分頻kalman.cKalman_Init / Kalman_UpdateQ_angle / Q_bias / R_measurepid.cPID_Calc / PID_Reset三環(huán) Kp / Ki / Kd、積分限幅motor.cMotor_SetPWM / Motor_InitPWM 頻率、死區(qū)補償、方向翻轉(zhuǎn)encoder.cEncoder_Read / Encoder_Init編碼器線數(shù)倍頻、方向判定control.cBalance_Control_Run目標角度、速度環(huán)輸出限幅這套源碼里一個值得學習的點電機和編碼器的方向判定用宏定義統(tǒng)一管理更換電機或把編碼器裝到另一側(cè)時只需要改一個宏不用在 PID 輸出里到處改正負號。初次移植時最容易翻車的就是方向問題電機轉(zhuǎn)了但編碼器計數(shù)方向反了直立環(huán)直接變成正反饋車越倒越加速。4. 三環(huán) PID 控制參數(shù)整定流程與輸出限幅實戰(zhàn)4.1 直立環(huán)、速度環(huán)、轉(zhuǎn)向環(huán)的分工與串聯(lián)方式平衡車控制系統(tǒng)的框架是三個控制環(huán)疊加在同一個輸出通道上。最內(nèi)層是直立環(huán)輸入是當前角度與目標角度的偏差輸出是電機力矩目標是讓車體保持垂直外層是速度環(huán)輸入是目標速度與當前速度的偏差輸出不是直接疊加到 PWM 上而是改變直立環(huán)的目標角度最外層是轉(zhuǎn)向環(huán)輸入是轉(zhuǎn)向目標與當前偏航角速度的偏差輸出疊加到左右電機的差模分量上。這種串聯(lián)方式的核心設(shè)計意圖是直立環(huán)必須最靈敏響應速度要快所以用 PD 控制Kp 負責把角度偏差轉(zhuǎn)化成恢復力矩Kd 提供阻尼防止過沖速度環(huán)用 PI 控制因為速度誤差的消除需要積分項來克服靜摩擦力轉(zhuǎn)向環(huán)和直立環(huán)一樣用 PD因為轉(zhuǎn)向動作本身是瞬態(tài)的不需要積分。三個環(huán)的輸出疊加順序是先算直立環(huán)得到基礎(chǔ) PWM再疊加速度環(huán)修正量最后疊加轉(zhuǎn)向環(huán)差模量等效于左電機 直立環(huán) 速度環(huán) 轉(zhuǎn)向環(huán)右電機 直立環(huán) 速度環(huán) - 轉(zhuǎn)向環(huán)。4.2 參數(shù)整定流程從靜止到運動的三個階段調(diào)參順序強烈建議按下面的步驟走跳過任何一步都會讓后面排查變得痛苦。第一步先把車體用繩子吊起來或者用手扶著讓車輪懸空第二步調(diào)直立環(huán) Kp從小到大逐步加直到用手把車體掰到一邊時能感覺到明顯的回復力矩并且車體不再一松手就軟塌塌地倒下第三步加 Kd抑制車體在目標角度附近的振蕩標準是車體被瞬時撥動后能在 1 到 2 個周期內(nèi)收斂而不是持續(xù)高頻抖動第四步把車放到地面調(diào)速度環(huán) Kp讓它能抵抗輕輕的推拉而不倒第五步加速度環(huán) Ki消除長時間勻速漂移最后調(diào)轉(zhuǎn)向環(huán)讓車體能按照遙控指令平滑轉(zhuǎn)向而不打轉(zhuǎn)。調(diào)參過程中一個可行的中間狀態(tài)判斷方法代碼里加一個串口調(diào)試輸出每 10ms 輸出一組數(shù)據(jù)格式為當前角度、目標角度、直立環(huán)輸出、速度環(huán)輸出、最終 PWM。調(diào)參時盯著這幾個數(shù)值的關(guān)系看如果車體靜止時直立環(huán)輸出不為零說明目標角度和實際零點不一致如果速度環(huán)輸出持續(xù)朝一個方向增長說明積分項在飽和需要檢查限幅。源碼里提供了這種調(diào)試宏開關(guān)默認注釋掉打開后編譯即可。直立環(huán)和速度環(huán)的代碼通常是這樣串聯(lián)的int16_t Balance_Control_Run(float angle, float gyro, int16_t speed_left, int16_t speed_right) { float angle_err g_balance.angle_target - angle; float angle_out g_pid_balance.Kp * angle_err g_pid_balance.Kd * (-gyro); float speed_avg (speed_left speed_right) / 2.0f; float speed_err g_balance.speed_target - speed_avg; float speed_out g_pid_speed.Kp * speed_err g_pid_speed.Ki * g_pid_speed.integral; g_pid_speed.integral speed_err * 0.005f; if (g_pid_speed.integral 1000.0f) g_pid_speed.integral 1000.0f; if (g_pid_speed.integral -1000.0f) g_pid_speed.integral -1000.0f; float pwm_base angle_out speed_out; return (int16_t)pwm_base; }這段代碼里有幾個關(guān)鍵細節(jié)直立環(huán)的微分項用的是陀螺儀的角速度 gyro而不是角度誤差的差分這樣能避免對加速度計噪聲做微分放大的問題速度環(huán)的積分項手動做了限幅防止車被持續(xù)外力推著時積分無限增大導致外力消失后車瘋狂朝反方向沖出去速度環(huán)輸出是疊加到 angle_out 上的這正好實現(xiàn)了“速度環(huán)通過改變目標角度來影響直立環(huán)”的設(shè)計意圖。限幅參數(shù)的經(jīng)驗范圍可以這樣記直立環(huán)輸出在 -700 到 700 之間速度環(huán)輸出在 -300 到 300 之間轉(zhuǎn)向環(huán)輸出在 -200 到 200 之間最終 PWM 輸出限幅在 -900 到 900。這些數(shù)值不是固定的具體要看電源電壓和電機額定參數(shù)。如果 PWM 滿偏值本身設(shè)定是 1000就不要讓最終輸出長期貼著 1000 跑否則電機處于飽和狀態(tài)速度環(huán)和轉(zhuǎn)向環(huán)都會失效車體會呈現(xiàn)明顯的“失控感”。5. 避坑排查五種高頻故障現(xiàn)象原因與解決記錄5.1 電機高頻“嗞嗞”聲且車體劇烈抖動現(xiàn)象上電后電機沒有正常轉(zhuǎn)動而是發(fā)出尖銳的高頻嘯叫同時車體以肉眼可見的頻率劇烈抖動用手按住車體時能感覺到明顯的振動力度但一松手就倒向一側(cè)。原因PWM 頻率設(shè)置過低常見值是幾百赫茲。電機驅(qū)動電路和電機的電感在這種頻率下產(chǎn)生明顯的電流波動導致力矩輸出斷續(xù)而直立環(huán)又把這種斷續(xù)當成角度誤差來響應形成正反饋振蕩。解決把 PWM 定時器的頻率配置成 10kHz 到 20kHz 之間。在源碼的 motor.c 里修改定時器分頻和重載值讓 PWM 頻率進入人耳聽不到的超聲范圍。修改后電機聲音會變得很干凈車體的高頻抖動會立刻消失。檢查手段是用示波器看 PWM 波形頻率或者聽聲音變化明顯可感知。5.2 MPU6050 初始化失敗I2C 通信卡死現(xiàn)象程序運行到 MPU6050_Init 時卡住串口沒有打印任何信息或者打印了初始化失敗提示復位后依舊如此。原因排除了接線錯誤后最常見的是 I2C 總線上沒有上拉電阻。部分開發(fā)板內(nèi)部已經(jīng)集成上拉但外接 MPU6050 模塊時如果連接線過長、接觸不良總線 SCL/SDA 處于高阻態(tài)單片機檢測不到從設(shè)備應答。解決給 SCL 和 SDA 各加一個 4.7k 歐姆上拉電阻到 3.3V另外 I2C 速率不要太高源碼里默認配置為 400kHz把分頻系數(shù)改大降速到 100kHz 試試能提高抗干擾能力。檢查手段是單獨寫一個只掃描 I2C 設(shè)備地址的測試函數(shù)看能否返回 0x68 或 0x69如果返回值不對優(yōu)先懷疑上拉和接線。5.3 靜止時角度輸出緩慢漂移現(xiàn)象車體靜止放在桌面上串口打印的姿態(tài)角度數(shù)值每隔幾秒就變化零點幾度時間越長漂移越明顯導致直立環(huán)誤以為車體在傾斜。原因最直接的原因是陀螺儀零偏未校準。MPU6050 出廠時存在溫漂和個體差異的零偏如果不做靜止校準陀螺儀積分會把這個固定偏差不斷累積導致角度隨時間漂移。另外一個常見原因是加速度計未做低通濾波高頻振動噪聲被直接引入角度計算。解決第一步在初始化后讓車體完全靜止 1 秒以上采樣陀螺儀輸出求平均值作為零偏補償值第二步確認濾波系數(shù)是否正確把加速度計原始數(shù)據(jù)經(jīng)過低通濾波后再算角度。這套源碼里已經(jīng)實現(xiàn)了 mpu6050_offset_cal 函數(shù)調(diào)用位置在主循環(huán)之前使用時要確認車體確實靜止否則會把運動狀態(tài)當成零偏記錄下來。5.4 電池電壓下降后車子突然變軟或朝一個方向加速現(xiàn)象滿電時車體直立穩(wěn)定運行幾分鐘后電池電壓降低車體開始慢慢前傾直立環(huán)輸出不足以把它拉回來最后朝前加速倒下。原因這是典型的 PWM 輸出未做電源電壓補償。當電池電壓下降時同樣占空比下電機實際電壓降低、力矩變小但控制環(huán)的 PID 計算依然按滿電壓的等效關(guān)系輸出相當于控制器對執(zhí)行機構(gòu)的控制能力打了折扣系統(tǒng)增益隨電壓變化。解決在讀取電池電壓的 AD 值后按電壓比例對最終 PWM 輸出做修正常見做法是維持一個基準電壓比如 12V 時輸出 1000 對應滿占空比當電壓降到 10V 時輸出限幅值同步按 10/12 縮放保持電機的實際出力不變。這樣電壓變化時控制參數(shù)基本不受影響。源碼里預留了 voltage_compensate 接口沒有實現(xiàn)完整邏輯建議自行填充 AD 采集代碼。5.5 裝好后轉(zhuǎn)向環(huán)正常一加速度環(huán)就左右甩尾現(xiàn)象單獨調(diào)直立環(huán)時車體很穩(wěn)手輕輕推也能恢復直立。但開啟速度環(huán)后車體開始前后往復擺動幅度越來越大最后變成左右甩尾式的失穩(wěn)。原因速度環(huán)積分系數(shù)設(shè)置過大或者速度環(huán)輸出限幅值設(shè)得太高。速度環(huán)輸出改變的是直立環(huán)的目標角度如果目標角度變化幅度過大、變化速度過快直立環(huán)會認為車體嚴重偏離目標產(chǎn)生一個很大的輸出導致電機猛烈加速反過來又讓速度誤差更大形成惡性循環(huán)。解決把速度環(huán)的 Ki 減半再觀察同時把速度環(huán)輸出限幅從 300 收窄到 150讓速度環(huán)對目標角度的改變更溫和。另外確認速度環(huán)的反饋方向是否正確——車體向前運動時編碼器值應為負速度誤差為負輸出應為正如果方向反了速度環(huán)會持續(xù)加大車速導致失控。判斷方法用手推動車體觀察串口打印的速度值符號和方向是否一致不對的話修改編碼器方向宏定義。6. 進階用法上電自檢、數(shù)據(jù)觀測與站立自鎖調(diào)試技巧最后分享三個在這套源碼基礎(chǔ)上常用的進階技巧能顯著減少調(diào)試時間和維護成本。第一個是上電自檢流程在進入主循環(huán)之前依次執(zhí)行傳感器初始化、零偏校準、電機 PWM 輸出置零、編碼器讀數(shù)清零然后延時 500ms 等待電源穩(wěn)定再解除電機剎車。這個流程看起來簡單但能避免一個問題——很多車一上電就突然加速是因為電機在初始化瞬間收到了一個殘留在 PWM 寄存器里的舊值。正確做法是初始化最后把 PWM 輸出強制寫到 0再做一次編碼器清零確保第一幀控制輸出時所有反饋都是干凈的。第二個是數(shù)據(jù)觀測方法。調(diào)平衡車必須能看到角度和輸出不然全靠手感是玄學。建議用串口以 100Hz 頻率輸出一段格式化數(shù)據(jù)終端里按列顯示角度、目標角度、直立環(huán)輸出、速度環(huán)輸出、最終 PWM。調(diào)試時重點看兩個指標一是靜止時角度和目標角度的差長期是否為零不為零就要重新做機械零點校準二是撥動車體一次后角度能否在兩個來回內(nèi)收斂到目標值附近如果連續(xù)振蕩四次以上就說明 Kd 不夠或者濾波環(huán)節(jié)的相位滯后太大。更快的方式是先用仿真工具對卡爾曼濾波和 PID 參數(shù)做一次離線驗證很多數(shù)學工具都能直接導入傳感器采集的數(shù)據(jù)調(diào)好參數(shù)再燒進單片機能明顯減少在實物上反復燒錄的次數(shù)。第三個是站立自鎖功能。平衡車有兩種常見工作模式上電即平衡、上電視為待機模式。待機模式更安全——上電后車子不轉(zhuǎn)按下按鍵后才啟動平衡算法。實現(xiàn)方式不復雜主循環(huán)檢測按鍵按下置位 enable 標志控制中斷里檢查該標志沒有置位時直接把 PWM 輸出置零且不執(zhí)行 PID 計算。這個習慣值得保留特別是調(diào)試階段車子放在桌面時不會被意外啟動的電機帶跑。初始化時我一般會強制走一遍整體檢查先看傳感器數(shù)據(jù)是否穩(wěn)定再看角度能否跟隨手動傾斜再確認電機方向與編碼器方向一致最后才松開手讓它進入平衡狀態(tài)。從那以后我每次移植新源碼或者換新硬件都強制走這一遍節(jié)省過太多排查時間希望幫到你。整套源碼的完整工程文件、依賴庫和燒錄說明都在壓縮包里按 README 里的引腳映射接好線燒錄后先調(diào)直立環(huán)就能站起來。本文還有配套的精品資源點擊獲取 二輪平衡車全套源碼：卡爾曼濾波與PID三環(huán)控制實戰(zhàn)
簡介這是一套面向電子愛好者與嵌入式初學者的二輪平衡車DIY完整資料包涵蓋從硬件搭建到軟件調(diào)試的全鏈路內(nèi)容。資源定位為STM32平臺平衡車的開源學習套件包含互補濾波、卡爾曼濾波、DMP三種姿態(tài)解算方案的源碼三套工程均編譯通過、可直接燒錄驗證適合希望動手制作平衡小車或深入理解濾波算法差異的讀者。壓縮包大小148.18MB內(nèi)置全部平衡小車相關(guān)資料類型上以Keil工程源碼、電路原理圖、PCB文件、連線說明文檔為主硬件與軟件配套呈現(xiàn)可以大幅降低DIY門檻。已有3129人學習瀏覽整體經(jīng)過本科階段完整調(diào)試驗證是一份少有的“從零到跑通”的參考資源。讀者可以獲得實物接線指引、電路設(shè)計源文件以及三種濾波方案的對比實現(xiàn)既能直接復刻樣車也能基于源碼二次開發(fā)用于課程設(shè)計、畢業(yè)設(shè)計或興趣項目。1. 平衡車全套源碼從零開始搭一輛能站穩(wěn)的兩輪車第一次接觸二輪平衡車的人往往以為難點在機械組裝等真把套件拼好、電機轉(zhuǎn)起來才發(fā)現(xiàn)真正讓人頭疼的是讓它站穩(wěn)。一個常見現(xiàn)象是車能啟動但兩三秒內(nèi)就朝一個方向沖出去或者原地高頻抖動這就是典型的角度環(huán)與速度環(huán)沒配合好。市面上能跑的平衡車源碼不少但大多數(shù)要么只給姿態(tài)解算片段要么把 PID 調(diào)參寫成玄學讓新手連入口都找不到。這套二輪平衡車全套源碼的價值在于它把從傳感器讀取、姿態(tài)解算到雙環(huán)/三環(huán)控制、PWM 輸出的完整鏈路都串起來了適合正在做課設(shè)、電賽備賽或想入門嵌入式控制的人直接照抄再改。2. 姿態(tài)解算選型卡爾曼濾波、DMP 與互補濾波的取舍和落地代碼2.1 三種解算方案的適用邊界為什么不是越復雜越好平衡車要解決的核心問題是MPU6050 輸出的加速度計和陀螺儀數(shù)據(jù)各自有硬傷。加速度計在靜止時能準確給出重力方向但車體一震動高頻噪聲會直接把角度信號淹沒陀螺儀積分得到的角度在短時間內(nèi)很平滑但積分漂移會隨時間累積幾秒后角度就開始“飄”。姿態(tài)解算的全部工作就是把這兩個都不完美的信號融合成一個相對可信的角度?？柭鼮V波的思路是把角度和陀螺儀零偏作為狀態(tài)量每次拿加速度計的角度做測量更新拿陀螺儀的角速度做預測更新。它收斂快、動態(tài)響應好但需要反復調(diào)協(xié)方差矩陣的過程噪聲和測量噪聲調(diào)不好就會出現(xiàn)角度滯后或者過沖。DMP 是 MPU6050 內(nèi)部的硬件解算引擎DMP 庫初始化后直接讀四元數(shù)再轉(zhuǎn)歐拉角CPU 占用極小但它把融合過程當黑匣子輸出是相對姿態(tài)想改濾波特性基本無從下手?；パa濾波則用一個系數(shù) alpha 把兩個信號按頻率高低切開陀螺儀負責高頻段加速度計負責低頻段。代碼只有幾行在平衡車這種垂直安裝、運行范圍不到 30 度的場景里完全夠用而且調(diào)試只要調(diào)一個 alpha。我的建議很簡單做課設(shè)或功能演示優(yōu)先互補濾波代碼短、好解釋、調(diào)參直觀做競賽或追求極限動態(tài)響應用卡爾曼濾波DMP 庫適合不想碰矩陣運算、把精力全放在 PID 上的人。這套源碼把三種方案都放了切換時只需要改融合函數(shù)入口不用動上層控制邏輯。2.2 卡爾曼濾波實現(xiàn)一維模型與核心代碼卡爾曼濾波在平衡車場景里可以簡化為一維模型——只對俯仰角pitch做融合橫滾角同理但角度變化幅度遠小于俯仰可以先不處理。狀態(tài)量有兩個角度 angle 和陀螺儀零偏 bias。預測方程用的是陀螺儀角速度減掉零偏后的值測量方程用的是加速度計算出的角度。下面是一份可直接嵌入單片機的卡爾曼濾波 C 代碼typedef struct { float angle; // 融合后的角度輸出 float bias; // 陀螺儀零偏 float P[2][2]; // 協(xié)方差矩陣 float Q_angle; // 角度過程噪聲 float Q_bias; // 零偏過程噪聲 float R_measure; // 測量噪聲 } Kalman_t; void Kalman_Init(Kalman_t *kf) { kf-angle 0.0f; kf-bias 0.0f; kf-P[0][0] 0.0f; kf-P[0][1] 0.0f; kf-P[1][0] 0.0f; kf-P[1][1] 0.0f; kf-Q_angle 0.001f; kf-Q_bias 0.003f; kf-R_measure 0.03f; } float Kalman_Update(Kalman_t *kf, float new_angle, float new_rate, float dt) { float rate new_rate - kf-bias; kf-angle dt * rate; kf-P[0][0] dt * (dt * kf-P[1][1] - kf-P[0][1] - kf-P[1][0] kf-Q_angle); kf-P[0][1] - dt * kf-P[1][1]; kf-P[1][0] - dt * kf-P[1][1]; kf-P[1][1] kf-Q_bias * dt; float S kf-P[0][0] kf-R_measure; float K[2]; K[0] kf-P[0][0] / S; K[1] kf-P[1][0] / S; float y new_angle - kf-angle; kf-angle K[0] * y; kf-bias K[1] * y; float P00_temp kf-P[0][0]; float P01_temp kf-P[0][1]; kf-P[0][0] - K[0] * P00_temp; kf-P[0][1] - K[0] * P01_temp; kf-P[1][0] - K[1] * P00_temp; kf-P[1][1] - K[1] * P01_temp; return kf-angle; }這套代碼的邏輯是每毫秒調(diào)用一次 Update傳入加速度計解算出的角度 new_angle 和陀螺儀的原始角速度 new_rate。注意代碼里 P 矩陣的更新方式和網(wǎng)絡上流傳的簡化版不太一樣這里是完整保留了協(xié)方差矩陣的遞推關(guān)系在車體低轉(zhuǎn)速時能明顯減少角度輸出的抖動。參數(shù)調(diào)整的經(jīng)驗是如果車體反應遲鈍、倒下前角度還沒跟上就把 Q_angle 調(diào)大一點如果角度噪聲大、車體原地微抖就把 R_measure 調(diào)大。 dt 必須和實際調(diào)用周期一致如果主循環(huán)是 2ms 調(diào)用一次dt 就傳 0.002寫死 0.001 會讓濾波輸出失真這也是很多人移植濾波代碼后發(fā)現(xiàn)角度“慢半拍”的直接原因。2.3 互補濾波與 DMP更輕量的替代方案互補濾波的實現(xiàn)思路非常直觀陀螺儀積分后的角度在短時間內(nèi)可信加速度計直接解算的角度在長時間內(nèi)可信所以每次取 0.98 倍的陀螺儀角度加上 0.02 倍的加速度計角度一秒內(nèi)完成約 50 次這樣的遞推后陀螺儀漂移就被不斷“拉回”到加速度計參考軸上。典型代碼如下#define ALPHA 0.98f float Complementary_Filter(float acc_angle, float gyro_rate, float dt) { static float angle 0.0f; angle ALPHA * (angle gyro_rate * dt) (1.0f - ALPHA) * acc_angle; return angle; }ALPHA 越大濾波結(jié)果越信任陀螺儀角度平滑但滯后明顯ALPHA 越小角度越容易受振動影響但跟隨加速度計的速度更快。平衡車上一般從 0.95 起步試車體前后傾斜時角度響應遲鈍就減小到 0.90高頻抖動就回調(diào)到 0.98。它雖然沒有卡爾曼濾波那種嚴格的統(tǒng)計最優(yōu)性但在平衡車這種控制周期只有 5ms、角度變化率有限的場景里性能差異幾乎感知不到。DMP 的接入路徑是官方庫配合 I2C 讀取。初始化時調(diào)用 dmp_init 并配置采樣率主循環(huán)里通過 dmp_read_fifo 獲取四元數(shù)再用四元數(shù)轉(zhuǎn)歐拉角的公式得到 pitch。需要注意三點一是 DMP 初始化后必須先調(diào)用 dmp_set_fifo_rate 把輸出頻率設(shè)到 100Hz 左右太高的頻率會導致 FIFO 頻繁溢出二是讀取時要用 dmp_get_pitch 這類接口而不是直接拿原始四元數(shù)算避免單位轉(zhuǎn)換錯誤三是 DMP 的 yaw 會隨機漂移但平衡車控制根本不依賴 yaw只取 pitch 和 roll 就夠了。3. 源碼架構(gòu)拆解數(shù)據(jù)流、工程結(jié)構(gòu)與啟動順序3.1 從傳感器到 PWM 輸出的完整數(shù)據(jù)鏈路這套源碼的工程結(jié)構(gòu)并不復雜核心可以分成四層傳感器層負責初始化 MPU6050 并讀取原始數(shù)據(jù)解算層負責把原始數(shù)據(jù)變成角度和角速度控制層跑 PID 三環(huán)輸出層把控制量換算成 PWM 占空比。數(shù)據(jù)從傳感器到電機輸出完整鏈路是MPU6050 原始數(shù)據(jù) → 加速度計/陀螺儀分量提取 → 姿態(tài)解算 → 機械角度與角速度 → 直立環(huán) PD → 速度環(huán) PI → 轉(zhuǎn)向環(huán) PD → 左右電機 PWM。這條鏈路里最容易斷的位置在解算層和控制層之間。解算層輸出的是“物理角度”單位是度控制層期望的輸入是“相對于機械中位”的角度偏差。如果車體安裝時傳感器不是絕對水平解算出的角度就帶了固定偏置這時必須做機械零點校準把這組偏置值記錄在結(jié)構(gòu)體里否則直立環(huán)會一直朝著一個方向推電機車一上電就沖出去。源碼里用了 Angle_Offset 結(jié)構(gòu)體來存三軸零偏這個設(shè)計很實用換車架時只需要重新校準不需要改控制代碼。3.2 主循環(huán)與控制周期設(shè)計整個控制實時性依賴定時器中斷而不是主循環(huán) while(1) 里裸跑。源碼用定時器產(chǎn)生 5ms 中斷中斷里依次執(zhí)行讀取 MPU6050 原始數(shù)據(jù)、姿態(tài)解算更新、直立環(huán)計算、速度環(huán)計算、轉(zhuǎn)向環(huán)計算、最終 PWM 輸出。這段代碼是核心中的核心按順序?qū)懬宄笃谡{(diào)參非常關(guān)鍵。主循環(huán)部分通常長這樣int main(void) { System_Init(); // 時鐘、GPIO、定時器初始化 MPU6050_Init(); // 傳感器初始化含自檢 Kalman_Init(kf); // 選擇卡爾曼濾波時啟用 PID_Init(); // 三環(huán) PID 參數(shù)初始化 Encoder_Init(); // 編碼器捕獲初始化 Motor_Init(); // PWM 定時器初始化 MPU6050_Calibrate(); // 靜止校準獲取零點偏置 while (1) { // 主循環(huán)只做低優(yōu)先級任務 // 串口打印調(diào)試數(shù)據(jù)、按鍵檢測、OLED 顯示 Debug_Print_Angle(); Delay(10); } }這里有一個值得注意的設(shè)計控制部分全部塞進中斷回調(diào)主循環(huán)只做調(diào)試信息輸出和按鍵掃描這樣 CPU 不會被耗時的浮點運算卡住控制節(jié)拍穩(wěn)定。代碼里涉及到一個關(guān)鍵的結(jié)構(gòu)體 Balance_Control_t里面保存了角度目標值、速度累加值、上一次誤差等狀態(tài)每次中斷更新它主循環(huán)讀它避免數(shù)據(jù)競爭。3.3 模塊劃分與關(guān)鍵參數(shù)速查表源碼把功能拆成了獨立的模塊文件命名和職責對應得非常清楚。下表列出主要文件、核心接口和修改參數(shù)時的關(guān)注點方便拿到源碼后快速定位文件模塊核心接口參數(shù)修改點mpu6050.cMPU6050_Init / MPU6050_Read低通濾波帶寬、采樣率分頻kalman.cKalman_Init / Kalman_UpdateQ_angle / Q_bias / R_measurepid.cPID_Calc / PID_Reset三環(huán) Kp / Ki / Kd、積分限幅motor.cMotor_SetPWM / Motor_InitPWM 頻率、死區(qū)補償、方向翻轉(zhuǎn)encoder.cEncoder_Read / Encoder_Init編碼器線數(shù)倍頻、方向判定control.cBalance_Control_Run目標角度、速度環(huán)輸出限幅這套源碼里一個值得學習的點電機和編碼器的方向判定用宏定義統(tǒng)一管理更換電機或把編碼器裝到另一側(cè)時只需要改一個宏不用在 PID 輸出里到處改正負號。初次移植時最容易翻車的就是方向問題電機轉(zhuǎn)了但編碼器計數(shù)方向反了直立環(huán)直接變成正反饋車越倒越加速。4. 三環(huán) PID 控制參數(shù)整定流程與輸出限幅實戰(zhàn)4.1 直立環(huán)、速度環(huán)、轉(zhuǎn)向環(huán)的分工與串聯(lián)方式平衡車控制系統(tǒng)的框架是三個控制環(huán)疊加在同一個輸出通道上。最內(nèi)層是直立環(huán)輸入是當前角度與目標角度的偏差輸出是電機力矩目標是讓車體保持垂直外層是速度環(huán)輸入是目標速度與當前速度的偏差輸出不是直接疊加到 PWM 上而是改變直立環(huán)的目標角度最外層是轉(zhuǎn)向環(huán)輸入是轉(zhuǎn)向目標與當前偏航角速度的偏差輸出疊加到左右電機的差模分量上。這種串聯(lián)方式的核心設(shè)計意圖是直立環(huán)必須最靈敏響應速度要快所以用 PD 控制Kp 負責把角度偏差轉(zhuǎn)化成恢復力矩Kd 提供阻尼防止過沖速度環(huán)用 PI 控制因為速度誤差的消除需要積分項來克服靜摩擦力轉(zhuǎn)向環(huán)和直立環(huán)一樣用 PD因為轉(zhuǎn)向動作本身是瞬態(tài)的不需要積分。三個環(huán)的輸出疊加順序是先算直立環(huán)得到基礎(chǔ) PWM再疊加速度環(huán)修正量最后疊加轉(zhuǎn)向環(huán)差模量等效于左電機 直立環(huán) 速度環(huán) 轉(zhuǎn)向環(huán)右電機 直立環(huán) 速度環(huán) - 轉(zhuǎn)向環(huán)。4.2 參數(shù)整定流程從靜止到運動的三個階段調(diào)參順序強烈建議按下面的步驟走跳過任何一步都會讓后面排查變得痛苦。第一步先把車體用繩子吊起來或者用手扶著讓車輪懸空第二步調(diào)直立環(huán) Kp從小到大逐步加直到用手把車體掰到一邊時能感覺到明顯的回復力矩并且車體不再一松手就軟塌塌地倒下第三步加 Kd抑制車體在目標角度附近的振蕩標準是車體被瞬時撥動后能在 1 到 2 個周期內(nèi)收斂而不是持續(xù)高頻抖動第四步把車放到地面調(diào)速度環(huán) Kp讓它能抵抗輕輕的推拉而不倒第五步加速度環(huán) Ki消除長時間勻速漂移最后調(diào)轉(zhuǎn)向環(huán)讓車體能按照遙控指令平滑轉(zhuǎn)向而不打轉(zhuǎn)。調(diào)參過程中一個可行的中間狀態(tài)判斷方法代碼里加一個串口調(diào)試輸出每 10ms 輸出一組數(shù)據(jù)格式為當前角度、目標角度、直立環(huán)輸出、速度環(huán)輸出、最終 PWM。調(diào)參時盯著這幾個數(shù)值的關(guān)系看如果車體靜止時直立環(huán)輸出不為零說明目標角度和實際零點不一致如果速度環(huán)輸出持續(xù)朝一個方向增長說明積分項在飽和需要檢查限幅。源碼里提供了這種調(diào)試宏開關(guān)默認注釋掉打開后編譯即可。直立環(huán)和速度環(huán)的代碼通常是這樣串聯(lián)的int16_t Balance_Control_Run(float angle, float gyro, int16_t speed_left, int16_t speed_right) { float angle_err g_balance.angle_target - angle; float angle_out g_pid_balance.Kp * angle_err g_pid_balance.Kd * (-gyro); float speed_avg (speed_left speed_right) / 2.0f; float speed_err g_balance.speed_target - speed_avg; float speed_out g_pid_speed.Kp * speed_err g_pid_speed.Ki * g_pid_speed.integral; g_pid_speed.integral speed_err * 0.005f; if (g_pid_speed.integral 1000.0f) g_pid_speed.integral 1000.0f; if (g_pid_speed.integral -1000.0f) g_pid_speed.integral -1000.0f; float pwm_base angle_out speed_out; return (int16_t)pwm_base; }這段代碼里有幾個關(guān)鍵細節(jié)直立環(huán)的微分項用的是陀螺儀的角速度 gyro而不是角度誤差的差分這樣能避免對加速度計噪聲做微分放大的問題速度環(huán)的積分項手動做了限幅防止車被持續(xù)外力推著時積分無限增大導致外力消失后車瘋狂朝反方向沖出去速度環(huán)輸出是疊加到 angle_out 上的這正好實現(xiàn)了“速度環(huán)通過改變目標角度來影響直立環(huán)”的設(shè)計意圖。限幅參數(shù)的經(jīng)驗范圍可以這樣記直立環(huán)輸出在 -700 到 700 之間速度環(huán)輸出在 -300 到 300 之間轉(zhuǎn)向環(huán)輸出在 -200 到 200 之間最終 PWM 輸出限幅在 -900 到 900。這些數(shù)值不是固定的具體要看電源電壓和電機額定參數(shù)。如果 PWM 滿偏值本身設(shè)定是 1000就不要讓最終輸出長期貼著 1000 跑否則電機處于飽和狀態(tài)速度環(huán)和轉(zhuǎn)向環(huán)都會失效車體會呈現(xiàn)明顯的“失控感”。5. 避坑排查五種高頻故障現(xiàn)象原因與解決記錄5.1 電機高頻“嗞嗞”聲且車體劇烈抖動現(xiàn)象上電后電機沒有正常轉(zhuǎn)動而是發(fā)出尖銳的高頻嘯叫同時車體以肉眼可見的頻率劇烈抖動用手按住車體時能感覺到明顯的振動力度但一松手就倒向一側(cè)。原因PWM 頻率設(shè)置過低常見值是幾百赫茲。電機驅(qū)動電路和電機的電感在這種頻率下產(chǎn)生明顯的電流波動導致力矩輸出斷續(xù)而直立環(huán)又把這種斷續(xù)當成角度誤差來響應形成正反饋振蕩。解決把 PWM 定時器的頻率配置成 10kHz 到 20kHz 之間。在源碼的 motor.c 里修改定時器分頻和重載值讓 PWM 頻率進入人耳聽不到的超聲范圍。修改后電機聲音會變得很干凈車體的高頻抖動會立刻消失。檢查手段是用示波器看 PWM 波形頻率或者聽聲音變化明顯可感知。5.2 MPU6050 初始化失敗I2C 通信卡死現(xiàn)象程序運行到 MPU6050_Init 時卡住串口沒有打印任何信息或者打印了初始化失敗提示復位后依舊如此。原因排除了接線錯誤后最常見的是 I2C 總線上沒有上拉電阻。部分開發(fā)板內(nèi)部已經(jīng)集成上拉但外接 MPU6050 模塊時如果連接線過長、接觸不良總線 SCL/SDA 處于高阻態(tài)單片機檢測不到從設(shè)備應答。解決給 SCL 和 SDA 各加一個 4.7k 歐姆上拉電阻到 3.3V另外 I2C 速率不要太高源碼里默認配置為 400kHz把分頻系數(shù)改大降速到 100kHz 試試能提高抗干擾能力。檢查手段是單獨寫一個只掃描 I2C 設(shè)備地址的測試函數(shù)看能否返回 0x68 或 0x69如果返回值不對優(yōu)先懷疑上拉和接線。5.3 靜止時角度輸出緩慢漂移現(xiàn)象車體靜止放在桌面上串口打印的姿態(tài)角度數(shù)值每隔幾秒就變化零點幾度時間越長漂移越明顯導致直立環(huán)誤以為車體在傾斜。原因最直接的原因是陀螺儀零偏未校準。MPU6050 出廠時存在溫漂和個體差異的零偏如果不做靜止校準陀螺儀積分會把這個固定偏差不斷累積導致角度隨時間漂移。另外一個常見原因是加速度計未做低通濾波高頻振動噪聲被直接引入角度計算。解決第一步在初始化后讓車體完全靜止 1 秒以上采樣陀螺儀輸出求平均值作為零偏補償值第二步確認濾波系數(shù)是否正確把加速度計原始數(shù)據(jù)經(jīng)過低通濾波后再算角度。這套源碼里已經(jīng)實現(xiàn)了 mpu6050_offset_cal 函數(shù)調(diào)用位置在主循環(huán)之前使用時要確認車體確實靜止否則會把運動狀態(tài)當成零偏記錄下來。5.4 電池電壓下降后車子突然變軟或朝一個方向加速現(xiàn)象滿電時車體直立穩(wěn)定運行幾分鐘后電池電壓降低車體開始慢慢前傾直立環(huán)輸出不足以把它拉回來最后朝前加速倒下。原因這是典型的 PWM 輸出未做電源電壓補償。當電池電壓下降時同樣占空比下電機實際電壓降低、力矩變小但控制環(huán)的 PID 計算依然按滿電壓的等效關(guān)系輸出相當于控制器對執(zhí)行機構(gòu)的控制能力打了折扣系統(tǒng)增益隨電壓變化。解決在讀取電池電壓的 AD 值后按電壓比例對最終 PWM 輸出做修正常見做法是維持一個基準電壓比如 12V 時輸出 1000 對應滿占空比當電壓降到 10V 時輸出限幅值同步按 10/12 縮放保持電機的實際出力不變。這樣電壓變化時控制參數(shù)基本不受影響。源碼里預留了 voltage_compensate 接口沒有實現(xiàn)完整邏輯建議自行填充 AD 采集代碼。5.5 裝好后轉(zhuǎn)向環(huán)正常一加速度環(huán)就左右甩尾現(xiàn)象單獨調(diào)直立環(huán)時車體很穩(wěn)手輕輕推也能恢復直立。但開啟速度環(huán)后車體開始前后往復擺動幅度越來越大最后變成左右甩尾式的失穩(wěn)。原因速度環(huán)積分系數(shù)設(shè)置過大或者速度環(huán)輸出限幅值設(shè)得太高。速度環(huán)輸出改變的是直立環(huán)的目標角度如果目標角度變化幅度過大、變化速度過快直立環(huán)會認為車體嚴重偏離目標產(chǎn)生一個很大的輸出導致電機猛烈加速反過來又讓速度誤差更大形成惡性循環(huán)。解決把速度環(huán)的 Ki 減半再觀察同時把速度環(huán)輸出限幅從 300 收窄到 150讓速度環(huán)對目標角度的改變更溫和。另外確認速度環(huán)的反饋方向是否正確——車體向前運動時編碼器值應為負速度誤差為負輸出應為正如果方向反了速度環(huán)會持續(xù)加大車速導致失控。判斷方法用手推動車體觀察串口打印的速度值符號和方向是否一致不對的話修改編碼器方向宏定義。6. 進階用法上電自檢、數(shù)據(jù)觀測與站立自鎖調(diào)試技巧最后分享三個在這套源碼基礎(chǔ)上常用的進階技巧能顯著減少調(diào)試時間和維護成本。第一個是上電自檢流程在進入主循環(huán)之前依次執(zhí)行傳感器初始化、零偏校準、電機 PWM 輸出置零、編碼器讀數(shù)清零然后延時 500ms 等待電源穩(wěn)定再解除電機剎車。這個流程看起來簡單但能避免一個問題——很多車一上電就突然加速是因為電機在初始化瞬間收到了一個殘留在 PWM 寄存器里的舊值。正確做法是初始化最后把 PWM 輸出強制寫到 0再做一次編碼器清零確保第一幀控制輸出時所有反饋都是干凈的。第二個是數(shù)據(jù)觀測方法。調(diào)平衡車必須能看到角度和輸出不然全靠手感是玄學。建議用串口以 100Hz 頻率輸出一段格式化數(shù)據(jù)終端里按列顯示角度、目標角度、直立環(huán)輸出、速度環(huán)輸出、最終 PWM。調(diào)試時重點看兩個指標一是靜止時角度和目標角度的差長期是否為零不為零就要重新做機械零點校準二是撥動車體一次后角度能否在兩個來回內(nèi)收斂到目標值附近如果連續(xù)振蕩四次以上就說明 Kd 不夠或者濾波環(huán)節(jié)的相位滯后太大。更快的方式是先用仿真工具對卡爾曼濾波和 PID 參數(shù)做一次離線驗證很多數(shù)學工具都能直接導入傳感器采集的數(shù)據(jù)調(diào)好參數(shù)再燒進單片機能明顯減少在實物上反復燒錄的次數(shù)。第三個是站立自鎖功能。平衡車有兩種常見工作模式上電即平衡、上電視為待機模式。待機模式更安全——上電后車子不轉(zhuǎn)按下按鍵后才啟動平衡算法。實現(xiàn)方式不復雜主循環(huán)檢測按鍵按下置位 enable 標志控制中斷里檢查該標志沒有置位時直接把 PWM 輸出置零且不執(zhí)行 PID 計算。這個習慣值得保留特別是調(diào)試階段車子放在桌面時不會被意外啟動的電機帶跑。初始化時我一般會強制走一遍整體檢查先看傳感器數(shù)據(jù)是否穩(wěn)定再看角度能否跟隨手動傾斜再確認電機方向與編碼器方向一致最后才松開手讓它進入平衡狀態(tài)。從那以后我每次移植新源碼或者換新硬件都強制走這一遍節(jié)省過太多排查時間希望幫到你。整套源碼的完整工程文件、依賴庫和燒錄說明都在壓縮包里按 README 里的引腳映射接好線燒錄后先調(diào)直立環(huán)就能站起來。本文還有配套的精品資源點擊獲取