机械臂避障路径规划仿真 路径平滑

结合前两部分，蚁群算法和碰撞检测，这部分将以上两部分的结果进行整合绘图展示，整体完成机械臂在通过障碍物时，保证机械臂不碰到机械臂前提下，减小路径长度，保证运动平滑，很好的完成机械臂避障工作。

clear;
clc
load（‘output1.mat‘）
ty=output1（:1）;
ty=ty‘;
tx=1:21;
tx1=1:0.2:21;
ty1=interp1（txtytx1‘cubic‘）;

load（‘output11.mat‘）
tyy=output11;
tyy_temp=tyy;
for i =1:21
   tyy（i:）=tyy_temp（22-i:）;
end


figure  
plot（txty‘o‘tx1ty1‘r‘）;  
title（‘三次多项式插值‘）  

q_c=[];
q_c1=[];
for i=1:6
    ty=output1（:i）;
    ty=ty‘;
    tx=1:21;
    tx1=1:0.2:21;
    q_c（i:）=interp1（txtytx1‘cubic‘）;
end
for i=1:6
    tyy1=tyy（:i）;
    tyy1=tyy1‘;
    txx=1:21;
    txx1=1:0.2:21;
    q_c1（i:）=interp1（txxtyy1txx1‘cubic‘）;
end
q_c=q_c‘;
q_c1=q_c1‘;

figure
bil=35;
bil1=bil*0.5;
t1 = [24 27;
      2 41;
      2 11];
x1=t1（12）*bil1;y1=t1（22）*bil1;z1=（t1（31）-1）*bil1;%角点
l=（t1（12）-t1（11）+1）*bil1;w=（t1（22）-t1（21）+1）*bil1;h=（t1（32）-t1（31）+1）*bil1;%长，宽，高
[a1b1c1]=meshgrid（[0 1]）;
p=alphaShape（l*a1（:）-（l-x1）w*b1（:）-（w-y1）h*c1（:）-（0-z1））;
plot（p‘edgecolor‘‘none‘）
xlabel（‘x‘）;ylabel（‘y‘）;zlabel（‘z‘）;
camlight
hold on
axis（[-10001000-10001000-10001500]）
view（[0500]）

bil1=bil*0.5;
t1 = [24 27;
      2 41;
      32 41];
x1=t1（12）*bil1;y1=t1（22）*bil1;z1=（t1（31）-1）*bil1;%角点
l=（t1（12）-t1（11）+1）*bil1;w=（t1（22）-t1（21）+1）*bil1;h=（t1（32）-t1（31）+1）*bil1;%长，宽，高
[a1b1c1]=meshgrid（[0 1]）;
p=alphaShape（l*a1（:）-（l-x1）w*b1（:）-（w-y1）h*c1（:）-（0-z1））;
plot（p‘edgecolor‘‘none‘）
xlabel（‘x‘）;ylabel（‘y‘）;zlabel（‘z‘）;
camlight
hold on
axis（[-10001000-10001000-10001500]）
view（[0500]）

%%
%建立机器人模型
% theta d a alpha offset
L1=link（[0 0 0 0 0 ]‘modified‘）; %连杆的D-H参数
L2=link（[0 149.09 0 -pi/2 0 ]‘modified‘）;
L3=link（[0 0 431.8 0 0 ]‘modified‘）;
L4=link（[0 433.07 20.32 -pi/2 0 ]‘modified‘）;
L5=link（[0 0 0 pi/2 0 ]‘modified‘）;
L6=link（[0 0 0 -pi/2 0 ]‘modified‘）;
robot=Seriallink（[L1 L2 L3 L4 L5 L6]‘name‘‘ABB1410‘‘base‘  ...
transl（0 0 0.62）* trotz（0））; %连接连杆，机器人取名puma560
q=[q_c1;q_c];
robot.plot（q）;
t=robot.fkine（q）;%运动学正解 

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2020-07-15 22:51  机械臂避障路径规划仿真\
     文件        1068  2020-06-30 20:09  机械臂避障路径规划仿真\output1.mat
     文件        1062  2020-07-02 14:10  机械臂避障路径规划仿真\output11.mat
     文件        2009  2020-07-03 21:34  机械臂避障路径规划仿真\sancichazhi.m