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');