從“能走路”到“能跟著人走”中間隔著好幾個真正的工程問題。很多做四足自研的人都有這種體驗單步走、原地轉身、越過簡單障礙這些單獨驗證都能跑但一旦把目標放在“自主跟隨一個正在走動的人”機器人就像換了一副面孔——要么把人跟丟要么速度忽快忽慢要么在轉向時晃到失去平衡。這篇文章就來拆解自研四足機器人“跟隨功能”背后的完整鏈路。我們不會只討論某一個神經網絡的精度而是把跟隨當作一個“感知—跟蹤—控制—反饋”的系統(tǒng)來分析和實現。讀完你會得到一個明確判斷四足跟隨不是算法競賽而是系統(tǒng)工程決定演示成敗的往往不是某一個模型的先進程度而是你有沒有把整條鏈路的時序、坐標、狀態(tài)和異常處理梳理清楚。1. 這篇文章真正要解決的問題如果你在搜索引擎里查“四足機器人 跟隨功能”會看到大量炫酷的展示視頻。機器人背著一個深度相機靈活地跟著操作員在草地、樓梯和室內穿行。這些視頻看多了容易讓人產生一個誤解跟隨功能是成熟的開箱即用能力我們只需要調一個模型再部署上去就行。真實情況要復雜得多。自研四足本體的跟隨功能演示表面上是做一個“追人”的機器人本質上包含四個獨立問題如何在機器人動態(tài)運動的情況下穩(wěn)定識別出“目標是誰”如何在目標走出畫面、轉身、被遮擋時仍然維持身份跟蹤如何把視覺給出的“像素框”換算成機器人坐標系里可以執(zhí)行的“距離和方位”如何把“距離和方位”轉換成四足機器人能穩(wěn)定執(zhí)行的速度指令同時不破壞步態(tài)平衡。四足和輪式機器人相比跟隨難度更高。輪式機器人底盤穩(wěn)定相機晃動小運動學模型簡單四足機器人走路時身體本身就在周期性地起伏、俯仰和橫滾哪怕站定時相機也會因為髖關節(jié)輕微震動而出現畫面抖動。與此同時四足的轉向更接近“先減速、后轉身、再加速”如果控制策略不及時響應目標位置變化機器人就會走出一個難看的弧線甚至因為轉向角速度過大導致傾覆風險。因此這篇文章的核心目標讀者有兩類正在用自研四足本體做巡檢、安防、陪伴演示的開發(fā)者希望把跟隨做成一個可穩(wěn)定復現的功能模塊準備從單一運動控制轉向“感知運動一體化”開發(fā)的機器人愛好者想理解一個完整機器人應用是如何組織代碼、狀態(tài)和數據的。文中代碼以結構演示與工程思路為主視覺模型參數和機器人結構參數都需要替換成你自己的實際數據但系統(tǒng)的拆解方法、狀態(tài)機設計、排錯路徑可以直接復用。2. 概念解析四足本體與跟隨功能的邊界在展開具體實現之前先把兩個容易混淆的概念講清楚。2.1 四足本體不是“機器人殼子”做四足開發(fā)的人常把“本體”和“整機”分開講。一次典型的自研四足本體至少包含這幾層機械層機身、四條腿、足端、關節(jié)連接件驅動層關節(jié)電機、驅動器、減速器、關節(jié)扭矩傳感器感知層IMU、關節(jié)編碼器、相機、激光雷達、超聲波模塊計算層運行運動控制和感知算法的主控板執(zhí)行層負責將速度指令變成腿足步態(tài)的步態(tài)控制器。跟隨功能會橫跨感知層到執(zhí)行層。很多團隊容易犯的錯是默認執(zhí)行層已經“穩(wěn)定到可用”然后把全部精力投入到感知模型訓練上。實際上四足本體的運動控制如果存在低頻抖動、步態(tài)切換遲滯或者速度響應非線性這些噪聲都會放大到感知端導致目標檢測框抖動甚至丟幀。2.2 跟隨功能在技術上包含什么從表現上看跟隨是“機器人始終和目標保持一段距離同時正面朝向目標并跟隨移動”。從算法實現上可以拆成更細的功能鏈人員檢測在圖像中找到人輸出邊界框目標鎖定在多個人出現時決定跟隨哪個目標跟蹤給目標分配一個唯一ID在連續(xù)幀之間維持身份深度映射獲得目標相對機器人的三維坐標跟隨規(guī)劃將目標坐標映射為線速度與角速度安全控制檢測近距障礙限制速度處理丟失后的恢復動作。不是每個跟隨系統(tǒng)都需要跑一個復雜的大模型。簡單場景可以只檢測“人腿”如果用到機載端到端模型則可以輸出人體全身框更復雜的會結合點云聚類和多目標跟蹤。選擇哪一種取決于你的平臺算力和演示場景的復雜度。2.3 四足跟隨與輪式跟隨最大的差異在哪里可以比較這樣一組場景一個輪式機器人底盤在跟人一個四足機器人本體在跟人。輪式機器人做跟隨主要是“車體坐標與目標坐標間的平面跟蹤問題”底盤運動模型相對線性控制頻率可以到50Hz到100Hz平滑輸出。四足機器人做跟隨需要同時處理相機外參隨身體姿態(tài)實時變化如果控制周期是100Hz那么每20毫秒相機的俯仰角都可能有幾度的變化四足的橫移能力弱跟隨主要靠轉向與前進來實現因此運動模型有很強的橫向約束步態(tài)切換從站立到小跑從直行到轉彎需要時間不能要求控制指令瞬時到達。這個差異決定了四足跟隨系統(tǒng)的調試方法絕對不能照搬輪式移動機器人。最好的辦法是先把控制問題簡化為兩步先修正身體姿態(tài)影響把目標坐標變換到水平投影平面再進行二維平面上的跟隨控制。3. 系統(tǒng)方案設計與軟件架構無論硬件多么復雜軟件上我都建議先把系統(tǒng)拆成四個模塊。這里有一張功能鏈路表先總覽再逐層拆解模塊主要任務典型數據流關鍵輸出感知采集獲取圖像與深度相機 → 機載處理單元彩色圖、深度圖、位姿目標感知檢測并跟蹤目標彩色圖 → 檢測器 → 跟蹤器目標邊界框、跟蹤ID坐標映射把目標位置換算到機器人坐標系邊界框 深度 外參 → 坐標變換目標相對機器人的距離與角度運動控制將目標位置轉化為速度指令坐標 → 控制器 → 運動底盤協(xié)議期望線速度、角速度、狀態(tài)切換指令3.1 工程上的推薦做法狀態(tài)機先行很多初學者會直接寫一個大循環(huán)讀圖像 → 目標檢測 → 計算誤差 → PID輸出。這樣做在空場地慢速演示時可能沒問題一旦遇到人轉身、走出畫面整個系統(tǒng)就會失去明確的行為。更穩(wěn)的做法是先用狀態(tài)機把機器人的行為歸一化先定義清楚狀態(tài)再寫功能SEARCH搜尋 視野中沒有目標機器人原地小角度旋轉搜索 TRACK跟蹤 目標穩(wěn)定出現在視野中計算速度跟隨 LOST丟失 目標曾經存在但連續(xù)N幀找不回減速并進入搜索 RECOVERY接管恢復 人重新出現后重新鎖定 STOP停止 近距離障礙觸發(fā)急?；虻却庸?。狀態(tài)機的好處有三個第一它把“有目標”“沒目標”“目標短暫消失”之間的行為區(qū)分開來避免機器人在丟失目標后仍然向前沖第二它讓日志和調試變得非常直觀你在看現場演示時一眼就知道當前行為是哪一層引起的第三它讓不同開發(fā)者的模塊可以并行工作感知組負責輸出目標控制組負責消費狀態(tài)。3.2 常用硬件方案與選型建議自研四足本體的算力選擇常見三類入門樹莓派或者Jetson Nano級別適合只跑輕量人腿檢測和簡單跟蹤中端Jetson Orin Nano / Orin NX 級別可以跑YOLO系列目標檢測和DeepSORT類跟蹤高端Jetson AGX Orin / 搭配獨立顯卡的工控機適合加載點云和端到端模型。關于感知傳感器入門演示推薦RGBD深度相機。原因很多最關鍵是“深度直接可用”四足跟隨最需要的距離信息可以直接從深度圖讀取而不必依賴單目深度估計模型。無論選擇什么相機品牌有一點必須做到拿到相機后先做內參標定并且確認深度圖和彩色圖時間戳對齊。如果深度和彩色圖像沒有對齊檢測框在深度圖上取到的距離會是錯的。位置差半米以內在視覺上可能看不出來在控制上卻可能造成明顯的忽遠忽近。激光雷達不是四足隨從的首選方案。原因在于四足本體在行走時雷達的高度和俯仰會周期性變化低線雷達對地面和墻體的反射噪聲很大。環(huán)境缺乏特征時還可能無法提供可靠的點云。激光雷達可以做近距安全防護但作為“跟隨目標感知”的主傳感器體驗不如RGBD相機來得直接。4. 感知鏈路設計從像素到目標位置這是跟隨功能里最難、也最值得花時間的部分。我們一步步拆。4.1 使用目標檢測鎖定行人目標檢測的任務是“在每一幀圖像中找到人的邊界框”。實際開發(fā)中通常有兩種選擇全身檢測用YOLO系列或更輕量的目標檢測模型輸出行人的全身矩形框。模型的泛化能力好對穿裙子和背包的行人都能檢測。腿部檢測專門針對腿部訓練檢測器輸出的框更小適合機器人視覺高度在膝蓋高度附近、只能拍到腿的場景。在空間開闊、相機安裝高度高于1米的場景優(yōu)先使用全身檢測因為人體特征更明顯。在較矮的四足平臺上如果相機高度低于0.5米全身檢測器會頻繁漏檢這時候就得考慮“腿部檢測 高度假定”的思路。檢測模型不能只跑一次就丟。僅靠單幀檢測結果目標框會有隨機抖動還可能出現同一人來回閃爍ID的情況因此必須引入目標跟蹤。4.2 目標跟蹤讓跟隨有“記憶”目標跟蹤在視覺模塊里要解決的是序列化問題。你可以選擇經典算法卡爾曼濾波 匈牙利匹配視覺上就體現為DeepSORT或者更簡單的SORT。也可以用現代的跟蹤器但核心邏輯一致對每一幀檢測到的目標框生成特征將當前幀的檢測框與上一幀已有的跟蹤軌跡做匹配匹配成功則更新軌跡連續(xù)多幀未匹配成功的軌跡標記為丟失。這里特別強調一點如果你只有單目相機、沒有做重識別ReID跨長時間遮擋后的目標身份恢復基本是不可能的。所以演示設計里要避免讓人走出畫面超過幾秒?？梢栽谝曈X層面增加目標特征也可以在狀態(tài)機層面增加“丟失后原地360度緩慢搜索”的行為機制。4.3 確定要跟隨誰跟隨系統(tǒng)的默認設定通常是“跟隨第一個被鎖定的目標”。更貼近實際演示的一種策略是進入跟隨模式后檢測畫面中面積最大、靠近畫面中央且持續(xù)數幀的人作為目標。當目標丟失需要重新鎖定時再執(zhí)行同樣的策略。為了避免現場出現誤跟情況推薦保留一個手動鎖定機制。例如遙控器上的一個按鍵作為“鎖定當前視野內目標”的觸發(fā)或者通過語音/視覺界面選擇。在自研演示階段最牢固的方法是使用遙控器的目標鎖定鍵不要完全依賴算法自動判斷尤其是演示環(huán)境可能站著多名觀眾時。4.4 坐標映射像素坐標到機器人坐標當目標被跟蹤后要通過傳感器的內外參數完成一次坐標變換。設目標框中心點的像素坐標為 (u, v)。需要做三步根據相機內參把 (u, v, depth) 反投影到相機坐標系下的三維點根據相機外參將相機坐標系下的點轉換到機體坐標系根據機器人當前姿態(tài)將機體坐標進一步投影到水平面或者轉換為以機身為原點的二維距離。核心偽代碼如下。# 偽代碼從像素坐標 深度值計算目標相對機器人的位置 import numpy as np def pixel_to_camera_point(u, v, depth, camera_matrix): fx camera_matrix[0, 0] fy camera_matrix[1, 1] cx camera_matrix[0, 2] cy camera_matrix[1, 2] # 針孔模型逆變換 z float(depth) if z 0: return None x (u - cx) * z / fx y (v - cy) * z / fy return np.array([x, y, z]) def camera_point_to_body(point_cam, R_cam_body, t_cam_body): # R_cam_body: 相機坐標系到機體坐標系的旋轉矩陣 # t_cam_body: 平移向量 point_body R_cam_body point_cam t_cam_body return point_body邊界框的中心并不一定是人身體的中心因為人的身體有寬度框的中心可能落在兩臂之間而非背部正中。如果要提高精度可以在框的下邊緣取底部中心點并結合人的典型高度來估計一個腳底附近的三維點因為四足跟隨要的“跟誰”本質是“跟到目標腳下/身后一個穩(wěn)定距離”取底部中心比取框中心更可靠。4.5 相機外參的實時補償四足行走帶來的姿態(tài)變化會讓相機外參不再是固定值。如果機器人運動較慢、演示場景平坦可以用固定外參近似。但一旦要做上下坡或大步態(tài)行走需要引入IMU數據實時補償相機位姿把圖像坐標系先投影到機體慣性系再做平面上控制。否則機器人低頭時相機視野向下目標會突然“跑到”很近的位置導致控制器誤以為需要急停。有一個判斷辦法在機器人原地踏步時觀察跟蹤畫面的目標位置穩(wěn)定性。如果目標檢測框沒有抖動但機器人自身姿態(tài)在變而跟隨的距離估算波動很大那大概率就是相機外參沒有同步補償機體姿態(tài)。5. 跟隨運動控制不讓機器人看起來“傻”跟隨運動控制是功能演示觀感最直接的模塊。這里要做的事有兩件決定機器人該多快前進決定機器人該轉多大的角度?？刂撇呗圆恍枰珡碗s分階段處理即可。5.1 縱向速度控制機器人對目標的期望距離需要在配置中給出。隨從速度按距離誤差分三段距離誤差較大時速度快速上升并設置限幅避免加速過猛導致四足失穩(wěn)接近目標后進入平滑速度段按PID或簡單的比例控制線性收斂距離小于安全距離時不前進甚至微后退。一種可行的數學模型是分段函數# 偽代碼縱向跟隨速度計算 def compute_linear_speed(distance, target_distance): error distance - target_distance if distance MIN_SAFE_DISTANCE: return 0.0 if abs(error) 0.1: return 0.0 speed K_V * error # 限幅 speed max(-MAX_BACK_SPEED, min(MAX_FORWARD_SPEED, speed)) return speed線性增益本身應當避免過大的值速度在跟隨時保持在小跑范圍內即可讓機器人呈現“平穩(wěn)跟人”而不是“猛撲式跟人”的觀感。5.2 轉向控制跟隨中目標不可能永遠正對機器人。所以需要在水平面上計算目標相對機器人正前方的夾角控制機器人轉向目標。另一種可選做法是“位置—角度混合控制”讓機器人始終正對目標用角速度消除角度誤差。一個比較通用的角度控制代碼如下# 偽代碼轉向角速度計算目標相對機體的夾角 yaw_error def compute_angular_speed(yaw_error): # yaw_error 單位弧度 # 設置死區(qū)避免頻繁擺動 if abs(yaw_error) ANGULAR_DEAD_ZONE: return 0.0 omega K_W * yaw_error omega max(-MAX_ANGULAR_SPEED, min(MAX_ANGULAR_SPEED, omega)) return omega先大幅轉向快速對準再小角度緩慢微調演示過程中就會比較自然。5.3 安全控制是演示成敗的生命線演示場合比實驗室更容易出問題。操作員后退、觀眾橫穿、機器人盲區(qū)有臺階任何一個意外都可能損壞設備。因此安全模塊必須有獨立優(yōu)先級它不跟隨目標而時刻監(jiān)視機器人與最近障礙物的距離。建議做兩條獨立的安全邏輯近距急停RGBD相機的深度圖與傳感器近距檢測共同組成防護當正前方一定范圍內出現未知物體時無論目標狀態(tài)如何機器人速度置零并進入STOP狀態(tài)。速度限幅跟隨過程中線速度和角速度都不能超過本體的穩(wěn)定性上限。這里的上限不能用輪式經驗拍腦袋需要結合本體的步態(tài)表與實測速度來確定。安全控制代碼應獨立于目標跟蹤邏輯優(yōu)先級最高。不依賴視覺目標判斷因為視覺檢測可能存在漏檢和延遲。6. 完整參考實現ROS2風格的感知與控制節(jié)點這里給出一個參考實現模塊劃分遵循“目標檢測 目標跟蹤 坐標映射 控制輸出”。由于不同團隊的視覺模型權重和四足底盤控制協(xié)議差別大我會把模型加載與底盤驅動留成接口重點演示數據結構和邏輯流程。6.1 行人檢測與跟蹤模塊下面的Python代碼使用OpenCV的DNN模塊以及一個簡化版的IOU跟蹤器。配合實際模型可以替換成YOLO官方的導出模型。# 文件路徑follow_demo/perception/tracker.py 簡化目標檢測 跟蹤模塊參考實現。 實際部署建議替換成 - 檢測器YOLO系列或移動端輕量檢測網絡 - 跟蹤器參考 SORT / DeepSORT 原理實現 這里僅演示完整鏈路。 class Detection: def __init__(self, bbox, score, class_id): # bbox 格式: (x1, y1, x2, y2) self.bbox bbox self.score score self.class_id class_id class Track: def __init__(self, track_id, bbox): self.track_id track_id self.bbox bbox self.last_seen 0 self.hit_streak 0 class SimpleTracker: def __init__(self, max_lost_frames30): self.tracks [] self.next_id 0 self.max_lost_frames max_lost_frames def update(self, detections, frame_index): updated_tracks [] # 這里簡化為“取最大面積檢測框作為目標” # 實際多目標場景建議使用 IoU 匹配 卡爾曼濾波 if len(detections) 0: max_area 0 best_det None for det in detections: x1, y1, x2, y2 det.bbox area (x2 - x1) * (y2 - y1) if area max_area: max_area area best_det det if best_det is not None: if self.tracks: t self.tracks[0] t.bbox best_det.bbox t.hit_streak 1 t.last_seen frame_index updated_tracks.append(t) else: nt Track(self.next_id, best_det.bbox) nt.last_seen frame_index nt.hit_streak 1 self.next_id 1 updated_tracks.append(nt) self.tracks updated_tracks return self.tracks這里的跟蹤器退化成了“取最大目標”但結構上預留了替換位置。真實項目里跟蹤目標時必須保存目標特征否則當畫面里出現一個更大的目標時機器人可能會突然切換跟隨對象。6.2 坐標映射與跟隨控制器下面的模塊演示如何由檢測框與深度圖產生控制指令。它接收一個深度圖像、檢測框和相機參數輸出距離、夾角以及速度指令。# 文件路徑follow_demo/core/follow_controller.py 跟隨控制器參考實現。 輸入目標框 對齊后的深度圖。 輸出線速度和角速度。 import numpy as np class FollowController: def __init__(self, camera_matrix, dist_coeffsNone): self.K camera_matrix self.target_distance 1.2 # 期望跟隨距離單位米按實際調整 self.min_safe_distance 0.5 # 安全距離閾值 self.k_v 0.8 # 速度增益需要實測標定 self.k_w 1.5 # 轉向增益需要實測標定 self.max_v 0.5 # 最大線速度 m/s self.max_w 0.8 # 最大角速度 rad/s def detect_to_center(self, bbox): x1, y1, x2, y2 bbox u (x1 x2) / 2.0 v (y1 y2) / 2.0 return u, v def get_depth_at_bbox_bottom_center(self, depth_image, bbox, safe_radius5): x1, y1, x2, y2 bbox cx int((x1 x2) / 2.0) cy int(y2) # 取檢測框底部中心接近人體腳底平面 h, w depth_image.shape[:2] cy min(max(cy, 0), h - 1) cx min(max(cx, 0), w - 1) region depth_image[max(0, cy-safe_radius):cysafe_radius1, max(0, cx-safe_radius):cxsafe_radius1] valid region[(region 0.1) (region 8.0)] if len(valid) 0: return None return float(np.median(valid)) def compute_control(self, target_point_body): # target_point_body: 目標在機體坐標系下的 XYZ x target_point_body[0] y target_point_body[1] distance float(np.sqrt(x * x y * y)) yaw_error float(np.arctan2(y, x)) if distance self.min_safe_distance: return 0.0, 0.0, distance, yaw_error # 縱向速度距離誤差越大速度越大但限制最高速度 error distance - self.target_distance linear self.k_v * error linear max(-0.2, min(self.max_v, linear)) # 轉向速度消除角度偏差 angular self.k_w * yaw_error angular max(-self.max_w, min(self.max_w, angular)) return linear, angular, distance, yaw_error這段參考代碼的關鍵之處是取檢測框底部中心作為目標測距點并在深度圖上取一個半徑區(qū)域的中位數而不是單點深度。因為單點深度往往落在目標邊緣或背景上導致測距跳變中位數可以顯著提升穩(wěn)定性。6.3 狀態(tài)機與主循環(huán)參考主循環(huán)按固定幀率運行把感知結果送給狀態(tài)機狀態(tài)機決定行為。需要將相機幀率與運動控制頻率解耦相機允許30幀控制輸出可限制在20Hz到30Hz。# 文件路徑follow_demo/main_loop.py 主循環(huán)參考實現精簡結構。 import cv2 import numpy as np from perception.tracker import SimpleTracker from core.follow_controller import FollowController class FollowState: SEARCH SEARCH TRACK TRACK LOST LOST class FollowDemo: def __init__(self, camera_matrix): self.tracker SimpleTracker() self.controller FollowController(camera_matrix) self.state FollowState.SEARCH self.lost_frames 0 def run_on_frame(self, color_image, depth_image, frame_index): # 1. 檢測目標這邊留占位替換為真實模型輸出 detections fake_detect_person(color_image) # 2. 更新跟蹤器 tracks self.tracker.update(detections, frame_index) if len(tracks) 0: self.lost_frames 1 if self.lost_frames 30: self.state FollowState.SEARCH return 0.0, 0.0, self.state else: self.lost_frames 0 track tracks[0] bbox [int(v) for v in track.bbox] # 3. 計算目標三維坐標 u, v self.controller.detect_to_center(bbox) depth self.controller.get_depth_at_bbox_bottom_center( depth_image, bbox) if depth is None: self.state FollowState.LOST return 0.0, 0.0, self.state # 這里省略像素反投影的內外參矩陣計算 # target_point_body depth camera_matrix 逆變換 外參 point_cam np.array([0.0, 0.0, depth]) point_body point_cam # 占位需要替換為真實坐標變換結果 # 4. 計算控制量 linear, angular, distance, yaw_error \ self.controller.compute_control(point_body) return linear, angular, self.state def fake_detect_person(image): # 占位函數實際應加載檢測模型 return []這個主循環(huán)仍然省略了坐標變換細節(jié)。在真實工程中強烈建議把“像素轉機體坐標”單獨抽成一個模塊輸出可視化效果方便調試。6.4 啟動腳本與參數配置示例# 文件路徑run_follow.sh #!/bin/bash # 先啟動相機驅動 ros2 launch realsense2_camera rs_launch.py sleep 5 # 啟動跟隨主程序以ROS2為例 ros2 run follow_demo follow_node --ros-args --params-file follow_params.yaml# 文件路徑follow_params.yaml follow_demo: ros__parameters: target_distance: 1.2 min_safe_distance: 0.5 linear_gain: 0.8 angular_gain: 1.5 max_linear_speed: 0.5 max_angular_speed: 0.8 use_imu_compensation: true配置文件的優(yōu)勢在于調參不用重新編譯。實際調試時可以把一組參數記錄下來體驗不同參數下機器人跟隨時“自然感”與“穩(wěn)定性”的差異。7. 運行調試與效果驗證7.1 先做仿真或小底盤驗證很多團隊把視覺模型部署到四足上的第一天就直接在真機上加步伐測試結果出了問題也分不清是視覺誤差還是控制振蕩。更穩(wěn)健的調試路徑是分三步走第一步把同一套感知代碼跑在一臺輪式小車上。如果輪式小車就能穩(wěn)定跟隨說明視覺鏈路基本可靠問題大概率出在四足運動的姿態(tài)干擾上。第二步把同一套視覺控制和四足本體放在靜止狀態(tài)由人緩慢繞機器人行走只測試轉向跟隨不測試前進跟隨。如果轉向控制也有抖動那就和步態(tài)關系不大需要調增益或處理測量噪聲。第三步才加入前后移動和小跑步態(tài)做全狀態(tài)跟隨。7.2 調試可視化開發(fā)階段的調試可視化至關重要。調試畫面必須顯示以下信息目標的跟蹤框與跟蹤ID目標框底部中心測距結果目標相對機體的距離與夾角機器人當前狀態(tài)SEARCH/TRACK/LOST當前發(fā)布的線速度與角速度值。可以使用OpenCV直接把信息繪制到畫面中。一張實時調試圖能省下大量猜測時間?？吹疆嬅嬷芯嚯x數值在10厘米內跳變說明測距存在問題看到線速度在控制周期內忽正忽負說明控制增益過大或深度噪聲太大。7.3 功能演示檢查清單一次順利的演示背后需要進行多次走場測試。以下檢查清單建議在演示開始前逐項確認相機視野內能檢測到操作員且檢測框覆蓋完整人體至少覆蓋軀干與腿部跟隨系統(tǒng)默認鎖定的是指定操作員而不是旁邊的觀眾操作員向前走、停下、轉身、走出畫面再回來機器人行為符合狀態(tài)機預期操作員走過一段直線后直角轉彎機器人能流暢跟隨沒有嚴重的超調旋轉正前方有突然出現的障礙物時機器人能及時停止遙控斷開后機器人進入停止狀態(tài)而不是繼續(xù)前沖光照變化時目標檢測仍能維持基本穩(wěn)定提前準備好強光或逆光場景的應對方案。7.4 量化評估與驗收可以簡單記錄三組數據來評價跟隨效果直線段平均跟隨距離誤差一般控制在10到20厘米以內轉彎時目標角度誤差的最大值控制在20度以內算可用目標丟失后的重捕獲時間現場演示盡量控制在3秒以內。這些指標不用追求極致但要能復現。當你在多輪演示中都得到接近的數值系統(tǒng)才具備交付給第三方演示的條件。8. 常見問題與排查思路下面整理自研四足跟隨里最高頻的幾類問題。問題現象可能原因排查方式解決方案人對準后機器人仍然忽遠忽近深度圖在目標邊緣取到背景距離或控制增益過高查看深度可視化在檢測框底部中心區(qū)域打印原始深度值測距使用區(qū)域中位數降低縱向速度增益并增加濾波畫面中有多人時機器人頻繁換目標跟蹤器沒有保留外觀特征最大面積目標切換導致ID跳變查看跟蹤ID是否在換人時改變引入重識別特征或鎖定按鈕固定目標ID機器人后仰起步時目標位置突變身體姿態(tài)變化未補償相機外參打印IMU的姿態(tài)角同步觀察目標坐標跳變將相機坐標經機體姿態(tài)旋轉到世界水平坐標系跟隨速度越快檢測框抖動越嚴重運動模糊或相機幀率不足降低速度復測檢查快門與曝光提升幀率縮短曝光必要時加機械減振機器人轉彎時人體目標丟失目標在畫面邊緣轉身角速度過大記錄轉向過程中目標框被截斷的幀數限制最大角速度減少目標走出畫面的概率目標檢測偶爾漏檢導致機器人停止單幀漏檢直接觸發(fā)狀態(tài)機進LOST看漏檢是否連續(xù)出現增加漏檢容忍幀數結合目標運動預測維持跟蹤激光雷達或超聲波誤觸發(fā)急停距離閾值過緊在坡道上誤檢測地面查看近距傳感器原始距離檢查安裝角根據安裝高度調整角度設置合理的閾值演示現場逆光人物臉部過曝相機自動曝光導致目標特征變暗開啟畫面曝光曲線調試固定曝光參數關閉過強的自動曝光這些問題的共同規(guī)律是多數不穩(wěn)定現象都不是某一個算法單獨造成的而是模塊之間數據不對稱造成的。所以排查時不要只在感知代碼里反復加濾波先從坐標變換、時間戳和標定參數是否一致查起。9. 最佳實踐與工程建議9.1 先讓系統(tǒng)“減速可復現”再談“智能”在做四足跟隨功能時最容易犯的錯誤是過早加入復雜的深度估計、端到端模型或多目標跟蹤。自研本體的算力有限現場環(huán)境的不可控因素又多。第一次跑通演示寧愿讓機器人速度比較保守也要保證行為可預測。等狀態(tài)機流程完全穩(wěn)定后再逐步提高速度和目標復雜度。9.2 把目標鎖定權交給操作員在演示中“抓住誰”和“怎么跟上”應該分成兩個權限。自動檢測負責候選遙控器選擇或確認負責最終決策。這種方式能避免在現場出現目標更換的尷尬情況。一個好的交互設計是操作員在畫面中央附近按下確認鍵機器人就把當前正前方最近的人作為唯一目標鎖住。9.3 記錄數據包是調優(yōu)的起點每次現場測試保存三個東西原始彩色圖與深度圖記錄每幀的檢測框、目標ID、深度值、坐標變換結果機器人實際控制指令與IMU姿態(tài)。只有在出問題時能把現場數據完整回放你才能分析出到底是感知誤判還是控制相位滯后。這部分前期工作帶來的收益會在真機聯(lián)調階段體現得非常明顯。9.4 重視標定與時間同步自研四足的機身結構如果發(fā)生過拆裝或碰撞相機與機體的外參就可能改變。建議在每次演示前做一次快速標定在機器人前方放置標定板或二維碼讓機器人保持站立姿態(tài)通過視覺特征確認相機位姿與預設外參是否一致。另外要保證彩色圖像與深度圖像時間戳接近否則運動物體的深度和彩色幀錯位會產生邊緣誤差。9.5 設計安全兜底最后也是最重要的真機測試必須設置急停按鈕和軟件看門狗。突發(fā)情況下操作員要能一鍵讓機器人進入停止狀態(tài)。開發(fā)階段不要越過安全環(huán)節(jié)直接調試高速跟隨保護設備與人員永遠是第一優(yōu)先級。10. 總結與后續(xù)學習方向做一次自研四足本體的跟隨功能演示真正要跨過去的坎并不是“識別一個人”。識別一個人的模型已經非常成熟真正的坎在于目標檢測的結果如何穩(wěn)定變成機器人能執(zhí)行的速度指令機器人運動起來之后感知數據如何保持可信以及目標丟失之后系統(tǒng)如何選擇恰當行為。這篇文章拆解了一條完整鏈路從四足本體的特點出發(fā)做了系統(tǒng)架構規(guī)劃講解了感知、坐標映射、運動控制和狀態(tài)機設計并給出了一套可運行的參考代碼。如果你要把它應用到自己的四足機器人上接下來值得深入的方向有三個第一把目標跟蹤的魯棒性做好引入多目標跟蹤與外觀特征讓機器人能夠長時間記住目標身份。第二把運動控制與步態(tài)更緊密地耦合起來根據目標狀態(tài)選擇不同的步態(tài)或速度檔位而不是讓底盤控制層自行切換。第三把多傳感器融合做扎實用近距雷達和深度相機互補降低視覺遮擋和光照變化對跟隨系統(tǒng)的影響。建議先基于本文的狀態(tài)機跑通最小閉環(huán)用自己的四足本體錄幾段不同場景的真實數據之后再圍繞調試視頻逐步優(yōu)化。穩(wěn)定、可復現、可排錯的跟隨系統(tǒng)比只能跑一次高難路徑的演示更有長期價值。 自研四足機器人跟隨功能全鏈路拆解：從目標檢測到運動控制
從“能走路”到“能跟著人走”中間隔著好幾個真正的工程問題。很多做四足自研的人都有這種體驗單步走、原地轉身、越過簡單障礙這些單獨驗證都能跑但一旦把目標放在“自主跟隨一個正在走動的人”機器人就像換了一副面孔——要么把人跟丟要么速度忽快忽慢要么在轉向時晃到失去平衡。這篇文章就來拆解自研四足機器人“跟隨功能”背后的完整鏈路。我們不會只討論某一個神經網絡的精度而是把跟隨當作一個“感知—跟蹤—控制—反饋”的系統(tǒng)來分析和實現。讀完你會得到一個明確判斷四足跟隨不是算法競賽而是系統(tǒng)工程決定演示成敗的往往不是某一個模型的先進程度而是你有沒有把整條鏈路的時序、坐標、狀態(tài)和異常處理梳理清楚。1. 這篇文章真正要解決的問題如果你在搜索引擎里查“四足機器人 跟隨功能”會看到大量炫酷的展示視頻。機器人背著一個深度相機靈活地跟著操作員在草地、樓梯和室內穿行。這些視頻看多了容易讓人產生一個誤解跟隨功能是成熟的開箱即用能力我們只需要調一個模型再部署上去就行。真實情況要復雜得多。自研四足本體的跟隨功能演示表面上是做一個“追人”的機器人本質上包含四個獨立問題如何在機器人動態(tài)運動的情況下穩(wěn)定識別出“目標是誰”如何在目標走出畫面、轉身、被遮擋時仍然維持身份跟蹤如何把視覺給出的“像素框”換算成機器人坐標系里可以執(zhí)行的“距離和方位”如何把“距離和方位”轉換成四足機器人能穩(wěn)定執(zhí)行的速度指令同時不破壞步態(tài)平衡。四足和輪式機器人相比跟隨難度更高。輪式機器人底盤穩(wěn)定相機晃動小運動學模型簡單四足機器人走路時身體本身就在周期性地起伏、俯仰和橫滾哪怕站定時相機也會因為髖關節(jié)輕微震動而出現畫面抖動。與此同時四足的轉向更接近“先減速、后轉身、再加速”如果控制策略不及時響應目標位置變化機器人就會走出一個難看的弧線甚至因為轉向角速度過大導致傾覆風險。因此這篇文章的核心目標讀者有兩類正在用自研四足本體做巡檢、安防、陪伴演示的開發(fā)者希望把跟隨做成一個可穩(wěn)定復現的功能模塊準備從單一運動控制轉向“感知運動一體化”開發(fā)的機器人愛好者想理解一個完整機器人應用是如何組織代碼、狀態(tài)和數據的。文中代碼以結構演示與工程思路為主視覺模型參數和機器人結構參數都需要替換成你自己的實際數據但系統(tǒng)的拆解方法、狀態(tài)機設計、排錯路徑可以直接復用。2. 概念解析四足本體與跟隨功能的邊界在展開具體實現之前先把兩個容易混淆的概念講清楚。2.1 四足本體不是“機器人殼子”做四足開發(fā)的人常把“本體”和“整機”分開講。一次典型的自研四足本體至少包含這幾層機械層機身、四條腿、足端、關節(jié)連接件驅動層關節(jié)電機、驅動器、減速器、關節(jié)扭矩傳感器感知層IMU、關節(jié)編碼器、相機、激光雷達、超聲波模塊計算層運行運動控制和感知算法的主控板執(zhí)行層負責將速度指令變成腿足步態(tài)的步態(tài)控制器。跟隨功能會橫跨感知層到執(zhí)行層。很多團隊容易犯的錯是默認執(zhí)行層已經“穩(wěn)定到可用”然后把全部精力投入到感知模型訓練上。實際上四足本體的運動控制如果存在低頻抖動、步態(tài)切換遲滯或者速度響應非線性這些噪聲都會放大到感知端導致目標檢測框抖動甚至丟幀。2.2 跟隨功能在技術上包含什么從表現上看跟隨是“機器人始終和目標保持一段距離同時正面朝向目標并跟隨移動”。從算法實現上可以拆成更細的功能鏈人員檢測在圖像中找到人輸出邊界框目標鎖定在多個人出現時決定跟隨哪個目標跟蹤給目標分配一個唯一ID在連續(xù)幀之間維持身份深度映射獲得目標相對機器人的三維坐標跟隨規(guī)劃將目標坐標映射為線速度與角速度安全控制檢測近距障礙限制速度處理丟失后的恢復動作。不是每個跟隨系統(tǒng)都需要跑一個復雜的大模型。簡單場景可以只檢測“人腿”如果用到機載端到端模型則可以輸出人體全身框更復雜的會結合點云聚類和多目標跟蹤。選擇哪一種取決于你的平臺算力和演示場景的復雜度。2.3 四足跟隨與輪式跟隨最大的差異在哪里可以比較這樣一組場景一個輪式機器人底盤在跟人一個四足機器人本體在跟人。輪式機器人做跟隨主要是“車體坐標與目標坐標間的平面跟蹤問題”底盤運動模型相對線性控制頻率可以到50Hz到100Hz平滑輸出。四足機器人做跟隨需要同時處理相機外參隨身體姿態(tài)實時變化如果控制周期是100Hz那么每20毫秒相機的俯仰角都可能有幾度的變化四足的橫移能力弱跟隨主要靠轉向與前進來實現因此運動模型有很強的橫向約束步態(tài)切換從站立到小跑從直行到轉彎需要時間不能要求控制指令瞬時到達。這個差異決定了四足跟隨系統(tǒng)的調試方法絕對不能照搬輪式移動機器人。最好的辦法是先把控制問題簡化為兩步先修正身體姿態(tài)影響把目標坐標變換到水平投影平面再進行二維平面上的跟隨控制。3. 系統(tǒng)方案設計與軟件架構無論硬件多么復雜軟件上我都建議先把系統(tǒng)拆成四個模塊。這里有一張功能鏈路表先總覽再逐層拆解模塊主要任務典型數據流關鍵輸出感知采集獲取圖像與深度相機 → 機載處理單元彩色圖、深度圖、位姿目標感知檢測并跟蹤目標彩色圖 → 檢測器 → 跟蹤器目標邊界框、跟蹤ID坐標映射把目標位置換算到機器人坐標系邊界框 深度 外參 → 坐標變換目標相對機器人的距離與角度運動控制將目標位置轉化為速度指令坐標 → 控制器 → 運動底盤協(xié)議期望線速度、角速度、狀態(tài)切換指令3.1 工程上的推薦做法狀態(tài)機先行很多初學者會直接寫一個大循環(huán)讀圖像 → 目標檢測 → 計算誤差 → PID輸出。這樣做在空場地慢速演示時可能沒問題一旦遇到人轉身、走出畫面整個系統(tǒng)就會失去明確的行為。更穩(wěn)的做法是先用狀態(tài)機把機器人的行為歸一化先定義清楚狀態(tài)再寫功能SEARCH搜尋 視野中沒有目標機器人原地小角度旋轉搜索 TRACK跟蹤 目標穩(wěn)定出現在視野中計算速度跟隨 LOST丟失 目標曾經存在但連續(xù)N幀找不回減速并進入搜索 RECOVERY接管恢復 人重新出現后重新鎖定 STOP停止 近距離障礙觸發(fā)急?；虻却庸?。狀態(tài)機的好處有三個第一它把“有目標”“沒目標”“目標短暫消失”之間的行為區(qū)分開來避免機器人在丟失目標后仍然向前沖第二它讓日志和調試變得非常直觀你在看現場演示時一眼就知道當前行為是哪一層引起的第三它讓不同開發(fā)者的模塊可以并行工作感知組負責輸出目標控制組負責消費狀態(tài)。3.2 常用硬件方案與選型建議自研四足本體的算力選擇常見三類入門樹莓派或者Jetson Nano級別適合只跑輕量人腿檢測和簡單跟蹤中端Jetson Orin Nano / Orin NX 級別可以跑YOLO系列目標檢測和DeepSORT類跟蹤高端Jetson AGX Orin / 搭配獨立顯卡的工控機適合加載點云和端到端模型。關于感知傳感器入門演示推薦RGBD深度相機。原因很多最關鍵是“深度直接可用”四足跟隨最需要的距離信息可以直接從深度圖讀取而不必依賴單目深度估計模型。無論選擇什么相機品牌有一點必須做到拿到相機后先做內參標定并且確認深度圖和彩色圖時間戳對齊。如果深度和彩色圖像沒有對齊檢測框在深度圖上取到的距離會是錯的。位置差半米以內在視覺上可能看不出來在控制上卻可能造成明顯的忽遠忽近。激光雷達不是四足隨從的首選方案。原因在于四足本體在行走時雷達的高度和俯仰會周期性變化低線雷達對地面和墻體的反射噪聲很大。環(huán)境缺乏特征時還可能無法提供可靠的點云。激光雷達可以做近距安全防護但作為“跟隨目標感知”的主傳感器體驗不如RGBD相機來得直接。4. 感知鏈路設計從像素到目標位置這是跟隨功能里最難、也最值得花時間的部分。我們一步步拆。4.1 使用目標檢測鎖定行人目標檢測的任務是“在每一幀圖像中找到人的邊界框”。實際開發(fā)中通常有兩種選擇全身檢測用YOLO系列或更輕量的目標檢測模型輸出行人的全身矩形框。模型的泛化能力好對穿裙子和背包的行人都能檢測。腿部檢測專門針對腿部訓練檢測器輸出的框更小適合機器人視覺高度在膝蓋高度附近、只能拍到腿的場景。在空間開闊、相機安裝高度高于1米的場景優(yōu)先使用全身檢測因為人體特征更明顯。在較矮的四足平臺上如果相機高度低于0.5米全身檢測器會頻繁漏檢這時候就得考慮“腿部檢測 高度假定”的思路。檢測模型不能只跑一次就丟。僅靠單幀檢測結果目標框會有隨機抖動還可能出現同一人來回閃爍ID的情況因此必須引入目標跟蹤。4.2 目標跟蹤讓跟隨有“記憶”目標跟蹤在視覺模塊里要解決的是序列化問題。你可以選擇經典算法卡爾曼濾波 匈牙利匹配視覺上就體現為DeepSORT或者更簡單的SORT。也可以用現代的跟蹤器但核心邏輯一致對每一幀檢測到的目標框生成特征將當前幀的檢測框與上一幀已有的跟蹤軌跡做匹配匹配成功則更新軌跡連續(xù)多幀未匹配成功的軌跡標記為丟失。這里特別強調一點如果你只有單目相機、沒有做重識別ReID跨長時間遮擋后的目標身份恢復基本是不可能的。所以演示設計里要避免讓人走出畫面超過幾秒?？梢栽谝曈X層面增加目標特征也可以在狀態(tài)機層面增加“丟失后原地360度緩慢搜索”的行為機制。4.3 確定要跟隨誰跟隨系統(tǒng)的默認設定通常是“跟隨第一個被鎖定的目標”。更貼近實際演示的一種策略是進入跟隨模式后檢測畫面中面積最大、靠近畫面中央且持續(xù)數幀的人作為目標。當目標丟失需要重新鎖定時再執(zhí)行同樣的策略。為了避免現場出現誤跟情況推薦保留一個手動鎖定機制。例如遙控器上的一個按鍵作為“鎖定當前視野內目標”的觸發(fā)或者通過語音/視覺界面選擇。在自研演示階段最牢固的方法是使用遙控器的目標鎖定鍵不要完全依賴算法自動判斷尤其是演示環(huán)境可能站著多名觀眾時。4.4 坐標映射像素坐標到機器人坐標當目標被跟蹤后要通過傳感器的內外參數完成一次坐標變換。設目標框中心點的像素坐標為 (u, v)。需要做三步根據相機內參把 (u, v, depth) 反投影到相機坐標系下的三維點根據相機外參將相機坐標系下的點轉換到機體坐標系根據機器人當前姿態(tài)將機體坐標進一步投影到水平面或者轉換為以機身為原點的二維距離。核心偽代碼如下。# 偽代碼從像素坐標 深度值計算目標相對機器人的位置 import numpy as np def pixel_to_camera_point(u, v, depth, camera_matrix): fx camera_matrix[0, 0] fy camera_matrix[1, 1] cx camera_matrix[0, 2] cy camera_matrix[1, 2] # 針孔模型逆變換 z float(depth) if z 0: return None x (u - cx) * z / fx y (v - cy) * z / fy return np.array([x, y, z]) def camera_point_to_body(point_cam, R_cam_body, t_cam_body): # R_cam_body: 相機坐標系到機體坐標系的旋轉矩陣 # t_cam_body: 平移向量 point_body R_cam_body point_cam t_cam_body return point_body邊界框的中心并不一定是人身體的中心因為人的身體有寬度框的中心可能落在兩臂之間而非背部正中。如果要提高精度可以在框的下邊緣取底部中心點并結合人的典型高度來估計一個腳底附近的三維點因為四足跟隨要的“跟誰”本質是“跟到目標腳下/身后一個穩(wěn)定距離”取底部中心比取框中心更可靠。4.5 相機外參的實時補償四足行走帶來的姿態(tài)變化會讓相機外參不再是固定值。如果機器人運動較慢、演示場景平坦可以用固定外參近似。但一旦要做上下坡或大步態(tài)行走需要引入IMU數據實時補償相機位姿把圖像坐標系先投影到機體慣性系再做平面上控制。否則機器人低頭時相機視野向下目標會突然“跑到”很近的位置導致控制器誤以為需要急停。有一個判斷辦法在機器人原地踏步時觀察跟蹤畫面的目標位置穩(wěn)定性。如果目標檢測框沒有抖動但機器人自身姿態(tài)在變而跟隨的距離估算波動很大那大概率就是相機外參沒有同步補償機體姿態(tài)。5. 跟隨運動控制不讓機器人看起來“傻”跟隨運動控制是功能演示觀感最直接的模塊。這里要做的事有兩件決定機器人該多快前進決定機器人該轉多大的角度?？刂撇呗圆恍枰珡碗s分階段處理即可。5.1 縱向速度控制機器人對目標的期望距離需要在配置中給出。隨從速度按距離誤差分三段距離誤差較大時速度快速上升并設置限幅避免加速過猛導致四足失穩(wěn)接近目標后進入平滑速度段按PID或簡單的比例控制線性收斂距離小于安全距離時不前進甚至微后退。一種可行的數學模型是分段函數# 偽代碼縱向跟隨速度計算 def compute_linear_speed(distance, target_distance): error distance - target_distance if distance MIN_SAFE_DISTANCE: return 0.0 if abs(error) 0.1: return 0.0 speed K_V * error # 限幅 speed max(-MAX_BACK_SPEED, min(MAX_FORWARD_SPEED, speed)) return speed線性增益本身應當避免過大的值速度在跟隨時保持在小跑范圍內即可讓機器人呈現“平穩(wěn)跟人”而不是“猛撲式跟人”的觀感。5.2 轉向控制跟隨中目標不可能永遠正對機器人。所以需要在水平面上計算目標相對機器人正前方的夾角控制機器人轉向目標。另一種可選做法是“位置—角度混合控制”讓機器人始終正對目標用角速度消除角度誤差。一個比較通用的角度控制代碼如下# 偽代碼轉向角速度計算目標相對機體的夾角 yaw_error def compute_angular_speed(yaw_error): # yaw_error 單位弧度 # 設置死區(qū)避免頻繁擺動 if abs(yaw_error) ANGULAR_DEAD_ZONE: return 0.0 omega K_W * yaw_error omega max(-MAX_ANGULAR_SPEED, min(MAX_ANGULAR_SPEED, omega)) return omega先大幅轉向快速對準再小角度緩慢微調演示過程中就會比較自然。5.3 安全控制是演示成敗的生命線演示場合比實驗室更容易出問題。操作員后退、觀眾橫穿、機器人盲區(qū)有臺階任何一個意外都可能損壞設備。因此安全模塊必須有獨立優(yōu)先級它不跟隨目標而時刻監(jiān)視機器人與最近障礙物的距離。建議做兩條獨立的安全邏輯近距急停RGBD相機的深度圖與傳感器近距檢測共同組成防護當正前方一定范圍內出現未知物體時無論目標狀態(tài)如何機器人速度置零并進入STOP狀態(tài)。速度限幅跟隨過程中線速度和角速度都不能超過本體的穩(wěn)定性上限。這里的上限不能用輪式經驗拍腦袋需要結合本體的步態(tài)表與實測速度來確定。安全控制代碼應獨立于目標跟蹤邏輯優(yōu)先級最高。不依賴視覺目標判斷因為視覺檢測可能存在漏檢和延遲。6. 完整參考實現ROS2風格的感知與控制節(jié)點這里給出一個參考實現模塊劃分遵循“目標檢測 目標跟蹤 坐標映射 控制輸出”。由于不同團隊的視覺模型權重和四足底盤控制協(xié)議差別大我會把模型加載與底盤驅動留成接口重點演示數據結構和邏輯流程。6.1 行人檢測與跟蹤模塊下面的Python代碼使用OpenCV的DNN模塊以及一個簡化版的IOU跟蹤器。配合實際模型可以替換成YOLO官方的導出模型。# 文件路徑follow_demo/perception/tracker.py 簡化目標檢測 跟蹤模塊參考實現。 實際部署建議替換成 - 檢測器YOLO系列或移動端輕量檢測網絡 - 跟蹤器參考 SORT / DeepSORT 原理實現 這里僅演示完整鏈路。 class Detection: def __init__(self, bbox, score, class_id): # bbox 格式: (x1, y1, x2, y2) self.bbox bbox self.score score self.class_id class_id class Track: def __init__(self, track_id, bbox): self.track_id track_id self.bbox bbox self.last_seen 0 self.hit_streak 0 class SimpleTracker: def __init__(self, max_lost_frames30): self.tracks [] self.next_id 0 self.max_lost_frames max_lost_frames def update(self, detections, frame_index): updated_tracks [] # 這里簡化為“取最大面積檢測框作為目標” # 實際多目標場景建議使用 IoU 匹配 卡爾曼濾波 if len(detections) 0: max_area 0 best_det None for det in detections: x1, y1, x2, y2 det.bbox area (x2 - x1) * (y2 - y1) if area max_area: max_area area best_det det if best_det is not None: if self.tracks: t self.tracks[0] t.bbox best_det.bbox t.hit_streak 1 t.last_seen frame_index updated_tracks.append(t) else: nt Track(self.next_id, best_det.bbox) nt.last_seen frame_index nt.hit_streak 1 self.next_id 1 updated_tracks.append(nt) self.tracks updated_tracks return self.tracks這里的跟蹤器退化成了“取最大目標”但結構上預留了替換位置。真實項目里跟蹤目標時必須保存目標特征否則當畫面里出現一個更大的目標時機器人可能會突然切換跟隨對象。6.2 坐標映射與跟隨控制器下面的模塊演示如何由檢測框與深度圖產生控制指令。它接收一個深度圖像、檢測框和相機參數輸出距離、夾角以及速度指令。# 文件路徑follow_demo/core/follow_controller.py 跟隨控制器參考實現。 輸入目標框 對齊后的深度圖。 輸出線速度和角速度。 import numpy as np class FollowController: def __init__(self, camera_matrix, dist_coeffsNone): self.K camera_matrix self.target_distance 1.2 # 期望跟隨距離單位米按實際調整 self.min_safe_distance 0.5 # 安全距離閾值 self.k_v 0.8 # 速度增益需要實測標定 self.k_w 1.5 # 轉向增益需要實測標定 self.max_v 0.5 # 最大線速度 m/s self.max_w 0.8 # 最大角速度 rad/s def detect_to_center(self, bbox): x1, y1, x2, y2 bbox u (x1 x2) / 2.0 v (y1 y2) / 2.0 return u, v def get_depth_at_bbox_bottom_center(self, depth_image, bbox, safe_radius5): x1, y1, x2, y2 bbox cx int((x1 x2) / 2.0) cy int(y2) # 取檢測框底部中心接近人體腳底平面 h, w depth_image.shape[:2] cy min(max(cy, 0), h - 1) cx min(max(cx, 0), w - 1) region depth_image[max(0, cy-safe_radius):cysafe_radius1, max(0, cx-safe_radius):cxsafe_radius1] valid region[(region 0.1) (region 8.0)] if len(valid) 0: return None return float(np.median(valid)) def compute_control(self, target_point_body): # target_point_body: 目標在機體坐標系下的 XYZ x target_point_body[0] y target_point_body[1] distance float(np.sqrt(x * x y * y)) yaw_error float(np.arctan2(y, x)) if distance self.min_safe_distance: return 0.0, 0.0, distance, yaw_error # 縱向速度距離誤差越大速度越大但限制最高速度 error distance - self.target_distance linear self.k_v * error linear max(-0.2, min(self.max_v, linear)) # 轉向速度消除角度偏差 angular self.k_w * yaw_error angular max(-self.max_w, min(self.max_w, angular)) return linear, angular, distance, yaw_error這段參考代碼的關鍵之處是取檢測框底部中心作為目標測距點并在深度圖上取一個半徑區(qū)域的中位數而不是單點深度。因為單點深度往往落在目標邊緣或背景上導致測距跳變中位數可以顯著提升穩(wěn)定性。6.3 狀態(tài)機與主循環(huán)參考主循環(huán)按固定幀率運行把感知結果送給狀態(tài)機狀態(tài)機決定行為。需要將相機幀率與運動控制頻率解耦相機允許30幀控制輸出可限制在20Hz到30Hz。# 文件路徑follow_demo/main_loop.py 主循環(huán)參考實現精簡結構。 import cv2 import numpy as np from perception.tracker import SimpleTracker from core.follow_controller import FollowController class FollowState: SEARCH SEARCH TRACK TRACK LOST LOST class FollowDemo: def __init__(self, camera_matrix): self.tracker SimpleTracker() self.controller FollowController(camera_matrix) self.state FollowState.SEARCH self.lost_frames 0 def run_on_frame(self, color_image, depth_image, frame_index): # 1. 檢測目標這邊留占位替換為真實模型輸出 detections fake_detect_person(color_image) # 2. 更新跟蹤器 tracks self.tracker.update(detections, frame_index) if len(tracks) 0: self.lost_frames 1 if self.lost_frames 30: self.state FollowState.SEARCH return 0.0, 0.0, self.state else: self.lost_frames 0 track tracks[0] bbox [int(v) for v in track.bbox] # 3. 計算目標三維坐標 u, v self.controller.detect_to_center(bbox) depth self.controller.get_depth_at_bbox_bottom_center( depth_image, bbox) if depth is None: self.state FollowState.LOST return 0.0, 0.0, self.state # 這里省略像素反投影的內外參矩陣計算 # target_point_body depth camera_matrix 逆變換 外參 point_cam np.array([0.0, 0.0, depth]) point_body point_cam # 占位需要替換為真實坐標變換結果 # 4. 計算控制量 linear, angular, distance, yaw_error \ self.controller.compute_control(point_body) return linear, angular, self.state def fake_detect_person(image): # 占位函數實際應加載檢測模型 return []這個主循環(huán)仍然省略了坐標變換細節(jié)。在真實工程中強烈建議把“像素轉機體坐標”單獨抽成一個模塊輸出可視化效果方便調試。6.4 啟動腳本與參數配置示例# 文件路徑run_follow.sh #!/bin/bash # 先啟動相機驅動 ros2 launch realsense2_camera rs_launch.py sleep 5 # 啟動跟隨主程序以ROS2為例 ros2 run follow_demo follow_node --ros-args --params-file follow_params.yaml# 文件路徑follow_params.yaml follow_demo: ros__parameters: target_distance: 1.2 min_safe_distance: 0.5 linear_gain: 0.8 angular_gain: 1.5 max_linear_speed: 0.5 max_angular_speed: 0.8 use_imu_compensation: true配置文件的優(yōu)勢在于調參不用重新編譯。實際調試時可以把一組參數記錄下來體驗不同參數下機器人跟隨時“自然感”與“穩(wěn)定性”的差異。7. 運行調試與效果驗證7.1 先做仿真或小底盤驗證很多團隊把視覺模型部署到四足上的第一天就直接在真機上加步伐測試結果出了問題也分不清是視覺誤差還是控制振蕩。更穩(wěn)健的調試路徑是分三步走第一步把同一套感知代碼跑在一臺輪式小車上。如果輪式小車就能穩(wěn)定跟隨說明視覺鏈路基本可靠問題大概率出在四足運動的姿態(tài)干擾上。第二步把同一套視覺控制和四足本體放在靜止狀態(tài)由人緩慢繞機器人行走只測試轉向跟隨不測試前進跟隨。如果轉向控制也有抖動那就和步態(tài)關系不大需要調增益或處理測量噪聲。第三步才加入前后移動和小跑步態(tài)做全狀態(tài)跟隨。7.2 調試可視化開發(fā)階段的調試可視化至關重要。調試畫面必須顯示以下信息目標的跟蹤框與跟蹤ID目標框底部中心測距結果目標相對機體的距離與夾角機器人當前狀態(tài)SEARCH/TRACK/LOST當前發(fā)布的線速度與角速度值。可以使用OpenCV直接把信息繪制到畫面中。一張實時調試圖能省下大量猜測時間?？吹疆嬅嬷芯嚯x數值在10厘米內跳變說明測距存在問題看到線速度在控制周期內忽正忽負說明控制增益過大或深度噪聲太大。7.3 功能演示檢查清單一次順利的演示背后需要進行多次走場測試。以下檢查清單建議在演示開始前逐項確認相機視野內能檢測到操作員且檢測框覆蓋完整人體至少覆蓋軀干與腿部跟隨系統(tǒng)默認鎖定的是指定操作員而不是旁邊的觀眾操作員向前走、停下、轉身、走出畫面再回來機器人行為符合狀態(tài)機預期操作員走過一段直線后直角轉彎機器人能流暢跟隨沒有嚴重的超調旋轉正前方有突然出現的障礙物時機器人能及時停止遙控斷開后機器人進入停止狀態(tài)而不是繼續(xù)前沖光照變化時目標檢測仍能維持基本穩(wěn)定提前準備好強光或逆光場景的應對方案。7.4 量化評估與驗收可以簡單記錄三組數據來評價跟隨效果直線段平均跟隨距離誤差一般控制在10到20厘米以內轉彎時目標角度誤差的最大值控制在20度以內算可用目標丟失后的重捕獲時間現場演示盡量控制在3秒以內。這些指標不用追求極致但要能復現。當你在多輪演示中都得到接近的數值系統(tǒng)才具備交付給第三方演示的條件。8. 常見問題與排查思路下面整理自研四足跟隨里最高頻的幾類問題。問題現象可能原因排查方式解決方案人對準后機器人仍然忽遠忽近深度圖在目標邊緣取到背景距離或控制增益過高查看深度可視化在檢測框底部中心區(qū)域打印原始深度值測距使用區(qū)域中位數降低縱向速度增益并增加濾波畫面中有多人時機器人頻繁換目標跟蹤器沒有保留外觀特征最大面積目標切換導致ID跳變查看跟蹤ID是否在換人時改變引入重識別特征或鎖定按鈕固定目標ID機器人后仰起步時目標位置突變身體姿態(tài)變化未補償相機外參打印IMU的姿態(tài)角同步觀察目標坐標跳變將相機坐標經機體姿態(tài)旋轉到世界水平坐標系跟隨速度越快檢測框抖動越嚴重運動模糊或相機幀率不足降低速度復測檢查快門與曝光提升幀率縮短曝光必要時加機械減振機器人轉彎時人體目標丟失目標在畫面邊緣轉身角速度過大記錄轉向過程中目標框被截斷的幀數限制最大角速度減少目標走出畫面的概率目標檢測偶爾漏檢導致機器人停止單幀漏檢直接觸發(fā)狀態(tài)機進LOST看漏檢是否連續(xù)出現增加漏檢容忍幀數結合目標運動預測維持跟蹤激光雷達或超聲波誤觸發(fā)急停距離閾值過緊在坡道上誤檢測地面查看近距傳感器原始距離檢查安裝角根據安裝高度調整角度設置合理的閾值演示現場逆光人物臉部過曝相機自動曝光導致目標特征變暗開啟畫面曝光曲線調試固定曝光參數關閉過強的自動曝光這些問題的共同規(guī)律是多數不穩(wěn)定現象都不是某一個算法單獨造成的而是模塊之間數據不對稱造成的。所以排查時不要只在感知代碼里反復加濾波先從坐標變換、時間戳和標定參數是否一致查起。9. 最佳實踐與工程建議9.1 先讓系統(tǒng)“減速可復現”再談“智能”在做四足跟隨功能時最容易犯的錯誤是過早加入復雜的深度估計、端到端模型或多目標跟蹤。自研本體的算力有限現場環(huán)境的不可控因素又多。第一次跑通演示寧愿讓機器人速度比較保守也要保證行為可預測。等狀態(tài)機流程完全穩(wěn)定后再逐步提高速度和目標復雜度。9.2 把目標鎖定權交給操作員在演示中“抓住誰”和“怎么跟上”應該分成兩個權限。自動檢測負責候選遙控器選擇或確認負責最終決策。這種方式能避免在現場出現目標更換的尷尬情況。一個好的交互設計是操作員在畫面中央附近按下確認鍵機器人就把當前正前方最近的人作為唯一目標鎖住。9.3 記錄數據包是調優(yōu)的起點每次現場測試保存三個東西原始彩色圖與深度圖記錄每幀的檢測框、目標ID、深度值、坐標變換結果機器人實際控制指令與IMU姿態(tài)。只有在出問題時能把現場數據完整回放你才能分析出到底是感知誤判還是控制相位滯后。這部分前期工作帶來的收益會在真機聯(lián)調階段體現得非常明顯。9.4 重視標定與時間同步自研四足的機身結構如果發(fā)生過拆裝或碰撞相機與機體的外參就可能改變。建議在每次演示前做一次快速標定在機器人前方放置標定板或二維碼讓機器人保持站立姿態(tài)通過視覺特征確認相機位姿與預設外參是否一致。另外要保證彩色圖像與深度圖像時間戳接近否則運動物體的深度和彩色幀錯位會產生邊緣誤差。9.5 設計安全兜底最后也是最重要的真機測試必須設置急停按鈕和軟件看門狗。突發(fā)情況下操作員要能一鍵讓機器人進入停止狀態(tài)。開發(fā)階段不要越過安全環(huán)節(jié)直接調試高速跟隨保護設備與人員永遠是第一優(yōu)先級。10. 總結與后續(xù)學習方向做一次自研四足本體的跟隨功能演示真正要跨過去的坎并不是“識別一個人”。識別一個人的模型已經非常成熟真正的坎在于目標檢測的結果如何穩(wěn)定變成機器人能執(zhí)行的速度指令機器人運動起來之后感知數據如何保持可信以及目標丟失之后系統(tǒng)如何選擇恰當行為。這篇文章拆解了一條完整鏈路從四足本體的特點出發(fā)做了系統(tǒng)架構規(guī)劃講解了感知、坐標映射、運動控制和狀態(tài)機設計并給出了一套可運行的參考代碼。如果你要把它應用到自己的四足機器人上接下來值得深入的方向有三個第一把目標跟蹤的魯棒性做好引入多目標跟蹤與外觀特征讓機器人能夠長時間記住目標身份。第二把運動控制與步態(tài)更緊密地耦合起來根據目標狀態(tài)選擇不同的步態(tài)或速度檔位而不是讓底盤控制層自行切換。第三把多傳感器融合做扎實用近距雷達和深度相機互補降低視覺遮擋和光照變化對跟隨系統(tǒng)的影響。建議先基于本文的狀態(tài)機跑通最小閉環(huán)用自己的四足本體錄幾段不同場景的真實數據之后再圍繞調試視頻逐步優(yōu)化。穩(wěn)定、可復現、可排錯的跟隨系統(tǒng)比只能跑一次高難路徑的演示更有長期價值。