令第一点调到固定点且相邻点距离不变怎么优化？

guohf

有一组n*3数组X(n个点的三维坐标，数据如下)，现要对各点进行微小调整x（3n个值），使相邻点间距离不变(lx=2e-5)，且将第一点调到固定位置[5e-6 0 5e-5]。现优化程序有问题(运行后第一点没调到固定点)，不知道怎么改，请高手帮忙指点一下，多谢了。

X=[
    5.000000807112509e-006    4.438188823382536e-012    4.999999999999996e-005
    5.000019466734432e-006    2.000041484956790e-005    5.000000000002813e-005
    5.000005381785101e-006    4.000035630116146e-005    4.999999999986301e-005
    5.000002199111159e-006    6.000030338908418e-005    5.000000003855406e-005
    5.000001310800677e-006    8.000028270773177e-005    4.999999999985877e-005
    5.000001139999288e-006    1.000002859376575e-004    5.000000000002505e-005
    5.000001105374056e-006    1.200002691391569e-004    4.999999999996041e-005
    5.000001422126761e-006    1.400002717169680e-004    5.000000001477024e-005
    5.000002175749727e-006    1.600002906163810e-004    4.999999999994554e-005
    5.000002819890397e-006    1.800002762363458e-004    5.000000000002112e-005
    5.000003503360227e-006    2.000002586221355e-004    4.999999999999951e-005
    5.000004206050277e-006    2.200002365369250e-004    4.999999999999058e-005
    5.000004822397571e-006    2.400002100393004e-004    4.999999999983640e-005
    5.000004457864783e-006    2.600001628445900e-004    5.000000004171483e-005
    5.000004410493534e-006    2.800001298186233e-004    4.999999999984020e-005
    5.000004786826385e-006    3.000001055148235e-004    4.999999999999983e-005
];

程序如下:

function Jdxyz=jizhi(lx,X,rx)
%这是三维节点间在满足各点间距离要求条件下的位置调整,lx是点间距离值，
%X是还没有施加杆间约束时的节点坐标，为n行3列;x是各点坐标微调距离的值,为3n;
%要求X+x满足节点间距离要求lx,第一点没调到固定点[5e-6 0 5e-5]。
m1=size(X,1);m2=size(X,2);             %m1,m2分别为杆件各节点坐标矩阵的行数、列数
%-------------数据传送到平台
assignin('base','Xz',X);
assignin('base','m1z',m1);
assignin('base','m2z',m2);
assignin('base','lxz',lx);
assignin('base','rxz',rx);
%-------------程序主体
x0=zeros(1,m1*m2);                     %x进行波动的初始值
options=optimset('LargeScale','off','display','off','Algorithm','interior-point','TolX',1e-13);
[x,fval]=fmincon(@myfun,x0,[],[],[],[],[],[],@mycon,options);
xd=zeros(m1,m2);
for i=1:m1
    for j=1:m2
        xd(i,j)=x((i-1)*m2+j);
    end
end
%%%%%%%%%%防止调整的微小位移过大的限制程序
for i=1:m1
    if sum(xd(i,:).^2)>2.5e-11   %既要求微小位移不能大于5e-6mm
       disp('调整的微小距离大于0.01mm,不对')
    end
end
%%%%%%%%%%%%%%%  
%xd,X;
Jdxyz=X+xd;                             %满足各项条件的节点坐标,调整后坐标
return
function f=myfun(x)
f=sum(x.^2);
return
function [c,ceq]=mycon(x)
%----------从平台引入数据
X=evalin('base','Xz');
m1=evalin('base','m1z');
m2=evalin('base','m2z');
lx=evalin('base','lxz');

%%%保证调整后第一点位置固定到[5e-6 0 5e-5]
ceq(m1)=X(1,1)+x(1)-5e-6;
ceq(m1+1)=X(1,2)+x(2);
ceq(m1+2)=X(1,3)+x(3)-5e-5;
for i=1:m1-1
    if m2==3
        ceq(i)=((X(i+1,1)+x(i*m2+1))-(X(i,1)+x((i-1)*m2+1)))^2+...
            ((X(i+1,2)+x(i*m2+2))-(X(i,2)+x((i-1)*m2+2)))^2+...
            ((X(i+1,3)+x(i*m2+3))-(X(i,3)+x((i-1)*m2+3)))^2-lx^2;
      
    end
end
c=[];
return