題目：運動目標控制與追蹤系統(tǒng) 本項目基于 K230 平臺實現(xiàn)運動目標位置復位、屏幕邊框巡航、膠帶正方形巡航、數(shù)字 8 循跡演示；并預留自動追蹤紅色光斑的擴展接口。

目錄

項目概述

題目要求—功能對照

硬件清單

接線與引腳映射

軟件與運行環(huán)境

標定與坐標映射（關鍵）

代碼結構與主要函數(shù)

使用方法

擴展：自動追蹤紅色光斑（對接題目(5)）

時間與性能估算

常見問題

安全與注意事項

項目源碼

項目概述

目標：在 1 m 處的白色屏幕（有效面積 ≥ 0.6×0.6 m2）上，用綠色激光光斑模擬“運動目標”，并按鍵觸發(fā)不同的軌跡控制；同時給出像素→電機角度的標定方法。

平臺：K230（攝像頭、FPIOA、GPIO、UART）。

核心思路：

攝像頭采圖與顯示；

像素坐標與雙電機角度的雙線性插值標定（pixel_to_angle）；

通過串口（UART1）下發(fā)“使能 / 清零 / 限速位置模式”控制幀驅動兩個電機。

使用手冊：

K230：https://wiki.01studio.cc/docs/canmv_k230/

張大頭步進電機：https://www.yuque.com/zhangdatouzhikong/gzng3d

電機控制工具：https://pan.baidu.com/share/init?surl=rpwA7QtdAglsVxj_y77mmQ&pwd=x7dy

題目要求—功能對照

說明：

題目“原點為正方形左上角”，代碼中已按此設定（Reset_posi() 將兩個電機角度設為 (0,0)）。

(3) 膠帶巡航與 (2) 邊框巡航本質相同，只需將軌跡點標定到膠帶中心線上（§6）。

硬件清單

K230 開發(fā)板（帶攝像頭接口）

攝像頭（RGB565，工作分辨率 320×240）

3D打印外殼 + 2 臺步進電機（串口協(xié)議控制）

綠色激光筆（光斑直徑 ≤ 1 cm）

紅色激光筆（用于擴展項自動追蹤）

4個按鍵（上拉輸入）

接線與引腳映射

UART1：TXD=IO3，RXD=IO4（FPIOA.set_function）

按鍵（上拉輸入）：

SW1→IO35（使能/失能 切換）

SW2→IO34（原點復位）

SW3→IO33（邊線/膠帶巡航）

SW4→IO32（數(shù)字 8 軌跡）

電機地址：電機1：0x01、電機2： 0x02

軟件與運行環(huán)境

固件 / SDK：K230 對應 Python 運行環(huán)境與 media.* 模塊（攝像頭、媒體）

主要依賴：time, os, sys, math, media.sensor, media.display, media.media, machine(UART/FPIOA/Pin)

串口參數(shù)：UART1，115200 bps

標定與坐標映射（關鍵）

代碼通過 四點標定 + 雙線性插值 將 像素 (x,y) 映射到 電機角度 (ax, ay)：

# 獲取電機角度defpixel_to_angle(x, y): # 像素的四角點 px_tl= (71,30) # 左上 px_tr= (248,29) # 右上 px_br= (246,206) # 右下 px_bl= (72,205) # 左下 # 對應的電機角度 ang_tl= (0.0,0.0) # 左上 ang_tr= (26.0,0.0) # 右上 ang_br= (26.2,25.8) # 右下 ang_bl= (-0.6,25.6) # 左下 # 把目標像素(x,y)映射到[0,1]×[0,1]的歸一化坐標(u,v) u= (x - px_tl[0]) / (px_tr[0] - px_tl[0]) v= (px_tl[1] - y) / (px_tl[1] - px_bl[1]) u= max(0, min(1, u)) v= max(0, min(1, v)) # 對x軸角/y軸角分別做雙線性插值 ax= (1-u)*(1-v)*ang_tl[0] + u*(1-v)*ang_tr[0] + u*v*ang_br[0] + (1-u)*v*ang_bl[0] ay= (1-u)*(1-v)*ang_tl[1] + u*(1-v)*ang_tr[1] + u*v*ang_br[1] + (1-u)*v*ang_bl[1] return(round(ax,1), round(ay,1))

