机器人运动学、轨迹规划、控制一体化仿真

 运动学模型建立

圆弧轨迹规划 

  

 伺服控制系统仿真

Matlab源码

clc
clear all
close all
% 尺度参数
b1=0.570;
b2=0.320;
b3=0.320;
a1=0.135;a2=0.135;a3=0.135;
e=0.345; 
%% 运动规律
a_max=10;
R=0.6;
T=sqrt(2*pi*2*pi*R/a_max);
t=linspace(0,T,100);
q1=zeros(size(t));
q2=zeros(size(t));
q3=zeros(size(t));
x_deta=zeros(size(t));
y_deta=zeros(size(t));
z_deta=zeros(size(t));
z_start=1;
for i=1:length(t)
    i;
    s(i)=a_max*T^2/(2*pi)*(t(i)/T-1/(2*pi)*sin(2*pi*t(i)/T));
    theta_deta=s(i)/R;
    x_deta(i)=-R*sin(theta_deta);
    y_deta(i)=-R*cos(theta_deta);
    z_deta(i)=z_start;
    %% 位置逆解
    x_p=x_deta(i);y_p=y_deta(i);z_p=z_deta(i);
    r_p=[x_p;y_p;z_p];
    q4=sqrt(r_p(1)^2+r_p(2)^2+r_p(3)^2)-e;
    rp=q4+e;
    theta=asin(x_p/rp);
    fai=atan(-y_p/z_p);
    Rp=[cos(theta) 0 sin(theta);
        sin(fai)*sin(theta) cos(fai) -sin(fai)*cos(theta);
        -cos(fai)*sin(theta) sin(fai) cos(fai)*cos(theta)];
    a10=[a1*cos(-pi/2);a1*sin(-pi/2);e];a20=[a2*cos(0);a2*sin(0);e];a30=[a3*cos(pi);a3*sin(pi);e];
    a_1=Rp*a10;a_2=Rp*a20;a_3=Rp*a30;
    b_1=[b1*cos(-pi/2);b1*sin(-pi/2);0];b_2=[b2*cos(0);b2*sin(0);0];b_3=[b3*cos(pi);b3*sin(pi);0];
    w_4=[sin(theta);-sin(fai)*cos(theta);cos(fai)*cos(theta)];
    q1(i)=norm(r_p+a_1-b_1);
    q2(i)=norm(r_p+a_2-b_2); 
    q3(i)=norm(r_p+a_3-b_3);
end
time(1:length(t))=t;
q1s=[time;q1-1.2683]';
q2s=[time;q2-1.5225]';
q3s=[time;q3-1.5225]';

Simulink仿真模型

仿真结果

  

仿真源文件地址 

https://pan.baidu.com/s/1zaXdfW8qkT1AcL1vPbEqlg?pwd=vqjl 提取码：vqjl