人工势场法进行六轴机械臂路径规划（MATLAB）

%% 清空变量 clear clc %% 初始化参数 startPoint = [-300 300 500]; % 起点 goalPoint = [300 350 600]; % 目标点 thr = 5; % 阈值（小于5停止迭代，结束路径规划） k = 0.000001; %引力增益系数 m = 2000000; %斥力增益系数 r = 100; %障碍物影响距离 （与障碍物距离小于100，斥力强，超过100，斥力减弱） iterStep = 1; % 每次移动的步长 iter = 1; % 计数器（实时表示当前的迭代次数） iterMax = 5000; % 最大迭代次数（迭代的终止条件） objMat = [-185 309 519; 103 349 568]; % 障碍物的位置信息（每一行代表一个障碍物的位置，列表示X,Y，Z的坐标） objNumber = size(objMat,1); % 获取障碍物数量（通过检测objMat的行数得到） objRadius = 90; % 障碍物半径 positionMat = zeros(3,iterMax); % 用于储存机械臂在每次迭代中的位置（它的大小为3行（分别为xyz坐标）* iterMax的列（代表最大的迭代次数）） gravitationForce = zeros(3,1); % 储存机械臂在每次迭代中受到的引力 repulsiveForce = zeros(3,1); % 储存机械臂在每次迭代中受到的斥力 sumForce = zeros(3,1); % 引力与斥力的和 angleGoal = zeros(3,1); % 机械臂与目标之间的角度的向量 angleObj = zeros(3,objNumber); % 储存机械臂与障碍物之间的角度信息 positionNow = startPoint; % 机械臂当前位置，初始化为起始点 positionMat(:,1) = positionNow; % 将机械臂的起始位置记录在positionMat矩阵的第一列 %% 迭代循环，寻找路径 while iter < iterMax % 如果迭代次数小于最大迭代次数，进行循环 %% 计算当前位置和目标位置以及障碍物的距离信息 [angleObj,angleGoal] = calcuAngle(objNumber,objMat,positionNow,angleObj,goalPoint); %% 计算引力 gravitationForce = calcuGravitationForce(positionNow,goalPoint,gravitationForce,k,angleGoal); GF(:,iter) = gravitationForce; %% 计算斥力 repulsiveForce = repulsiveForce*0; % 清零之前的斥力，每次迭代，斥力都需要重新计算 repulsiveForce = calcuRepulsiveForce(repulsiveForce,objNumber,positionNow,objMat,angleObj,r,m); RF(:,iter) = repulsiveForce; % 将计算得到的斥力 存储在矩阵 RF 的第 iter 列中。 % RF 矩阵用于存储每次迭代中计算得到的斥力，每一列对应一个迭代步骤。 % RF 矩阵的每一行代表斥力的一个分量（x、y、z 方向），每一列代表一个迭代步骤 %% 计算合力 sumForce = repulsiveForce+gravitationForce; %% 计算下一步位置（当前位置＋步长*合力的单位向量） positionNow = positionNow+iterStep*sumForce'./(sum(abs(sumForce))); iter = iter+1; positionMat(:,iter) = positionNow;% 将当前位置储存在位置矩阵的ITER列中 %% 判断当前位置与目标点位置是否小于阈值 if sqrt((positionNow(1)-goalPoint(1))^2+(positionNow(2)-goalPoint(2))^2+(positionNow(3)-goalPoint(3))^2)<thr break; end end %% 画障碍物（障碍物半径，障碍物数量，障碍物位置） drawObject(objRadius,objNumber,objMat) %% 画路径图 positionMat(:,iter+1:end) = []; % 清除不需要的数据（迭代结束的数据） line(positionMat(1,:),positionMat(2,:),positionMat(3,:),'Color','r','LineWidth',2) %画路径线，第一行为x,第二行为y，第三行为z plot3(startPoint(1),startPoint(2),startPoint(3),'bo','MarkerSize',4) % 画起始点，起始点名字：bo，蓝色，大小为4个像素 plot3(goalPoint(1),goalPoint(2),goalPoint(3),'bo','MarkerSize',4) %画结束点 text(startPoint(1),startPoint(2),startPoint(3),'start'); % 添加文本信息 text(goalPoint(1),goalPoint(2),goalPoint(3),'goal') xlabel('x') ylabel('y') zlabel('z') position = positionMat(:,1:20:end); % 创建变量，储存路径信息 jointAngle = zeros(size(position,2),6); % 创建一个全零矩阵 %% 配置机械臂参数 Theta = [0 -90 180 0 90 0]; Arm_1.alpha = -90*pi/180; Arm_1.a = 64.2; Arm_1.d = 149.58; Arm_1.theta = Theta(1)*pi/180; Arm_2.alpha = 0*pi/180; Arm_2.a = 305; Arm_2.d = 0; Arm_2.theta = Theta(2)*pi/180; Arm_3.alpha = 90*pi/180; Arm_3.a = 0; Arm_3.d = 0; Arm_3.theta = Theta(3)*pi/180; Arm_4.alpha = -90*pi/180; Arm_4.a = 0; Arm_4.d = 213.91; Arm_4.theta = Theta(4)*pi/180; Arm_5.alpha = 90*pi/180; Arm_5.a = 0; Arm_5.d = 0; Arm_5.theta = Theta(5)*pi/180; Arm_6.alpha = 0*pi/180; Arm_6.a = 0; Arm_6.d = 74.91; Arm_6.theta = Theta(6)*pi/180; % T = [nx ox ax px; % ny oy ay py; % nz oz az pz; % 0 0 0 1 ]; %% 建立机械臂模型 % theta d a alpha offset L(1)=Link([0 Arm_1.d Arm_1.a Arm_1.alpha ]); %定义连杆的D-H参数 L(2)=Link([-pi/2 Arm_2.d Arm_2.a Arm_2.alpha ]); L(3)=Link([pi Arm_3.d Arm_3.a Arm_3.alpha ]); L(4)=Link([0 Arm_4.d Arm_4.a Arm_4.alpha ]); L(5)=Link([pi/2 Arm_5.d Arm_5.a Arm_5.alpha ]); L(6)=Link([0 Arm_6.d Arm_6.a Arm_6.alpha ]); robot = SerialLink(L,'name','HyundaiRobot-6-dof'); T = zhengyundongx(Arm_1,Arm_2,Arm_3,Arm_4,Arm_5,Arm_6); for k1 = 1:size(position,2) T(1,4) = position(1,k1); T(2,4) = position(2,k1); T(3,4) = position(3,k1); inverseEightAngle = niyundongx(Arm_1,Arm_2,Arm_3,Arm_4,Arm_5,Arm_6,T); if k1 == 1 jointAngle(k1,:) = inverseEightAngle(1,:); else stepPreJoint = zhengyundongx2(Arm_1,Arm_2,Arm_3,Arm_4,Arm_5,Arm_6,jointAngle(k1-1,:)); sumJoint = inf; indexJoint = -1; for k2 = 1:size(inverseEightAngle,1) stepNextJoint = zhengyundongx2(Arm_1,Arm_2,Arm_3,Arm_4,Arm_5,Arm_6,inverseEightAngle(k2,:)); if sumJoint>sum(sum(stepNextJoint-stepPreJoint)) indexJoint = k2; sumJoint = sum(sum(stepNextJoint-stepPreJoint)); end end jointAngle(k1,:) = inverseEightAngle(indexJoint,:); end end hold on robot.plot(jointAngle(2:end,:))