一种串联机器人六维力传感器受力感知算法的制作方法

本发明涉及串联机器人六维力传感器力控，尤其涉及一种串联机器人六维力传感器受力感知算法。

背景技术：

1、在串联机器人末端法兰安装六维力传感器进行相关力控的应用中，需要实时的计算六维力传感器所受的外力。该外力不是简单的从六维力传感器反馈的力直接读取的，因为六维力传感器实际反馈的力或力矩中，主要由三部分组成：1)传感器自身的误差(零漂)；2)传感器末端及附加载荷的重力作用；3)外界环境施加的力。而外力感知其指的是外界环境施加的力，因此传感器标定指的是进行传感器自身的误差(零漂)和传感器末端及附加载荷的重力作用的标定。

2、在当前串联机器人六维力传感器标定算法中，主流的做法是通过机器人法兰上的机械定位进行六维力传感器的安装，以此来假设末端法兰坐标系与传感器坐标系是对齐的状态。但是通过机械定位总是存在微小的误差，而常见的标定算法中，并没有考虑该误差；除此之外，对于外力的感知，需要精确的传感器参数的标定，而当前常见的标定算法处理方式是一次性通过最小二乘法将所需的参数都辨识出来，这样的处理会导致辨识的参数结果误差较大，间接的导致传感器受力感知的计算误差增大。

技术实现思路

1、根据以上存在的技术问题，本发明提高一种在算法上补偿传感器与法兰安装角度定位的微小误差和分步辨识标定参数来提升计算出受力的精确度。

2、本发明提供一种串联机器人六维力传感器受力感知算法，具体技术步骤如下：

3、步骤1：根据六维力传感器实际反馈的力/力矩中由传感器自身的误差、传感器末端及附加载荷的重力作用和外界环境施加的力组成，得出外界环境施加的力为：

4、sfextenal＝sffeedback-sfoffset-sg  (1)

5、式中：

6、sfextenal——基于传感器坐标系描述的外界环境施加的力/力矩

7、sffeedback——基于传感器坐标系描述的实际反馈的力/力矩

8、sfoffset——基于传感器坐标系描述的传感器自身的误差(零漂)

9、sg——基于传感器坐标系描述的传感器末端及附加载荷的重力作用标定过程中将sfextenal视为0，即标定的过程中外界环境施加的力为0，得到六维力传感器实际反馈的力/力矩为：

10、sffeedback＝sg+sfoffset (5)

11、在步骤1中，已知：

12、sffeedback＝[sfx sfy sfz smx smy smz]t (2)

13、sfoffset＝[sfoffsetx sfoffsety sfoffsetz smoffsetx smoffsety smoffsetz]t (3)

14、sg＝[sfg smg]t＝[sfgx sfgy sfgz smgx smgy smgz]t (4)

15、将式(2)、式(3)和式(4)代入式式(5)中得到：

16、

17、步骤2：基于传感器坐标系描述的六维力传感器末端及载荷的重力作用，将传感器末端及附加载荷的重力作用的质心在传感器坐标系的描述为(spx,spy,spz)，通过式(7)计算得到重力g作用在x,y,z轴上的力矩为：

18、

19、将正负考虑进去进一步推导式(7)为：

20、

21、步骤3：已知的六维力传感器末端及载荷的重力作用g是基于世界坐标系{w}进行描述的，g在世界坐标系中的描述如下：

22、wfg＝[0 0 -g]t (9)

23、式(9)即表示重力g只在世界坐标系{w}的z负方向上有力的作用，需将式(9)转换到力传感器坐标系中进行描述，假设机器人基座坐标系{b}绕世界坐标系{w}的x轴转动的角度记为rx，绕y轴转动的角度记为ry；进一步假设六维力传感器与机器人末端安装法兰的安装面是平行且贴合的，因此只存在力传感器坐标系{s}与机器人末端坐标系{e}之间有z方向的安装偏差，记为角度rz。则此时重力g在力传感器坐标系{s}的描述为：

24、

25、在步骤3中，其中的值分别如下：

26、

27、通过串联机器人的运动学推导得到旋转算子的具体值，在此记：

28、

29、

30、步骤4：结合式(8)和式(10)得到最终的sg为：

31、

32、步骤5：结合式(5)和式(14)可得无外界环境施加力情况下的力传感器的反馈值为：

33、

34、将式(3)、式(11)、式(12)以及式(13)带入到式(15)中，可得：

35、

36、式(16)中的6个方程我们可以得知，六维力传感器需要标定的参数有

37、{g,px,py,pz,sfoffsetx,sfofsety,sfoffsetz,smoffsetx,smoffsety,smoffsetz,sinrz,cosrz}这12个，采用分步辨识法，优先辨识部分参数，再将辨识得到的结果用在后续的辨识过程中。在步骤5中，所述分步辨识法包含如下三步：

38、第一步：辨识g,sfoffsetz

39、由式(16)中的第3个方程可知：

40、

41、设：

42、y1＝s fz (20)

43、a1＝[-r33 1] (21)

44、

45、通过采集多组数据，利用最小二乘法，可得到

46、第二步：辨识cos rz,sin rz，sfoffsetx,sfoffsety

47、由式(16)的第1个和第2个方程可知：

48、

49、设：

50、

51、

52、

53、通过采集多组数据，利用最小二乘法，可得到

54、第三步：辨识px,py,pz,smoffsetx,smoffsety,smoffsetz

55、由式(16)中的第4至第6这3个方程可知：

56、

57、设：

58、

59、

60、

61、通过采集多组数据，利用最小二乘法，可得到

62、步骤6：将x1，x2，x3代入到式(16)中，结合机器人的姿态描述矩阵即可完成六维力传感器的标定。此时得到无外界环境施加力时sg+sfoffset的值，所以当存在外界环境施加的力情况时，已知sffeedback，还已知sg+sfoffset的值，所以通过式(1)即可计算得到外界环境施加的力，由此完成外力精确感知的算法。

63、本发明的有益效果：

64、本发明无需通过精确的机械定位进行六维力传感器与末端法兰的安装，因为本发明在标定时考虑了传感器与法兰安装的角度问题，在算法的标定参数中包含了安装角度的偏移值，从算法上进行补偿；

65、本发明对分步辨识标定参数，从而能提高标定参数的辨识精确性，以此提高所计算的外界环境力的精确度。