手術(shù)機(jī)器人中主手重復(fù)直線運(yùn)動(dòng)精度標(biāo)定

手術(shù)機(jī)器人中主手重復(fù)直線運(yùn)動(dòng)精度標(biāo)定基于最小二乘法的空間直線擬合機(jī)械工程1011201049張國(guó)凱在手術(shù)機(jī)器人的設(shè)計(jì)過程中，具有高精度，靈活性高的機(jī)器人對(duì)于完成手術(shù)操作具有重要的意義。對(duì)機(jī)器人精度標(biāo)定對(duì)以了解機(jī)器人的性能以及對(duì)機(jī)器人的誤差進(jìn)行補(bǔ)償具有重要作用，主手的重復(fù)直線運(yùn)動(dòng)精度作為機(jī)器人精度中的一部分，其標(biāo)定對(duì)于整個(gè)機(jī)器人的精度標(biāo)定具有代表和基礎(chǔ)意義。重復(fù)運(yùn)動(dòng)精度則是指在相同條件下，操作方法不變，進(jìn)行規(guī)定次數(shù)操作所得連續(xù)結(jié)果的一致程度。為了驗(yàn)證力反饋主操作手的重復(fù)運(yùn)動(dòng)精度，本文利用空?qǐng)D1主操作手重復(fù)運(yùn)動(dòng)精度測(cè)試間直線的往復(fù)運(yùn)動(dòng)來測(cè)量，主操作手的往復(fù)直線運(yùn)動(dòng)采用日本THK高精度直線導(dǎo)軌制作的滑動(dòng)平臺(tái)來實(shí)現(xiàn)，將主操作手末端固定在與導(dǎo)軌滑動(dòng)平面上，主操作手末端隨之一起做直線往復(fù)運(yùn)動(dòng)，如圖1所示。讓力反饋型主操作手末端沿高精度導(dǎo)軌重復(fù)運(yùn)動(dòng)，記錄各個(gè)關(guān)節(jié)的在各離散時(shí)刻的關(guān)節(jié)角度值，最后通過正運(yùn)動(dòng)學(xué)求解算出末端點(diǎn)的坐標(biāo)，在matlab中繪出各離散時(shí)刻的末端坐標(biāo)點(diǎn)，如圖2所示。trajectoryofreference

圖2主操作手末末端運(yùn)動(dòng)軌跡通過利用最小二乘法擬合出末端點(diǎn)的運(yùn)動(dòng)直線，出空間中各點(diǎn)到擬合直線的距離，即是主操作手的末端點(diǎn)運(yùn)動(dòng)偏差。利用最小二乘法擬合空間直線原理[56][57]如下：空間直線的標(biāo)準(zhǔn)方程：二=二=二（1）mnp由式（4-1）得直線映射方程式：x=m(z-z0)+x0=az+b（2圖2主操作手末末端運(yùn)動(dòng)軌跡通過利用最小二乘法擬合出末端點(diǎn)的運(yùn)動(dòng)直線，出空間中各點(diǎn)到擬合直線的距離，即是主操作手的末端點(diǎn)運(yùn)動(dòng)偏差。利用最小二乘法擬合空間直線原理[56][57]如下：空間直線的標(biāo)準(zhǔn)方程：二=二=二（1）mnp由式（4-1）得直線映射方程式：x=m(z-z0)+x0=az+b（2）其中:y=—(z-z0)+y0=cz+d、b=-mz+x、c=n、d=--z+yp00pp00ma=—p所求直線實(shí)際為上述2個(gè)方程表示的空間平面的交線，所以可以分別對(duì)2個(gè)方程進(jìn)行數(shù)據(jù)擬合。用最小二乘法分別對(duì)每個(gè)方程進(jìn)行數(shù)據(jù)擬合，求出參數(shù)a、b、c、d，即可求出直線。當(dāng)所測(cè)得各x,和y,值與擬合直線上各估計(jì)值之間偏差的平方和:Q=￡[x-(az+b)]2,=1Q=Y[y-(cz+d)]2i=1當(dāng)所測(cè)得各x,和y,值與擬合直線上各估計(jì)值之間偏差的平方和最小時(shí)，可以認(rèn)為擬合的直線為所求直線。此時(shí):8Q化解得:x-da=0,dQx

