一种冗余参数自主规划的机械臂逆运动学解算方法

本发明涉及机器人领域，具体地说是一种冗余参数自主规划的机械臂逆运动学解算方法。

背景技术：

1、在与人协作类机器人中，七自由度拟人构型机械臂是一种常见结构，此类机械臂被广泛应用于柔性化工业生产线、电子元器件装配、医疗与服务等行业，与传统的六自由度机械臂相比，七自由度拟人构型机械臂具有更大、更灵活的工作空间，特别适合应用在与人交互的工作环境中。

2、机械臂逆运动学规划是机械臂轨迹规划和控制的基础，传统工业机械臂的逆运动学规划方法无法直接用于七自由度拟人构型机械臂上，其原因在于：六自由度操作空间轨迹如何映射到七自由度空间轨迹存在问题。目前已有的解算方法多采用“数值解法”，即在每个迭代周期内通过求解雅可比伪逆方法进行求解，“数值解法”虽然能够处理所有具有冗余自由度构型的机械臂逆运动学问题，但存在误差大、计算效率低等缺点，若将此类方法应用于七自由度机械臂轨迹规划中，不仅控制精度会存在问题，还会降低控制器的运行效率，甚至会降低机械臂反应的实时性。

技术实现思路

1、本发明的目的在于提供一种冗余参数自主规划的机械臂逆运动学解算方法，通过定义参考平面和冗余角的方法实现了空间六自由度与机械臂关节七自由度的映射关系，从而提高了机械臂控制精度和控制器运行效率。

2、本发明的目的是通过以下技术方案来实现的：

3、一种冗余参数自主规划的机械臂逆运动学解算方法，包括如下步骤：

4、步骤一：建立机械臂模型，并定义机械臂各个关节由基座至机械臂末端依次为关节1至关节7，θi(i＝1-7)为第i个关节的旋转角度，将机械臂配置成具有“肩-肘-腕”结构，其中关节1至关节3旋转轴相交于一点ps并配置为肩关节，关节4旋转中心为pe并配置为肘关节，关节5至关节7的旋转轴相交于一点pw并配置为腕关节；

5、步骤二：确定“肩-肘-腕”结构的运动参数为：lbs、lse、lew、lwt，其中：lbs表示基座坐标系原点到ps点的距离，lse表示ps点到pe点距离，lew表示pe点到pw点距离，lwt表示pw到机械臂末端的距离，根据机械臂末端期望位置和姿态求解出pspw直线段在基座坐标系中的向量

6、

7、步骤三：求解基座坐标系下关节1至关节4的旋转角度θ1_0、θ2_0、θ3_0、θ4_0，并将θ1_0、θ2_0、θ3_0、θ4_0确定的平面定义为参考平面；

8、步骤四：定义肩-肘-腕各关节中心pspepw三点形成的任意空间平面与步骤三中定义的参考平面之间的夹角为冗余角建立肩关节角度θ1、θ2、θ3以及腕关节角度θ5、θ6、θ7与所述冗余角的函数关系其中θ＝[θ1，θ2，θ3，θ5，θ6，θ7]t；

9、步骤五：假定冗余角为0，根据上述解出初始逆解关节组合；

10、步骤六：根据初始逆解关节组合、冗余角变化速率以及预期的机械臂末端轨迹规划路线求解最优冗余角

11、步骤七：将最优冗余角代入关系式中最终求解出肩关节角度θ1、θ2、θ3以及腕关节角度θ5、θ6、θ7；

12、步骤八：将解算获得的7个关节旋转角度θ1～θ7作为规划的期望轨迹发送给机械臂各个关节进行控制，其中θ4由步骤三获得。

13、步骤三中，先根据肩-肘中心点连线长度lse、肘-腕中心点连线长度lew以及肩-腕中心点连线长度构成的三角形求解θ4_0：

14、

15、然后根据下式(3)求解肩-腕中心点ps pw直线段在关节3坐标系中的向量

16、

17、再结合步骤二向量同时定义关节3角度固定不变θ3_0＝0，求解此时基座坐标系下的关节1角度θ1_0和关节2角度θ2_0，其中：

18、θ2_0求解如下：

19、

20、

21、上式(4)中，表示当θ3_0＝0时，由关节1角度θ1_0确定的关节1相对于基座的旋转矩阵，表示当θ3_0＝0时，由关节2角度θ2_0确定的关节2相对于关节1的旋转矩阵，表示关节3坐标系相对于关节2坐标系的旋转矩阵，另外上式(4)中：

22、a＝lse+lew*cos(θ4_0)

23、b＝-lew*sin(θ4_0)

