RRPR机械臂simulink建模及仿真
RRPR机械臂建模及仿真
（1）机器人建模
在solidworks中绘制机械臂的简单模型，将零件文件导入simulink中，完成坐标系变换的设置，完成机械臂的搭建。
Simulink机械臂搭建图
完成搭建后使用改进DH方法建立连杆坐标系，完成DH参数表，并求解工具坐标系到基坐标系的变换矩阵。
DH模型图
DH参数表为：
通过程序fk_test求得工具坐标系到基坐标系的变换矩阵为：
最后设置机械臂材料的密度为2.7g/cm3
，完成建模。
Simulink 仿真结果图
（2）反解模块
function [theta1,theta2,d,theta3] = my_ik(x,y,z,fi)
%输入x,y，z以及抓手的旋转角度
%输出关节空间的位置
p=sqrt(x^2+y^2);
u0=sqrt(1-((p^2+49/400-1/16)/((7/10)*p))^2); %也可取负值
u1=(p^2+49/400-1/16)/(7/10*p);
theta1=atan2(u0,u1)+atan2(y,x);
theta2=atan2(4*(cos(theta1)*y-sin(theta1)*x),4*(cos(theta1)*x+sin(theta1)*y-7/20));
d=1/8-z;
theta3=theta1+theta2-atan2(sin(fi),cos(fi));
end
（3）轨迹规划模块
采用关节空间的规划方法，根据教材上介绍的具有中间点的路径的三次多项式方法，当中间点速度已知时，可以得到
function [sdd,sd,s] = my_traj(Q,tf,t)
%输入关节经过的位置Q，Q为各个位置构成的向量，tf为位置点间的间隔时间，t为自然时间（仿真时间）
%输出轨迹，轨迹速度，轨迹加速度
n=length(Q)-1;
qd=[0];
for i=1:n-1
qd=[qd my_vmid(Q(i),Q(i+1),Q(i+2),tf)]; %求中间点速度
end
qd0=qd;
qdf=[qd(2:end) 0];
if(t==0) %跳过对0时间点的运算，否则仿真不成功
i=1;
else
i=ceil(t/tf); %向上取整
end
X = [0 0 0 1;
tf^3 tf^2 tf 1;
0 0 1 0;
3*tf^2 2*tf 1 0
];
coeffs = (X \ [Q(i) Q(i+1) qd0(i) qdf(i)]')';
coeffs_d = coeffs(1:3) .* (3:-1:1);
coeffs_dd = coeffs_d(1:2) .* (2:-1:1);
p = polyval(coeffs, t-(i-1)*tf);
pd = polyval(coeffs_d, t-(i-1)*tf);
pdd = polyval(coeffs_dd, t-(i-1)*tf);
s=p;
sd=pd;
sdd=pdd;
end
function v=my_vmid(x1,x2,x3,tf)
v1=(x2-x1)/tf;
v2=(x3-x2)/tf;
if(v1*v2>=0)
v=(v1+v2)/2;
else
v=0;
end
end
（4）单关节控制器
f为输出的力矩，q为位置反馈，w为速度反馈；qdd_in，qd_in，q_in分别为轨迹规划模块产生的期望的加速度，速度，位置的值。
通过仿真调节PID参数，得到
关节1控制器的Kp=74500，Kv=332，Ki=230000；位置误差曲线为
关节2控制器的Kp=89000，Kv=75，Ki=500000；位置误差曲线为
关节3控制器的Kp=365000，Kv=490，Ki=490000；位置误差曲线为
关节4控制器的Kp=99000，Kv=130，Ki=350000；位置误差曲线为
综上，四个控制器的误差总体控制在1%左右。
机械臂simulink仿真模型图
这是笔者完成的一个课程大作业，源文件已上传，于（2020.6.12）
链接：https://download.csdn.net/download/weixin_42075625/12518458