├── README.md ├── image └── README │ ├── 1672140581114.png │ └── 1672140697780.png ├── input.txt ├── pathplanning.m ├── robot_DHtable.m ├── robot_fkin.m ├── rotx.m ├── roty.m ├── rotz.m ├── trans.m └── worckspace.m /README.md: -------------------------------------------------------------------------------- 1 | ### RRT算法 2 | 3 | RRT（Rapid-exploration Random Tree）是一种随机搜索算法，可以快速进行搜索，Random指的是搜索的方式，通过在环境中随机采样的方式探索整个环境。Tree指的是已搜索的位置通过一棵树来存储，每个位置都有自己的父节点和子节点。搜索完成的路径通常是从树的根节点到一个叶节点的路径。 4 | 5 | ![1672140581114](image/README/1672140581114.png) 6 | 7 | 常规的避障使用二维或三维RRT搜索，但是机械臂需要考虑各轴与物体的碰撞，因此需要在六维空间进行扩展，然后通过正运动解算做碰撞检测。 8 | 9 | ### 依赖 10 | 11 | 本脚本运行需要以下工具包 12 | 13 | [geom3d](https://ww2.mathworks.cn/matlabcentral/fileexchange/24484-geom3d) 14 | 15 | [Robotics Toolbox for MATLAB](https://petercorke.com/toolboxes/robotics-toolbox/#Downloading_the_Toolbox) 16 | 17 | ### 配置 18 | 19 | 机械臂DH参数可在 robot_DHtable.m 中设置，默认为如图机械臂模型![1672140697780](image/README/1672140697780.png) 20 | 21 | 22 | 采样次数和步长可在 pathplanning.m 中设置，应保证足够的采样次数以获得正确的结果 23 | 24 | ### 输入 25 | 26 | 运行 pathplanning.m ，脚本会读取input.txt的内容。input.txt中 27 | 28 | 第一行为初始位姿 29 | 30 | 第二行为目标位姿 31 | 32 | 第三行为障碍物个数 33 | 34 | 下面是障碍物的空间坐标与半径 35 | -------------------------------------------------------------------------------- /image/README/1672140581114.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/carlsberg03/6-axis_robot_pathplanning/1fa15283156fb4db6b274d18dcdfc92ec6e41cc3/image/README/1672140581114.png -------------------------------------------------------------------------------- /image/README/1672140697780.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/carlsberg03/6-axis_robot_pathplanning/1fa15283156fb4db6b274d18dcdfc92ec6e41cc3/image/README/1672140697780.png -------------------------------------------------------------------------------- /input.txt: -------------------------------------------------------------------------------- 1 | 1.17 -1.29 -1.05 -2.91 1.15 -3.14 2 | 0.42 0.21 0.97 -0.81 1.57 1.99 3 | 2 4 | 0.3 0 0.5 0.05 5 | 0.2 0.1 0.4 0.1 6 | -------------------------------------------------------------------------------- /pathplanning.m: -------------------------------------------------------------------------------- 1 | %% 清空工作区 2 | clear; 3 | close all; 4 | clc; 5 | %% 读取DH参数 6 | DHtable = robot_DHtable(); 7 | L(1)=Link([DHtable(1,:),0],'modified'); 8 | L(2)=Link([DHtable(2,:),0],'modified'); 9 | L(3)=Link([DHtable(3,:),0],'modified'); 10 | L(4)=Link([DHtable(4,:),0],'modified'); 11 | L(5)=Link([DHtable(5,:),0],'modified'); 12 | L(6)=Link([DHtable(6,:),0],'modified'); 13 | %s为机械臂初始姿态 14 | s=[pi -pi/2 0 -pi/2 0 0]; 15 | plotopt = {'noraise', 'nowrist', 'nojaxes', 'delay',0}; 16 | robot=SerialLink(L,'name','robot'); 17 | %% 步长和采样次数，请确保足够大的采样次数以获得正确结果 18 | 19 | %如果运行报错，可以尝试增大samples或者重新运行 20 | samples = 20000; 21 | segmentlength = 0.02; 22 | %% 读取input.txt 23 | fid=fopen("input.txt"); 24 | init = (fgetl(fid)); 25 | init = str2num(init); 26 | target = fgetl(fid); 27 | target = str2num(target); 28 | obstacles_num = str2num(fgetl(fid)); 29 | obstacles=[]; 30 | for i = 1:obstacles_num 31 | temp=fgetl(fid); 32 | temp=str2num(temp); 33 | obstacles=[obstacles;temp]; 34 | end 35 | fclose(fid); 36 | 37 | %% 获取路径 38 | [path , tree] = RRT(init,target,obstacles_num,obstacles,samples,segmentlength); 39 | q = path(:,1:6); 40 | for i = 1:size(q,1) 41 | q(i,:)=q(i,:)+s; 42 | end 43 | 44 | T=robot.fkine(q); 45 | num = size(path,1); 46 | TJ=zeros(4,4,num); 47 | 48 | %% 画图 49 | figure(1); 50 | clf;hold on;axis equal; 51 | xlim([-1 1]); 52 | ylim([-1 1]); 53 | zlim([-1 1]); 54 | for i = 1:obstacles_num 55 | h(i)=drawSphere(obstacles(i,:)); % 画圆，用到geom3d工具包 56 | end 57 | 58 | robot.plot(init + s); % 画出初始位姿 59 | 60 | % 计算每个关节坐标下的齐次变换矩阵 61 | for ii = 1:num 62 | TJ(:,:,ii)=T(ii).T; 63 | end 64 | 65 | 66 | plot3(squeeze(TJ(1,4,:)),squeeze(TJ(2,4,:)),squeeze(TJ(3,4,:)));%输出末端轨迹 67 | robot.plot(q) % 输出运动动画 68 | 69 | 70 | %% RRT函数 71 | function [paths,tree] = RRT(init,target,NumObstacles,Obstacles,samples,segmentLength) 72 | 73 | dim=6; 74 | start_cord = init; 75 | goal_cord = target; 76 | 77 | 78 | world = createWorld(NumObstacles,Obstacles); 79 | 80 | start_node = [start_cord,0,0,0]; 81 | end_node = [goal_cord,0,0,0]; 82 | paths=[]; 83 | % 建立树结构 84 | tree = start_node; 85 | numPaths = 0; 86 | a = clock; 87 | % 检查初始节点和目标节点能否直接相连 88 | if ( (norm(start_node(1:dim)-end_node(1:dim))0) 102 | paths = findMinimumPath(tree,end_node,dim); 103 | end 104 | end 105 | 106 | 107 | 108 | 109 | 110 | function world = createWorld(NumObstacles, Obstacles) 111 | 112 | world.NumObstacles = NumObstacles; 113 | world.endcorner = [pi pi pi pi pi pi ]; 114 | world.origincorner = [-pi -pi -pi -pi -pi -pi]; 115 | 116 | for i=1:NumObstacles 117 | world.cx(i) = Obstacles(i,1); 118 | world.cy(i) = Obstacles(i,2); 119 | world.cz(i) = Obstacles(i,3); 120 | world.radius(i) = Obstacles(i,4); 121 | end 122 | end 123 | 124 | 125 | 126 | 127 | %% 碰撞检测 128 | function collision_flag = collision(node, world) 129 | collision_flag = 0; 130 | dim = 6; 131 | 132 | for i=1:dim 133 | if (node(i)>world.endcorner(i))||(node(i)0.9 187 | randomPoint=end_node(1:6); 188 | else 189 | for i=1:dim 190 | randomPoint(1,i) = world.origincorner(i)+(world.endcorner(i)-world.origincorner(i))*rand; 191 | end 192 | end 193 | tmp = tree(:,1:dim)-ones(size(tree,1),1)*randomPoint; 194 | sqrd_dist = sqr_eucl_dist(tmp,dim); %返回各点到随机点直线距离的平方值 195 | [~,idx] = min(sqrd_dist); 196 | new_point = (randomPoint-tree(idx,1:dim)); 197 | if(norm(new_point) 0 268 | 269 | % find minimum cost last node 270 | [~,idx] = min(connectingNodes(:,dim+2)); 271 | 272 | % construct lowest cost path 273 | path = [connectingNodes(idx,:); end_node]; 274 | parent_node = connectingNodes(idx,dim+3); 275 | while parent_node>1, 276 | parent_node = tree(parent_node,dim+3); 277 | path = [tree(parent_node,:); path]; 278 | end 279 | 280 | else 281 | path = []; 282 | end 283 | 284 | end 285 | 286 | -------------------------------------------------------------------------------- /robot_DHtable.m: -------------------------------------------------------------------------------- 1 | function DHtable = robot_DHtable (~) 2 | %robot_DHtable creates a DH table for an industrial robot. 3 | 4 | % 完善函数robot_DHtable的注释，根据正运动学给出DH参数的值。 5 | 6 | DHtable = []; 7 | 8 | theta1 = 180/180*pi; 9 | theta2 = -90/180*pi; 10 | theta3 = 0; 11 | theta4 = -90/180*pi; 12 | theta5 = 0; 13 | theta6 = 0; 14 | 15 | d1 = 0.0985; 16 | d2 = 0.1405; 17 | d3 = 0; 18 | d4 = -0.019; 19 | d5 = 0.1025; 20 | d6 = 0.094; 21 | 22 | a1 = 0; 23 | a2 = 0; 24 | a3 = 0.408; 25 | a4 = 0.376; 26 | a5 = 0; 27 | a6 = 0; 28 | 29 | alpha1 = 0; 30 | alpha2 = -90/180*pi; 31 | alpha3 = 180/180*pi; 32 | alpha4 = 180/180*pi; 33 | alpha5 = -90/180*pi; 34 | alpha6 = 90/180*pi; 35 | 36 | DHtable = [theta1,d1,a1,alpha1;... 37 | theta2,d2,a2,alpha2;... 38 | theta3,d3,a3,alpha3;... 39 | theta4,d4,a4,alpha4;... 40 | theta5,d5,a5,alpha5;... 41 | theta6,d6,a6,alpha6]; 42 | 43 | end 44 | 45 | -------------------------------------------------------------------------------- /robot_fkin.m: -------------------------------------------------------------------------------- 1 | function [H, H_i] = robot_fkin(DHtable,q) 2 | % robot_fkin根据DH表和关节角度q生成末端到基座的齐次变换矩阵H和相邻连杆的齐次变换矩阵H_i 3 | % 输入参数： 4 | % DHtable是一个nX4 的矩阵，满足DH改进约定的DH表，可从robot_DHtable()中读取； 5 | % q是一个1xN的向量，表示关节坐标 6 | % 输出参数： 7 | % H表示末端到基座的 4X4 齐次变换矩阵； 8 | % H_i表示相邻连杆的 4X4 齐次变换矩阵，由于有n个连杆，因此H_i应该为包含n个元素的cell； 9 | 10 | num_link = size(DHtable,1); 11 | H = eye(4); 12 | H_i = cell(1,num_link); 13 | 14 | for i = 1:num_link 15 | H_i{1,i} = rotx(DHtable(i,4))*trans(DHtable(i,3),0,0)... 16 | *rotz(DHtable(i,1)+q(i))*trans(0,0,DHtable(i,2)); 17 | H = H*H_i{1,i}; 18 | end 19 | 20 | end 21 | -------------------------------------------------------------------------------- /rotx.m: -------------------------------------------------------------------------------- 1 | function T = rotx(s) 2 | % 代码补充完整 3 | a = s; 4 | T = [1,0,0,0;... 5 | 0,cos(a),-sin(a),0;... 6 | 0,sin(a),cos(a),0;... 7 | 0,0,0,1]; 8 | end 9 | -------------------------------------------------------------------------------- /roty.m: -------------------------------------------------------------------------------- 1 | function T = roty(s) 2 | % 代码补充完整 3 | a = s; 4 | T = [cos(a),0,sin(a),0;... 5 | 0,1,0,0;... 6 | -sin(a),0,cos(a),0;... 7 | 0,0,0,1]; 8 | end -------------------------------------------------------------------------------- /rotz.m: -------------------------------------------------------------------------------- 1 | function T = rotz(s) 2 | % 代码补充完整 3 | a = s; 4 | T = [cos(a),-sin(a),0,0;... 5 | sin(a),cos(a),0,0;... 6 | 0,0,1,0;... 7 | 0,0,0,1]; 8 | end -------------------------------------------------------------------------------- /trans.m: -------------------------------------------------------------------------------- 1 | function T = trans(a,b,c) 2 | % trans 用于生成机器人的平移齐次变换矩阵 3 | % 输入参数： 4 | % a,b,c: 分别沿着x,y,z轴的移动量，单位(mm) 5 | % 输出参数： 6 | % T：4x4齐次变换矩阵 7 | 8 | % 版本号V1.0，编写于2022.10.1，修改于2022.10.31，作者：Chen 9 | 10 | % 函数主体 11 | % 判断输入变量的数量 12 | if nargin<1 13 | a=0;b=0;c=0; 14 | elseif nargin<2 15 | b=0;c=0; 16 | elseif nargin<3 17 | c=0; 18 | elseif nargin > 3 19 | error('输入变量过多！'); 20 | end 21 | 22 | % 根据平移齐次变换矩阵，定义矩阵 23 | T = [1,0,0,a;0,1,0,b;0,0,1,c;0,0,0,1]; 24 | 25 | end 26 | -------------------------------------------------------------------------------- /worckspace.m: -------------------------------------------------------------------------------- 1 | % workspace_computation 2 | clear; 3 | clc; 4 | 5 | % load DHtable 6 | figure(1) 7 | hold on 8 | robot.plot([0 0 0 0 0 0], plotopt{:}); 9 | hold off 10 | % angle limit for each joint 11 | format short; 12 | num=10000;%仿真点数 13 | limit = ones(6,2); 14 | limit(:,1)=-deg2rad(178); 15 | limit(:,2)=deg2rad(178); 16 | 17 | % set step for worskpace computation 18 | step = 10; 19 | % TODO 20 | q1_rand = limit(1,1) + rand(num,1)*(limit(1,2) - limit(1,1)); 21 | q2_rand = limit(2,1) + rand(num,1)*(limit(2,2) - limit(2,1)); 22 | q3_rand = limit(3,1) + rand(num,1)*(limit(3,2) - limit(3,1)); 23 | q4_rand = limit(4,1) + rand(num,1)*(limit(4,2) - limit(4,1)); 24 | q5_rand = limit(5,1) + rand(num,1)*(limit(5,2) - limit(5,1)); 25 | q6_rand = rand(num,1)*0; 26 | q = [q1_rand q2_rand q3_rand q4_rand q5_rand q6_rand]; 27 | 28 | % compute workspace 29 | tic; 30 | % TODO 31 | T_cell = cell(num,1); 32 | [T_cell{:,1}]=robot.fkine(q).t;%正向运动学仿真函数 33 | disp(['运行时间：',num2str(toc)]); 34 | figure('name','机械臂工作空间') 35 | hold on 36 | plotopt = {'noraise', 'nowrist', 'nojaxes', 'delay',0}; 37 | robot.plot([0 0 0 0 0 0], plotopt{:}); 38 | figure_x=zeros(num,1); 39 | figure_y=zeros(num,1); 40 | figure_z=zeros(num,1); 41 | for cout=1:1:num 42 | figure_x(cout,1)=T_cell{cout}(1); 43 | figure_y(cout,1)=T_cell{cout}(2); 44 | figure_z(cout,1)=T_cell{cout}(3); 45 | end 46 | plot3(figure_x,figure_y,figure_z,'r.','MarkerSize',3); 47 | hold off 48 | t0=toc; 49 | disp(['Computational time: ',num2str(t0)]); 50 | 51 | 52 | 53 | 54 | --------------------------------------------------------------------------------