我正在做一个与机器人技术相关的项目，我正在使用fminconmatlab 中的函数来最小化点x_0和x_f. 基本上任务是使机械手从 移动x_0到x_f永远不会离开圆环区域。所以我认为这是一个有约束的优化问题。我很难弄清楚找到两个中间点的策略是什么（如下图所示），因此给定x_0and x_f，它给了我们两个点 [x1,y1,z1] 和 [x2,y2,z2]用作通过点。

你能弄清楚我应该如何编写目标函数。到目前为止的代码如下：

    function [x, fval, history] = solver( x0, xn, R, r )

% x0 = initial position, xn = final position, R = distance from the center of the tube to the center of the torus
% r is the radius of the tube.
% plotting torus
th=linspace(0,2*pi,60); % e.g. 60 partitions along perimeter of the tube
phi=linspace(0,pi,60); % e.g. 60 partitions along azimuth of torus
[Phi,Th]=meshgrid(phi,th);
x=0+(R+r.*cos(Th)).*cos(Phi); 
y=0+(R+r.*cos(Th)).*sin(Phi);
z=0+r.*sin(Th);
s=surface(x,y,z);
s.EdgeColor='none'
axis equal;
daspect([1 1 1 ])
hold off 
alpha(s,0.2);
% defining constraints and objective function
history = [];
options = optimset('OutputFcn', @myoutput);
[x, fval] = fmincon(@(x)objfun(x,xn),x0,[],[],[],[],[],[],@noncol,options);

    function stop = myoutput(x,optimvalues,state)
        stop = false;
        if isequal(state,'iter')
            history = [history; x];
        end
    end

    function z = objfun(x,p)   
        z = sqrt((p(1)-x(1))^2+(p(2)-x(2))^2+(p(3)-x(3))^2);
    end
    function [y, yeq] = noncol( x ) % x ==== [ x(1)   x(2)    x(3) ]
        y = (sqrt(x(1)^2+x(2)^2)-R)^2 + x(3)^2-r^2; 
        yeq =[];
    end
end

$x_0$到$x_f$在这个环形区域内">