簡(jiǎn)介基于意法半導(dǎo)體 STM32F407ZGT6 芯片與 MPU6050 姿態(tài)傳感器利用速度環(huán)加位置環(huán)串級(jí) PID 控制算法實(shí)現(xiàn)兩輪自平衡車的技術(shù)資料包內(nèi)容覆蓋完整工程源碼與 Keil 項(xiàng)目配置適合電子信息、自動(dòng)化、計(jì)算機(jī)等專業(yè)學(xué)生作為課程設(shè)計(jì)、期末大作業(yè)或畢業(yè)設(shè)計(jì)參考資料也適合具備 C 語(yǔ)言與 PID 基礎(chǔ)、希望深入理解平衡車控制原理的嵌入式開(kāi)發(fā)者自學(xué)與二次開(kāi)發(fā)。壓縮包共有 188 個(gè)文件大小約 5.33MB主要構(gòu)成為 59 個(gè)頭文件、49 個(gè) C 源文件同時(shí)包含工程配置文件、編譯生成的固件與映射文件以及說(shuō)明文檔和輔助腳本目錄結(jié)構(gòu)清晰便于按模塊閱讀和重新編譯目前已有 357 人學(xué)習(xí)或下載。資料價(jià)值體現(xiàn)在源碼完整可直接燒錄驗(yàn)證速度環(huán)與位置環(huán)兩級(jí) PID 以及 MPU6050 姿態(tài)解算實(shí)現(xiàn)思路明確算法模塊相對(duì)獨(dú)立附帶工程配置與清理腳本可減少環(huán)境處理時(shí)間便于后續(xù)進(jìn)行參數(shù)調(diào)試、算法對(duì)比或功能擴(kuò)展。1. 從串級(jí) PID 理解兩輪車平衡為什么內(nèi)環(huán)不倒、外環(huán)不跑每次有同行來(lái)問(wèn)“為什么我的兩輪車裝了 MPU6050 還是倒”我第一個(gè)問(wèn)題不是問(wèn) PID 參數(shù)而是問(wèn)他速度環(huán)是不是沒(méi)加限幅。標(biāo)題里這三個(gè)詞——速度環(huán)、位置環(huán)、串級(jí) PID——描述的其實(shí)是同一個(gè)控制骨架最內(nèi)層用角度環(huán)把車身拉回豎直外層用速度環(huán)和位置環(huán)把整車?yán)卦亍：芏噘Y料把“速度環(huán)和位置環(huán)串級(jí)”掛在標(biāo)題上但真正讓車不倒的不是外環(huán)而是被外環(huán)驅(qū)動(dòng)的那個(gè)內(nèi)環(huán)角度反饋。用 STM32F407ZGT6 跑這套系統(tǒng)168MHz 主頻和 FPU 算三個(gè)回路綽綽有余MPU6050 也只提供加速度和角速度兩組原始數(shù)據(jù)。適合剛?cè)胧肿云胶廛嚨那度胧焦こ處熞策m合手里有資料包但對(duì)“內(nèi)外環(huán)怎么嵌套、參數(shù)怎么一步一步調(diào)出來(lái)”還含糊的人。下面按姿態(tài)解算、回路搭建、硬件接線、調(diào)參、定點(diǎn)停車這個(gè)順序往下講。2. MPU6050 姿態(tài)解算先讓 STM32F407ZGT6 拿到能用的角度2.1 配置 MPU6050 的要點(diǎn)量程、采樣率、數(shù)字低通MPU6050 在平衡車?yán)锏慕巧皇恰敖嵌葌鞲衅鳌倍恰敖撬俣葌鞲衅? 加速度傳感器”。陀螺儀輸出的是繞三個(gè)軸的角速度加速度計(jì)輸出的是三個(gè)軸上的比力。平衡車只需要繞輪軸方向的車身傾角也就是 pitch 角但對(duì)角速度的實(shí)時(shí)性要求很高因?yàn)閮?nèi)環(huán)微分項(xiàng)直接使用陀螺儀瞬時(shí)值。先看量程配置。陀螺儀量程選 ±2000dps靈敏度 16.4 LSB/(°/s)這樣車身快速傾倒時(shí)不會(huì)削頂加速度計(jì)量程選 ±16g 或 ±8g對(duì)應(yīng)靈敏度 2048 或 4096 LSB/g。量程不是越大越好大了分辨率低靜止時(shí)加速度計(jì)的噪聲會(huì)直接反映到角度里所以加速度計(jì)不建議選 ±16g 以下的選項(xiàng)。陀螺儀量程靈敏度 LSB/(°/s)加速度量程靈敏度 LSB/g±250131±2g16384±50065.5±4g8192±100032.8±8g4096±200016.4±16g2048寄存器配置順序一般是先設(shè)電源管理寄存器 0x6B 喚醒芯片再寫 0x1B 陀螺儀量程、0x1C 加速度量程、0x1A 數(shù)字低通濾波器 DLPF最后通過(guò) 0x19 設(shè)置采樣率分頻。DLPF 帶寬對(duì)平衡車非常關(guān)鍵帶寬太低角度數(shù)據(jù)滯后內(nèi)環(huán)容易振蕩帶寬太高噪聲直接進(jìn) PID電機(jī)發(fā)燙。常見(jiàn)選擇是 DLPF 配置為 0x03對(duì)應(yīng)陀螺儀帶寬約 44Hz加速度計(jì)帶寬約 44Hz采樣率用 1kHz控制周期取 2ms也就是 500Hz 讀取一次。void MPU6050_Init(void) { uint8_t reg; reg 0x00; // 清除 SLEEP 位喚醒傳感器 HAL_I2C_Mem_Write(hi2c1, 0xD0, 0x6B, 1, reg, 1, 50); reg 0x03; // 陀螺儀 ±2000dps HAL_I2C_Mem_Write(hi2c1, 0xD0, 0x1B, 1, reg, 1, 50); reg 0x18; // 加速度計(jì) ±16g HAL_I2C_Mem_Write(hi2c1, 0xD0, 0x1C, 1, reg, 1, 50); reg 0x03; // DLPF 約 44Hz HAL_I2C_Mem_Write(hi2c1, 0xD0, 0x1A, 1, reg, 1, 50); reg 0x04; // 采樣率 1kHz / (1 4) 200Hz HAL_I2C_Mem_Write(hi2c1, 0xD0, 0x19, 1, reg, 1, 50); }上面這段 HAL 庫(kù)代碼里的 0xD0 是 MPU6050 的 7 位地址 0x68 左移一位后的寫地址HAL_I2C_Mem_Write最后一個(gè)參數(shù)是超時(shí)時(shí)間。需注意配置寫完后要有 10ms 以上延時(shí)等傳感器內(nèi)部首次數(shù)據(jù)穩(wěn)定再開(kāi)始讀取否則前幾十個(gè)原始值全是 0。2.2 從加速度計(jì)計(jì)算傾角與陀螺儀積分互補(bǔ)純加速度計(jì)算傾角公式是angle_acc atan2(-acc_x, acc_z)前提是加速度計(jì)按常見(jiàn)方式安裝芯片平放X 軸指向車身前進(jìn)方向Z 軸豎直向上。安裝方向不同軸對(duì)應(yīng)關(guān)系要重算。經(jīng)驗(yàn)做法是上電后把車身人工扶正串口打印三個(gè)軸的原始值判斷哪個(gè)軸朝下再確定公式里的符號(hào)。全角速度積分也能得到角度但陀螺儀零偏會(huì)隨時(shí)間累積漂移幾秒鐘后角度就偏了。所以工程上最常用互補(bǔ)濾波高頻段信任角速度積分低頻段信任加速度計(jì)角度。由于平衡車振動(dòng)大純加速度計(jì)的傾角高頻噪聲很嚴(yán)重濾波系數(shù)通常在 0.95 到 0.98 之間但系數(shù)調(diào)到 0.99 以上會(huì)讓角度響應(yīng)極慢外環(huán)速度調(diào)節(jié)還沒(méi)反應(yīng)車已經(jīng)倒了。另一個(gè)不可忽視的點(diǎn)陀螺儀零漂。每次上電后靜止采樣 500 組陀螺儀原始值取平均作為本次運(yùn)行的零漂補(bǔ)償。溫度變化大時(shí)零漂會(huì)變所以啟動(dòng)階段就標(biāo)定一次的方案在空調(diào)房里沒(méi)問(wèn)題在戶外溫差大的環(huán)境最好每隔幾分鐘自動(dòng)補(bǔ)一次。2.3 互補(bǔ)濾波代碼與零漂處理控制周期內(nèi)做一次姿態(tài)更新讀取原始數(shù)據(jù)、轉(zhuǎn)物理單位、做互補(bǔ)濾波。#define RAD_TO_DEG 57.29578f #define GYRO_SCALE 16.4f // ±2000dps 靈敏度 #define ACC_SCALE 2048.0f // ±16g 靈敏度 #define DT 0.002f // 2ms 控制周期 float angle 0.0f, gyro_rate 0.0f, gyro_offset 0.0f; void MPU6050_Read_Raw(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) { uint8_t buf[6]; HAL_I2C_Mem_Read(hi2c1, 0xD0, 0x3B, 1, buf, 6, 50); *ax (buf[0] 8) | buf[1]; *ay (buf[2] 8) | buf[3]; *az (buf[4] 8) | buf[5]; HAL_I2C_Mem_Read(hi2c1, 0xD0, 0x43, 1, buf, 6, 50); *gx (buf[0] 8) | buf[1]; *gy (buf[2] 8) | buf[3]; *gz (buf[4] 8) | buf[5]; } void Attitude_Update(void) { int16_t ax, ay, az, gx, gy, gz; float acc_pitch, gyro_deg; MPU6050_Read_Raw(ax, ay, az, gx, gy, gz); acc_pitch atan2f((float)(-ax), (float)(az)) * RAD_TO_DEG; gyro_deg (float)gx / GYRO_SCALE - gyro_offset; angle 0.98f * (angle gyro_deg * DT) 0.02f * acc_pitch; gyro_rate gyro_deg; } void Gyro_Calibrate(void) { int16_t ax, ay, az, gx, gy, gz; float sum 0.0f; for (int i 0; i 500; i) { MPU6050_Read_Raw(ax, ay, az, gx, gy, gz); sum gx; HAL_Delay(2); } gyro_offset sum / 500.0f / GYRO_SCALE; }互補(bǔ)濾波公式中0.98 是角速度積分的權(quán)重0.02 是加速度計(jì)角度的修正權(quán)重兩者之和必須為 1。angle gyro_deg * DT是角度積分acc_pitch是絕對(duì)基準(zhǔn)。陀螺儀短時(shí)間積分可信所以給它更高權(quán)重加速度計(jì)長(zhǎng)期無(wú)漂移所以用它修正積分的緩慢偏移。這段代碼里gyro_offset的單位已經(jīng)轉(zhuǎn)換成 °/s和gyro_deg保持一致。讀取原始數(shù)據(jù)時(shí)要注意MPU6050 的寄存器地址在連續(xù)讀取時(shí)會(huì)自動(dòng)遞增所以 6 個(gè)字節(jié)一次讀完比單獨(dú)讀每個(gè)寄存器效率高也更不容易出現(xiàn) I2C 總線競(jìng)爭(zhēng)導(dǎo)致的角度跳變。若 I2C 讀返回 HAL_TIMEOUT本次直接沿用上一次的 angle不要清零否則濾波狀態(tài)突變會(huì)讓車猛抽一下。3. 串級(jí) PID 的回路結(jié)構(gòu)位置環(huán)、速度環(huán)與內(nèi)層角度環(huán)的嵌套3.1 為什么需要串級(jí)先壓住姿態(tài)再控制速度單環(huán) PID 直接拿角度誤差算 PWM在靜態(tài)能站住但人推一下車車會(huì)加速往一個(gè)方向逃直到角度誤差足夠大才剎車表現(xiàn)為“跑飛”。原因是 PWM 直接控制的是輪子的加速度而角度誤差來(lái)自車身傾斜兩者之間隔著動(dòng)力學(xué)耦合純比例控制沒(méi)法同時(shí)保證“姿態(tài)穩(wěn)定”和“位置不漂移”。串級(jí) PID 的思路是把控制目標(biāo)分級(jí)內(nèi)環(huán)角度環(huán)的期望值不是固定的 0而是由外環(huán)速度環(huán)給出的目標(biāo)角度速度環(huán)的期望速度又由位置環(huán)給出。這樣車的控制鏈路是位置誤差 → 目標(biāo)速度 → 速度誤差 → 目標(biāo)角度 → 角度誤差 → PWM 占空比。標(biāo)題里的“速度環(huán)和位置環(huán)串級(jí)”指的就是這條鏈上的外兩層。最常見(jiàn)說(shuō)法其實(shí)叫“三環(huán)串級(jí)”內(nèi)環(huán)角度環(huán)是獨(dú)立的支撐回路速度環(huán)負(fù)責(zé)抵消車輪漂移位置環(huán)負(fù)責(zé)定點(diǎn)或限位。網(wǎng)上一些資料只提“速度環(huán) 位置環(huán)”是為了強(qiáng)調(diào)讓車站穩(wěn)之后真正決定控制品質(zhì)的是這兩個(gè)外環(huán)的設(shè)計(jì)。速度環(huán)給內(nèi)環(huán)的“目標(biāo)角度”是有限幅的一般限制在 ±15° 以內(nèi)。如果外環(huán)出現(xiàn)大的速度誤差目標(biāo)角度瞬間給到 30°內(nèi)環(huán)角度環(huán)就算拼盡全力車的傾角已經(jīng)太大物理上就救不回來(lái)了所以限幅本身就是一種安全保護(hù)。3.2 內(nèi)外環(huán)的時(shí)間間隔與控制頻率內(nèi)環(huán)必須高于外環(huán)一個(gè)數(shù)量級(jí)。我一般把角度環(huán)放在 500Hz也就是 2ms 一次速度環(huán) 100Hz位置環(huán) 50Hz。原因是內(nèi)環(huán)是系統(tǒng)穩(wěn)定性的根本外環(huán)只負(fù)責(zé)緩慢修正如果內(nèi)外環(huán)同頻速度環(huán)的噪聲會(huì)直接注入角度環(huán)的期望值讓車身頻繁“點(diǎn)頭”?；芈奉l率周期作用位置環(huán)50Hz20ms輸出目標(biāo)速度限制車移動(dòng)范圍速度環(huán)100Hz10ms輸出目標(biāo)角度抵消車輪漂移角度環(huán)500Hz2ms輸出 PWM維持車身豎直具體實(shí)現(xiàn)不一定要開(kāi)三個(gè)定時(shí)器中斷。常見(jiàn)做法是在一個(gè) 2ms 定時(shí)器中斷里做計(jì)數(shù)分頻每次執(zhí)行角度環(huán)每 5 次執(zhí)行一次速度環(huán)每 10 次執(zhí)行一次位置環(huán)。這樣的好處是時(shí)間基準(zhǔn)統(tǒng)一不會(huì)出現(xiàn)多個(gè)中斷互相搶占導(dǎo)致角度環(huán)周期抖動(dòng)。void TIM_PeriodElapsedCallback(TIM_HandleTypeDef* htim) { static uint8_t tick 0; if (htim-Instance ! TIM7) return; Attitude_Update(); target_angle Angle_PID(angle, gyro_rate); Motor_PWM(target_angle); if (tick 5) { tick 0; float speed Get_Speed_From_Encoder(); target_angle Speed_PID(0.0f, speed); } }注意這里代碼順序角度環(huán)每次都執(zhí)行速度環(huán)每 5ms 執(zhí)行一次但目標(biāo)角度target_angle在角度環(huán)計(jì)算之前已經(jīng)由速度環(huán)更新過(guò)了。實(shí)際工程中更穩(wěn)妥的做法是先執(zhí)行外環(huán)再執(zhí)行內(nèi)環(huán)因?yàn)閮?nèi)環(huán)要用最新的目標(biāo)值所以上面的代碼里速度環(huán)target_angle Speed_PID(...)放在了角度環(huán)之后下一次 2ms 中斷的角度環(huán)才會(huì)用到新目標(biāo)這正好形成外環(huán) 10ms 周期、內(nèi)環(huán) 2ms 周期的錯(cuò)位執(zhí)行。無(wú)人機(jī)串級(jí) PID 也是同樣的思路內(nèi)環(huán)角速度環(huán)跑 1000Hz外環(huán)姿態(tài)環(huán)跑 250Hz外環(huán)給定的是期望姿態(tài)角速度。二輪平衡車因?yàn)轵?qū)動(dòng)是輪子而非旋翼動(dòng)力學(xué)更慢頻率可以整體降低一檔。3.3 核心代碼位置環(huán)→速度環(huán)→角度環(huán)的一次控制周期把三個(gè)環(huán)寫進(jìn)一個(gè)統(tǒng)一的控制函數(shù)便于調(diào)試時(shí)單獨(dú)使能或禁掉某環(huán)。typedef struct { float kp, ki, kd; float integral, last_err; float out_max, integral_max; } PID_t; float PID_Update(PID_t* pid, float target, float feedback, float dt) { float err target - feedback; float p_out pid-kp * err; pid-integral err * dt; // 積分限幅 if (pid-integral pid-integral_max) pid-integral pid-integral_max; if (pid-integral -pid-integral_max) pid-integral -pid-integral_max; float i_out pid-ki * pid-integral; float d_out pid-kd * (err - pid-last_err) / dt; pid-last_err err; float out p_out i_out d_out; // 輸出限幅 if (out pid-out_max) out pid-out_max; if (out -pid-out_max) out -pid-out_max; return out; } void Balance_Control_Cycle(PID_t* pos_pid, PID_t* speed_pid, PID_t* angle_pid) { float pos_err target_position - current_position; float target_speed PID_Update(pos_pid, target_position, current_position, 0.02f); target_speed CLAMP(target_speed, -0.3f, 0.3f); if (fabsf(pos_err) 0.03f) { target_speed 0.0f; // 位置死區(qū)防止往復(fù)振蕩 } float target_angle PID_Update(speed_pid, target_speed, current_speed, 0.01f); target_angle CLAMP(target_angle, -15.0f, 15.0f); float pwm_out PID_Update(angle_pid, target_angle, angle, 0.002f); Motor_SetPWM(pwm_out); }這段代碼體現(xiàn)了串級(jí) PID 的關(guān)鍵特征每一環(huán)的輸出是下一環(huán)的給定值而且每一環(huán)都有獨(dú)立的限幅。位置環(huán)的輸出限幅 0.3m/s意思是位置誤差再大車的最大期望速度也只有 0.3m/s防止起步猛沖速度環(huán)的輸出限幅 15°防止目標(biāo)角度給得過(guò)大角度環(huán)的輸出限幅沒(méi)有寫在函數(shù)里但Motor_SetPWM內(nèi)部會(huì)做占空比限幅。pos_err死區(qū) 3cm 的用意車上有編碼器噪聲、轉(zhuǎn)動(dòng)摩擦、地面不平位置誤差在 ±3cm 以內(nèi)時(shí)實(shí)際已經(jīng)算到位強(qiáng)行走反而引起震蕩。積分限幅integral_max尤其重要速度環(huán)積分項(xiàng)如果無(wú)限累積停車后電機(jī)會(huì)持續(xù)輸出一個(gè)偏置電流車會(huì)緩慢往一個(gè)方向爬。調(diào)參數(shù)時(shí)先設(shè)積分限幅為輸出限幅的一半再逐步調(diào)整。4. 硬件連接與工程初始化從原理圖到定時(shí)器編碼器配置4.1 對(duì)著原理圖和 stm32f407zgt6 數(shù)據(jù)手冊(cè)先確認(rèn)三件事拿到二輪平衡車資料包先別急著解壓代碼。工程環(huán)境千差萬(wàn)別芯片一樣、外設(shè)連接不一樣代碼能直接跑的概率不大。我一般先看原理圖確認(rèn)三件事第一供電結(jié)構(gòu)。電機(jī)驅(qū)動(dòng)和單片機(jī)是不是同一路電源常見(jiàn)設(shè)計(jì)是鋰電池經(jīng)過(guò)降壓模塊同時(shí)給 STM32F407ZGT6 和 TB6612 電機(jī)驅(qū)動(dòng)供電電機(jī)堵轉(zhuǎn)電流大時(shí)系統(tǒng)電壓會(huì)跌落幾伏板載穩(wěn)壓若壓差不足單片機(jī)會(huì)掉電重啟。至少要讓編碼器供電和邏輯電源獨(dú)立濾波。第二BOOT 配置。F407 的 BOOT0 引腳電平?jīng)Q定啟動(dòng)方式很多資料板出廠默認(rèn) BOOT0 懸空或拉低從主 Flash 啟動(dòng)。如果發(fā)現(xiàn)下不進(jìn)程序先量 BOOT0 是不是低電平。第三MPU6050 的上拉電阻。I2C 總線上拉電阻在 2.2k 到 4.7k 之間是正常范圍有些精簡(jiǎn)版資料板板載電阻根本沒(méi)焊I2C 通信偶爾超時(shí)表現(xiàn)為上電能跑、跑著跑著角度跳變。原理圖上用萬(wàn)用表量一下 SDA/SCL 到 3.3V 的阻值即可確認(rèn)。數(shù)據(jù)手冊(cè)在這里的用處主要是查引腳復(fù)用功能表和時(shí)鐘樹(shù)。F407 的定時(shí)器時(shí)鐘來(lái)源不看手冊(cè)容易搞錯(cuò)TIM1/TIM8 掛在 APB2168MHzTIM2-TIM5 掛在 APB184MHz如果分頻系數(shù)配錯(cuò)PWM 頻率和編碼器計(jì)數(shù)周期都會(huì)差一倍。4.2 引腳分配與定時(shí)器配置以常見(jiàn)的 STM32F407ZGT6 平衡車方案為例引腳分配如下表所示。不同板子的引腳復(fù)用功能在數(shù)據(jù)手冊(cè)里都能查到遇到資源沖突時(shí)優(yōu)先調(diào)整編碼器定時(shí)器因?yàn)榫幋a器對(duì)引腳的復(fù)用功能依賴較強(qiáng)。外設(shè)引腳定時(shí)器/功能說(shuō)明MPU6050 SCLPB8I2C1_SCL4.7kΩ 上拉到 3.3VMPU6050 SDAPB9I2C1_SDA4.7kΩ 上拉到 3.3V左電機(jī) PWMPA8TIM1_CH110kHz-20kHz右電機(jī) PWMPA9TIM1_CH2與左電機(jī)共定時(shí)器左編碼器PB6 / PB7TIM4_CH1 / TIM4_CH2正交解碼模式右編碼器PA0 / PA1TIM2_CH1 / TIM2_CH2正交解碼模式PWM 用 TIM1 輸出比較模式編碼器用 TIM2 和 TIM4 的正交編碼器模式。實(shí)際上很多資料板左右編碼器對(duì)調(diào)配在 TIM2 和 TIM4 上因?yàn)檫@兩個(gè)定時(shí)器的編碼器接口引腳正好分布在 GPIOA 和 GPIOB方便 PCB 布線。4.3 電機(jī) PWM 輸出與編碼器測(cè)速初始化TIM1 是高級(jí)定時(shí)器初始化 PWM 后必須開(kāi)啟主輸出使能否則引腳上量不到波形。HAL 庫(kù)中的配置片段如下TIM_OC_InitTypeDef sConfigOC {0}; sConfigOC.OCMode TIM_OCMODE_PWM1; sConfigOC.Pulse 0; sConfigOC.OCPolarity TIM_OCPOLARITY_HIGH; sConfigOC.OCNPolarity TIM_OCNPOLARITY_HIGH; sConfigOC.OCFastMode TIM_OCFAST_DISABLE; sConfigOC.OCIdleState TIM_OCIDLESTATE_RESET; sConfigOC.OCNIdleState TIM_OCNIDLESTATE_RESET; HAL_TIM_PWM_ConfigChannel(htim1, sConfigOC, TIM_CHANNEL_1); HAL_TIM_PWM_ConfigChannel(htim1, sConfigOC, TIM_CHANNEL_2); HAL_TIM_PWM_Start(htim1, TIM_CHANNEL_1); HAL_TIM_PWM_Start(htim1, TIM_CHANNEL_2); __HAL_TIM_MOE_ENABLE(htim1); // 高級(jí)定時(shí)器主輸出使能這行不能少__HAL_TIM_MOE_ENABLE很多人漏掉。高級(jí)定時(shí)器的輸出由 MOE 位總開(kāi)關(guān)控制CubeMX 生成的初始化代碼通常不會(huì)默認(rèn)打開(kāi)結(jié)果 PWM 配置全對(duì)但電機(jī)就是不動(dòng)。編碼器定時(shí)器配置TIM_Encoder_InitTypeDef sEncoderConfig {0}; sEncoderConfig.EncoderMode TIM_ENCODERMODE_TI12; sEncoderConfig.IC1Polarity TIM_ICPOLARITY_RISING; sEncoderConfig.IC2Polarity TIM_ICPOLARITY_RISING; sEncoderConfig.IC1Selection TIM_ICSELECTION_DIRECTTI; sEncoderConfig.IC2Selection TIM_ICSELECTION_DIRECTTI; sEncoderConfig.IC1Prescaler 0; sEncoderConfig.IC2Prescaler 0; HAL_TIM_Encoder_Init(htim4, sEncoderConfig); HAL_TIM_Encoder_Start(htim4, TIM_CHANNEL_ALL);編碼器模式TIM_ENCODERMODE_TI12表示兩路輸入同時(shí)參與計(jì)數(shù)每轉(zhuǎn)一個(gè)編碼器格計(jì)數(shù)器增減 4 倍最大分辨率。測(cè)速時(shí)直接讀__HAL_TIM_GET_COUNTER(htim4)得到的是一個(gè)帶符號(hào)的整數(shù)正負(fù)表示旋轉(zhuǎn)方向。速度換算公式float wheel_speed_ms (float)counter / (ticks_per_rev * 4) * wheel_circumference / dt;其中ticks_per_rev是電機(jī)輸出軸每轉(zhuǎn)對(duì)應(yīng)的編碼器線數(shù)如果是減速電機(jī)要乘以減速比。比如編碼器線數(shù) 13、減速比 30那么ticks_per_rev 13 * 30 390四倍頻后每轉(zhuǎn)計(jì) 1560 個(gè)脈沖。輪徑設(shè)為 65mm 時(shí)輪周長(zhǎng)約 0.204m。dt 是測(cè)速周期速度環(huán) 100Hz 時(shí) dt 0.01s。讀計(jì)數(shù)值的代碼要注意用int32_t強(qiáng)轉(zhuǎn)因?yàn)開(kāi)_HAL_TIM_GET_COUNTER返回值類型是 uint32_t如果兩個(gè)方向計(jì)數(shù)導(dǎo)致 CNT 為負(fù)直接按無(wú)符號(hào)數(shù)讀會(huì)得到一個(gè)很大的正數(shù)。5. 調(diào)參順序和實(shí)戰(zhàn)踩坑從“砸地”到“站住”再到“不漂”5.1 摘掉外環(huán)先調(diào)內(nèi)環(huán)角度環(huán)的 Kp 與 Kd串級(jí) PID 調(diào)參的第一原則摘掉外環(huán)先只調(diào)內(nèi)環(huán)。把速度環(huán)和位置環(huán)的輸出全部清零目標(biāo)角度直接給 0然后從小到大加角度環(huán)的 Kp。我習(xí)慣從 Kp 10 開(kāi)始用手扶著車體感受電機(jī)是否在對(duì)抗傾斜。Kp 太小車完全無(wú)力松手直接倒Kp 慢慢加到 20 到 40 區(qū)間車身開(kāi)始恢復(fù)直立但會(huì)持續(xù)抖動(dòng)。這個(gè)階段不要急著加 Kd先把 Kp 提上去找到“能短暫直立”的點(diǎn)。然后加 Kd每次增加 0.5直到抖動(dòng)明顯收斂。Kd 的值對(duì)應(yīng)的是阻尼它抑制的是角速度本身不是角度的導(dǎo)數(shù)所以直接讀陀螺儀的gyro_rate參與運(yùn)算不要對(duì) angle 做差分——差分會(huì)放大高頻噪聲而陀螺儀本身已經(jīng)是很干凈的角速度信號(hào)。內(nèi)環(huán)能站住后驗(yàn)證一個(gè)關(guān)鍵現(xiàn)象用手推一下車車應(yīng)該快速回正但回正過(guò)程有輕微超調(diào)。如果回正后反向倒下去說(shuō)明 Kd 不夠如果回正過(guò)程發(fā)悶、遲鈍說(shuō)明 Kd 過(guò)大。5.2 增量式速度環(huán)怎么調(diào)從防漂移到抗擾動(dòng)內(nèi)環(huán)站住后如果車開(kāi)始朝一個(gè)方向持續(xù)加速這就是速度環(huán)缺失的表現(xiàn)。原因很簡(jiǎn)單車身略微前傾時(shí)內(nèi)環(huán)為了讓車保持直立會(huì)讓車輪前進(jìn)如果沒(méi)有任何外環(huán)修正車就一直往前加速直到角度徹底失衡。速度環(huán)的目標(biāo)是讓“車輪速度”始終趨向 0從而抵消漂移。速度環(huán)通常用 PI不用 D。原因是速度反饋本身來(lái)自編碼器差分已經(jīng)有量化噪聲再加 D 會(huì)引入毛刺。調(diào)整時(shí)先關(guān)掉積分項(xiàng)只加 Kp_speed從小值 0.3 開(kāi)始觀察車是否還持續(xù)加速。Kp_speed 太小漂移減得很慢Kp_speed 過(guò)大車會(huì)來(lái)回“點(diǎn)頭”因?yàn)樗俣日`差會(huì)變成大幅度的角度目標(biāo)擺動(dòng)。速度環(huán) Kp 穩(wěn)定后再慢慢加 KiKi 的作用是消除靜態(tài)誤差也就是長(zhǎng)時(shí)間朝一個(gè)方向的緩慢漂移。增量式速度環(huán)的常見(jiàn)實(shí)現(xiàn)是每次累加誤差增量而不是累加絕對(duì)誤差這樣能避免起步瞬間積分飽和void Speed_Update_Inc(float target_speed, float current_speed, float dt) { static float last_err 0.0f; float err target_speed - current_speed; float delta_out speed_pid.kp * (err - last_err) speed_pid.ki * err * dt; speed_pid.integral delta_out; speed_pid.integral CLAMP(speed_pid.integral, -5.0f, 5.0f); last_err err; }增量式的好處是輸出量是“變化量”而非“絕對(duì)值”即使控制周期偶爾丟一次也不會(huì)導(dǎo)致輸出突變。缺點(diǎn)是抗積分限幅能力不如位置式所以integral必須做限幅否則長(zhǎng)期單方向漂移會(huì)讓積分項(xiàng)悄悄爬滿。5.3 幾個(gè)容易反復(fù)踩的坑現(xiàn)象可能原因解決辦法上電后角度跳變I2C 上拉電阻缺失或時(shí)鐘線干擾量 SCL/SDA 電平補(bǔ) 4.7kΩ 上拉車朝一個(gè)方向加速內(nèi)環(huán)極性反了把 Kp 設(shè)為負(fù)值試試判斷反饋方向靜止時(shí)電機(jī)嗡嗡抖動(dòng)內(nèi)環(huán) D 項(xiàng)過(guò)大或 DLPF 帶寬過(guò)高降低 Kd把 DLPF 調(diào)到 20Hz 左右速度環(huán)一開(kāi)就振蕩速度環(huán)限幅太大或 Kp_speed 過(guò)大目標(biāo)角度限幅降到 ±10°Kp_speed 減半編碼器讀數(shù)方向異常A/B 相接反交換編碼器兩相輸入或讀值取反跑幾分鐘后開(kāi)始漂移陀螺儀零漂受熱變化運(yùn)行中周期性重新標(biāo)定零漂下不進(jìn)程序BOOT0 配置錯(cuò)誤確認(rèn) BOOT0 低電平從 Flash 啟動(dòng)調(diào)參時(shí)一個(gè)很有用的技巧把a(bǔ)ngle、target_angle、current_speed通過(guò)串口打印出來(lái)。但注意打印頻率不要超過(guò) 50Hz否則大量串口中斷會(huì)干擾 2ms 控制周期。打印數(shù)據(jù)時(shí)用浮點(diǎn)數(shù)格式化F407 的 FPU 能扛住但printf重定向到串口時(shí)建議加一個(gè) DMA 發(fā)送避免阻塞控制循環(huán)。6. 用位置環(huán)做定點(diǎn)停車死區(qū)、軟限位與串口驗(yàn)證位置環(huán)是串級(jí)鏈條的最外層它的實(shí)際作用是“限定車的活動(dòng)范圍”。做定點(diǎn)停車時(shí)目標(biāo)位置設(shè) 0位置反饋來(lái)自速度積分。實(shí)現(xiàn)上有兩種一種是把編碼器計(jì)數(shù)值直接當(dāng)作位置另一種是每次速度環(huán)更新時(shí)累加speed * dt。前者更準(zhǔn)確但要注意編碼器計(jì)數(shù)值的范圍避免溢出后者簡(jiǎn)單但有累積誤差。定點(diǎn)停車的核心參數(shù)有兩個(gè)位置環(huán) Kp 和位置死區(qū)。Kp 決定車距目標(biāo)多近時(shí)開(kāi)始減速Kp 太大車在目標(biāo)點(diǎn)附近反復(fù)沖過(guò)頭Kp 太小車到不了目標(biāo)就停了。死區(qū)給 2cm 到 5cm車速在該區(qū)間內(nèi)直接置零防止編碼器噪聲引起持續(xù)微調(diào)。void Position_Loop(void) { float pos_err target_position - current_position; if (fabsf(pos_err) 0.03f) { target_speed 0.0f; } else { // 軟限位誤差越大目標(biāo)速度越大但最大不超過(guò) 0.3m/s target_speed position_pid.kp * CLAMP(pos_err, -0.3f, 0.3f); } }這里用CLAMP而不是直接線性放大是為了避免位置誤差大時(shí)速度目標(biāo)超限。軟限位的思路可以延展到其他場(chǎng)景比如給車設(shè)定前后各 1m 的活動(dòng)范圍位置環(huán)目標(biāo)設(shè)為區(qū)間中值車就會(huì)自動(dòng)維持在區(qū)間內(nèi)把目標(biāo)位置改成遙控給定的變量就變成了遙控駐車模式。驗(yàn)證定位精度時(shí)把車放在地面用直尺量實(shí)際停止位置和目標(biāo)位置的距離差。前提是先把車調(diào)穩(wěn)否則位置環(huán)收斂得再好內(nèi)環(huán)不穩(wěn)也是白搭。串口打印pos_err和target_speed正常收斂過(guò)程應(yīng)該是誤差逐漸減小到死區(qū)內(nèi)目標(biāo)速度同步衰減到 0沒(méi)有來(lái)回穿越。數(shù)據(jù)可以通過(guò) Vofa 或匿名上位機(jī)畫曲線觀察目標(biāo)速度和實(shí)際速度兩條曲線是否同步收斂如果實(shí)際速度在零附近反復(fù)跳動(dòng)說(shuō)明位置死區(qū)還不夠大或者速度環(huán)積分限幅偏高。調(diào)試到這里二輪車的平衡、防漂移和定點(diǎn)停車就形成了一個(gè)完整閉環(huán)剩下的工作就是根據(jù)實(shí)際機(jī)械結(jié)構(gòu)微調(diào)濾波器系數(shù)和死區(qū)范圍。本文還有配套的精品資源點(diǎn)擊獲取