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

一、基础知识

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

二、具体案例

对机械臂进行逆运动学求解：
(1)

(2)
(3)
（4）
（5）建立如下图所示的二自由度机械臂

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.1x=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:150theta2=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 offsubplot(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]);endfunction plotline(x1,x2,y1,y2,c)
xx=[x1,x2];
yy=[y1,y2];
plot(xx,yy,c)
end

实现效果

思考与讨论

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

长期从事机器人学相关研究，可指导课程设计及毕业设计，详询QQ：3531225003。