function fit=fitnessfunc_3D_yuanxin_m(u,n,r,xt,yt,zt,COB,rob) %%%%u is represented by the first n values of arc length l, the middle n values of central Angle theta, and the last n values of rotational motion Angle
lambda1=0.1;
lambda2=0.8;
lambda3=0.1;
P=[0 0 0]; %%P is used to store the arc endpoint coordinates

SD=1;%%%SD indicates safe distance

K=1;

length=sum(u(1,1:n));

o0=[r 0 0];

pit=[0 0 1]';

OC=zeros(n+1,3); %%%%OC is used to store the center coordinates of each arc
OC(1,:)=o0; %%%%OC The first row stores the non-rotating coordinates, the second row stores the first circular arc coordinates, the third row stores the second circular arc coordinates, and so on

Tt=eye(4,4);
for i=1:n
    alpha=u(:,2*n+i);
    L=u(:,i);
    theta=u(:,n+i);
    
    %%%Find rotation matrix
    Rotza=[cos(alpha) -sin(alpha) 0 0
    sin(alpha) cos(alpha) 0 0
    0 0 1 0
    0 0 0 1];
    %%r=L/theta;
    h=r*(1-cos(theta));
    d=r*sin(theta);
    Transh0d=[1 0 0 h
            0 1 0 0
            0 0 1 d
            0 0 0 1];
    Rotyt=[cos(theta) 0 sin(theta) 0
            0         1     0        0
            -sin(theta) 0 cos(theta)  0
             0 0 0 1];
    T=Rotza*Transh0d*Rotyt;
    %%%%Find rotation matrix
    
   
    
    Tt=Tt*T;
    P=[P;Tt(1:3,4)'];
    
    Pi1=[P(i,1) P(i,2) P(i,3)]; %%%%Pi1 is the end point of the arc
    
% %     pit=T(1:3,1:3)*pit; %%%pit is the starting point tangent vector of the arc, pit1 is the end point tangent vector of the arc
   
    
    Oi1=Pi1-((Pi1-OC(i,:))*cos(alpha)+cross(pit',(Pi1-OC(i,:)))*sin(alpha) + pit'*dot(pit',(Pi1-OC(i,:)))*(1-cos(alpha)));
    OC(i+1,:)=Oi1;
    
% %     TRT=Rotza*Rotyt;
% %     pit=TRT(1:3,1:3)*pit;
% %     %%pit=(pit'/norm(pit'))';
    
    vnc=cross(P(i,:)-OC(i+1,:),P(i+1,:)-OC(i+1,:));%%%vnc represents the tangent vector of the plane
    vnc=vnc/norm(vnc);
    pit=(pit'*cos(theta)+cross(vnc,pit')*sin(theta)+vnc*dot(vnc,pit')*(1-cos(theta)))';
    
    
    
    
end

dist=sqrt((xt-P(n+1,1))^2+(yt-P(n+1,2))^2+(zt-P(n+1,3))^2);

fob=zeros(n,size(COB,1));
for i=1:n
    for j=1:size(COB,1)%%%COB represents the set of coordinates of obstacles, expressed in vector form
        cobp1=P(i,:)-COB(j,:);
        cobp2=P(i+1,:)-COB(j,:);
        cobce=OC(i+1,:)-COB(j,:);
        
        if dot(cross(cobp1,cobce),cross(cobce,cobp2))>0
            L=sqrt((OC(i+1,1)-COB(j,1))^2+(OC(i+1,2)-COB(j,2))^2+ (OC(i+1,3)-COB(j,3))^2); %L represents the distance between the obstacle and the center of the circle
            %%% dso=abs(L-r)-rob; %%dso represents the minimum distance from the obstacle to the trajectory
            vn=cross(OC(i+1,:)-P(i,:),OC(i+1,:)-P(i+1,:));%%%vn represents the normal vector of the plane where the locus circle is located
            angphy=asin(dot(vn,cobce)/(norm(vn)*L));%%%angphy is the Angle between the vector between the center of the trajectory circle and the center of the obstacle sphere and the plane of the trajectory circle
            dso=sqrt(r^2+L^2-2*r*L*cos(angphy))-rob;
        else
            dso1=sqrt((COB(j,1)-P(i,1))^2+(COB(j,2)-P(i,2))^2 + (COB(j,3)-P(i,3))^2)-rob;
            dso2=sqrt((COB(j,1)-P(i+1,1))^2+(COB(j,2)-P(i+1,2))^2 + (COB(j,3)-P(i+1,3))^2)-rob;
            dso=min(dso1,dso2);
        end
        
% %         if dso < rob      %%%%The safety distance is set to rob and can be changed
% %             fob(i,j)=+inf;
% %         else
% %             fob(i,j)=K*rob/dso;
% %         end
        
        if dso < SD      %%%%The safety distance is set to SD instead of obstacle radius rob
            fob(i,j)=+inf;
        else
            fob(i,j)=K*SD/dso;
        end
    end
end

fit=lambda1*length+lambda2*dist+lambda3*sum(sum(fob));