1 内容介绍
基于简化几何解法的六轴机械臂位置规划附matlab代码
2 部分代码
clc;
clear;
%载入数据
importfile('shuiping.mat');
theta_shuiping=shuiping;
%参数,单位/m
a=0.1;
L1=0.43;
L2=0.65;
L3=0.7;
L4=0.088;
H=pi/180;
% %给定初始和终止角度
% alpha1_st=80*pi/180;
% alpha2_st=45*pi/180;
% alpha1_en=50*pi/180;
% %角度值
% alpha1_st1=80;
% alpha2_st1=45;
% alpha1_en1=50;
% jiange=3;
% %开始计算
% deltah=L2*sin(alpha1_st)-L3*sin(alpha2_st);
% for i=1:(alpha1_st1-alpha1_en1)/jiange+1
% alpha1(i)=alpha1_st1-(i-1)*jiange;
% end
alpha1_st=theta_shuiping(1)*H;
alpha2_st=45*pi/180;
deltah=L2*sin(alpha1_st)-L3*sin(alpha2_st);
alpha1=theta_shuiping;
theta2=alpha1;
alpha2=asin((L2*sin(alpha1*H)-deltah)/L3);
theta3=90-theta2-alpha2/H;
theta5=90-alpha2/H;
%画图
r2_s=a;
h2_s=L1; %L2起点
r3_s=a+L2*cos(alpha1*H);
h3_s=L1+L2*sin(alpha1*H); %L2终点，L3起点；
r4_s=r3_s+L3*cos(alpha2);
h4_s=h3_s-L3*sin(alpha2); %L3终点，L4起点；
r4_e=r4_s;
h4_e=h4_s-L4;
%画连杆2，3，4
figure(1)
N=size(theta2,2);
for i=1:N
a=drawline(r2_s,r3_s(i),h2_s,h3_s(i));
a=drawline(r3_s(i),r4_s(i),h3_s(i),h4_s(i));
a=drawline(r4_s(i),r4_e(i),h4_s(i),h4_e(i));
end
%画底座和连杆1
a=drawline2(-0.2,0.2,0,0);
a=drawline2(0,0,0,0.2);
a=drawline2(0,0.1,0.2,0.43);
plot(r4_e,h4_e,'b','LineWidth',3)
%题目横纵坐标及范围
axis([-0.25 1.1 0 1.2])
xlabel('r/m');
ylabel('z/m');
grid on
axis equal
% title('机械臂水平方向直线运动');
figure(2)
plot(theta2,'-');
hold on
plot(theta3,'-.');
hold on
plot(theta5,'--');
% title('各关节运动曲线');
legend('theta2','theta3','theta5')
axis([1 20 -40 90])
xlabel('序列号');
ylabel('关节角/°');
set(gcf,'color','white');
set(gca,'FontSize',15);
function a=drawline(x1,x2,y1,y2)
x=[x1,x2];
y=[y1,y2];
plot(x,y,'k','LineWidth',0.8)
hold on
a=1;
set(gcf,'color','white');
set(gca,'FontSize',15);
end
function a=drawline2(x1,x2,y1,y2)
x=[x1,x2];
y=[y1,y2];
plot(x,y,'k','LineWidth',2)
hold on
a=1;
end
3 运行结果
4 参考文献
[1]鲁飞. 基于改进蚁群算法的机械臂路径规划研究[D]. 合肥工业大学.
