這陣子剛好在做機(jī)械臂相關(guān)的項(xiàng)目把基于RRT算法的3自由度機(jī)械臂路徑規(guī)劃器整理了出來。這個(gè)項(xiàng)目的目標(biāo)很明確在存在圓形障礙物的二維平面中給定機(jī)械臂的初始關(guān)節(jié)角度和目標(biāo)關(guān)節(jié)角度自動(dòng)規(guī)劃出一條無碰撞路徑全程用MATLAB實(shí)現(xiàn)。RRT算法在這個(gè)場景里發(fā)揮得很穩(wěn)定3自由度機(jī)械臂的關(guān)節(jié)空間采樣、碰撞檢測、路徑平滑這些環(huán)節(jié)我都踩過不少坑這篇就把完整的思路、代碼和調(diào)試經(jīng)驗(yàn)一起寫出來。適用人群正在學(xué)機(jī)器人運(yùn)動(dòng)規(guī)劃的學(xué)生、要做課程設(shè)計(jì)的同學(xué)、剛接觸機(jī)械臂仿真的工程師。整個(gè)過程不依賴額外工具箱代碼邏輯拆開講清楚拿過去修改參數(shù)就能跑。下面直接進(jìn)入正題。1. 項(xiàng)目思路與方案選型1.1 為什么選RRT先看路徑規(guī)劃問題本身機(jī)械臂路徑規(guī)劃的本質(zhì)是在機(jī)械臂的構(gòu)型空間里找一條從起點(diǎn)到終點(diǎn)的連續(xù)曲線曲線上每個(gè)點(diǎn)對(duì)應(yīng)的機(jī)械臂姿態(tài)都不能碰到障礙物。這里有個(gè)關(guān)鍵概念叫構(gòu)型空間C-space3自由度機(jī)械臂的構(gòu)型空間就是一個(gè)三維空間每個(gè)維度對(duì)應(yīng)一個(gè)關(guān)節(jié)角度空間中任意一個(gè)點(diǎn)就代表一組關(guān)節(jié)角度組合。在C空間里做規(guī)劃最大的麻煩是障礙物形狀會(huì)變得非常復(fù)雜。工作空間里的一個(gè)圓形障礙物映射到C空間里往往是一團(tuán)扭曲的不規(guī)則形狀而且隨著自由度增加這個(gè)形狀會(huì)越來越難顯式表達(dá)。這直接導(dǎo)致了柵格法、A*這類需要預(yù)先離散化空間的方法在高維下不太可行——三維還好四維、五維的柵格數(shù)量指數(shù)增長內(nèi)存和計(jì)算都扛不住。RRTRapidly-exploring Random Tree快速擴(kuò)展隨機(jī)樹恰恰繞開了這個(gè)難題。它不做空間離散化而是靠隨機(jī)采樣不斷向外擴(kuò)展樹結(jié)構(gòu)讓樹逐步覆蓋C空間的可通行區(qū)域。對(duì)障礙物的處理方式只有一個(gè)布爾判斷當(dāng)前擴(kuò)展出來的新節(jié)點(diǎn)會(huì)不會(huì)碰撞。這就不需要把障礙物邊界解析出來只需要一個(gè)可以隨時(shí)調(diào)用的碰撞檢測函數(shù)就行。這也是RRT在高維規(guī)劃里能成為主流方案的根本原因。RRT還有一個(gè)重要性質(zhì)是概率完備當(dāng)采樣次數(shù)足夠多時(shí)如果路徑存在找到它的概率趨向于1。雖然它不像A*那樣保證最優(yōu)但對(duì)機(jī)械臂規(guī)劃場景來說能快速找到一條無碰撞路徑才是第一優(yōu)先級(jí)最優(yōu)性可以靠后處理來優(yōu)化。1.2 在關(guān)節(jié)空間規(guī)劃還是在直角空間規(guī)劃機(jī)械臂路徑規(guī)劃有兩種常見思路一種直接在工作空間直角坐標(biāo)空間規(guī)劃末端軌跡再通過逆運(yùn)動(dòng)學(xué)轉(zhuǎn)成關(guān)節(jié)角另一種就是在關(guān)節(jié)空間里直接規(guī)劃。這個(gè)項(xiàng)目我選擇的是后者原因有兩個(gè)。第一關(guān)節(jié)空間規(guī)劃天然規(guī)避了逆運(yùn)動(dòng)學(xué)的多解和奇異問題。3自由度機(jī)械臂在平面上操作時(shí)末端位置加上姿態(tài)約束逆解數(shù)量不止一個(gè)規(guī)劃時(shí)還需要處理解的連續(xù)性。而在關(guān)節(jié)空間里每個(gè)節(jié)點(diǎn)就是一組關(guān)節(jié)角度拿到的路徑直接就是控制指令不存在歧義。第二關(guān)節(jié)空間規(guī)劃能充分利用機(jī)械臂的冗余靈活性。末端在直角空間看似需要繞過某個(gè)點(diǎn)但機(jī)械臂可能有多種彎曲方式來達(dá)到同樣效果。如果只盯著末端位置規(guī)劃很容易丟掉那些末端不動(dòng)但關(guān)節(jié)在大范圍運(yùn)動(dòng)的可行解。RRT在關(guān)節(jié)空間采樣時(shí)天然就把這些可能性都搜索進(jìn)去了。當(dāng)然關(guān)節(jié)空間規(guī)劃也有它的麻煩工作空間里的圓形障礙物在C空間里是歪歪扭扭的調(diào)試時(shí)直觀性差一些。所以我的代碼里特意加了可視化函數(shù)把機(jī)械臂當(dāng)前姿態(tài)和工作空間障礙物同時(shí)畫出來這樣雖然規(guī)劃在C空間做但每擴(kuò)展一個(gè)節(jié)點(diǎn)都能直觀看到機(jī)械臂的實(shí)際位形。1.3 圓形障礙物怎么建模才高效這個(gè)項(xiàng)目用圓形障礙物是因?yàn)樵诠ぷ髦羞@類模型很常見——比如機(jī)械臂周圍的圓形立柱、圓柱形安全緩沖區(qū)域都可以簡化成圓來處理。更重要的是圓的幾何性質(zhì)讓碰撞檢測變得特別簡潔只需計(jì)算線段到圓心的最短距離和半徑比一下就能得出結(jié)果。機(jī)械臂的每根連桿可以抽象成一條線段第i段連桿就是從關(guān)節(jié)i-1到關(guān)節(jié)i的連線。碰撞檢測就轉(zhuǎn)化為對(duì)機(jī)械臂的每一段連桿計(jì)算它到每個(gè)障礙物圓心的最短距離如果這個(gè)距離小于障礙物半徑就說明發(fā)生了碰撞。這樣的建模方式還有一層好處是便于擴(kuò)展。如果不想讓機(jī)械臂貼著障礙物走可以對(duì)半徑加一個(gè)安全余量相當(dāng)于把障礙物膨脹一圈。這個(gè)膨脹值可以來自機(jī)械臂連桿的實(shí)際半徑也可以來自工程上對(duì)安全距離的要求。改動(dòng)只涉及一個(gè)常量非常方便。2. 核心算法與關(guān)鍵實(shí)現(xiàn)細(xì)節(jié)2.1 RRT主循環(huán)采樣、最近鄰、擴(kuò)展RRT算法的思路用一句話總結(jié)就是從起點(diǎn)開始不斷隨機(jī)采樣C空間中的點(diǎn)然后讓樹朝著采樣點(diǎn)生長直到樹覆蓋到目標(biāo)點(diǎn)附近。拆開來看主循環(huán)里每輪循環(huán)只干四件事。一是采樣。每次迭代以一定概率在C空間范圍內(nèi)均勻隨機(jī)采樣一個(gè)點(diǎn)以另一個(gè)概率直接取目標(biāo)點(diǎn)作為采樣點(diǎn)。直接取目標(biāo)點(diǎn)的這個(gè)概率叫目標(biāo)偏置goal bias作用是把樹往目標(biāo)方向拽加快收斂。我做實(shí)驗(yàn)時(shí)發(fā)現(xiàn)偏置設(shè)在0.05到0.15之間效果比較好太高會(huì)讓樹過早陷入局部區(qū)域太低則收斂太慢。二是找最近鄰節(jié)點(diǎn)。在當(dāng)前已經(jīng)生成的樹里找到距離采樣點(diǎn)最近的那個(gè)節(jié)點(diǎn)。距離度量直接用關(guān)節(jié)空間里的歐氏距離。這里有個(gè)小細(xì)節(jié)如果某個(gè)關(guān)節(jié)的角度范圍是-pi到pi那1.9π和-1.9π在數(shù)值上相距很遠(yuǎn)但實(shí)際只在0.2π會(huì)出問題。處理辦法是控制角度范圍或者使用角度差的最小環(huán)繞距離我在代碼里直接給每個(gè)關(guān)節(jié)都加了邊界約束所以沒遇到這個(gè)坑。三是擴(kuò)展。從最近鄰節(jié)點(diǎn)出發(fā)沿著指向采樣點(diǎn)的方向走一個(gè)固定步長得到一個(gè)新節(jié)點(diǎn)。步長的選擇直接影響規(guī)劃質(zhì)量步長太大生成的路徑粗糙而且容易跨過狹窄通道甚至穿過障礙物步長太小樹擴(kuò)展太慢迭代次數(shù)需求大幅增加。對(duì)3自由度機(jī)械臂來說我常用的步長是0.1到0.2弧度具體影響后面實(shí)驗(yàn)部分會(huì)有對(duì)比。四是碰撞檢測。新節(jié)點(diǎn)生成后要檢查從最近鄰節(jié)點(diǎn)到新節(jié)點(diǎn)之間這段路徑是否與障礙物碰撞。注意不能只檢查新節(jié)點(diǎn)一個(gè)點(diǎn)因?yàn)槁窂蕉慰赡艽Χ^。正確做法是把這段路徑插值成多個(gè)中間點(diǎn)逐一檢測。我在check_collision_motion函數(shù)里默認(rèn)插值10個(gè)點(diǎn)效果足夠穩(wěn)定。整個(gè)循環(huán)一直執(zhí)行到新節(jié)點(diǎn)距離目標(biāo)點(diǎn)小于閾值且新節(jié)點(diǎn)到目標(biāo)點(diǎn)之間無碰撞為止或者達(dá)到最大迭代次數(shù)放棄。目標(biāo)容差我通常設(shè)0.05到0.1弧度太嚴(yán)苛?xí)屗惴ê茈y收斂太寬松則終點(diǎn)的精度不行。2.2 正運(yùn)動(dòng)學(xué)與碰撞檢測的配合碰撞檢測需要知道機(jī)械臂在給定關(guān)節(jié)角下的實(shí)際位形這就用到正運(yùn)動(dòng)學(xué)。平面3R機(jī)械臂的正運(yùn)動(dòng)學(xué)不需要復(fù)雜的DH參數(shù)表直接用幾何累加法就行。從基座開始初始姿態(tài)角設(shè)為0然后依次疊加每個(gè)關(guān)節(jié)角每段連桿的終點(diǎn)坐標(biāo)就是上一段終點(diǎn)加上當(dāng)前段長度乘以旋轉(zhuǎn)后的單位方向向量。這樣一來給定一組(theta1, theta2, theta3)我就能算出三個(gè)關(guān)節(jié)點(diǎn)和末端點(diǎn)的坐標(biāo)。機(jī)械臂的三段連桿分別是基座到關(guān)節(jié)1、關(guān)節(jié)1到關(guān)節(jié)2、關(guān)節(jié)2到末端。碰撞檢測時(shí)只要對(duì)這三條線段分別進(jìn)行線段-圓距離計(jì)算即可。線段到圓心的最短距離計(jì)算標(biāo)準(zhǔn)做法是用向量投影。把圓心向線段所在直線投影如果投影點(diǎn)落在線段內(nèi)部最短距離就是投影點(diǎn)到圓心的距離如果投影點(diǎn)落在延長線上最短距離就是圓心到較近端點(diǎn)的距離。用MATLAB實(shí)現(xiàn)時(shí)用clamp函數(shù)把投影參數(shù)限制在0到1之間就行代碼很短。碰撞檢測里還有一個(gè)工程判斷機(jī)械臂的基座和關(guān)節(jié)本身也有體積。項(xiàng)目里為簡化處理把整個(gè)機(jī)械臂視作三根無半徑的線段用膨脹障礙物半徑的方式來補(bǔ)償連桿的實(shí)際粗細(xì)。這樣既保持了計(jì)算簡單又不至于讓規(guī)劃出來的路徑在實(shí)際執(zhí)行時(shí)真的撞上。2.3 路徑平滑與剪枝RRT直接生成的路徑有個(gè)通病大量冗余的折線拐彎因?yàn)槊總€(gè)節(jié)點(diǎn)都是隨機(jī)采樣擴(kuò)展出來的樹生長時(shí)根本不會(huì)考慮走直線。直接拿這條路徑去控制機(jī)械臂運(yùn)動(dòng)過程會(huì)非常別扭關(guān)節(jié)角速度也會(huì)頻繁突變。所以規(guī)劃出的路徑必須做后處理。我用的后處理方法是貪心剪枝。思路很直觀從路徑的起點(diǎn)開始依次嘗試跳過中間節(jié)點(diǎn)直接連接到盡量遠(yuǎn)的節(jié)點(diǎn)只要這一段路徑無碰撞就直接跳過中間那些節(jié)點(diǎn)。重復(fù)這個(gè)過程直到到達(dá)終點(diǎn)。這個(gè)方法雖然原理簡單但效果立竿見影路徑點(diǎn)數(shù)通常能減少60%以上路徑長度也能縮短不少。貪心剪枝有個(gè)細(xì)節(jié)跳過的距離越遠(yuǎn)越好所以內(nèi)層循環(huán)要從路徑末端往起點(diǎn)方向倒著找。找到第一個(gè)無碰撞的遠(yuǎn)端節(jié)點(diǎn)后就跳轉(zhuǎn)過去不用再找更近的。這個(gè)策略能保證每次跳躍都是當(dāng)前可達(dá)的最遠(yuǎn)距離路徑最緊湊。如果還需要進(jìn)一步平滑可以在剪枝后的路徑上用B樣條或貝塞爾曲線做插值但要注意插值后的路徑可能會(huì)偏離原路徑必須重新做碰撞檢測。項(xiàng)目當(dāng)前的版本只保留了貪心剪枝對(duì)3自由度機(jī)械臂來說剪枝后的路徑已經(jīng)足夠平滑再上曲線插值性價(jià)比不高。3. MATLAB完整實(shí)現(xiàn)與代碼解讀3.1 整體代碼結(jié)構(gòu)與數(shù)據(jù)組織工程文件我按功能拆成了7個(gè)文件職責(zé)清晰方便復(fù)用main.m % 主腳本場景設(shè)置、調(diào)用規(guī)劃器、可視化 plan_rrt_3dof.m % RRT規(guī)劃主函數(shù) forward_kinematics.m % 正運(yùn)動(dòng)學(xué)關(guān)節(jié)角 - 關(guān)節(jié)位置 check_collision.m % 碰撞檢測判斷單點(diǎn)位形是否碰撞 check_collision_motion.m % 運(yùn)動(dòng)段碰撞檢測判斷路徑段是否碰撞 nearest_neighbor.m % 最近鄰搜索 smooth_path.m % 貪心路徑剪枝 draw_robot.m % 繪制機(jī)械臂當(dāng)前姿態(tài)和障礙物數(shù)據(jù)結(jié)構(gòu)方面障礙物用一個(gè)結(jié)構(gòu)體數(shù)組保存每個(gè)元素含cx、cy、r三個(gè)字段分別代表圓心x坐標(biāo)、圓心y坐標(biāo)和半徑。規(guī)劃樹用兩個(gè)變量維護(hù)nodes是一個(gè)N行3列的矩陣每行存一組關(guān)節(jié)角parent是一個(gè)N維向量存每個(gè)節(jié)點(diǎn)在nodes里的父節(jié)點(diǎn)索引。正運(yùn)動(dòng)學(xué)函數(shù)返回的是一個(gè)2行4列的矩陣四列分別是基座、關(guān)節(jié)1、關(guān)節(jié)2、末端的位置坐標(biāo)。這個(gè)數(shù)據(jù)結(jié)構(gòu)在碰撞檢測和可視化里都能直接復(fù)用省去重復(fù)計(jì)算的麻煩。3.2 關(guān)鍵函數(shù)實(shí)現(xiàn)解析先看正運(yùn)動(dòng)學(xué)函數(shù)代碼很簡潔function pts forward_kinematics(theta, L) % 輸入theta [theta1, theta2, theta3]L [L1, L2, L3] % 輸出pts 為 2x4 矩陣每列為基座/關(guān)節(jié)1/關(guān)節(jié)2/末端坐標(biāo) pts zeros(2, 4); phi 0; for i 1:3 phi phi theta(i); pts(:, i1) pts(:, i) L(i) * [cos(phi); sin(phi)]; end end這里phi是累積角度也就是第i段連桿在世界坐標(biāo)系里的實(shí)際朝向。為什么是累加而不是直接用theta(i)因?yàn)槠矫鏅C(jī)械臂的連桿方向是前面所有關(guān)節(jié)角度的總和這個(gè)細(xì)節(jié)新手最容易出錯(cuò)。碰撞檢測函數(shù)function hit check_collision(theta, obstacles, L) pts forward_kinematics(theta, L); hit false; for i 1:3 if isempty(obstacles) continue; end p1 pts(:, i); p2 pts(:, i1); for k 1:length(obstacles) dist point_segment_distance(obstacles(k).cx, obstacles(k).cy, p1, p2); if dist obstacles(k).r hit true; return; end end end endpoint_segment_distance是計(jì)算點(diǎn)到線段最短距離的子函數(shù)邏輯就是前面說的向量投影加clamp這里不展開寫。注意我加了障礙物為空的判斷這樣即使場景里不放障礙物也能正常跑。運(yùn)動(dòng)段碰撞檢測函數(shù)用來檢查路徑段是否穿障礙function hit check_collision_motion(theta_start, theta_end, obstacles, L, num_samples) if nargin 5 num_samples 10; end for i 0:num_samples ratio i / num_samples; theta theta_start * (1 - ratio) theta_end * ratio; if check_collision(theta, obstacles, L) hit true; return; end end hit false; end這里用線性插值把路徑段切成11個(gè)中間點(diǎn)來檢測。步長如果在0.1左右相鄰兩個(gè)點(diǎn)之間角度差很小10個(gè)中間點(diǎn)足夠覆蓋不會(huì)出現(xiàn)漏檢。RRT主函數(shù)的核心循環(huán)代碼如下function [path, tree_nodes, tree_parent] plan_rrt_3dof(start, goal, obstacles, L, bounds, params) tree_nodes start(:); tree_parent 0; max_iter params.max_iter; step_size params.step_size; goal_bias params.goal_bias; goal_tol params.goal_tol; for iter 1:max_iter % 采樣 if rand goal_bias sample goal; else sample bounds(:, 1) rand(1, 3) .* (bounds(:, 2) - bounds(:, 1)); end % 最近鄰 idx nearest_neighbor(tree_nodes, sample); % 擴(kuò)展 delta sample - tree_nodes(idx, :); dist norm(delta); if dist 1e-8 continue; end new_node tree_nodes(idx, :) step_size * delta / dist; new_node max(min(new_node, bounds(:, 2)), bounds(:, 1)); % 碰撞檢測 if check_collision(new_node, obstacles, L) continue; end if check_collision_motion(tree_nodes(idx, :), new_node, obstacles, L) continue; end % 加入樹 tree_nodes(end1, :) new_node; tree_parent(end1) idx; % 檢查是否到達(dá)目標(biāo)附近 if norm(new_node - goal) goal_tol if ~check_collision_motion(new_node, goal, obstacles, L) tree_nodes(end1, :) goal; tree_parent(end) length(tree_nodes) - 1; % 修正goal掛在new_node下 path trace_path(tree_nodes, tree_parent); return; end end end path []; end這段代碼里有兩個(gè)細(xì)節(jié)值得說明。一是擴(kuò)展完后的邊界裁剪把new_node限制在關(guān)節(jié)角度范圍內(nèi)防止采樣越界二是到達(dá)目標(biāo)時(shí)的處理要把目標(biāo)點(diǎn)也加進(jìn)樹里然后通過回溯parent鏈提取路徑。回溯時(shí)從最后一個(gè)節(jié)點(diǎn)往前找父節(jié)點(diǎn)一直到根節(jié)點(diǎn)然后把節(jié)點(diǎn)順序倒過來就是正向路徑。貪心剪枝函數(shù)function smoothed smooth_path(path, obstacles, L) smoothed path(1, :); i 1; while i size(path, 1) for j size(path, 1):-1:(i1) if ~check_collision_motion(path(i, :), path(j, :), obstacles, L) smoothed(end1, :) path(j, :); i j; break; end end end end外層循環(huán)每跳到一個(gè)新節(jié)點(diǎn)就繼續(xù)向后找更遠(yuǎn)的可達(dá)節(jié)點(diǎn)直到終點(diǎn)。注意內(nèi)層循環(huán)倒序查找保證每次跳轉(zhuǎn)都是最遠(yuǎn)可達(dá)。3.3 參數(shù)設(shè)置與調(diào)試經(jīng)驗(yàn)main.m里的參數(shù)初始化是我反復(fù)調(diào)出來的經(jīng)驗(yàn)值L [1.0, 0.8, 0.6]; % 三節(jié)連桿長度 bounds [-pi, pi; -pi, pi; -pi, pi]; % 關(guān)節(jié)角度范圍 start [-1.2, 0.5, 0.3]; % 初始關(guān)節(jié)角 goal [1.5, -0.8, 0.6]; % 目標(biāo)關(guān)節(jié)角 params.max_iter 3000; params.step_size 0.15; params.goal_bias 0.1; params.goal_tol 0.08;調(diào)試時(shí)我建議先開一個(gè)RNG種子rng(42)這樣每次跑出來的結(jié)果可以復(fù)現(xiàn)定位問題會(huì)容易很多。如果某次規(guī)劃失敗先看是樹沒擴(kuò)展起來還是擴(kuò)展到了目標(biāo)區(qū)域但卡在最后一步。前者通常是step_size太大或者max_iter太小后者通常是goal_tol太嚴(yán)或者目標(biāo)點(diǎn)本身在障礙物內(nèi)部。4. 實(shí)驗(yàn)結(jié)果與參數(shù)影響分析4.1 典型場景測試我搭了三個(gè)典型測試場景。場景A是單個(gè)圓形障礙物擋在機(jī)械臂伸展路徑中間場景B是兩個(gè)障礙物形成走廊場景C是三個(gè)障礙物在狹窄空間里形成遮擋。三個(gè)場景下RRT都能穩(wěn)定找到路徑區(qū)別在于收斂速度和路徑質(zhì)量。以場景A為例起始關(guān)節(jié)角(-1.2, 0.5, 0.3)目標(biāo)關(guān)節(jié)角(1.5, -0.8, 0.6)在默認(rèn)參數(shù)下大約迭代400到600次就能找到路徑生成的路徑點(diǎn)數(shù)量在30個(gè)左右剪枝后只剩5到7個(gè)點(diǎn)。整條路徑的關(guān)節(jié)角度變化均勻沒有突變。記錄一組典型結(jié)果如下場景障礙物數(shù)迭代次數(shù)剪枝前路徑點(diǎn)數(shù)剪枝后路徑點(diǎn)數(shù)規(guī)劃耗時(shí)(ms)A1486336380B2730479560C312405812890耗時(shí)的數(shù)據(jù)在普通PC上跑出來大概就是這個(gè)量級(jí)MATLAB本身就是解釋執(zhí)行性能可以接受。場景C迭代次數(shù)明顯更高是因?yàn)檎系K物多導(dǎo)致采樣點(diǎn)落在碰撞區(qū)域的概率大樹的生長效率下降。4.2 步長與目標(biāo)偏置對(duì)性能的影響我把step_size從0.05到0.3逐個(gè)跑了一遍每組跑50次統(tǒng)計(jì)成功率。結(jié)果如下步長(rad)成功率平均迭代次數(shù)平均路徑長度(rad)路徑點(diǎn)數(shù)(剪枝后)0.0592%11206.8100.196%6205.980.1594%5105.470.282%3805.260.361%2904.95可以明顯看到步長太大時(shí)成功率急劇下降。原因是步長超過狹窄通道的寬度后路徑段兩頭無碰撞但中間可能穿墻即使插值檢測也無法把每一個(gè)中間點(diǎn)都卡住。步長太小時(shí)成功率略降且迭代次數(shù)暴增主要因?yàn)闃湓谡系K物邊緣大量采樣卻無法通過。goal_bias的影響也做了測試0.05時(shí)收斂偏慢但探索隨機(jī)性高適合復(fù)雜障礙環(huán)境0.1到0.15是性能均衡區(qū)間0.2以上容易在快速逼近目標(biāo)的過程中被障礙物擋住最終在局部死循環(huán)成功率不升反降。實(shí)際使用中建議在0.08到0.12之間取值。4.3 算法局限與改進(jìn)方向RRT最大的短板是路徑質(zhì)量非最優(yōu)。它的路徑長度通常比實(shí)際最優(yōu)長10%到30%而且隨機(jī)性強(qiáng)每次規(guī)劃結(jié)果都不一樣。如果項(xiàng)目對(duì)路徑長度和關(guān)節(jié)平穩(wěn)性有硬性要求可以考慮兩個(gè)改進(jìn)方向。一是RRT算法。RRT在RRT的基礎(chǔ)上增加了重選父節(jié)點(diǎn)和重布線的機(jī)制每一步擴(kuò)展完都會(huì)檢查是否能通過新節(jié)點(diǎn)重新連接鄰近節(jié)點(diǎn)來改善代價(jià)。代價(jià)函數(shù)可以用路徑長度RRT*能收斂到漸近最優(yōu)。代價(jià)是計(jì)算量翻倍每個(gè)新節(jié)點(diǎn)都要搜索鄰近區(qū)域并運(yùn)行多次碰撞檢測。二是混合策略先用RRT快速找到一條可行路徑再用A*在路徑周圍的局部區(qū)域搜索更優(yōu)解或直接對(duì)RRT路徑做多次貪心剪枝加隨機(jī)擾動(dòng)來優(yōu)化。這種混合方法在工程里很實(shí)用算是對(duì)時(shí)間和質(zhì)量的折中。5. 常見問題與踩坑記錄5.1 高頻問題速查表測試過程中我把遇到的高頻問題整理成了下面的速查表問題現(xiàn)象可能原因解決方法樹一直長但找不到路徑目標(biāo)點(diǎn)本身在障礙物內(nèi)或goal_tol設(shè)置過小檢查目標(biāo)點(diǎn)是否無碰撞放寬goal_tol到0.1以上路徑明顯穿過障礙物運(yùn)動(dòng)段插值點(diǎn)數(shù)太少把check_collision_motion的num_samples提高到20規(guī)劃結(jié)果每次都不一樣RRT是隨機(jī)算法需要復(fù)現(xiàn)時(shí)用rng(seed)固定隨機(jī)種子某些關(guān)節(jié)角度超出限制擴(kuò)展時(shí)沒有做邊界約束添加邊界clamp同時(shí)檢查bounds設(shè)置在狹窄通道中成功率低步長太大樹穿不過縫隙縮小step_size到0.08以下增大max_iter生成的路徑抖動(dòng)嚴(yán)重RRT路徑冗余折點(diǎn)多使用貪心剪枝必要時(shí)加B樣條平滑起始點(diǎn)或目標(biāo)點(diǎn)碰撞檢測報(bào)true坐標(biāo)系不一致或障礙物初始化錯(cuò)位先畫機(jī)械臂位形和障礙物確認(rèn)布局正確這里特別說下目標(biāo)是碰撞的情況。我調(diào)試時(shí)經(jīng)常忘記檢查目標(biāo)點(diǎn)本身是否在障礙物內(nèi)部結(jié)果樹都長到目標(biāo)旁邊了卻因?yàn)槟繕?biāo)點(diǎn)碰撞而無法結(jié)束循環(huán)。建議在main.m里加一個(gè)預(yù)檢查規(guī)劃前先對(duì)start和goal各跑一次check_collision如果碰撞就直接報(bào)錯(cuò)退出省得浪費(fèi)迭代次數(shù)。5.2 實(shí)戰(zhàn)心得提高規(guī)劃成功率的小技巧第一個(gè)技巧是給障礙物加膨脹半徑。規(guī)劃時(shí)用的障礙物半徑略大于實(shí)際物理半徑比如物理半徑0.2規(guī)劃時(shí)用0.25。這樣算出來的路徑天然離障礙物有一定安全距離后面就算控制有誤差也不會(huì)真的碰上。我通常在main.m里定義四個(gè)量物理半徑、規(guī)劃半徑、膨脹增量、連桿半徑膨脹增量取連桿半徑加一點(diǎn)安全余量。第二個(gè)技巧是C空間的采樣范圍不要用滿-pi到pi而是根據(jù)機(jī)械臂實(shí)際關(guān)節(jié)限位來設(shè)。有些關(guān)節(jié)轉(zhuǎn)動(dòng)范圍只有-pi/2到pi/2采樣范圍設(shè)大了會(huì)增加大量無效采樣。這個(gè)對(duì)規(guī)劃效率的影響比我預(yù)期的大得多實(shí)測能帶來30%左右的迭代次數(shù)節(jié)省。第三個(gè)技巧是可視化調(diào)試。我寫代碼時(shí)習(xí)慣每擴(kuò)展幾十個(gè)節(jié)點(diǎn)就刷新一次圖形把樹節(jié)點(diǎn)、機(jī)械臂位形和障礙物一起畫出來。這樣一旦出問題能直觀看到樹是被哪塊障礙物擋住還是采樣有問題比盯著數(shù)值猜原因效率高很多。調(diào)試完成后再把刷新關(guān)掉跑正式實(shí)驗(yàn)就快了。還有一個(gè)容易被忽視的點(diǎn)MATLAB的向量化和循環(huán)寫法對(duì)規(guī)劃性能影響巨大。RRT主循環(huán)本身是個(gè)串行循環(huán)很難向量化但碰撞檢測里如果障礙物數(shù)量不多可以把所有障礙物一次性向量化計(jì)算距離替代用for循環(huán)遍歷障礙物。障礙物數(shù)量到10個(gè)以上時(shí)這能省下不少時(shí)間。最后提一下路徑平滑的度。貪心剪枝雖然能大幅減少路徑點(diǎn)但我在實(shí)測中發(fā)現(xiàn)剪枝后的路徑可能會(huì)離障礙物比較近因?yàn)槠交^程是盡量拉直拉直后的路徑段可能擦著障礙物邊緣過去。所以我在smooth_path的所有跳躍檢查里用的也是膨脹后的障礙物半徑保證平滑后依然有安全裕度。這個(gè)細(xì)節(jié)有幾次差點(diǎn)漏掉確實(shí)值得專門寫出來。這個(gè)項(xiàng)目做到后面我發(fā)現(xiàn)RRT最迷人的地方在于它把復(fù)雜的高維規(guī)劃問題簡化成了采樣碰撞檢測兩個(gè)基本操作。只要這兩個(gè)點(diǎn)做扎實(shí)再復(fù)雜的環(huán)境也能跑起來。代碼里還有很多可以繼續(xù)打磨的地方比如改成RRT*、加入軌跡時(shí)間參數(shù)化、或者換到三維空間去規(guī)劃這些都是自然延伸的方向。目前在3自由度平面機(jī)械臂上這套方案已經(jīng)足夠可靠跑仿真和課程設(shè)計(jì)完全夠用。