www.gusucode.com > matlab编程求解机器人的正逆解 采用遗传算法对机器人进行路径规划源码程序 > code/基本代码/Path_program.m

    
%求初始时刻的关节角
Target = [0,0,-1,5.5;-1,0,0,0.75;
          0,1,0,0.5;0,0,0,1];
theta = solve(Target);

%给定目标点及运行时间
qf_x = 5;qf_y =1;qf_z = 2;time = 5;
Target(1,4) = qf_x;Target(2,4) = qf_y;Target(3,4) = qf_z;
tf = solve(Target);

%求三次多项式的系数
a2 = zeros(1,7);a3 = zeros(1,7);
a2 = 3/time^2*(tf-theta);
a3 = -3/time^3*(tf-theta);

Theta = cell(1,7);
for i=1:7
    k=1;
    for t = 0:0.1:5;
    Theta{i}(k) = theta(i)+a2(i)*t^2+a3(i)*t^3;
    k=k+1;
    end
end

t = 0:0.1:5;
% F1 = plot(t,Theta{1}*180/pi,'r');title('theta1');
% F2 = plot(t,Theta{2}*180/pi,'g');title('theta2');
% F3 = plot(t,Theta{3}*180/pi,'g');title('theta3');
% F4 = plot(t,Theta{4}*180/pi,'g');title('theta4');
%  F5 = plot(t,Theta{5}*180/pi,'g');title('theta5');
%  F6 = plot(t,Theta{6}*180/pi,'g');title('theta6');
 F7 = plot(t,Theta{7}*180/pi,'g');title('theta7');