曲柄的回转

回转曲柄对人来讲是最简单的。但对于机器人却是极为困难的动作之一。曲柄是沿着空间所确定的圆轨道运动的，如果让机器人必须按这个约束条件不断回转曲柄，则是困难的，位置控制的机器人，在空中可以描出近乎正确的圆轨道。但是，作为目标值所给出的圆轨道，若比由曲柄实际运动得到的圆轨道稍微偏离一点的话，则在强制其接近目标值时，可能不是曲柄折断就是机器人自身被损坏。为了不加上强制力，按照由曲柄所给的约束条件不断平滑地回转，则必须对作业中手所受的反作用力进行检测，并调节手的动作。即从物理意义上讲，为了沿着外部约束的轨道移动机器人的手，只有位置控制是不够的，还需要有力反馈控制。

当曲柄的中心位置(X,Yo,Z₀)和半径R给出时，试编写一个按一定速度回转曲柄的程序。回转曲柄的动作，是由在曲柄回转圆的切线方向驱动曲柄的过程，和进行力反馈让法线方向的反作用力变为0的过程，协调实现的。

下面我们把在曲柄回转圆的切线方向上所给的一定速度指令，在各个采样时刻，和在切线方向上前进一定距离的点被看成假目标点的情况下，置换为位置控制。如图5.21所示，手位于H点

(X,Y,Z,,αr),作为单位时间后的目标位置，可以考虑是在切线方向仅前进了距离v(与回转速度相对应)的点H.,因为V的

X,Y分量是

Vx—V*(Y,一Yo)/RVy=·(X,—X₀)/R

所以，为了向H.点移动臂，给予臂的新目标值只要作如下更新即可：

X,=X,—V·(Y,—Y₀)/R

Y,←Y,+V·(X,一X₀)/R

上述目标值不一定与曲柄的回转圆相一致、因此，我们可以考虑利用装在手腕部的力传感器，根据让法线方向的反作用力变为0的控制，就可以让臂的位置适应曲柄的圆轨道。如果手的姿势角α被保持为0,从力传感器所测定的手坐标系中的fxsfy的值求法线方向的反作用力，则为

f。=fr·(X,一X₀)/R+fy+(Y,一Y₀)/R

为了让反作用力为0,只要在反作用力的作用方向上移动臂即可.即若设K为比例常数，则仅以K·f.修正臂的目标即可。因此，为了这种适应动作，可对臂的目标值作如下更新：

Y,一#Y,+K·fo·(Y,Y₀)/R

若按照以上想法编写回转曲柄的程序，则为

processcranking;begin

cobeginturning;following;coend;

end;

processturning;

begin

whilestop=0dobegin

X=X,一V·(Y,一Yo)/RY,:=Y,十V·(X,一X,)/R

startIOput-arm-reference(X,,Y,,Z,,0);delay(T。);

end;end;

proce8sfollowing;begin

whilentop=0dobegin

就art10get-wrist-force(fysf,,fys州，my,m.);

g;一f·(X,-X,)/R+Jy(Y,-Yo)/R;

X,=X,+K+f.·(X,-X₀)/Ry

Y,:=Y,十K·f(Y,-Yc)/R;

Z,:=Z,+K·f

tart¹0put-arm~refereace(X,,Y,,2,,0);delay(T₀);end;

end;