CSDN話題挑戰賽第2期
參賽話題：學習筆記

1、前記

?????? 傳統機器人通過人工示教的方式編寫機器人的運動程序，在結構化的環境中應用廣泛，但對非專業機器人工程師，機器人編程有很大的難度。機器人的路徑規劃問題可以通過算法的方式在作業環境中尋找一個無碰撞的運動路徑，對機器人安全運動很重要。常見的運動路徑規劃包括基于搜索的方法【A* and D* search】和基于采樣【rapidly-exploring random tree (RRT)，probabilistic roadmap (PRM)
】的方法，以及基于這兩類方法為基礎的后續路徑平滑等優化方法。

?????? 在這里，我們利用matlab環境，通過機器人系統工具箱來演示通過內置RRT規劃器實現基于采樣的無碰撞路徑規劃問題的demo演示。

2、內容

（1）RRT簡單描述

機器人系統工具箱中的RRT算法是基于雙向快速探索隨機樹(RRT)算法，并帶有可選的連接啟發式以潛在地提高速度。雙向RRT計劃器在指定的開始配置和目標配置處創建兩個根節點樹。為了擴展每個樹，規劃器生成一個隨機配置，如果有效，則根據MaxConnectionDistance屬性從最近的節點執行一步采樣擴展。在每次擴展之后，計劃器嘗試使用新的擴展和對面樹上最近的節點連接兩個樹。與環境沖突的無效配置或連接不會添加到樹中。

參考：【Plan motion for rigid body tree using bidirectional RRT - MATLAB- MathWorks 中國】

（2）代碼，描述部分在代碼中有對應注釋，這里不展開l描述，按小節運行可以看到每小節代碼結果。

%% RRT規劃器避障測試 % https://www.mathworks.com/help/robotics/ug/check-for-environmental-collisions-with-manipulators.html % https://www.mathworks.com/help/robotics/ref/manipulatorrrt.html %% 構建環境 % Create two platforms clc clear platform1 = collisionBox(0.5,0.5,0.25); platform1.Pose = trvec2tform([-0.5 0.4 0.2]); platform2 = collisionBox(0.5,0.5,0.25); platform2.Pose = trvec2tform([0.5 0.2 0.2]); % Add a light fixture, modeled as a sphere lightFixture = collisionSphere(0.25); lightFixture.Pose = trvec2tform([0 0.6 0.75]); % Store in a cell array for collision-checking worldCollisionArray = {platform1 platform2 lightFixture};%1，2，3；worldCollisionArray{1} % show(worldCollisionArray)%環境還得單獨show %% 創建figure對象顯示環境：可以直接用exampleHelperVisualizeCollisionEnvironment(worldCollisionArray); % 為了演示修改顏色做下 figure; ax=gca; % 顯示桌子1并上色 [~, patchObj]=show(platform1,'Parent', ax); patchObj.FaceColor = [1 0.6 0.9]; axis([-0.8,1,-0.8,1.2,0,1.4])%調整圖框范圍 view(141,22)%調節視角 hold on %顯示桌子2并上色 [~, patchObj]=show(platform2,'Parent', ax); patchObj.FaceColor = [1 0.6 0.9]; % 顯示燈球并上色 [~, patchObj]=show(lightFixture,'Parent', ax); patchObj.FaceColor = [0 0.8 0.8]; %% 添加機器人模型：可更改為DH方向構建。 robot = loadrobot("kinovaGen3","DataFormat","column","Gravity",[0 0 -9.81]); show(robot,homeConfiguration(robot),"Parent",ax); %% 給機器人添加任務起點和終點 startPose = trvec2tform([-0.5,0.5,0.4])*axang2tform([1 0 0 pi]); endPose = trvec2tform([0.5,0.2,0.4])*axang2tform([1 0 0 pi]);%% 使用逆運動學求解起點個終點位姿的關節構型并顯示 rng(0);% Use a fixed random seed to ensure repeatable results ik = inverseKinematics("RigidBodyTree",robot); weights = ones(1,6); startConfig = ik("EndEffector_Link",startPose,weights,robot.homeConfiguration); endConfig = ik("EndEffector_Link",endPose,weights,robot.homeConfiguration); endEffector="EndEffector_Link"; % Show initial and final positions show(robot,startConfig); show(robot,endConfig); %% RRT屬于采樣類，一些屬性參數：MaxConnetionDistance等會影響結果 rrt=manipulatorRRT(robot,worldCollisionArray); path=plan(rrt,startConfig',endConfig'); path2=interpolate(rrt,path,14);%沒兩個插入14個，還有shorten用于shorten path q=path2; for i = 1:size(path2,1)show(robot,q(i,:)',"PreservePlot",false);%false 不留下重影，true留下poseNow = getTransform(robot, q(i,:)', endEffector);%正運動學plot3(poseNow(1,4), poseNow(2,4), poseNow(3,4),'b.','MarkerSize',15)%末端軌跡drawnow limitrate end q1=q';%轉換格式，配合檢查碰撞的機器人構型 % 初始化輸出 inCollision = false(length(q1),1); %===> zeros(length(q),1) worldCollisionPairIdx = cell(length(q1),1); % 元胞數組，保存與環境碰撞的部件和關節配置的索引 for i = 1:length(q1)[inCollision(i),sepDist] = checkCollision(robot,q1(:,i),worldCollisionArray,"IgnoreSelfCollision","on","Exhaustive","on");% collisions.免除自身碰撞的檢查，因為關節限制保證了大多數自碰撞情況。只尋找構型與環境的碰撞情況[bodyIdx,worldCollisionObjIdx] = find(isnan(sepDist)); % 找到碰撞的部件。距離是空NaN，則發生碰撞worldCollidingPairs = [bodyIdx,worldCollisionObjIdx]; worldCollisionPairIdx{i} = worldCollidingPairs; %機器人部件索引第一列，環境物體索引第二列 end isTrajectoryInCollision = any(inCollision) if isTrajectoryInCollision==0disp('No collison happen')title('checkCollisionReslut：MotionWithoutCollision') end

（3）結果：

?3、小結

記錄如何利用matlab的機器人系統工具箱【robotics system toolbox】中內置的RRT算法再復雜環境中規劃中一個從起始點到目標點的無碰撞路徑。

總結

以上是生活随笔為你收集整理的Robotics System Toolbox中的机器人运动（7）--RRT规划避障路径的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。