Robotics System Toolbox中的机器人运动（7）--RRT规划避障路径

112 阅读 0 评论 74 点赞

我是靠谱客的博主靓丽裙子，这篇文章主要介绍Robotics System Toolbox中的机器人运动（7）--RRT规划避障路径，现在分享给大家，希望可以做个参考。

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==0
    disp('No collison happen')
    title('checkCollisionReslut：MotionWithoutCollision')
end

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
%% 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==0
    disp('No collison happen')
    title('checkCollisionReslut：MotionWithoutCollision')
end