(注：這個部分需要自行通過攝像頭識別四個角點的像素坐標以及控制電機轉動到四個角點并記錄下角度，方便后續(xù)做像素→電機角度的映射)

u,v 為像素在四邊形內(nèi)的歸一化坐標；

通過雙線性插值得到 (ax, ay)，并四舍五入到 0.1°；

下發(fā)控制前再放大 10 倍（協(xié)議以 0.1° 為單位）：bx=int(ax*10)、by=int(ay*10)。

（注：放大10倍是因為命令里面位置角度：范圍為00000000-FFFFFFFF，即0-4294967295，保留一位小數(shù)，值為1表示0.1°，值為10表示1°，以此類推）

如何滿足 (2)/(3) 的“距邊線≤2 cm”：

在標定時，將四角像素點精確對齊到屏幕上 0.5 m 正方形 的四個頂點；

巡航點列（x1..x4, y1..y4 或自定義數(shù)組）沿膠帶中軸布置；

調整 speed 與采樣密度，確保弧度/直線段逼近膠帶路徑。

代碼結構與主要函數(shù)

串口協(xié)議：

Set_zero(addr)：將當前位置角度清零；

# 將當前位置角度清零defSet_zero(addr): # addr=電機地址 cmd =bytearray(4) # 定義長度為4的指令幀 cmd[0] = addr # 地址（電機ID） cmd[1] = 0x0A # 功能碼 cmd[2] = 0x6D # 輔助碼 cmd[3] = 0x6B # 校驗碼 uart.write(cmd) # 通過串口發(fā)送這條指令

Enable_elect(addr, state)：電機使能控制；

# 電機使能控制defEnable_elect(addr, state): # addr=電機地址,state=使能狀態(tài) cmd= bytearray(6) # 定義長度為6的指令幀 cmd[0] = addr # 地址（電機ID） cmd[1] = 0xF3 # 功能碼 cmd[2] = 0xAB # 輔助碼 cmd[3] = state # 使能狀態(tài):00/01分別表示不使能/使能 cmd[4] = 0x00 # 同步標志 cmd[5] = 0x6B # 校驗碼 uart.write(cmd) # 通過串口發(fā)送這條指令

Posi_ctrl(addr, vel, ang)：直通限速位置模式控制。

# 直通限速位置模式控制defPosi_ctrl(addr, vel, ang): # addr=電機地址, vel=速度, ang=角度 ifang >50: # 角度限制 pass cmd= bytearray(12) # 定義長度為12的指令幀 cmd[0] = addr # 地址（電機ID） cmd[1] = 0xFB # 功能碼 cmd[2] = 0x01 # 方向 cmd[3] = (vel >>8) &0xFF # 速度(高8位字節(jié)) cmd[4] = vel &0xFF # 速度(低8位字節(jié)) cmd[5] = (ang >>24) &0xFF # 位置角度(高8位字節(jié)bit24 - bit31) cmd[6] = (ang >>16) &0xFF # 位置角度(bit16 - bit23) cmd[7] = (ang >>8) &0xFF # 位置角度(bit8 - bit15) cmd[8] = ang &0xFF # 位置角度(低8位字節(jié)bit0 - bit7) cmd[9] = 0x01 # 運動模式 cmd[10] =0x00 # 同步標志 cmd[11] =0x6B # 校驗碼 uart.write(cmd) # 通過串口發(fā)送這條指令

系統(tǒng)參數(shù)：

addr_1/addr_2、speed、time_1/ time_2、預設角點 (x0..x4, y0..y4)

(x1..x4, y1..y4)這四個坐標是通過實踐得出的，為了不完全脫離膠帶稍微做了一些調整。

