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

    function Theta = parametric( a,t )
%此为正弦函数参数化关方法来表示关节角的函数
%   此处显示详细说明

theta_i0 = [90 90 90 0 90 -90 0]*pi/180;
theta_id = [90 80 70 25 90 -75 0]*pi/180;
delta_i1 = pi;
delta_i2 = 0;
tf = 20;

ai0 = asin((theta_i0-delta_i2)/delta_i1);
ai1 = 0;
ai2 = 0;
ai6 = a(2:2:14);
ai7 = a(1:2:13);

ai3 = 3*ai7*tf^7+ai6*tf^6-10*(asin((theta_id-delta_i2)/delta_i1)-ai0);
ai3 = -ai3/tf^3;

ai4 = 8*ai7*tf^7+3*ai6*tf^6-15*(asin((theta_id-delta_i2)/delta_i1)-ai0);
ai4 = ai4/tf^4;

ai5 = 6*ai7*tf^7+3*ai6*tf^6-6*(asin((theta_id-delta_i2)/delta_i1)-ai0);
ai5 = -ai5/tf^5;

theta = delta_i1*sin((ai7*t^7+ai6*t^6+ai5*t^5+ai4*t^4+ai3*t^3+ai2*t^2+ai1*t+ai0))+delta_i2;
theta1 = delta_i1*cos((ai7*t^7+ai6*t^6+ai5*t^5+ai4*t^4+ai3*t^3+ai2*t^2+ai1*t+ai0))...
         .*(7*ai7*t^6+6*ai6*t^5+5*ai5*t^4+4*ai4*t^3+3*ai3*t^2+2*ai2*t^1+ai1);
theta2 = -delta_i1*sin((ai7*t^7+ai6*t^6+ai5*t^5+ai4*t^4+ai3*t^3+ai2*t^2+ai1*t+ai0))...
         .*(7*ai7*t^6+6*ai6*t^5+5*ai5*t^4+4*ai4*t^3+3*ai3*t^2+2*ai2*t^1+ai1)...
         +delta_i1*cos((ai7*t^7+ai6*t^6+ai5*t^5+ai4*t^4+ai3*t^3+ai2*t^2+ai1*t+ai0))...
         .*(42*ai7*t^5+30*ai6*t^4+20*ai5*t^3+12*ai4*t^2+6*ai3*t+2*ai2);

     
Theta = [theta' theta1' theta2'];