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