# 設置運動目標控制系統(tǒng)所需參數(shù)addr_1=0x01 # 電機1地址addr_2=0x02 # 電機2地址time_1=0.03 # 30mstime_2=2 # 2sspeed =30 # 速度state =1 # 電機狀態(tài)x0=0 ; y0 = 0 # 原點x1=2 ; y1 = 3 # 左上x2=257; y2 = 3 # 右上x3=260; y3 = 256 # 右下x4=0 ; y4 = 255 # 左下

系統(tǒng)功能：

Clean_elect()：雙電機清零；

# 將電機xy值清零def Clean_elect(): Set_zero(addr_1) time.sleep(time_1) Set_zero(addr_2)

Enabled_elect(state)：雙電機使能；

# 不使能/使能電機def Enabled_elect(state): Enable_elect(addr_1,state) time.sleep(time_1) Enable_elect(addr_2,state)

Reset_posi()：回到原點（左上角）；

# 啟動運動目標控制系統(tǒng)def Reset_posi(): Posi_ctrl(addr_1, speed, x0) time.sleep(time_1) Posi_ctrl(addr_2, speed, y0)

Start_system()：按序到達 5 個角點，實現(xiàn)邊框/膠帶巡航；

# 啟動運動目標控制系統(tǒng)def Start_system(): Posi_ctrl(addr_1, speed, x1) time.sleep(time_1) Posi_ctrl(addr_2, speed, y1) time.sleep(time_2) Posi_ctrl(addr_1, speed, x2) time.sleep(time_1) Posi_ctrl(addr_2, speed, y2) time.sleep(time_2) Posi_ctrl(addr_1, speed, x3) time.sleep(time_1) Posi_ctrl(addr_2, speed, y3) time.sleep(time_2) Posi_ctrl(addr_1, speed, x4) time.sleep(time_1) Posi_ctrl(addr_2, speed, y4) time.sleep(time_2) Posi_ctrl(addr_1, speed, x1) time.sleep(time_1) Posi_ctrl(addr_2, speed, y1)

（注：這部分的代碼還未優(yōu)化，比較冗余，這樣寫意義不大?？梢孕薷某色@取四個角點的像素點，通過pixel_to_angle(x, y)這個函數(shù)得到電機轉動的角度進而控制電機運動）

pixel_to_angle()：像素→角度（雙線性插值）；（詳見 §6 標定）

get_motor_angles_from_pixel()：單點下發(fā)；

# 傳入像素坐標并返回電機角度defget_motor_angles_from_pixel(x, y): ax, ay = pixel_to_angle(x, y) bx =int(ax *10) # 電機控制需要乘以10 by =int(ay *10) # 電機控制需要乘以10 print(f"輸入像素坐標: ({x},{y}) -> 電機角度: ({ax},{ay}) -> 換算角度: ({bx},{by})") Posi_ctrl(addr_1,100, bx) time.sleep(0.01) Posi_ctrl(addr_2,100, by) time.sleep(0.01)

Reco_eight()：攝像頭找圓（find_circles），采樣 N=48 點/圓并縮放至 96% 半徑；

# 識別數(shù)字8def Reco_eight(): # ----- 拍一張圖 ----- for _ inrange(5): sensor.snapshot() img = sensor.snapshot() # 用于標記每個圓的采樣點編號 point_number =1 scale_factor =0.96 # 檢測圓形 for c in img.find_circles(threshold=6000, x_margin=10, y_margin=10, r_margin=10, r_min=35, r_max=45, r_step=4): # 畫出圓 r = c.r() img.draw_circle(c.x(), c.y(), r, color=(255,0,0), thickness=2) print("找到圓: 圓心=(", c.x(),",", c.y(),") 半徑=", c.r()) # ----- 在圓周上采樣48個點 ----- N =48 for i inrange(N): theta =2* math.pi * i / N - math.pi /2 px =int(c.x() + r * math.cos(theta)) py =int(c.y() + r * math.sin(theta)) # 計算新的縮小后的點坐標 new_px =int(c.x() + (px - c.x()) * scale_factor) new_py =int(c.y() + (py - c.y()) * scale_factor) trajectory_points.append((new_px, new_py)) point_number +=1 # 增加點編號 # 顯示結果 Display.show_image(img, x=round((lcd_width - sensor.width()) /2), y=round((lcd_height - sensor.height()) /2))

