用fmincon求相邻点距离不变问题超限
 

问题描述：
在一个长（x向）0.2mm，宽（y向）0.4mm，高（z向）0.1mm的方体内有N个有序已知点（顺序相连可成一条曲线，可设N点坐标为一个N*3的矩阵X），要求除第一个点不动外，对其余N-1个点进行微调((N-1)*3的矩阵xd），使调整后的相邻点间距离为一定值lx，当然调整后的点不能出界。
用fmincon进行最优化，但第一个点也被调整了，且不知是提取的数学问题不对还是程序有问题，得到的微调值很大，超限很多，请高手帮忙指点一下，先谢了。

数学问题：
目标函数：Σx(j)^2 x(j)为微调xd的各分量，j=1, 2……3N-3
约束条件：|X(i+1)+xd(i+1)- X(i)- xd(i)|=lx, i=1, 2…..N
0<X(i,1)+xd(i,1)<0.2, 0<X(i,2)+xd(i,2)<0.4, 0<X(i,3)+xd(i,3)<0.1

假设N=20，已知这20个点构成的坐标矩阵X, lx=1e-5;

程序：
format long e %因所给量很小，为避免舍入误差太大，用long e

X=[1.000000000000000e-005 0 5.000000000000000e-005
1.084212146258562e-004 1.374317617706318e-004 3.353201365358312e-005
1.743335211270057e-004 3.211374518483659e-004 3.183321977317479e-005
1.981769753590540e-004 -9.021471158199482e-004 4.409472569732121e-005
3.607778176281379e-005 -6.450571057147658e-004 5.362661879261433e-005
1.992600048898033e-005 1.115269281749919e-004 5.289233224855776e-005
1.830891687691136e-005 3.502329268561499e-004 5.041792172515980e-005
1.724894446411056e-005 2.082276257844818e-004 4.989077981552538e-005
1.442138466276086e-005 2.856263517830034e-004 4.968606079655937e-005
1.599026055319464e-005 2.196338795975163e-004 4.906672591045520e-005
2.649812284492222e-005 3.016661141552573e-004 4.797951158729175e-005
3.501905903500460e-005 1.947676718678416e-004 4.656622210738030e-005
3.715026059364602e-005 2.017765869090804e-004 4.685650795284104e-005
5.924772262711515e-005 1.078990962840916e-004 4.935954495744724e-005
8.305504484629322e-005 1.320501850534990e-004 5.104872456086212e-005
7.104610355191896e-005 1.015464768267407e-004 5.116284821760612e-005
9.016579851488848e-005 3.569370145761626e-004 5.109457596677840e-005
1.786477300524624e-004 1.542941709813852e-004 5.048413376542842e-005
2.247149253348480e-004 1.823588732474208e-004 4.997876808826507e-005
2.164931765012710e-004 1.740574934462233e-004 4.998654719288205e-005]

lx=1e-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);
%-------------程序主体
x0=zeros(1,m1*m2); %x进行波动的初始值

%%%求线性上、下限
%%x,y,z边界-X(i,1)<=x(i,1)<=2e-4-rx-X(i,1); rx-X(i,2)<=x(i,2)<=4e-4-rx-X(i,2); rx-X(i,3)<=x(i,3)<=1e-4-rx-X(3,1)
lb=zeros(1,m1*m2);ub=zeros(1,m1*m2);
for i=2:m1
mm=3*(i-1);
lb(mm+1)=rx-X(i,1); ub(mm+1)=2e-4-rx-X(i,1);
lb(mm+2)=rx-X(i,2); ub(mm+2)=4e-4-rx-X(i,2);
lb(mm+3)=rx-X(i,3); ub(mm+3)=1e-4-rx-X(i,3);
end

%options=optimset('LargeScale','off','display','off');
options=optimset('LargeScale','off','display','off','Algorithm','active-set');
[x,fval]=fmincon(@myfun,x0,[],[],[],[],lb,ub,@mycon,options);
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)>1e-12 %既要求微小位移不能大于1e-6mm
disp('调整的微小距离大于0.001mm,不对')
end
end
xd, %显示的xd值很怪，超限很多
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');
%----------节点限制条件的数学表示
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
%防止调整的微小位移过大，限定矢量xd的模小于jx，但这步好像没起作用
jx=5e-6;
for i=1:m1
if m2==3
c(i)=x((i-1)*m2+1)^2+x((i-1)*m2+2)^2+x((i-1)*m2+3)^2-jx^2;
end
end
return