基于几何法的机器人逆运动学求解--工业机器人前三个关节

利用几何法求解工业机器人逆运动学是将空间中的机器人结转换为几个正交平面下的几何法求解方法，就有简单，直观的效果。但是，对于机器人的一个空间位置姿态可能对于多种关节角的情况，采用几何法的求解可能会造成，考虑不充分，求解结果变少的情况。但基于它所具有的优点和直观性，我觉得还是可以和大家分享一下我的求解例子。我以工业机器人staubli为例，机器人的前三个关节确定机器人的位置，后三个关节确定机器人的姿态，将第六各关节坐标系建立在机器人的腕关节处。
第一关节角的求解如下图所示：

对应的matlab代码如下：
首先定义好机器人的姿态关系：

n1 = -1;   % 'l'
 n2 = -1;   % 'u'
 n4 = -1;   % 'n'
 if sol(1)==1%~isempty(strfind(configuration, 'l'))
     n1 = -1;
 end
 if sol(1)==2%~isempty(strfind(configuration, 'r'))
     n1 = 1;
 end
 if sol(2)==1%~isempty(strfind(configuration, 'u'))
     if n1 == 1
         n2 = 1;
     else
         n2 = -1;
     end
 end
 if sol(2)==2%~isempty(strfind(configuration