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