CSDN話題挑戰(zhàn)賽第2期
參賽話題：學(xué)習(xí)筆記

1、前記

?????? 傳統(tǒng)機(jī)器人通過(guò)人工示教的方式編寫(xiě)機(jī)器人的運(yùn)動(dòng)程序，在結(jié)構(gòu)化的環(huán)境中應(yīng)用廣泛，但對(duì)非專(zhuān)業(yè)機(jī)器人工程師，機(jī)器人編程有很大的難度。機(jī)器人的路徑規(guī)劃問(wèn)題可以通過(guò)算法的方式在作業(yè)環(huán)境中尋找一個(gè)無(wú)碰撞的運(yùn)動(dòng)路徑，對(duì)機(jī)器人安全運(yùn)動(dòng)很重要。常見(jiàn)的運(yùn)動(dòng)路徑規(guī)劃包括基于搜索的方法【A* and D* search】和基于采樣【rapidly-exploring random tree (RRT)，probabilistic roadmap (PRM)
】的方法，以及基于這兩類(lèi)方法為基礎(chǔ)的后續(xù)路徑平滑等優(yōu)化方法。

?????? 在這里，我們利用matlab環(huán)境，通過(guò)機(jī)器人系統(tǒng)工具箱來(lái)演示通過(guò)內(nèi)置RRT規(guī)劃器實(shí)現(xiàn)基于采樣的無(wú)碰撞路徑規(guī)劃問(wèn)題的demo演示。

2、內(nèi)容

（1）RRT簡(jiǎn)單描述

機(jī)器人系統(tǒng)工具箱中的RRT算法是基于雙向快速探索隨機(jī)樹(shù)(RRT)算法，并帶有可選的連接啟發(fā)式以潛在地提高速度。雙向RRT計(jì)劃器在指定的開(kāi)始配置和目標(biāo)配置處創(chuàng)建兩個(gè)根節(jié)點(diǎn)樹(shù)。為了擴(kuò)展每個(gè)樹(shù)，規(guī)劃器生成一個(gè)隨機(jī)配置，如果有效，則根據(jù)MaxConnectionDistance屬性從最近的節(jié)點(diǎn)執(zhí)行一步采樣擴(kuò)展。在每次擴(kuò)展之后，計(jì)劃器嘗試使用新的擴(kuò)展和對(duì)面樹(shù)上最近的節(jié)點(diǎn)連接兩個(gè)樹(shù)。與環(huán)境沖突的無(wú)效配置或連接不會(huì)添加到樹(shù)中。

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

（2）代碼，描述部分在代碼中有對(duì)應(yīng)注釋，這里不展開(kāi)l描述，按小節(jié)運(yùn)行可以看到每小節(jié)代碼結(jié)果。

%% RRT規(guī)劃器避障測(cè)試 % https://www.mathworks.com/help/robotics/ug/check-for-environmental-collisions-with-manipulators.html % https://www.mathworks.com/help/robotics/ref/manipulatorrrt.html %% 構(gòu)建環(huán)境 % 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)%環(huán)境還得單獨(dú)show %% 創(chuàng)建figure對(duì)象顯示環(huán)境：可以直接用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])%調(diào)整圖框范圍 view(141,22)%調(diào)節(jié)視角 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]; %% 添加機(jī)器人模型：可更改為DH方向構(gòu)建。 robot = loadrobot("kinovaGen3","DataFormat","column","Gravity",[0 0 -9.81]); show(robot,homeConfiguration(robot),"Parent",ax); %% 給機(jī)器人添加任務(wù)起點(diǎn)和終點(diǎn) 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]);%% 使用逆運(yùn)動(dòng)學(xué)求解起點(diǎn)個(gè)終點(diǎn)位姿的關(guān)節(jié)構(gòu)型并顯示 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屬于采樣類(lèi)，一些屬性參數(shù)：MaxConnetionDistance等會(huì)影響結(jié)果 rrt=manipulatorRRT(robot,worldCollisionArray); path=plan(rrt,startConfig',endConfig'); path2=interpolate(rrt,path,14);%沒(méi)兩個(gè)插入14個(gè)，還有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);%正運(yùn)動(dòng)學(xué)plot3(poseNow(1,4), poseNow(2,4), poseNow(3,4),'b.','MarkerSize',15)%末端軌跡drawnow limitrate end q1=q';%轉(zhuǎn)換格式，配合檢查碰撞的機(jī)器人構(gòu)型 % 初始化輸出 inCollision = false(length(q1),1); %===> zeros(length(q),1) worldCollisionPairIdx = cell(length(q1),1); % 元胞數(shù)組，保存與環(huán)境碰撞的部件和關(guān)節(jié)配置的索引 for i = 1:length(q1)[inCollision(i),sepDist] = checkCollision(robot,q1(:,i),worldCollisionArray,"IgnoreSelfCollision","on","Exhaustive","on");% collisions.免除自身碰撞的檢查，因?yàn)殛P(guān)節(jié)限制保證了大多數(shù)自碰撞情況。只尋找構(gòu)型與環(huán)境的碰撞情況[bodyIdx,worldCollisionObjIdx] = find(isnan(sepDist)); % 找到碰撞的部件。距離是空NaN，則發(fā)生碰撞worldCollidingPairs = [bodyIdx,worldCollisionObjIdx]; worldCollisionPairIdx{i} = worldCollidingPairs; %機(jī)器人部件索引第一列，環(huán)境物體索引第二列 end isTrajectoryInCollision = any(inCollision) if isTrajectoryInCollision==0disp('No collison happen')title('checkCollisionReslut：MotionWithoutCollision') end

（3）結(jié)果：

?3、小結(jié)

記錄如何利用matlab的機(jī)器人系統(tǒng)工具箱【robotics system toolbox】中內(nèi)置的RRT算法再?gòu)?fù)雜環(huán)境中規(guī)劃中一個(gè)從起始點(diǎn)到目標(biāo)點(diǎn)的無(wú)碰撞路徑。

總結(jié)

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

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。