簡介這是基于STM32F103與OpenMV視覺模塊的二維舵機云臺追蹤程序包適合做電磁炮、視覺瞄準(zhǔn)或低成本追蹤云臺的電子愛好者與嵌入式開發(fā)者。程序主體為32端PID控制邏輯利用OpenMV顏色識別返回色塊在x、y軸坐標(biāo)驅(qū)動舵機完成二維實時追蹤PID參數(shù)需按實際硬件微調(diào)舵機精度越高表現(xiàn)越好。包內(nèi)共455個文件約11.86MB以C源文件151個c和頭文件131個h為主同時包含編譯過程產(chǎn)生的.o、.crf、.axf、.hex等工程文件以及png圖片、txt說明和Keil工程配置uvprojx/uvoptx等目錄涵蓋標(biāo)準(zhǔn)庫、HZ主工程與腳本便于直接查看代碼結(jié)構(gòu)。目前已有8233人學(xué)習(xí)下載適合具備一定STM32與PID基礎(chǔ)、希望快速搭建視覺追蹤原型的開發(fā)者參考。1. 項目定位一套把PID閉環(huán)跑在舵機云臺上的二維視覺追蹤代碼調(diào)試追蹤云臺最常遇到的一個現(xiàn)象是色塊識別沒問題、坐標(biāo)也送過來了但舵機就是不能把目標(biāo)穩(wěn)定鎖在畫面中心——要么追著目標(biāo)來回掃要么目標(biāo)稍微偏一點就整個過沖。這個基于STM32F103和OpenMV的工程恰好把二維追蹤鏈路完整打通了OpenMV在畫面里識別色塊計算出目標(biāo)與畫面中心的偏移量通過串口發(fā)給F103F103跑PID算法輸出PWM波驅(qū)動水平、垂直兩顆舵機讓云臺實時跟隨目標(biāo)。整套代碼的定位不是“能識別就行”而是“能穩(wěn)定鎖住”。適合正在做智能車、機械臂視覺跟隨或想搞懂PID實物調(diào)試的開發(fā)者。你拿到手里可以直接燒錄也可以把OpenMV端的色塊追蹤換成其他識別算法控制端不用動。2. 系統(tǒng)架構(gòu)與通信協(xié)議OpenMV到F103的六字節(jié)數(shù)據(jù)鏈路這個項目拆開看是“三段式”結(jié)構(gòu)視覺端OpenMV、控制端STM32F103、執(zhí)行端雙舵機云臺。視覺端不是每幀都發(fā)完整圖像而是只發(fā)目標(biāo)偏移量串口負(fù)擔(dān)極小控制端也不做圖像處理只專注PID閉環(huán)。分離式設(shè)計有一個明顯好處換識別算法不用動控制代碼換舵機不用動視覺代碼。2.1 硬件鏈路兩路PWM加一路UART常見硬件選型是MG996R或SG90舵機電源需要5V左右電流能供2A以上最好。云臺架通常是兩自由度舵機云臺水平pan、垂直tilt對應(yīng)STM32F103的TIM2通道1/通道2一般接PA0和PA1。OpenMV的UART3P4作TX、P5作RX接到F103的USART1PA9是TX、PA10是RXTX/RX交叉對接。信號OpenMVSTM32F103UART3 TXP4PA10 (USART1_RX)UART3 RXP5PA9 (USART1_TX)GNDGNDGND必須共地PAN舵機信號—TIM2_CH1 (PA0)TILT舵機信號—TIM2_CH2 (PA1)舵機電源獨立5V電源GND與F103共地同左有一點值得強調(diào)舵機供電禁止從OpenMV或F103的3.3V引腳取電舵機啟動和堵轉(zhuǎn)時的瞬時電流會直接拉崩主控電壓導(dǎo)致串口波形畸變、程序反復(fù)復(fù)位。獨立電源加粗線共地是最省心的接法。2.2 串口幀協(xié)議為什么固定六字節(jié)最省心協(xié)議設(shè)計的目標(biāo)是簡單可靠。幀格式固定為0xAA 0x55 Type Dx Dy ChecksumType字段0x01表示有目標(biāo)0x00表示沒有目標(biāo)。Dx、Dy是目標(biāo)中心相對于圖像中心的偏移歸一化到-100~100。Checksum是前面五個字節(jié)求和后取低8位。為什么用固定字節(jié)而不是JSON或字符串嵌入式里逐字節(jié)狀態(tài)機解析比字符串split省內(nèi)存、快得多而且OpenMV端發(fā)的是原始int不用做格式化轉(zhuǎn)換。歸一化還有一個好處OpenMV換分辨率時只需要改一個系數(shù)K協(xié)議本身不用動。比如320x240畫面Dx原始范圍是±160除以1.6變成±100換成640x480后K也跟著變但F103端無需感知。2.3 目錄結(jié)構(gòu)zip解壓后的三段式布局壓縮包解壓后是典型的“OpenMV端 F103端 文檔”三段結(jié)構(gòu)01_OpenMV端main.py是主程序blob_track.py封裝色塊識別pid_pre.py預(yù)留了視覺端預(yù)處理的擴展位02_STM32F103端Core/Inc下是main.h、usart.h、pid.h、servo.hCore/Src下是main.c、usart.c、pid.c、servo.c是標(biāo)準(zhǔn)的MDK工程布局03_調(diào)參記錄pids.md記錄了幾組實測參數(shù)test_log.txt是聯(lián)調(diào)時的串口日志調(diào)試時可以直接對照。main.py負(fù)責(zé)sensor初始化和主循環(huán)usart.c里是串口中斷接收狀態(tài)機servo.c負(fù)責(zé)PWM初始化和角度映射。各部分職責(zé)清晰改哪一塊都很容易定位。3. 核心代碼拆解從色塊坐標(biāo)到舵機PWM的完整閉環(huán)這一章看關(guān)鍵代碼。整條鏈路由四段組成OpenMV端識別出目標(biāo)偏移 → 串口發(fā)送 → F103中斷解析 → PID計算并驅(qū)動舵機。每一段我都拆開講一遍。3.1 OpenMV端閾值標(biāo)定、色塊識別與偏差打包import sensor, image, time from pyb import UART # LAB色彩空間紅色閾值按實際環(huán)境用IDE閾值編輯器標(biāo)定 THRESHOLD (30, 80, 25, 80, -60, -30) sensor.reset() sensor.set_pixformat(sensor.RGB565) sensor.set_framesize(sensor.QVGA) # 320x240幀率優(yōu)先 sensor.skip_frames(30) sensor.set_auto_gain(False) # 關(guān)自動增益 sensor.set_auto_whitebal(False) # 關(guān)白平衡 uart UART(3, 115200, timeout_char1000) # 歸一化dx范圍 -160..160 - -100..100 K 100.0 / (sensor.width() / 2) while True: img sensor.snapshot() blobs img.find_blobs([THRESHOLD], pixels_threshold100, area_threshold200, mergeTrue) if blobs: blob max(blobs, keylambda b: b.area()) img.draw_rectangle(blob.rect()) img.draw_cross(blob.cx(), blob.cy()) dx int((blob.cx() - sensor.width() / 2) * K) dy int((blob.cy() - sensor.height() / 2) * K) type_id 0x01 else: dx dy 0 type_id 0x00 b bytearray([0xAA, 0x55, type_id, dx 0xFF, dy 0xFF, (0xAA 0x55 type_id dx dy) 0xFF]) uart.write(b) time.sleep_ms(20) # 約50Hz的發(fā)送節(jié)奏這段代碼有幾個關(guān)鍵決策點。第一find_blobs里的pixels_threshold和area_threshold用來過濾噪聲和小色塊mergeTrue會把重疊的色塊合并避免同一個目標(biāo)被拆成多塊第二max(blobs, keylambda b: b.area())是取最大面積色塊作為主目標(biāo)多目標(biāo)場景下會一直鎖住最大的那個第三關(guān)閉自動增益和白平衡很重要否則色塊閾值在不同光線下會漂移。參數(shù)說明dx為正表示目標(biāo)在畫面中心右側(cè)為負(fù)表示在左側(cè)dy為正表示目標(biāo)在中心下方。歸一化后dx、dy的范圍是-100~100恰好能塞進(jìn)一個int8_t串口解析時不會遇到符號擴展問題。無目標(biāo)時發(fā)type0F103收到后會走“丟目標(biāo)保護(hù)”分支不會繼續(xù)更新舵機角度。3.2 F103端串口中斷六字節(jié)狀態(tài)機解析typedef struct { uint8_t state; uint8_t type; volatile int8_t dx; volatile int8_t dy; volatile uint8_t valid; } Frame_t; Frame_t frame {0}; void USART1_IRQHandler(void) { if (USART_GetITStatus(USART1, USART_IT_RXNE) ! RESET) { uint8_t byte USART_ReceiveData(USART1); switch (frame.state) { case 0: if (byte 0xAA) frame.state 1; break; case 1: frame.state (byte 0x55) ? 2 : 0; break; case 2: frame.type byte; frame.state 3; break; case 3: frame.dx (int8_t)byte; frame.state 4; break; case 4: frame.dy (int8_t)byte; frame.state 5; break; case 5: if (byte (uint8_t)(0xAA 0x55 frame.type frame.dx frame.dy)) { frame.valid 1; } frame.state 0; break; default: frame.state 0; break; } } }狀態(tài)機的邏輯每個新字節(jié)進(jìn)入后都在五個狀態(tài)之間流轉(zhuǎn)任何一幀的字節(jié)對不上就回退到state0重新找?guī)^。dx、dy強轉(zhuǎn)成int8_t是因為OpenMV發(fā)送的-100在字節(jié)流里是0x9C只有按有符號數(shù)解釋才能還原成-100。校驗和這里有個容易踩的坑中斷里計算校驗和時frame.dx已經(jīng)是int8_t負(fù)數(shù)而OpenMV端計算時dx 0xFF是按無符號數(shù)算的。好在加法求模的結(jié)果不受符號解釋影響只要兩端都按“五個字節(jié)求和取低8位”這個規(guī)則算結(jié)果必然一致。這也是這個協(xié)議設(shè)計得聰明的地方——不需要在F103端做一次無符號還原。3.3 PID計算位置式PID、積分限幅與死區(qū)typedef struct { float kp; float ki; float kd; float integral; float last_err; float out_max; // 輸出限幅 float dead_zone; // 死區(qū) } PID_TypeDef; float PID_Calc(PID_TypeDef *pid, float measure_err) { if (fabsf(measure_err) pid-dead_zone) { measure_err 0.0f; } pid-integral measure_err; if (pid-integral pid-out_max * 0.3f) pid-integral pid-out_max * 0.3f; if (pid-integral -pid-out_max * 0.3f) pid-integral -pid-out_max * 0.3f; float out pid-kp * measure_err pid-ki * pid-integral pid-kd * (measure_err - pid-last_err); pid-last_err measure_err; if (out pid-out_max) out pid-out_max; if (out -pid-out_max) out -pid-out_max; return out; }這里用的是位置式PID輸出直接是“云臺這次要轉(zhuǎn)多少角度”方便限幅、方便在日志里觀察。為什么不選增量式增量式輸出的是每次調(diào)整量累加過程放在舵機側(cè)調(diào)試時看不到絕對角度丟目標(biāo)后的狀態(tài)也很難恢復(fù)。位置式把角度累積放在主循環(huán)里出問題時一眼能看出當(dāng)前云臺到哪里了。死區(qū)dead_zone對舵機云臺很有用目標(biāo)已經(jīng)接近畫面中心時誤差可能只有兩三個像素?fù)Q算成角度不到0.1°此時讓舵機反復(fù)微調(diào)只會制造高頻抖動。誤差小于死區(qū)就當(dāng)作0給出去云臺自然靜止。積分限幅是防“起飛”的關(guān)鍵。如果目標(biāo)突然快速移動誤差很大積分項會一路累加等目標(biāo)回到畫面中心時積分還在輸出一個很大的恢復(fù)量云臺會沖過頭。這里的out_max * 0.3f是把積分作用限制在輸出上限的30%以內(nèi)算一個比較穩(wěn)妥的初值實際按調(diào)試效果調(diào)整。3.4 主循環(huán)與舵機映射誤差怎么變成角度// 方向宏安裝方向與圖像坐標(biāo)一致時保持1.0f #define PAN_DIR 1.0f #define TILT_DIR 1.0f while (1) { if (frame.valid) { frame.valid 0; // 丟目標(biāo)復(fù)位PID內(nèi)部狀態(tài)避免恢復(fù)瞬間跳變 if (frame.type 0x00) { PID_Reset(pid_pan); PID_Reset(pid_tilt); continue; } float err_pan (float)frame.dx * PAN_DIR; float err_tilt (float)frame.dy * TILT_DIR; float out_pan PID_Calc(pid_pan, err_pan); float out_tilt PID_Calc(pid_tilt, err_tilt); pan_angle out_pan; tilt_angle out_tilt; clampf(pan_angle, -90.0f, 90.0f); clampf(tilt_angle, -90.0f, 90.0f); set_servo_angle(SERVO_PAN, pan_angle); set_servo_angle(SERVO_TILT, tilt_angle); } delay_ms(10); }方向宏這里值得單獨解釋。OpenMV圖像坐標(biāo)系原點在左上角x向右增大dx為正表示目標(biāo)偏右云臺理應(yīng)向右轉(zhuǎn)。但不同云臺的舵機安裝方向可能相反有的云臺舵機正方向恰好和圖像x軸反向此時把PAN_DIR改成-1.0f即可不需要動任何算法代碼。這個宏就是給安裝方向留的口子。丟目標(biāo)保護(hù)的處理邏輯目標(biāo)消失時frame.type 0x00直接復(fù)位PID內(nèi)部狀態(tài)并保留當(dāng)前角度。如果不清積分目標(biāo)恢復(fù)瞬間會產(chǎn)生一個巨大的誤差跳變云臺會猛甩一下復(fù)位后無縫銜接追蹤表現(xiàn)會平滑很多。舵機角度映射的實現(xiàn)void set_servo_angle(uint8_t ch, float angle) { // 0度對應(yīng)0.5ms脈寬180度對應(yīng)2.5ms脈寬 float pulse 500.0f angle / 180.0f * 2000.0f; if (pulse 500.0f) pulse 500.0f; if (pulse 2500.0f) pulse 2500.0f; // 定時器預(yù)分頻后計數(shù)頻率1MHz1us脈寬1個計數(shù) TIM_SetCompare1(TIM2, (uint16_t)pulse); // ch參數(shù)決定用TIM_SetCompare1還是TIM_SetCompare2 }定時器配置72MHz主頻預(yù)分頻71得到1MHz計數(shù)頻率自動重裝值19999即20ms周期對應(yīng)50Hz這是模擬舵機標(biāo)準(zhǔn)頻率。脈寬500us對應(yīng)0度2500us對應(yīng)180度角度到脈寬是線性映射。上電時最好先輸出一次1500us中位脈寬讓云臺回到零位再進(jìn)入主循環(huán)。4. 工程落地編譯燒錄、接線檢查與閾值標(biāo)定完整流程把工程跑起來真正麻煩的不是代碼而是環(huán)境匹配和聯(lián)調(diào)順序。下面按我實際操作的順序過一遍。4.1 環(huán)境準(zhǔn)備與燒錄OpenMV端安裝OpenMV IDE打開01_OpenMV端/main.py用USB連接OpenMV點IDE左上角的綠色運行按鈕腳本會直接跑在開發(fā)板上。F103端打開02_STM32F103端里現(xiàn)成的MDK工程編譯后用SWD下載器燒錄。SWD只需要四根線3.3V、SWDIO、SWCLK、GND下載速度設(shè)置1MHz比較穩(wěn)太快有時會連不上這是老熟人翻車點。燒錄完成后先別接舵機用邏輯分析儀或示波器看PA0和PA1上有沒有20ms周期的PWM波確認(rèn)初始脈寬在1.5ms左右。這一步能排除“代碼燒了但PWM沒配置對”的情況把問題限定在控制鏈路之前。4.2 硬件上電點檢順序上電檢查按順序走一遍能省大量排查時間先看OpenMV的電源指示燈亮不亮USB線是否帶數(shù)據(jù)功能再看F103核心板指示燈串口接沒接反OpenMV的P4必須接到F103的PA10P5必須接到PA9然后確認(rèn)舵機獨立電源已經(jīng)供電且GND和F103共地最后確認(rèn)兩根舵機信號線分別接在PA0和PA1。提示第一次聯(lián)調(diào)建議把“數(shù)據(jù)鏈”和“控制鏈”分開驗證——先不接舵機只看串口數(shù)據(jù)正不正確確認(rèn)沒問題再接舵機跑閉環(huán)。4.3 閾值標(biāo)定與串口數(shù)據(jù)驗證OpenMV IDE自帶的閾值編輯器是標(biāo)定這個項目最順手的工具采集一幀畫面框選目標(biāo)物體工具會自動生成一組LAB閾值把數(shù)值填回main.py里的THRESHOLD。標(biāo)定的時候把自動增益和白平衡關(guān)掉代碼里已經(jīng)做了否則換一個光線環(huán)境閾值就廢了。想在電腦上先看數(shù)據(jù)是否正?？梢园袿penMV的USB口當(dāng)串口跑一段Python腳本讀取和F103端解析的是同一幀數(shù)據(jù)import serial ser serial.Serial(COM3, 115200, timeout1) while True: data ser.read(6) if len(data) 6 and data[0] 0xAA and data[1] 0x55: dx data[3] - 256 if data[3] 0x80 else data[3] dy data[4] - 256 if data[4] 0x80 else data[4] print(type%d dx%d dy%d chk%d % (data[2], dx, dy, data[5]))在Windows上跑需要先裝pyserialLinux下把COM3換成/dev/ttyUSB0。讀到的dx、dy應(yīng)該是平滑變化的如果數(shù)值亂跳檢查波特率是否一致以及OpenMV和電腦之間是否共地。數(shù)據(jù)驗證通過后再接舵機跑閉環(huán)問題定位會快很多。5. 常見問題與避坑抖動、串口亂碼與PID發(fā)散的排查實錄這個章節(jié)記錄我在調(diào)試中反復(fù)遇到的五類問題現(xiàn)象、原因、解決辦法一條條列清楚調(diào)試時可以直接對照。5.1 高頻抖動舵機在目標(biāo)附近不停微顫現(xiàn)象目標(biāo)靜止時云臺還在高頻咯咯響角度輸出曲線在±5度范圍內(nèi)顫抖。原因最大元兇是供電不足。USB的5V帶不動舵機瞬時電流舵機每次脈沖都在拉垮電壓導(dǎo)致PWM波形畸變其次是Kp調(diào)得過大輸出隨噪聲放大。解決先換獨立電源驗證物理問題把供電問題排除后再把Kp降到0.8左右試一次。如果還抖把Kd直接歸零微分環(huán)節(jié)對噪聲最敏感。5.2 目標(biāo)高速移動時跟不上積分飽和現(xiàn)象目標(biāo)從畫面一端快速移到另一端舵機飛快打到限位還停不下來目標(biāo)消失后繼續(xù)走一段才停。原因目標(biāo)移動期間誤差一直很大積分項持續(xù)累加超出安全范圍后形成了“慣性”等誤差變小時積分輸出仍然很大。解決在PID_Calc里做積分限幅前述的out_max * 0.3f再把丟目標(biāo)時的PID_Reset寫進(jìn)主循環(huán)。這兩招配合后高速移動也能有效收斂。5.3 串口亂碼與丟幀現(xiàn)象串口收不到完整幀或者dx、dy出現(xiàn)離譜的大數(shù)值。原因多半是OpenMV的TX/RX沒有和F103交叉接或者沒有共地其次是波特率不一致。還有一類隱蔽情況OpenMV的uart.write發(fā)送太頻繁緩沖沒寫完又塞新數(shù)據(jù)。解決交叉接線、統(tǒng)一115200、GND共地。如果OpenMV端連續(xù)發(fā)幀時丟數(shù)據(jù)在uart.write(b)后加一句uart.wait_sent()等待發(fā)送完成再進(jìn)入下一輪循環(huán)。5.4 閾值漂移導(dǎo)致誤識別現(xiàn)象白天標(biāo)定好晚上開燈或者陽光一照色塊識別就抓空或者抓成別的物體。原因自動增益和白平衡開著閾值自身給得太寬。解決關(guān)閉自動增益和白平衡用LAB閾值重新標(biāo)定閾值盡量收窄到一個區(qū)間寧可偶爾跟丟也不要誤識別——PID對丟目標(biāo)有保護(hù)邏輯但對誤識別沒有任何保護(hù)。5.5 舵機堵轉(zhuǎn)發(fā)熱現(xiàn)象調(diào)試中云臺轉(zhuǎn)到機械限位、舵機發(fā)出嗡嗡聲外殼明顯發(fā)熱。原因主循環(huán)里沒有角度限位或者限位范圍超過了機械行程。解決把clampf的限位范圍改成實際云臺的機械行程多數(shù)兩軸云臺在±90度以內(nèi)上電復(fù)位先回中位再開主循環(huán)避免上電瞬間跑到極限位置。5.6 PID參數(shù)速查表現(xiàn)象優(yōu)先調(diào)整調(diào)整方向副作用高頻抖動Kp減小響應(yīng)變慢穩(wěn)態(tài)存在固定偏差Ki增大超調(diào)變大來回過沖Kd增大噪聲放大靜止?fàn)顟B(tài)微顫dead_zone增大死區(qū)內(nèi)的微小誤差不響應(yīng)目標(biāo)消失后猛沖積分限幅收緊大誤差下收斂變慢調(diào)參順序固定先P后I再D每次只動一個量。改完一個參數(shù)觀察至少5秒的追蹤效果再動下一個不要同時改兩個參數(shù)否則出問題根本不知道是哪個改壞的。6. 二次開發(fā)目標(biāo)預(yù)測、前饋補償與三軸擴展硬件鏈路完全復(fù)用代碼結(jié)構(gòu)預(yù)留了三個改造口按可移植性從低到高排列。6.1 目標(biāo)預(yù)測一階低通加微分前饋舵機機械響應(yīng)有滯后目標(biāo)快速移動時純PID總是慢半拍。一個性價比很高的改法是在誤差進(jìn)入PID之前加一階低通平滑再補一個前饋量float alpha dt / (0.10f dt); // tau約0.1秒 err_smooth alpha * (err_raw - err_smooth); float vff (err_smooth - err_last_smooth) / dt * 0.15f; out PID_Calc(pid, err_smooth) vff;時間常數(shù)tau0.1s會讓誤差曲線平滑很多vff對快速移動的目標(biāo)提前給舵機一個“預(yù)判轉(zhuǎn)角”。實測目標(biāo)做勻速運動時前饋能把跟蹤誤差縮小一半以上。注意前饋對噪聲敏感誤差先過平滑否則噪聲會被放大。6.2 三軸擴展與協(xié)議升級擴展Z軸距離或俯仰時需要把串口幀從6字節(jié)改成8字節(jié)新增dz字段校驗和相應(yīng)改變。這里有個血淚經(jīng)驗協(xié)議設(shè)計初期就把type字段的高兩位預(yù)留給版本號低六位表示狀態(tài)。這次改幀格式只需要在F103端按版本號走兩條解析分支不用所有舊板子跟著報廢。F103側(cè)再加一路定時器通道驅(qū)動第三顆舵機主循環(huán)里多一個PID實例即可。整套代碼結(jié)構(gòu)對多路PID的擴展成本很低因為PID_Calc是結(jié)構(gòu)體指針傳入不依賴任何全局變量。6.3 讓OpenMV直接輸出PID如果只想做演示不想在F103里維護(hù)PID可以直接把PID_Calc邏輯搬到OpenMV端每幀算完誤差后直接輸出PWM。代價是閉環(huán)頻率受視覺幀率限制通常只有20到30fpsF103端跑閉環(huán)可以做到100Hz左右動態(tài)響應(yīng)明顯更好。如果要拿這個項目練手PID建議保留F103數(shù)據(jù)鏈路比視覺端更穩(wěn)定也更容易用串口日志畫誤差曲線。從那以后我每次改完P(guān)ID參數(shù)都會順手把誤差曲線用串口打出來先看曲線形態(tài)再決定動Kp還是動Ki而不是憑感覺盲調(diào)。這個習(xí)慣幫我把調(diào)試時間幾乎砍了一半希望幫到你。本文還有配套的精品資源點擊獲取