基于RRT算法的六自由度机械臂轨迹规划

clear all; close all; clc; %theta d a alpha L(1)=Link([0 330 -50 pi/2 ] , 'standard'); L(2)=Link([0 0 440 0 ],'standard'); L(3)=Link([0 0 35 -pi/2 ],'standard'); L(4)=Link([0 420 0 pi/2 ],'standard'); L(5)=Link([0 0 0 -pi/2 ],'standard'); L(6)=Link([0 0 0 0 ] , 'standard'); robot=SerialLink(L,'name','Funuc'); % X = zeros(1,6); start_pos = [0, pi/2, 0, 0, 0, 0]; %初始位置 end_pos = [-41.1859,26.6172,-175.3405,0,-31.2767,-6.1189]*pi/180; %目标位置 %关节范围 min_pos = -[180,120,180,180,125,180]*pi/180; max_pos = [180,120,180,180,125,180]*pi/180; %障碍物中心位置，半径 obstacle_centers = [-300,-350,200;100,-200,300;100,-200,800]; obstacle_radii = [100,80,80]; %RRT算法参数 num_nodes = 1000; max_dist = 0.1; goal_dist_thresh = 0.08; random_goal_prob = 0.1; node_list(1).pos = start_pos; node_list(1).parent = 0; for i=2:num_nodes if rand()<random_goal_prob rnd_pos = end_pos; else rnd_pos = min_pos + (max_pos - min_pos).*rand(1,6); end [nearest_node,nearest_dist] = find_nearest_node(node_list,rnd_pos); new_pos = extend(nearest_node,rnd_pos,max_dist); if check_collision(new_pos,obstacle_centers,obstacle_radii) == 1 %flag标志位，1为未碰撞 node_list(i).pos = new_pos; node_list(i).parent = nearest_node.id; else node_list(i).pos = node_list(i-1).pos; node_list(i).parent = node_list(i-1).parent; end if norm(node_list(i).pos - end_pos)<goal_dist_thresh break; end end path = []; cur_node = node_list(end); while cur_node.parent ~= 0 path = [cur_node.pos;path]; cur_node = node_list(cur_node.parent); end % path = [start_pos;path;end_pos]; path = [start_pos;path]; %绘图 figure(); obstacles(obstacle_centers,obstacle_radii); hold on %%寻找路径过程 for i = 1:size(struct2cell(node_list),3) t(i,:) = node_list(i).pos; p(i,:) = node_list(i).parent; end x= [];y=[];z=[]; % figure() for i = 1:size(t,1) T = robot.fkine(t(i,:)); T = double(T); x(i) = T(1,4); y(i) = T(2,4); z(i) = T(3,4); end for i = 1:size(t,1) for j = 1:size(t,1) if p(j) == i plot3([x(i),x(j)],[y(i),y(j)],[z(i),z(j)]); hold on end end end %最终路径 for i = 1:size(path,1) T = robot.fkine(path(i,:)); T = double(T); X(i) = T(1,4); Y(i) = T(2,4); Z(i) = T(3,4); end plot3(X,Y,Z,'-r','LineWidth',1) % q = []; % for i = 1:size(path,1)-1 % [Q,~,~] = jtraj(path(i,:),path(i+1,:),10); % q = [q;Q]; % end % x = []; y = []; z = []; % for i = 1:size(q,1) % T = robot.fkine(q(i,:)); % T = double(T); % x(i) = T(1,4); % y(i) = T(2,4); % z(i) = T(3,4); % end % plot3(x,y,z,'-k','LineWidth',2) % robot.plot(q); % axis equal; xlabel('X'); ylabel('Y'); zlabel('Z'); title('6D RRT'); function [nearest_node,nearest_dist] = find_nearest_node(node_list,rnd_pos) for i = 1:size(struct2cell(node_list),3) distances(i) = vecnorm(node_list(i).pos-rnd_pos,2); end [a,b] = min(distances); nearest_dist = a; nearest_node.pos = node_list(b).pos; nearest_node.id = b; end function new_pos = extend(nearest_node,rnd_pos,max_dist) dist = norm(rnd_pos-nearest_node.pos); if dist>max_dist new_pos = nearest_node.pos+max_dist*(rnd_pos-nearest_node.pos); else new_pos = rnd_pos; end end %%碰撞检测 function feasible = check_collision(new_pos,obstacle_centers,obstacle_radii) feasible = true; %theta d a alpha L(1)=Link([0 330 -50 pi/2 ] , 'standard'); L(2)=Link([0 0 440 0 ],'standard'); L(3)=Link([0 0 35 -pi/2 ],'standard'); L(4)=Link([0 420 0 pi/2 ],'standard'); L(5)=Link([0 0 0 -pi/2 ],'standard'); L(6)=Link([0 0 0 0 ] , 'standard'); robot=SerialLink(L,'name','Funuc'); [~,all] = robot.fkine(new_pos); T = double(all); %检测第2，3，45，6连杆和2，3，5关节碰撞 link2_point1 = [0,0,160]; link2_point2 = T(1:3,4,1)'; r_1 = 75 ; %连杆2 link3_point1 = T(1:3,4,1)'; link3_point2 = T(1:3,4,2)'; r_3 = 70 ; %连杆3 link45_point1 = T(1:3,4,3)'; link45_point2 = T(1:3,4,6)'; r_5 = 60; %连杆456 joint2 = T(1:3,4,1)'; r2 = 95; %关节2 joint3 = T(1:3,4,3)'; r4 = 75; %关节3 joint56 = T(1:3,4,5)'; r6 = 80; %关节56 for i = 1:size(obstacle_radii,2) if dis_pp(joint2,obstacle_centers(i,:)) <= r2+obstacle_radii(i) feasible = false; % disp([关节2与障碍物碰撞']) end if dis_pp(joint3,obstacle_centers(i,:)) <= r2+obstacle_radii(i) feasible = false; % disp([关节3与障碍物碰撞']) end if dis_pp(joint56,obstacle_centers(i,:)) <= r2+obstacle_radii(i) feasible = false; % disp([关节56与障碍物碰撞']) end if dis_pl(obstacle_centers(i,:),link2_point1,link2_point2) <= r_1+obstacle_radii(i) feasible = false; % disp([障碍物与连杆2碰撞']) end if dis_pl(obstacle_centers(i,:),link3_point1,link3_point2) <= r_1+obstacle_radii(i) feasible = false; % disp([障碍物与连杆3碰撞']) end if dis_pl(obstacle_centers(i,:),link45_point1,link45_point2) <= r_1+obstacle_radii(i) feasible = false; % disp([障碍物与连杆456碰撞']) end end end function d_pp = dis_pp(point1,point2) %求关节至球体最小距离 d_pp = sqrt(dot(point2-point1,point2-point1)); end function d_pl = dis_pl(point1,point2,point3) %求障碍物球体与连杆之间的最小距离 % x1 = point1(1);y1 = point1(2);z1 = point1(3); %点 % x2 = point2(1);y2 = point2(2);z2 = point2(3); %线段起点1 % x3 = point3(1);y3 = point3(2);z3 = point3(3); %线段终点2 s = dot(point1-point2,point3-point2)/dot(point3-point2,point3-point2); point4 = point2+s*(point3-point2); if s<0 d_pl = sqrt(dot(point1-point2,point1-point2)); end if (0<=s)&&(s<=1) d_pl = sqrt(dot(point1-point4,point1-point4)); end if s>1 d_pl = sqrt(dot(point1-point3,point1-point3)); end end %%创建障碍物 function obstacles(obstacle_centers,obstacle_radii) for i = 1:size(obstacle_radii,2) t=linspace(0,pi,50); p=linspace(0,2*pi,50); [theta,phi]=meshgrid(t,p); x=obstacle_centers(i,1)+obstacle_radii(i)*sin(theta).*sin(phi); y=obstacle_centers(i,2)+obstacle_radii(i)*sin(theta).*cos(phi); z=obstacle_centers(i,3)+obstacle_radii(i)*cos(theta); plot2d=surf(x,y,z,'linestyle','none','FaceAlpha',0.8); hold on end end