图4.2 机械手的坐标系建立简图
操作人员给出了目标点的位置坐标后，就得通过算法算出每个电机的控制命令，设回转关节的回转角度为angle1,肩关节的回转角度为angle2,肘关节的回转角度为angle3,手爪电机的回转角度为angle3。同时规定图示的状态就是机械手各个关节，各个电机的初始状态。
假设目标点的位置坐标为（X，Y，Z），机械手的工作空间是一定的，为了使机械手能够到达目标点要求X²+Y²+Z²≤（手臂长+手腕长）²。
机械手实际工作时先转动腰关节，当手臂和手腕组成的平面和目标点在同一平面内时停止，再转动肩关节，接着转动肘关节，最后转动手爪电机使手爪闭合。
腰部回转角度angle1跟坐标x,y有关， angle1=arctan(y/x),若为angle1<0则正转，若angle1>0则反转，转动角度为其绝对值。
当腰部关节转动结束使得手臂和手腕组成的平面和目标点在同一平面内。转动结束后如图4.3所示，其中X’²=X²+Y²。
 
图4.3 腰关节转动结束后坐标图
肩关节转动角度为angle2,手臂长为L1,手腕长为L2，通过正余弦定理分别求出角ψ和角β的大小， 当z>0时，angle2 = 90º-（β﹢ψ），当z<0时angle2=90º-（ψ-β），若angle2<0则正转，若angle2>0则反转。
肘关节转动角度为angle3,由余弦定理可求出其大小。
最终VB程序实现如下：
(1) 腰部回转关节角度计算
Dim x, y, z As Double       '坐标变量
Dim a, b, d, e As Double    'a为θ余弦，b为θ正弦，d、e为ψ正余弦
x = Textx.Text              '坐标变量赋值
y = Texty.Text
z = Textz.Text
angle1 = Atn(y / x)      
(2)肘关节角度计算
x = Sqr(x * x + y * y)
a = (shoubi*shoubi+shouwan * shouwan -x*x- z * z)/(2 * shoubi * shouwan)
b = Sqr(1 - a * a)
If a < 0 Then
angle3 = -Atn(b / a) - 3.1415926 / 2       
Else
  angle3 = 3.1415926 / 2 - Atn(b / a)
End If
(3)肩关节计算
d = (x*x+z*z+shoubi*shoubi-shouwan*shouwan)/(2*shoubi*Sqr(x*x+z*z))
e = Sqr(1 - d * d)
If d > 0 Then
If z > 0 Then
    angle2 = Atn(Abs(z) / x) + Atn(e / d)  
    angle2 = 3.1415926 / 2 - angle2
  Else
    angle2 = Atn(e / d) - Atn(Abs(z) / x)
    angle2 = 3.1415926 / 2 - angle2
  End If
Else
  If z > 0 Then
    angle2 = Atn(Abs(z) / x) + 3.1515926 + Atn(e / d)
    angle2 = 3.1515926 / 2 - angle2
  Else
    angle2 = 3.1415926 + Atn(e / d) - Atn(Abs(z) / x)
    angle2 = 3.1415926 / 2 - angle2
  End If
End If
当电机抓完物体后，需要返回时，上位机只需将各电机的转动方向取反，其余命令信号不变，这样机械手就能原路返回。
4.1.4 操作方式
具体操作上位机界面时，自动运行和手动控制对应的命令按钮分别为“自动抓取、自动返回”和“手动控制”。自动运行时，只需将目标位置坐标输入文本框，程序通过算法计算出相应的控制命令并会将控制命令通过文本框在界面中显示出来，然后命令传给下位机，而手动控制时，电机的具体转速、转向以及转动圈数得由操作人员实现计算出来并输入到文本框中，然后通过串口将命令传送出去。为了节省界面间，自动运行和手动控制的与电机命令文本框公用。例如自动运行时，假如目标位置为（50,50,50）则点击完“自动运行”按钮后会在“设定圈数”和“电机转向”文本框中将控制命令显示出来，如图4.4所示： AT89S51单片机全地形八足机器人机械手的设计(13):http://www.751com.cn/jixie/lunwen_2398.html