function [robot_end_pose] = robot_forward_kinematics(initial_theta)

    if length(initial_theta) ~= 6
        error('关节角度初始值必须为1x6向量');
    end

    % 机器人DH结构参数（固定参数）
    d1 = 0;      a1 = 0.00;   alfa1 = -pi/2;  seta1_offset = 0.00;
    d2 = 305;    a2 = 0;      alfa2 = pi/2;   seta2_offset = pi/2;
    d3 = 0;      a3 = 785;    alfa3 = 0;      seta3_offset = pi/2;
    d4 = 0;      a4 = 774;    alfa4 = 0;      seta4_offset = 0;
    d5 = 0;      a5 = 0;      alfa5 = pi/2;   seta5_offset = pi/2;
    d6 = 485;    a6 = 0;      alfa6 = 0;      seta6_offset = 0; 
    seta1 = initial_theta(1);
    seta2 = initial_theta(2);
    seta3 = initial_theta(3);
    seta4 = initial_theta(4);
    seta5 = initial_theta(5);
    seta6 = initial_theta(6);

    seta1_offset=0.00;seta2_offset=pi/2;seta3_offset=pi/2;seta4_offset=0;seta5_offset=pi/2;seta6_offset=0;
    
    seta1=seta1+seta1_offset;
    seta2=seta2+seta2_offset;
    seta3=seta3+seta3_offset;
    seta4=seta4+seta4_offset;
    seta5=seta5+seta5_offset;
    seta6=seta6+seta6_offset;
    
    n_x =  cos(seta1)*sin(seta2)*sin(seta6) - cos(seta6)*(sin(seta5)*(cos(seta4)*(cos(seta3)*sin(seta1) + cos(seta1)*cos(seta2)*sin(seta3)) - sin(seta4)*(sin(seta1)*sin(seta3) - cos(seta1)*cos(seta2)*cos(seta3))) + cos(seta5)*(cos(seta4)*(sin(seta1)*sin(seta3) - cos(seta1)*cos(seta2)*cos(seta3)) + sin(seta4)*(cos(seta3)*sin(seta1) + cos(seta1)*cos(seta2)*sin(seta3))));
    n_y =  cos(seta6)*(sin(seta5)*(cos(seta4)*(cos(seta1)*cos(seta3) - cos(seta2)*sin(seta1)*sin(seta3)) - sin(seta4)*(cos(seta1)*sin(seta3) + cos(seta2)*cos(seta3)*sin(seta1))) + cos(seta5)*(cos(seta4)*(cos(seta1)*sin(seta3) + cos(seta2)*cos(seta3)*sin(seta1)) + sin(seta4)*(cos(seta1)*cos(seta3) - cos(seta2)*sin(seta1)*sin(seta3)))) + sin(seta1)*sin(seta2)*sin(seta6);
    n_z =  cos(seta2)*sin(seta6) + cos(seta6)*(cos(seta5)*(sin(seta2)*sin(seta3)*sin(seta4) - cos(seta3)*cos(seta4)*sin(seta2)) + sin(seta5)*(cos(seta3)*sin(seta2)*sin(seta4) + cos(seta4)*sin(seta2)*sin(seta3)));
    o_x =  sin(seta6)*(sin(seta5)*(cos(seta4)*(cos(seta3)*sin(seta1) + cos(seta1)*cos(seta2)*sin(seta3)) - sin(seta4)*(sin(seta1)*sin(seta3) - cos(seta1)*cos(seta2)*cos(seta3))) + cos(seta5)*(cos(seta4)*(sin(seta1)*sin(seta3) - cos(seta1)*cos(seta2)*cos(seta3)) + sin(seta4)*(cos(seta3)*sin(seta1) + cos(seta1)*cos(seta2)*sin(seta3)))) + cos(seta1)*cos(seta6)*sin(seta2);
    o_y =  cos(seta6)*sin(seta1)*sin(seta2) - sin(seta6)*(sin(seta5)*(cos(seta4)*(cos(seta1)*cos(seta3) - cos(seta2)*sin(seta1)*sin(seta3)) - sin(seta4)*(cos(seta1)*sin(seta3) + cos(seta2)*cos(seta3)*sin(seta1))) + cos(seta5)*(cos(seta4)*(cos(seta1)*sin(seta3) + cos(seta2)*cos(seta3)*sin(seta1)) + sin(seta4)*(cos(seta1)*cos(seta3) - cos(seta2)*sin(seta1)*sin(seta3))));
    o_z =  cos(seta2)*cos(seta6) - sin(seta6)*(cos(seta5)*(sin(seta2)*sin(seta3)*sin(seta4) - cos(seta3)*cos(seta4)*sin(seta2)) + sin(seta5)*(cos(seta3)*sin(seta2)*sin(seta4) + cos(seta4)*sin(seta2)*sin(seta3)));
    a_x =  cos(seta5)*(cos(seta4)*(cos(seta3)*sin(seta1) + cos(seta1)*cos(seta2)*sin(seta3)) - sin(seta4)*(sin(seta1)*sin(seta3) - cos(seta1)*cos(seta2)*cos(seta3))) - sin(seta5)*(cos(seta4)*(sin(seta1)*sin(seta3) - cos(seta1)*cos(seta2)*cos(seta3)) + sin(seta4)*(cos(seta3)*sin(seta1) + cos(seta1)*cos(seta2)*sin(seta3)));
    a_y =  sin(seta5)*(cos(seta4)*(cos(seta1)*sin(seta3) + cos(seta2)*cos(seta3)*sin(seta1)) + sin(seta4)*(cos(seta1)*cos(seta3) - cos(seta2)*sin(seta1)*sin(seta3))) - cos(seta5)*(cos(seta4)*(cos(seta1)*cos(seta3) - cos(seta2)*sin(seta1)*sin(seta3)) - sin(seta4)*(cos(seta1)*sin(seta3) + cos(seta2)*cos(seta3)*sin(seta1)));
    a_z =  sin(seta5)*(sin(seta2)*sin(seta3)*sin(seta4) - cos(seta3)*cos(seta4)*sin(seta2)) - cos(seta5)*(cos(seta3)*sin(seta2)*sin(seta4) + cos(seta4)*sin(seta2)*sin(seta3));
    p_x =  a1*cos(seta1) - d6*(sin(seta5)*(cos(seta4)*(sin(seta1)*sin(seta3) - cos(seta1)*cos(seta2)*cos(seta3)) + sin(seta4)*(cos(seta3)*sin(seta1) + cos(seta1)*cos(seta2)*sin(seta3))) - cos(seta5)*(cos(seta4)*(cos(seta3)*sin(seta1) + cos(seta1)*cos(seta2)*sin(seta3)) - sin(seta4)*(sin(seta1)*sin(seta3) - cos(seta1)*cos(seta2)*cos(seta3)))) - d2*sin(seta1) - a4*cos(seta4)*(sin(seta1)*sin(seta3) - cos(seta1)*cos(seta2)*cos(seta3)) - a4*sin(seta4)*(cos(seta3)*sin(seta1) + cos(seta1)*cos(seta2)*sin(seta3)) - a5*cos(seta5)*(cos(seta4)*(sin(seta1)*sin(seta3) - cos(seta1)*cos(seta2)*cos(seta3)) + sin(seta4)*(cos(seta3)*sin(seta1) + cos(seta1)*cos(seta2)*sin(seta3))) - a5*sin(seta5)*(cos(seta4)*(cos(seta3)*sin(seta1) + cos(seta1)*cos(seta2)*sin(seta3)) - sin(seta4)*(sin(seta1)*sin(seta3) - cos(seta1)*cos(seta2)*cos(seta3))) + a2*cos(seta1)*cos(seta2) + d3*cos(seta1)*sin(seta2) + d4*cos(seta1)*sin(seta2) + d5*cos(seta1)*sin(seta2) - a3*sin(seta1)*sin(seta3) + a3*cos(seta1)*cos(seta2)*cos(seta3);
    p_y =  d6*(sin(seta5)*(cos(seta4)*(cos(seta1)*sin(seta3) + cos(seta2)*cos(seta3)*sin(seta1)) + sin(seta4)*(cos(seta1)*cos(seta3) - cos(seta2)*sin(seta1)*sin(seta3))) - cos(seta5)*(cos(seta4)*(cos(seta1)*cos(seta3) - cos(seta2)*sin(seta1)*sin(seta3)) - sin(seta4)*(cos(seta1)*sin(seta3) + cos(seta2)*cos(seta3)*sin(seta1)))) + d2*cos(seta1) + a1*sin(seta1) + a4*cos(seta4)*(cos(seta1)*sin(seta3) + cos(seta2)*cos(seta3)*sin(seta1)) + a4*sin(seta4)*(cos(seta1)*cos(seta3) - cos(seta2)*sin(seta1)*sin(seta3)) + a5*cos(seta5)*(cos(seta4)*(cos(seta1)*sin(seta3) + cos(seta2)*cos(seta3)*sin(seta1)) + sin(seta4)*(cos(seta1)*cos(seta3) - cos(seta2)*sin(seta1)*sin(seta3))) + a5*sin(seta5)*(cos(seta4)*(cos(seta1)*cos(seta3) - cos(seta2)*sin(seta1)*sin(seta3)) - sin(seta4)*(cos(seta1)*sin(seta3) + cos(seta2)*cos(seta3)*sin(seta1))) + a2*cos(seta2)*sin(seta1) + a3*cos(seta1)*sin(seta3) + d3*sin(seta1)*sin(seta2) + d4*sin(seta1)*sin(seta2) + d5*sin(seta1)*sin(seta2) + a3*cos(seta2)*cos(seta3)*sin(seta1);
    p_z =  d1 - d6*(cos(seta5)*(cos(seta3)*sin(seta2)*sin(seta4) + cos(seta4)*sin(seta2)*sin(seta3)) - sin(seta5)*(sin(seta2)*sin(seta3)*sin(seta4) - cos(seta3)*cos(seta4)*sin(seta2))) + d3*cos(seta2) + d4*cos(seta2) + d5*cos(seta2) - a2*sin(seta2) + a5*sin(seta5)*(cos(seta3)*sin(seta2)*sin(seta4) + cos(seta4)*sin(seta2)*sin(seta3)) - a3*cos(seta3)*sin(seta2) + a5*cos(seta5)*(sin(seta2)*sin(seta3)*sin(seta4) - cos(seta3)*cos(seta4)*sin(seta2)) - a4*cos(seta3)*cos(seta4)*sin(seta2) + a4*sin(seta2)*sin(seta3)*sin(seta4);

    robot_end_pose = [n_x,o_x,a_x,p_x;
                      n_y,o_y,a_y,p_y;
                      n_z,o_z,a_z,p_z;
                      0,0,0,1];
end