簡(jiǎn)介面向多機(jī)器人編隊(duì)控制場(chǎng)景這份MATLAB資源實(shí)現(xiàn)了經(jīng)典的領(lǐng)航跟隨法適用于機(jī)器人控制、自動(dòng)化等方向的初學(xué)者理解編隊(duì)原理也適合研究者作為快速驗(yàn)證的基礎(chǔ)模板。該算法通過(guò)指定領(lǐng)航者運(yùn)動(dòng)軌跡控制跟隨者保持相對(duì)距離與角度從而形成穩(wěn)定編隊(duì)。資源包內(nèi)只有1個(gè)m文件整體大小約816B代碼量精簡(jiǎn)方便逐步閱讀和調(diào)試能夠直觀展示矩陣位姿更新、誤差計(jì)算等關(guān)鍵編程思路便于在此基礎(chǔ)上擴(kuò)展隊(duì)形切換、避障等高級(jí)功能。已有1073人瀏覽學(xué)習(xí)對(duì)于入門(mén)級(jí)示例來(lái)說(shuō)具有一定參考價(jià)值。整體而言這份代碼既是一份可運(yùn)行的編隊(duì)控制樣例也是一份便于拆解的MATLAB教學(xué)素材可幫助使用者通過(guò)實(shí)際仿真理解多機(jī)器人協(xié)同控制的實(shí)現(xiàn)路徑。1. 領(lǐng)航跟隨法多機(jī)器人編隊(duì)里最容易被低估的控制范式做多機(jī)器人編隊(duì)的人大多從一致性Consensus或虛擬結(jié)構(gòu)Virtual Structure入手但真正在實(shí)驗(yàn)室里跑過(guò)幾臺(tái)差速小車(chē)之后你會(huì)發(fā)現(xiàn)領(lǐng)航跟隨Leader-Follower才是工程上最省事、也最坑的方案。坑不在理論而在跟隨者對(duì)領(lǐng)航者位姿的估計(jì)怎么建模、控制器增益怎么匹配輪速以及參考點(diǎn)選在車(chē)體哪個(gè)位置。MATLAB 里跑仿真只需要一份l_f.m和對(duì)應(yīng)的.rar數(shù)據(jù)文件但要把曲線調(diào)穩(wěn)、把兩車(chē)或多車(chē)編隊(duì)推廣到任意隊(duì)形你需要把相對(duì)運(yùn)動(dòng)學(xué)方程掰開(kāi)揉碎。本文就用這份經(jīng)典的l_f.rar源碼包把領(lǐng)航跟隨法的模型推導(dǎo)、控制器設(shè)計(jì)、參數(shù)整定和擴(kuò)展方式完整過(guò)一遍適合正在做多機(jī)器人編隊(duì)仿真、準(zhǔn)備投機(jī)器人課程設(shè)計(jì)或想把編隊(duì)算法遷移到 ROS 的工程師。2. 領(lǐng)航跟隨法的運(yùn)動(dòng)學(xué)模型與參考點(diǎn)選取2.1 為什么用相對(duì)位姿而不是全局坐標(biāo)多機(jī)器人編隊(duì)常用的全局坐標(biāo)控制器需要每臺(tái)機(jī)器人知道自己的絕對(duì)位姿一旦 GPS 或全局視覺(jué)失效整個(gè)編隊(duì)就散了。領(lǐng)航跟隨法的核心思想是跟隨者只需要感知領(lǐng)航者的相對(duì)距離和相對(duì)角度就能維持隊(duì)形。這正符合實(shí)際機(jī)器人搭載的傳感器特點(diǎn)——激光雷達(dá)、UWB、視覺(jué)標(biāo)簽給的都是相對(duì)測(cè)量量而不是全局坐標(biāo)。在l_f.m中模型的建立基于一個(gè)經(jīng)典的兩輪差速底盤(pán)假設(shè)左右輪速度分別為 (v_L) 和 (v_R)車(chē)體線速度 (v (v_L v_R)/2)角速度 (\omega (v_R - v_L)/L)其中 (L) 是輪距。這個(gè)假設(shè)在不同資料里可能寫(xiě)成 (v (v_Rv_L)/2)但符號(hào)定義不影響邏輯關(guān)鍵是后續(xù)控制器設(shè)計(jì)時(shí)要把線速度和角速度映射回左右輪速。2.2 領(lǐng)航者與跟隨者的相對(duì)運(yùn)動(dòng)學(xué)方程設(shè)領(lǐng)航者位姿為 ([x_L, y_L, \theta_L]^T)跟隨者位姿為 ([x_F, y_F, \theta_F]^T)。定義相對(duì)距離 (\rho) 和相對(duì)角度 (\alpha)[ \rho \sqrt{(x_L - x_F)^2 (y_L - y_F)^2} ][ \alpha \text{atan2}(y_L - y_F, x_L - x_F) - \theta_F ]注意這里的 (\alpha) 是跟隨者朝向與領(lǐng)航者方向之間的夾角不是全局坐標(biāo)系下的絕對(duì)角度。這個(gè)定義直接決定了控制律的形式。對(duì)上述兩式求導(dǎo)可以得到經(jīng)典的領(lǐng)航跟隨運(yùn)動(dòng)學(xué)模型[ \dot{\rho} v_L \cos(\theta_L - \theta_F - \alpha) - v_F \cos\alpha ][ \dot{\alpha} \frac{1}{\rho} \left[ v_L \sin(\theta_L - \theta_F - \alpha) v_F \sin\alpha \right] - \omega_F \omega_L ]這個(gè)方程組的意義在于距離的變化率由領(lǐng)航者速度在相對(duì)方向上的投影減去跟隨者速度在相對(duì)方向上的投影決定角度的變化率則由兩者角速度及相對(duì)幾何關(guān)系共同決定。2.3 (\rho)-(\alpha) 參考點(diǎn)模型與 (\rho)-(\varphi) 模型的差異l_f.m中實(shí)際使用的是 (\rho-\alpha) 模型即把參考點(diǎn)放在跟隨者車(chē)體中心。另一種常見(jiàn)的 (\rho-\varphi) 模型把參考點(diǎn)放在跟隨者前方某處兩者的區(qū)別是特征(\rho-\alpha) 模型(\rho-\varphi) 模型參考點(diǎn)位置跟隨者車(chē)體中心跟隨者前方 (d) 處狀態(tài)變量距離 (\rho)、角度 (\alpha)距離 (\rho)、角度 (\varphi)控制器復(fù)雜度需要兩路解耦控制前饋?lái)?xiàng)更多適用場(chǎng)景低速、編隊(duì)隊(duì)形固定高速、避障頻繁我一般建議課程設(shè)計(jì)或驗(yàn)證性仿真用 (\rho-\alpha)因?yàn)榭刂破鹘Y(jié)構(gòu)簡(jiǎn)單參數(shù)少調(diào)參速度快。工程落地時(shí)如果車(chē)體前方裝有激光雷達(dá)(\rho-\varphi) 會(huì)更自然因?yàn)槟銣y(cè)到的最近障礙物距離天然是前方參考點(diǎn)的距離。2.4 代碼中的關(guān)鍵參數(shù)定義在l_f.m開(kāi)頭大部分情況下能看到類(lèi)似這樣的結(jié)構(gòu)% 領(lǐng)航者初始位姿 [x, y, theta] leader_pose [0, 0, 0]; % 跟隨者初始位姿 follower_pose [0, -1.5, 0]; % 期望編隊(duì)相對(duì)距離和相對(duì)角度 rho_d 1.5; alpha_d 0; % 正前方編隊(duì) % 控制器增益 k_rho 1.2; k_alpha 2.0; % 仿真步長(zhǎng)與總時(shí)長(zhǎng) dt 0.02; T 20;這里rho_d 1.5表示期望兩車(chē)中心距 1.5 米alpha_d 0表示跟隨者在領(lǐng)航者正后方。如果你想做三角形編隊(duì)第三輛車(chē)和領(lǐng)航者之間的alpha_d應(yīng)該設(shè)為 (60^\circ) 或 (-60^\circ)具體取決于編隊(duì)開(kāi)口方向。3. 控制器設(shè)計(jì)與左右輪速映射3.1 反推法設(shè)計(jì)控制律(\rho-\alpha) 模型是一個(gè)典型的欠驅(qū)動(dòng)系統(tǒng)只有兩個(gè)控制輸入(v_F, \omega_F)但需要同時(shí)穩(wěn)定兩個(gè)狀態(tài)(\rho, \alpha)。常規(guī)做法是用反推法設(shè)計(jì)[ v_F v_L \cos(\theta_L - \theta_F - \alpha) k_\rho (\rho - \rho_d) \cos\alpha ][ \omega_F \omega_L \frac{1}{\rho} \left[ v_L \sin(\theta_L - \theta_F - \alpha) v_F \sin\alpha \right] k_\alpha (\alpha - \alpha_d) ]其中 (k_\rho 0) 控制距離收斂速度(k_\alpha 0) 控制角度收斂速度。第一式的前半部分補(bǔ)償領(lǐng)航者運(yùn)動(dòng)對(duì)相對(duì)距離的影響后半部分是距離誤差的比例反饋。第二式同理前兩項(xiàng)是領(lǐng)航者角速度和相對(duì)角度變化的補(bǔ)償?shù)谌?xiàng)是角度誤差反饋。3.2 為什么增益要滿足 (\rho_d \cdot k_\alpha k_\rho)這個(gè)不等式很多人在調(diào)參時(shí)直接忽略但它是閉環(huán)系統(tǒng)局部穩(wěn)定的充分條件之一。直覺(jué)解釋是如果角度增益 (k_\alpha) 相對(duì)距離增益 (k_\rho) 太小跟隨者會(huì)優(yōu)先貼近距離而忽略方向?qū)е滤鼪_到領(lǐng)航者側(cè)面甚至前方最后繞著領(lǐng)航者轉(zhuǎn)圈。反過(guò)來(lái)如果 (k_\alpha) 太大車(chē)頭會(huì)劇烈擺動(dòng)距離還沒(méi)收斂角度已經(jīng)振蕩。在l_f.m里你可以直接驗(yàn)證這個(gè)規(guī)律把k_alpha從 2.0 改到 0.5保持k_rho 1.2觀察相對(duì)距離曲線會(huì)出現(xiàn)明顯的超調(diào)相對(duì)角度會(huì)在正負(fù)之間來(lái)回穿越。恢復(fù)k_alpha 2.0后兩條曲線大約在 5 秒內(nèi)平滑收斂。3.3 從角速度到左右輪速的映射仿真中控制器的輸出是 (v_F) 和 (\omega_F)但實(shí)際機(jī)器人或者 Simulink 模型里需要的是左右輪速。常見(jiàn)的映射函數(shù)function [vL, vR] wheel_velocities(v, omega, wheel_base) % v: 線速度, omega: 角速度, wheel_base: 輪距 vL v - omega * wheel_base / 2; vR v omega * wheel_base / 2; end把線速度和角速度分別加減半個(gè)輪距乘角速度就得到左右輪速。注意這里的符號(hào)約定如果 omega 為正表示逆時(shí)針旋轉(zhuǎn)那么左輪減速、右輪加速。如果你的 MATLAB 版本里機(jī)器人坐標(biāo)系方向不同把wheel_base / 2前面的符號(hào)換一下即可。3.4 領(lǐng)航者軌跡生成與速度輸入領(lǐng)航者的軌跡可以用時(shí)間參數(shù)化曲線定義也可用預(yù)存軌跡點(diǎn)插值。在l_f.m里常見(jiàn)做法是定義一個(gè)領(lǐng)航者速度函數(shù)% 領(lǐng)航者線速度和角速度隨時(shí)間變化 v_L 0.5; % 恒定線速度 0.5 m/s omega_L 0.1 * sin(0.5 * t); % 正弦變化的角速度 % 領(lǐng)航者位姿更新歐拉法 leader_pose(3) leader_pose(3) omega_L * dt; leader_pose(1) leader_pose(1) v_L * cos(leader_pose(3)) * dt; leader_pose(2) leader_pose(2) v_L * sin(leader_pose(3)) * dt;用正弦角速度的好處是它同時(shí)考驗(yàn)距離通道和角度通道的動(dòng)態(tài)響應(yīng)——直線編隊(duì)測(cè)試測(cè)不出角度控制器的真實(shí)品質(zhì)只有領(lǐng)頭軌跡彎曲時(shí)跟隨者才會(huì)暴露滯后問(wèn)題。3.5 仿真主循環(huán)中的狀態(tài)更新順序很多人仿真結(jié)果發(fā)散不是因?yàn)榭刂破麇e(cuò)誤而是狀態(tài)更新順序不對(duì)。正確的流程是for k 1:round(T/dt) t (k-1) * dt; % 1. 計(jì)算跟隨者期望控制量 [v_F, omega_F] leader_follower_controller(...); % 2. 映射到左右輪速 [vL, vR] wheel_velocities(v_F, omega_F, L); % 3. 實(shí)際速度可加電機(jī)延遲或噪聲 v_actual (vL vR) / 2; omega_actual (vR - vL) / L; % 4. 更新跟隨者位姿 follower_pose(3) follower_pose(3) omega_actual * dt; follower_pose(1) follower_pose(1) v_actual * cos(follower_pose(3)) * dt; follower_pose(2) follower_pose(2) v_actual * sin(follower_pose(3)) * dt; % 5. 更新領(lǐng)航者位姿 leader_pose(3) leader_pose(3) omega_actual * dt; leader_pose(1) leader_pose(1) v_actual * cos(leader_pose(3)) * dt; leader_pose(2) leader_pose(2) v_actual * sin(leader_pose(3)) * dt; end如果先更新跟隨者再用更新后的位姿去算控制量相當(dāng)于引入了一個(gè)額外的預(yù)測(cè)步這會(huì)讓閉環(huán)系統(tǒng)對(duì)增益變化更敏感曲線看起來(lái)抖動(dòng)更重。保持同步更新、用同一時(shí)刻狀態(tài)計(jì)算控制量是仿真和實(shí)機(jī)一致性的關(guān)鍵。4. 多機(jī)器人編隊(duì)擴(kuò)展從兩車(chē)到 N 車(chē)4.1 跟隨-跟隨級(jí)聯(lián)結(jié)構(gòu)與隊(duì)形矩陣兩車(chē)編隊(duì)穩(wěn)定之后擴(kuò)展成多車(chē)編隊(duì)最直接的方法是級(jí)聯(lián)結(jié)構(gòu)第三輛車(chē)跟隨第二輛車(chē)第二輛車(chē)跟隨第一輛車(chē)以此類(lèi)推。這種結(jié)構(gòu)的優(yōu)勢(shì)是傳感器部署簡(jiǎn)單每輛車(chē)只需要感知它的直接前驅(qū)。代價(jià)是隊(duì)形誤差會(huì)沿著鏈路累積越靠后的車(chē)位置偏差越大。隊(duì)形用期望距離向量 ([\rho_{i,d}, \alpha_{i,d}]) 矩陣描述每一行代表當(dāng)前車(chē)輛與其直接前驅(qū)的期望關(guān)系formation [ 1.5, 0; % 2號(hào)車(chē)距離1.5m正后方 1.5, 0; % 3號(hào)車(chē)距離1.5m正后方縱向鏈 ];矩陣的第一列是期望相對(duì)距離第二列是期望相對(duì)角度。修改第二列的alpha_d就能在同一個(gè)框架內(nèi)實(shí)現(xiàn)三角形、楔形、橫排等不同隊(duì)形。4.2 隊(duì)形切換控制編隊(duì)控制里切換隊(duì)形是最常見(jiàn)的需求比如無(wú)人機(jī)編隊(duì)或者地面車(chē)輛編隊(duì)在通過(guò)窄道時(shí)從橫隊(duì)切成縱隊(duì)。在l_f.m的擴(kuò)展版本里我一般用一個(gè)簡(jiǎn)單的狀態(tài)機(jī)實(shí)現(xiàn)formation_phase column; % 或 line, triangle switch formation_phase case column alpha_d 0; rho_d 1.5; case line alpha_d pi / 2; % 右側(cè)橫排 rho_d 1.5; case triangle alpha_d pi / 4; rho_d 1.8; end切換瞬間rho_d和alpha_d發(fā)生跳變控制器會(huì)自動(dòng)驅(qū)使跟隨者運(yùn)動(dòng)到新的期望位置。由于控制律是連續(xù)的隊(duì)形切換不需要停機(jī)或重啟動(dòng)只是過(guò)渡過(guò)程中相對(duì)距離和相對(duì)角度會(huì)出現(xiàn)短暫超調(diào)超調(diào)量取決于增益大小這也是驗(yàn)證編隊(duì)魯棒性的好場(chǎng)景。4.3 領(lǐng)航者速度突變時(shí)的編隊(duì)保持性測(cè)試只做直線勻速仿真無(wú)法暴露控制器的缺陷。我常用的驗(yàn)證方式是讓領(lǐng)航者做一個(gè)減速-急停-再加速的動(dòng)作序列如下if t 5 v_L 1.0; elseif t 8 v_L 0.3; elseif t 10 v_L 0; else v_L 0.8; end跟隨者會(huì)在 5 秒時(shí)因?yàn)榍败?chē)突然減速而出現(xiàn)距離縮小如果k_rho不夠大最小距離可能小于安全閾值。急停時(shí)角度誤差會(huì)先被拉大再逐漸收斂。這段測(cè)試能讓編隊(duì)控制的動(dòng)態(tài)特性一目了然——只看穩(wěn)態(tài)收斂曲線看不出系統(tǒng)的真實(shí)性能。5. 參數(shù)靈敏度分析與 PID 改型的邊界5.1 增益對(duì)跟蹤性能的影響以 (k_\rho) 和 (k_\alpha) 為核心做一次簡(jiǎn)單的網(wǎng)格掃描統(tǒng)計(jì)最大距離誤差k_rho_range 0.5:0.2:2.0; k_alpha_range 0.5:0.2:2.0; err_max zeros(length(k_rho_range), length(k_alpha_range)); for i 1:length(k_rho_range) for j 1:length(k_alpha_range) err_max(i,j) run_simulation(k_rho_range(i), k_alpha_range(j)); end end我跑過(guò)的典型結(jié)果是(k_\rho) 增大可以顯著減小距離誤差的峰值但當(dāng) (k_\rho \cdot d k_\alpha) 時(shí)角度誤差會(huì)振蕩加劇(k_\alpha) 增大可以加快角度收斂但會(huì)導(dǎo)致跟隨者橫向擺動(dòng)頻繁速度給入左右輪速后有明顯抖動(dòng)。5.2 控制器從反推法到 PID 的遷移反推控制律雖然穩(wěn)定但它依賴(lài)模型精確性。如果仿真中加入了電機(jī)延遲或輪徑誤差反推法的效果會(huì)下降。此時(shí)我更推薦把控制器拆成兩個(gè)獨(dú)立的 PIDerror_rho rho - rho_d; error_alpha wrapToPi(alpha - alpha_d); v_F rho_error_pid(error_rho); % 線速度 PID 對(duì)象 omega_F alpha_error_pid(error_alpha); % 角速度 PID 對(duì)象這種結(jié)構(gòu)的好處是解耦了距離控制和方向控制壞處是失去了前饋補(bǔ)償。領(lǐng)航者轉(zhuǎn)彎時(shí)跟隨者會(huì)明顯滯后尤其在高速轉(zhuǎn)彎場(chǎng)景下誤差會(huì)放大。如果一定要用 PID給距離環(huán)增加前饋?lái)?xiàng)即把領(lǐng)航者速度在相對(duì)方向上的投影v_L * cos(...)加到輸出上就能抵消大部分跟蹤誤差。5.3 輸入飽和與防積分飽和仿真階段很多人忽略控制量限幅實(shí)機(jī)卻逃不掉。左右輪速的物理上限決定了v_F和omega_F的輸出范圍v_F max(-1.0, min(1.0, v_F)); omega_F max(-0.8, min(0.8, omega_F));如果控制器輸出超過(guò)限幅值誤差積分項(xiàng)持續(xù)累積等到領(lǐng)航者速度恢復(fù)時(shí)跟隨者會(huì)沖過(guò)期望位置。PID 實(shí)現(xiàn)里要注意積分分離只在誤差絕對(duì)值小于閾值時(shí)啟用積分項(xiàng)。5.4 傳感器噪聲對(duì)編隊(duì)精度的影響l_f.m默認(rèn)是理想感知但實(shí)際傳感器的相對(duì)距離和相對(duì)角度都有測(cè)量噪聲。在距離測(cè)量上添加 0.01% 的高斯噪聲在角度測(cè)量上添加 0.5° 的高斯噪聲后編隊(duì)穩(wěn)態(tài)誤差會(huì)從接近零變成 ±0.02 米和 ±0.5° 左右的抖動(dòng)。此時(shí)可以增加低通濾波器rho_filtered 0.9 * rho_filtered_prev 0.1 * rho_measure; alpha_filtered 0.9 * alpha_filtered_prev 0.1 * alpha_measure;濾波系數(shù) 0.9 對(duì)應(yīng)的是較重的平滑系統(tǒng)響應(yīng)會(huì)變慢相位滯后變大。實(shí)際工程中更常見(jiàn)的是用卡爾曼濾波做位置融合但在一階仿真階段滑動(dòng)平均足夠。6. 把編隊(duì)算法應(yīng)用到 Simulink 與實(shí)機(jī)前的三個(gè)技巧6.1 Simulink 中復(fù)用l_f.m控制器的常見(jiàn)做法把 MATLAB 腳本函數(shù)化然后在 Simulink 中通過(guò) Interpreted MATLAB Function 或 MATLAB Function 塊調(diào)用是最快的遷移方式。但注意腳本內(nèi)部不要包含繪圖和pause只保留輸入輸出計(jì)算邏輯。通常在l_f.m中控制器的核心輸出是[v_F, omega_F]在 Simulink 里將傳感器的相對(duì)距離和角度作為輸入控制器函數(shù)作為輸出即可無(wú)縫連接。6.2 收斂速度與振蕩的調(diào)和增益調(diào)度固定增益在起點(diǎn)距離誤差較大時(shí)容易產(chǎn)生劇烈角速度輸出。我一般用增益調(diào)度的方式將 (k_\rho) 和距離誤差關(guān)聯(lián)誤差大用大增益快速收斂誤差小用小增益防振蕩。角度增益也可以做同樣的處理但要注意兩條增益調(diào)度曲線的切換點(diǎn)重合否則會(huì)出現(xiàn)控制量的跳變。6.3 編隊(duì)起點(diǎn)發(fā)散時(shí)先檢查這四處實(shí)際運(yùn)行l(wèi)_f.m時(shí)如果編隊(duì)一直不能收斂?jī)?yōu)先檢查四項(xiàng)第一alpha是否做了角度歸一化未做wrapToPi的角度累加會(huì)導(dǎo)致控制量突變第二左右輪速映射時(shí)是符號(hào)是否正確前后輪距取值是否與初始位姿一致第三領(lǐng)航者的角速度是否與線速度同時(shí)變化速度突變會(huì)讓跟隨者觸發(fā)飽和限幅第四仿真步長(zhǎng)是否過(guò)大步長(zhǎng)超過(guò) 0.05 秒時(shí)離散化誤差會(huì)掩蓋控制器的真實(shí)性能常見(jiàn)解決方法是把步長(zhǎng)調(diào)至 0.01 或更小并對(duì)比兩次仿真的偏差。如果以上都正常但編隊(duì)仍是振蕩發(fā)散則回頭檢查增益條件 (k_\rho \approx k_\alpha / \rho_d) 是否成立。本文還有配套的精品資源點(diǎn)擊獲取