24、

25、上式(5)中表示在基座坐标系中向量的z向分量；

26、θ1_0求解如下：

27、

28、和分别表示在基座坐标系中向量的x向分离和y向分量，另外上式(6)中：

29、m＝sin(θ2_0)*(lse+lew*cos(θ4_0))

30、n＝lew*sin(θ4_0)*cos(θ2_0)  (7)；

31、θ1_0、θ2_0、θ4_0解出后，将此时由θ1_0、θ2_0、θ3_0、θ4_0确定的肩-肘-腕所在平面定义为参考平面，其中θ3_0＝0。

32、步骤三中，θ4_0求解后存在正负两种解，当机械臂末端操作空间轨迹点为离散情况时，θ4_0实际存在两组解，由人为选取正负符号，当机械臂末端操作空间轨迹点为连续情况时，θ4_0根据前一周期关节角度θ4确定正负符号。

33、步骤三中，θ2_0求解后存在正负两种解，当机械臂末端操作空间轨迹点为离散情况时，θ2_0实际存在两组解，由人为选取正负符号，当机械臂末端操作空间轨迹点为连续情况时，实际存在一组解并根据最小位移原则选取，即选取前后两个规划周期内的转角之差在阀值范围内的一组解。

34、步骤四中，表达如下：

35、θ1、θ2、θ3表达为：

36、

37、

38、

39、上式(8)中，参数asij，bsij，csij分别为矩阵as，bs，cs的第i行第j列的数值，i、j＝1…3，as，bs，cs由下式(9)确定：

40、

41、

42、

43、

44、

45、θ5、θ6、θ7表达为：

46、

47、

48、

49、上式(10)中，参数awij，bwij，cwij分别表示矩阵aw，bw和cw第i行第j列的数值，i、j＝1…3，aw，bw和cw由下式(11)确定：

50、

51、

52、

53、

54、步骤四中，θ2求解均存在正负两种情况，当机械臂末端操作空间轨迹为离散情况时，人为选取符号正负，当机械臂末端操作空间轨迹为连续情况时，θ2根据前一规划周期角度值确定符号正负。

55、步骤四中，θ6求解均存在正负两种情况，当机械臂末端操作空间轨迹为离散情况时，人为选取符号正负，当机械臂末端操作空间轨迹为连续情况时，θ6根据前一规划周期角度值确定符号正负。

56、步骤五中，根据总共获得四组不同的关节解组合，当机械臂末端操作空间轨迹点为离散情况时，实际存在4组解，由人为选取，当机械臂末端操作空间轨迹点为连续情况时，实际存在1组解，此时根据最小位移原则选取，即选取前后两个规划周期内的转角之差在阀值范围内的一组定为初始逆解关节组合。

57、步骤六中，机械臂末端预期规划轨迹路线经过基座坐标系内的a(xa，ya，za)、b(xb，yb，zb)、c(xc，yc，zc)三个点，三点形成一个轨迹平面，最优冗余角求解如下：

58、步骤a：求解轨迹平面的法向量：

59、

60、

61、

62、上式(12)中，表示点a到b的向量，表示点a到c的向量，表示轨迹平面的法向量；

63、步骤b：求解步骤五中确定的初始逆解关节组合所形成的肩-肘-腕中心pspepw平面与轨迹平面的夹角：

64、

65、

66、上式(13)中，表示pspepw平面的法向量，α为轨迹平面和pspepw平面的夹角；

67、步骤c：当步骤b计算的α＝0时，此时的冗余角＝0即为最优化冗余角当α不等于0时，以冗余角变化速率变化并不断重复下式(14)的计算，使pspepw平面和轨迹平面夹角αi+1不断趋向0度：

68、

69、

70、

71、直到αi+1达到极值时，解得最优化冗余角

72、本发明的优点与积极效果为：

73、本发明将七自由度机械臂各个关节配置成肩-肘-腕式结构，先求解关节1-4角度，然后将关节1-4角度确定的肩-肘-腕平面定义为参考平面，并定义肩-肘-腕各关节中心形成的任意空间平面与所述参考平面夹角为冗余角进而建立冗余角与肩关节角度θ1、θ2、θ3和腕关节角度θ5、θ6、θ7的函数关系并假定为0获得初始逆解关节组合，然后系统根据初始逆解关节组合、设定的冗余角变化速率以及预期的机械臂末端轨迹规划路线自主求解合适的最优冗余角并将最优冗余角代入从而最终获得各个关节旋转角度作为规划的期望轨迹实现控制，实现了空间六自由度与关节七自由度的映射关系，并提高了机械臂控制精度和控制器运行效率。