代码如下:
clear
clc
format long
%标定时参数c=[L1 L2 qc xp0 yp0 m n dm dn q1z q2z]
c=[270 250 0 280 -140 8 5 40 40 2 2];
L1=c(1);L2=c(2);qc=c(3);xp0=c(4);yp0=c(5);m=c(6);n=c(7);dm=c(8);dn=c(9);q1z=c(10);q2z=c(11);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%待标定参数q1z,q2z,qc,xp0,yp0,
%载入测量点的关节角度
encoder=load('ctest2');
[rencoder,lencoder]=size(encoder);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%计算
for i=0:(rencoder-1)
qas(i+1,1)=encoder(i+1,1)+sym('x(1)')/360*pi;
qas(i+1,2)=encoder(i+1,2)+sym('x(2)')/360*pi;
p(i+1,1)=L1*cos(qas(i+1,1))+L2*cos(qas(i+1,1)+qas(i+1,2))-rem(i,n)*dn*cos(sym('x(3)'))+fix(i/n)*dm*sin(sym('x(3)'))-sym('x(4)');
p(i+1,2)=L1*sin(qas(i+1,1))+L2*sin(qas(i+1,1)+qas(i+1,2))-rem(i,n)*dn*sin(sym('x(3)'))-fix(i/n)*dm*cos(sym('x(3)'))-sym('x(5)');
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fun=inline(p);
%设定迭代初值,请注意调整迭代初值
x0=[0 0 0 275 -145];
%进行迭代计算标定。
disp(('>>标定运算中................'))
myoptions=optimset('LevenbergMarquardt','on','NonlEqnAlgorithm','lm','MaxIter',10000,'TolFun',1e-6,'Tolx',1e-6,'MaxFunEvals',10000);
[x,Fval,exitflag,output]=fsolve(fun,x0,myoptions);
%[x,exitflag,output,resnorm,residual]=fsolve(fun,x0,myoptions);
%myoptions.TolFun;myoptions.TolX
%plot(residual(:,1))
%查看标定结果。
x,Fval,exitflag,output
程序执行结果如下:
>>标定运算中................
Optimization terminated: directional derivative along
search direction less than TolFun and infinity-norm of
gradient less than 10*(TolFun+TolX).
x =
1.0e+002 *
-0.00157063263327 0.01999999639038 -0.00018823937648 2.77315197169297 -1.45245585453119
Fval =
1.0e-004 *
-0.01594640878011 -0.12048655690933
0.03428882394019 -0.03766079572642
0.03962466678331 -0.16617129830365
0.02481593242010 0.03753617619395
0.01692886144156 0.00008686754427
-0.05308237234658 -0.06403248079323
-0.02576540737209 0.11849102776296
0.02211054209056 -0.01946831304167
-0.00197759675302 0.04430346109530
0.02211379012351 -0.00505518414684
0.03217430844416 0.08018792101439
-0.07483193201097 -0.00094193410405
-0.05233907188540 0.09332183253719
0.00274875787909 0.01332473601678
0.06932629389667 -0.10225775412209
-0.01252338961422 -0.02912995171300
-0.06191226759711 0.06072040690697
0.03385061916106 0.00057519628172
0.06426964205275 -0.05061323122391
0.03675691573335 -0.04299255465412
-0.01354735786663 -0.00303138165236
-0.07229292123156 0.05114632756431
0.03069964805036 0.00130948251353
0.06652136391949 -0.05083241575221
0.03795018130859 -0.03537865978842
0.00070312523803 0.00564309232232
-0.09476660068231 0.06300757689814
-0.09079808933166 0.08536141251625
-0.01131872863880 0.02868075029028
0.08621919846519 -0.07459986676395
-0.05258040744138 0.01491118354124
-0.09602083650861 0.05066673139709
0.01701882069938 -0.01661088418814
-0.03767661894472 0.04545984353399
0.01345104294614 -0.01193452618509
0.02590453675566 -0.01323077214010
0.02501625829154 0.00573993276021
0.09590536592441 -0.02829373215718
-0.02042577989414 0.03779048796559
-0.01059285523297 0.03445793964829
exitflag =
1
output =
iterations: 4
funcCount: 34
stepsize: 1
cgiterations: []
firstorderopt: []
algorithm: 'medium-scale: Levenberg-Marquardt, line-search'
message: [1x147 char]
理想值应该是0 0 0 280 -140;运算出来的结果和理想值偏差太多,虽然Fval很小,且exitflag = 1,是不是和提示有关:Optimization terminated: directional derivative along
search direction less than TolFun and infinity-norm of
gradient less than 10*(TolFun+TolX).
什么原因导致优化停止。是程序中的rem(i,n)*dn*cos(sym('x(3)'))+fix(i/n)*dm*sin(sym('x(3)'))造成的么?
我将程序中sin(sym('x(3)'))用零代替(因为理论值就是零),只保留cos(sym('x(3)'))这一部分,结果就对了。对于这样待标定值既有正弦又有余弦运算的,怎么才能计算出可用结果。
因为x(3)在零附近,我将sin(sym('x(3)'))用sym('x(3)')代替;cos(sym('x(3)'))用1-(sym('x(3)'))^2/2代替,仍旧不行。难道只能保留正弦或余弦,另外一个用1或0代替。那如果'x(3)'的真实值比方
说pi/8,那就不好办了。 |