dbdQ=0,*ydcdQ=0,*——y

ddi=1bLz+a￡z2=lLxzd￡z+c?2=lLyZ?。悍匠探M可寫為:FY其中：A=[ab]'A=[ab]';X=[x1xk『;B=[cd]';Y=[y1*]'。根據(jù)實(shí)驗(yàn)所測(cè)得的k個(gè)點(diǎn)就可擬合出直線。取p=1,最后求出:n=cp=c,m=ap=a；x=az+b,y=cz+d0000最后得直線方程式為：x—(x—(az+b)ay—(cz^+dz—z^c1(3)在擬合直線中，直線過xoy平面，故可取%=0，得:(4)x—by—d==z(4)求實(shí)驗(yàn)數(shù)據(jù)點(diǎn)到擬合空間直線的距離：p—|.-l.I-.c古仆I兀：Ax+By+Cz=0口匚八—空間中點(diǎn)P(x,y,z)到空間直線I:J11/1的距離公式為：'ii?切:Ax+By+Cz=0(Ax+By+CzJn—(Ax2+By+Cz)nd=1i1i1i22i2i2i1(5)'n1'n2|根據(jù)上述直線擬合方法，如圖4-8與圖4-9所示，繪出擬合空間直線和求出所主操作手末端位置誤差。從實(shí)驗(yàn)所擬合直線和主操作手末端偏差來看，主操作手的重復(fù)運(yùn)動(dòng)精度在0.5mm以內(nèi)。trajectoryofreference,擬合直線運(yùn)動(dòng)軌跡點(diǎn)200220150X(mm)Y(mm)圖4末端點(diǎn)運(yùn)動(dòng)偏差圖3末端軌跡點(diǎn)直線擬合附：主手重復(fù)直線運(yùn)動(dòng)精度matlab程序clearalltrajectoryofreference,擬合直線運(yùn)動(dòng)軌跡點(diǎn)200220150X(mm)Y(mm)圖4末端點(diǎn)運(yùn)動(dòng)偏差圖3末端軌跡點(diǎn)直線擬合d4=53;fori=1:nq(i)=1;k1=THETA(i,1);%%%實(shí)際采集的數(shù)據(jù)k1k2=THETA(i,2);%%實(shí)際采集的數(shù)據(jù)為k2k3=THETA(i,3);%%實(shí)際采集的數(shù)據(jù)為k3A10=[cos(k1)-sin(k1)0a1*cos(k1);sin(k1)cos(k1)0a1*sin(k1);0010;0001];k21=k2-k1;A21=[cos(k21+pi/2)0-sin(k21+pi/2)0;sin(k21+pi/2)0cos(k21+pi/2)0;0-100;0001];A32=[cos(k3)-sin(k3)0a3*cos(k3);sin(k3)cos(k3)0a3*sin(k3);001+d4;0001];A30=A10*A21*A32;Q=A30*[0;0;0;1];Qx(i)=Q(1);Qy(i)=Q(2);Qz(i)=Q(3);endx=Qx;y=Qy;z=Qz;F=[z;q];M=F*F';N=F*x';O=F*y';A=(M\N)';B=(M\O)';x1=A(1)*z+A(2);y1=B(1)*z+B(2);figure(1)plot3(x1,y1,z,'k',x,y,z,'.')%plot3(x,y,z,'.')%gridonxlabel('X(mm)')ylabel('Y(mm)')zlabel('Z(mm)')%title('trajectoryofreference')legend('fittedline','Trajectorypoint')holdona=A(1);b=A(2);c=B(1);d=B(2);figure(2)fori=1:nA1=x(i)-a*z(i)-b;B1=y(i)-c*z(i)-d;n2=[1;0;-a];n1=[0;1;-c];dd(i)=norm((B1*n2-A1*n1))/norm((cross(n1,n2)));endj=1;fori=1:nifdd(i)<0.6bb(j)=dd(i);j=j+1;elseendendfigure(2)m=size(bb,2);i=1:m;plot(i,bb(i),'