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

    function  F = evaluate(a)
%计算目标函数并求适应度
%   此处显示详细说明

%机械臂参数初始化
Initialization

%建立惯性系及求出各刚体质心坐标
CoorTransform  

t = 0;
K = cell(1,7);            %角速度的方向
JT = cell(1,7);
Jr = cell(1,7);
V_w = cell(1,7);
Jtw = zeros(3,7);
Hw = I_sat;
Hwf = zeros(3,7);
Hb2 = zeros(3,3);
Hb3 = zeros(3,3);
Jm = [];
delta_t = 0.05;

 while t<20
    %求给定时刻的关节角，角速度，角加速度
    temp = parametric(a,t);
    theta = temp(:,1);
    dtheta = temp(:,2);
    d2theta = temp(:,3);
    
    R = CD_T20(theta);
    r_0 = R{1};
    r = R{2}(:,1:7);
    p = R{2}(:,7:14);
    %求角速度的方向序列
    K{1} = r{1}-r_0;
    for i=1:6
        K{i+1} = p{i+1}-p{i};
    end
    
    %求解Jti和Jri矩阵
    for i=1:7
        V_w{i} = cross(K{i},(r{i}-p{i}));
        Jt{i} = [];
        Jr{i} = [];
        for j = 1:i
            Jt{i} = [Jt{i} V_w{j}];
            Jr{i} = [Jr{i} K{i}];
        end
        Jt{i} = [Jt{i} zeros(3,7-i)];
        Jr{i} = [Jr{i} zeros(3,7-i)];
    end
    
    %求解Jtw和Hw
    for i=1:7
        Jtw = m(i)*Jt{i}+Jtw;
        temp = r{i}-r_0;
        temp1 = Cross_operator(temp);
        Hwf = Hwf + I{i}*Jr{i}+m(i)*temp1*Jt{i};
        Hw = Hw+I{i}+m(i)*(temp1')*temp1;
        Hb2 = Hb2+m(i)*(temp1');
        Hb3 = Hb3+m(i)*temp1;
    end
    
    %求解Hb和Hbm
    Hb = [eye(3) Hb2;Hb3 Hw];
    Hbm = [Jtw;Hwf];
    Jbm = -Hb\Hbm;
    
    %求解Jm
    for i=1:7
        temp = [cross(K{i},p{7}-p{i});K{i}];
        Jm = [Jm temp];
    end
    
    %求解Jg、Jb
    Jb = [eye(3),-Cross_operator(p{7}-r_0);zeros(3),eye(3)];
    Jg = Jm+Jb*Jbm;
    Jbm_v = Jbm(1:3,:);Jg_v = Jg(1:3,:);
    Jbm_w = Jbm(4:6,:);Jg_w = Jg(4:6,:);
    
    V0 = Jbm_v*dtheta;Ve = Jg_v*dtheta;
    W0 = Jbm_w*dtheta;We = Jg_w*dtheta;
    
    Coordinate = Coordinate+V0*delta_t;
    
    %求解t时刻的位置和姿态
    dQ0 = [-q0';yita0*I_sum-Cross_operator(q0)]*Jbm_w*dtheta;
    dQe = [-qe';yitae*I_sum-Cross_operator(qe)]*Jg_w*dtheta;
    dPe = Jg_v*dtheta;
    
    Q_0 = Q0+dQ0*delta_t;
    Q_e = Qe+dQe*delta_t;
    P_e = Pe+dPe*delta_t;
    
    
    t = t+delta_t;
    
    %雅可比矩阵初始化进行下一次循环
    
    Jtw = zeros(3,7);
    Hw = I_sat;
    Hwf = zeros(3,7);
    Hb2 = zeros(3,3);
    Hb3 = zeros(3,3);
    Jm = [];
    
 end
    
%求解姿态与位置偏差量
yita_0f = Q_0(1);
q_0f = Q_0(2:4);
yita_ef = Q_e(1);
q_ef = Q_e(2:4);

delta_qe = yita_ef*q_ed-yita_ed*q_ef-Cross_operator(q_ef)*q_ed;
delta_pe = Ped-P_e;
delta_q0 = yita_0f*q0-yita0*q_0f-Cross_operator(q_0f)*q0;

F1 = [delta_qe;delta_pe];
F2 = delta_q0;    

F = sum(abs(F1))*sum(abs(F2));
disp('完成一次适应度评估');