Eight_track(points, t)：按“左圓上半圈→右圓上半圈→右圓下半圈→左圓下半圈”的順/逆時針順序形成 8 字軌跡。

# 數(shù)字8循跡def Eight_track(trajectory_points,times): # 順時針走第一個圓的前半圈 for(x, y)intrajectory_points[:24]: get_motor_angles_from_pixel(x, y) time.sleep(times) # 延時times秒 # 順時針走第二個圓的前半圈 for(x, y)intrajectory_points[48:72]: get_motor_angles_from_pixel(x, y) time.sleep(times) # 延時times秒 # 逆時針走第二個圓的后半圈 for(x, y)intrajectory_points[72:96]: get_motor_angles_from_pixel(x, y) time.sleep(times) # 延時times秒 # 逆時針走第一個圓的后半圈 for(x, y)intrajectory_points[24:48]: get_motor_angles_from_pixel(x, y) time.sleep(times) # 延時times秒 x,y = trajectory_points[1] get_motor_angles_from_pixel(x, y)

使用方法

上電與初始化：

上電前我們需要手動把電機置于原點；

代碼開頭完成 UART、按鍵、攝像頭初始化；

默認 Enabled_elect(1) 使能電機并執(zhí)行 Clean_elect()，記錄當前位置為原點。

按鍵操作：

SW1：電機使能/失能切換；

SW2：回到原點（左上角）；

SW3：沿預設邊框/膠帶點位順時針巡航；

SW4：拍照識別兩個圓，生成 8 字軌跡并沿軌跡運行（Eight_track(trajectory_points, 0.1)）。

擴展：自動追蹤紅色光斑（對接題目(5)）

當前代碼暫未實現(xiàn)自動追蹤紅光斑，后續(xù)可以按照如下開發(fā)路徑進行完善：

檢測紅色光斑：在攝像頭圖像做 HSV/閾值分割，尋找最大連通域質心 (xr, yr)；

像素→角度：調用 pixel_to_angle(xr, yr) 得到期望角度 (ax, ay)；

閉環(huán)控制：周期性下發(fā) Posi_ctrl(vel, ang)，或加入 PID（對誤差 e=目標角-當前角）；

收斂判定與提示：誤差持續(xù) < 閾值（例如 0.2°）且時間 < 2 s，觸發(fā)蜂鳴器/LED 連續(xù)提示；

抗抖與安全：對質心做時間濾波，限制角速度與加速度。

時間與性能估算

邊框巡航：Start_system() 在四邊與對角點之間各等待 time_2 = 2 s，總時長約 8~10 s（遠小于 30 s 限制）。如需更“貼邊”，可在側邊插入更多中間點。

數(shù)字 8：每個圓 N=48 點，Eight_track(..., 0.1) 單圈約 48×2×0.1 = 9.6 s，可按評分需求調節(jié) N 與 times。

常見問題

光斑不在邊線上 / 偏差大：檢查 §6 標定四點是否準確落在 0.5 m 正方形頂點；必要時重新測量角度對照表（ang_*）。

運動不連貫 / 抖動：適當降低 speed，增加過渡點；在串口下發(fā)間隙留足 time.sleep。

找不到圓 / 8 字不閉合：調節(jié) find_circles 的 threshold / r_min / r_max；提高屏幕對比、降低環(huán)境光。

電機無響應：確認 Enable_elect(..., 1) 已使能；串口連線正確；地址 0x01/0x02 與實物一致。

安全與注意事項

激光直視有危險，請確保對人眼安全；

設備擺放與圖示一致（屏幕 1 m 處），防止反射；

運行前先執(zhí)行 Clean_elect()，避免機械撞限位。

項目源碼

GitHub倉庫地址：https://github.com/ruanqiuqiu/RSOC_K230_project

RT-Thread Github 開源倉庫，歡迎撒個星(Star)支持，更期待你的代碼貢獻：https://github.com/RT-Thread/rt-thread