一、基础知识
(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.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
实现效果
思考与讨论
根据调速跟踪位置的代码,写出用PID调节加速度跟踪位置的代码
长期从事机器人学相关研究,涉及机械臂、轮式机器人、四足机器人的建模及仿真,可共同探讨机器人相关问题,可指导课程设计及毕业设计,详询QQ:3531225003。