0
点赞
收藏
分享

微信扫一扫

【机器人学2】二自由度机械臂建模及控制


一、基础知识

(1) PID控制:Kp比例、Ki积分、Kd微分;
(2) 坐标变换基础 (3) atan2(y,x)双参数反正切

二、具体案例

【机器人学2】二自由度机械臂建模及控制_自动驾驶


对机械臂进行逆运动学求解:

(1)

【机器人学2】二自由度机械臂建模及控制_Time_02

(2)

【机器人学2】二自由度机械臂建模及控制_matlab_03


(3)

【机器人学2】二自由度机械臂建模及控制_自动驾驶_04


(4)

【机器人学2】二自由度机械臂建模及控制_算法_05


(5)建立如下图所示的二自由度机械臂

【机器人学2】二自由度机械臂建模及控制_机械臂_06


P点坐标:

P=Rot(theta2,‘z’)*[a;0;0];

E点坐标:

E=P+Rot(theta2,‘z’)Rot(phi,‘z’)[b;0;0];

若给定的E点坐标与实际利用上式求解的坐标一致则说明逆解正确

代码实现

(1)正运动学

while 1
clc;
clear;

%圆心坐标
x0=110;
y0=110;
%半径
R=40;
%连杆长度
a=100;
b=100;
t=1;
for i=0:0.1:2*pi+0.1
    
    x=x0+R*cos(i);
    y=y0+R*sin(i);
%     x=(a+b)*cos(i);
%     y=(a+b)*sin(i);
    theta1=atan2(y,x); 
    % theta1=acos(x/sqrt(x*x+y*y));
    c=sqrt(x*x+y*y); % 末端到原点的距离
    theta3=acos((c*c+a*a-b*b)/(2*a*c));
    theta2=theta1-theta3; % 关节1 角度
    phi=pi-acos((a*a+b*b-c*c)/(2*a*b)); %关节2角度
    
    %连杆 P 位置
    P=Rot(theta2,'z')*[a;0;0];
    
    % 连杆末端位置(正运动学验证)
    E=P+Rot(theta2,'z')*Rot(phi,'z')*[b;0;0];
    
    %连杆连接处P的坐标
    Px=a*cos(theta2);
    Py=a*sin(theta2);
    
    %末端绘制圆的坐标
    rolx(t)=x;
    roly(t)=y;
    t=t+1;
    plotrobot(P(1),P(2),E(1),E(2),rolx,roly);  % 绘图验证
    pause(0.0000001)
    hold off
end
end

(2)PID

调速跟踪位置
 
clc;
clear;

%圆心坐标
x0=110;
y0=110;
%半径
R=40;
%连杆长度
a=100;
b=100;
t=1;

% 控制周期 
dt=0.05 % 秒
Time=[0]; % 当前时间
realTheta=[0];% 关节1实际角度
realPhi=[0];%关节2实际角度
errTehta=[0]; % 关节1 角度误差
errPhi=[0]; % 关节2 角度误差
errThetaSum=0;%关节1累积误差
errPhiSum=0;%关节2累积误差
derrTheta=0;%关节1 本次角度误差与上一次角度误差的差值
derrPhi=0;%关节2 本次角度误差与上一次角度误差的差值
% PID 参数(关节1)
Kp1=10;
Ki1=2;
Kd1=4;
% PID 参数(关节2)
Kp2=8;
Ki2=1;
Kd2=2;

for i=0:1:150
    theta2=i/150*2*pi;
    phi=i/150*pi;

    
    %连杆 P 位置
    P=Rot(theta2,'z')*[a;0;0];
    
    % 连杆末端位置(正运动学验证)
    E=P+Rot(theta2,'z')*Rot(phi,'z')*[b;0;0];
    
    % PID 控制
    w1=Kp1*(theta2-realTheta(end))+derrTheta*Kd1+Ki1*errThetaSum;%关节1 瞬时角速度
    w2=Kp2*(phi-realPhi(end))+derrPhi*Kd2+Ki1*errPhiSum;%关节2 瞬时角速度
    realTheta(end+1)=realTheta(end)+w1*dt;
    realPhi(end+1)=realPhi(end)+w2*dt;
    
    % 误差
    errTehta(end+1)=theta2- realTheta(end);
    errPhi(end+1)=phi- realPhi(end);
    errThetaSum=errThetaSum+errTehta(end);
    errPhiSum=errPhiSum+errPhi(end);
    derrTheta= errTehta(end)-errTehta(end-1);
    derrPhi= errPhi(end)-errPhi(end-1);
    % 当前时间
    Time(end+1)=dt*i;
    
    %连杆 P 位置
    realP=Rot(realTheta(end),'z')*[a;0;0];
    % 连杆末端位置(正运动学验证)
    realE=P+Rot(realTheta(end),'z')*Rot(realPhi(end),'z')*[b;0;0];
    
    %末端绘制圆的坐标
    rolx(t)=E(1);
    roly(t)=E(2);
    t=t+1;
    subplot(121);
    plotrobot(realP(1),realP(2),realE(1),realE(2),rolx,roly);  % 绘图验证
    hold off
    subplot(122);
    plot(Time,errTehta,'k',Time,errPhi,'r');
    pause(0.0000001)
   
end

(3)绘图代码

function plotrobot(Px,Py,x,y,rolx,roly)
xx=0;
yy=0;
plotline(xx,Px,yy,Py,'r')%
%axis equal;
axis square
hold on
plotline(Px,x,Py,y,'r')%
hold on
%axis equal;
plot(rolx,roly);
axis([-300,300,-300,300]);

end

function plotline(x1,x2,y1,y2,c)
xx=[x1,x2];
yy=[y1,y2];
plot(xx,yy,c)
end

实现效果

【机器人学2】二自由度机械臂建模及控制_自动驾驶_07

思考与讨论

根据调速跟踪位置的代码,写出用PID调节加速度跟踪位置的代码

长期从事机器人学相关研究,涉及机械臂、轮式机器人、四足机器人的建模及仿真,可共同探讨机器人相关问题,可指导课程设计及毕业设计,详询QQ:3531225003。


举报

相关推荐

0 条评论