机器人底座六维力传感器在线监测与误差补偿方法及系统

本发明属于机器人外力感知，具体涉及机器人底座六维力传感器在线监测与误差补偿方法及系统。

背景技术：

1、随着机器人在焊接、搬运、装配等场景中应用越来越广泛，更为精确的动力学感知能力为机器人所必需，通过外力的精确感知，机器人不仅可以获得一个较强的作业、环境适应能力，而且还能够根据外力及时调整相关状态，完成人机紧密协作的复杂作业任务。目前，针对机器人的外力感知技术方案主要有无外力传感和有外力传感两种方式，其中，无外力传感方式主要通过检测电流、设计广义动量观测器等间接方式辨识外力实现感知，有外力传感方式则是通过增加额外的如电子皮肤、关节力传感器、六维力传感器等外部力传感器的布置直接测量外力实现外力感知。两种方案中，尽管后者需要增加额外的力传感器，但有着算法相对简单、检测精度高、响应速度快等优点，因而在工程中常被采用。

2、在机器人底座上布置六维力传感器是实现机器人全臂外力感知的常用方案之一。通过底座六维力传感器的力检测，机器人系统能够获得与机器人全臂接触的外力，从而保证机器人具有较强的感知和环境适应能力。然而，由于六维力传感器安装在机器人底座上，其不可避免的存在安装误差，再加上长期受力、零点漂移以及外部工作环境变化等因素影响，底座六维力传感器不可避免地存在一定测量误差，长期使用后工作状态改变可能存在失效的风险，容易引发安全问题。为此，能够对底座六维力传感器进行在线的监测和误差补偿显得尤为重要。

技术实现思路

1、本发明针对现有技术中的不足以及对底座六维力传感器监测和误差补偿的需求，提供一种机器人底座六维力传感器在线监测与误差补偿方法及系统，目的是实现对底座六维力传感器的在线监测和六维力测量误差的在线补偿，保证六维力传感器能够持续正常工作，并具有一个较高的测量精度。

2、为实现上述目的，本发明采用以下技术方案：

3、机器人底座六维力传感器在线监测与误差补偿方法，其特征在于，包括：

4、步骤1：采集机器人运行过程中的状态数据和底座六维力传感器的测量值，并根据设定的筛选条件选取合适的数据；

5、步骤2：建立底座六维力传感器的受力求解模型，带入筛选后的数据求解底座六维力传感器的预测值；

6、步骤3：求解底座六维力传感器的预测值与测量值之间的均方根残差，当求解结果连续多次超过设定阈值时，判断底座六维力传感器出现异常；

7、步骤4：对于正常工作的底座六维力传感器，基于在线参数估计的误差补偿算法，带入底座六维力传感器的预测值和测量值求解误差补偿矩阵，选用补偿精度较高的误差补偿矩阵补偿底座六维力传感器的测量值；

8、步骤5：按照固定步长循环步骤1到步骤4，实现底座六维力传感器的在线监测和误差补偿。

9、为优化上述技术方案，采取的具体措施还包括：

10、进一步地，步骤1中，所述状态数据包括机器人机械臂中串联的各个连杆对应关节的角位移θi(t)、角速度和加速度n为关节个数，t为采样时刻；所述测量值为和分别对应于底座六维力传感器x轴、y轴和z轴的力测量值，和分别对应于底座六维力传感器x轴、y轴和z轴的力矩测量值。

11、进一步地，所述状态数据通过在各关节处布置角位移传感器获取关节的角位移θi(t)，再利用数值微分方法计算关节的角速度和加速度计算公式如下：

12、

13、

14、其中，h为采样时间间隔。

15、进一步地，所述状态数据通过在各关节处布置位置传感器、角速度传感器和角加速度传感器直接获得各关节的角位移θi(t)、角速度和加速度

16、进一步地，步骤1中，所述设定的筛选条件如下：

17、a：机器人不带负载；

18、b：只有机器人的末端关节处于加速状态，其它关节处于非零位的静止状态或匀速状态；

19、在采集的状态数据和测量值中，选取满足条件a和b的θi(t)、和组合形成采集数据序列作为传感器监测和误差补偿的输入数据：

20、

21、其中，θ(tk)、和分别表示各连杆对应关节tk时刻的角位移、角速度和角加速度数据集合，表示底座六维力传感器tk时刻的测量值。

22、进一步地，步骤2具体如下：

23、首先，通过如下方式建立底座六维力传感器的受力求解模型：

24、基于牛顿-欧拉动力学方程，建立如下公式求解机器人靠近底座的第1连杆t时刻作用在底座六维力传感器的力力fs(t)和力矩ns(t)：

25、fs(t)＝r1f1(t)

26、ns(t)＝r1n1(t)+p1(t)×r1(t)f1(t)

27、其中，r1为机器人底座相对底座六维传感器的旋转矩阵，p1为机器人底座相对底座六维传感器的位置矢量；

28、对机器人的每个连杆应用牛顿-欧拉动力学方程，从机器人末端开始向机器人第1连杆递推得到机器人第i连杆t时刻受到的力fi(t)和力矩ni(t)：

29、fi(t)＝ri+1(t)fi+1(t)+fi(t)

30、

31、机器人第i连杆t时刻受到的惯性力fi(t)和惯性力矩ni(t)用如下公式计算：

32、

33、

34、其中，mi表示第i连杆的质量，ri+1(t)表示第i+1连杆t时刻相对第i连杆的旋转矩阵，ωi(t)和分别表示第i连杆t时刻的角速度和角加速度，表示第i连杆t时刻的质心线加速度；pi+1表示第i连杆坐标系原点至第i+1关节坐标系原点的矢量，表示第i连杆的质心位置矢量，表示第i连杆质心惯性张量；

35、然后，将tk时刻的状态数据通过机器人连杆速度和加速度迭代公式计算得到各连杆的角速度ωi(tk)、角加速度和质心线加速度接着带入到底座六维传感器受力求解模型，最终获得底座六维力传感器的预测值w(tk)为：

36、w(tk)＝[fx(tk) fy(tk) fz(tk)nx(tk) ny(tk) nz(tk)]

37、其中，fx(tk)、fy(tk)和fz(tk)分别对应于底座六维力传感器x轴、y轴和z轴的力预测值，nx(tk)、ny(tk)和nz(tk)分别对应于底座六维力传感器x轴、y轴和z轴的力矩预测值。

38、进一步地，步骤3具体如下：

39、采用如下公式求解tk时刻底座六维力传感器的预测值和测量值的均方根残差r(tk)：

40、

41、

42、当在连续设定次数m内，底座六维力传感器的预测值和测量值的均方根残差都大于设定阈值δ时，则判定底座六维力传感器出现测量异常。

43、进一步地，步骤4中，所述误差补偿矩阵根据底座六维力传感器的测量原理和各方向力和力矩的相关性，定义为：

44、

45、其中，a(t)表示误差补偿矩阵，a11(t)，a12(t)，…，a66(t)为补偿因子；

46、所述误差补偿算法采用带遗忘因子的递推最小二乘在线估计算法求解误差补偿矩阵。

47、进一步地，步骤4具体包括如下步骤：

48、s1：初始化，设置遗忘常数λ，设置中间变量y(t)的初始参数y(0)＝i6/106，i6为6×6单位矩阵，采用如下公式求解a(t)的初始值a(0)：

49、

50、其中，[·]+表示矩阵的moore-penrose广义逆；

51、s2：将底座六维力传感器tk时刻的预测值w(tk)和测量值带入如下公式计算中间变量φ(tk)和y(tk)：

52、

53、

54、s3：利用如下公式求解tk时刻的误差补偿矩阵a(tk)：

55、

56、s4：将a(tk)带入补偿公式进行补偿，并与上一时刻使用的误差补偿矩阵进行比较，选取其中补偿精度较高的误差补偿矩阵作为新的误差补偿矩阵使用，补偿公式如下：

57、

58、其中，是对应于的补偿值。

59、此外，本发明还提出了一种机器人底座六维力传感器在线监测与误差补偿系统，所述六维力传感器布置在机器人底座，用于检测机器人所受的外力；其特征在于，包括：

60、数据采集模块，用于采集机器人运行过程中的状态数据和底座六维力传感器的测量值；

61、数据筛选模块，用于根据设定的筛选条件从数据采集模块采集的状态数据和测量值中选取合适的数据；

62、数据处理模块，用于建立底座六维力传感器的受力求解模型，带入数据筛选模块筛选后的数据求解底座六维力传感器的预测值，以及求解底座六维力传感器的预测值与测量值之间的均方根残差；

63、传感器监测模块，用于根据数据处理模块的求解结果来判断底座六维力传感器的工作状态，当求解结果连续多次超过设定阈值时，判断底座六维力传感器出现异常；

64、误差补偿模块，用于对于正常工作的底座六维力传感器，基于在线参数估计的误差补偿算法，带入底座六维力传感器的预测值和测量值求解误差补偿矩阵，选用补偿精度较高的误差补偿矩阵补偿底座六维力传感器的测量值；

65、机器人控制系统，用于根据传感器监测模块和误差补偿模块的结果，对机器人进行控制。

66、本发明的有益效果是：

67、1)本发明提供了一种底座六维力传感器状态在线监测方案，监测布置在底座上的六维力传感器工作状态是否正常，当底座六维力传感器异常时及时给出提示。该方案通过连续采集机器人状态数据和底座六维力传感器的测量值，并带入底座六维力传感器的受力求解模型获得传感器预测值，而后将预测值与测量值进行均方根残差比较，以均方根残差值为依据对底座六维力传感器进行在线监测，针对底座六维力传感器长期使用中可能存在传感器工作状态改变甚至失效等问题，在先监测并及时发现传感器异常，确保六维力传感器能持续稳定工作；

68、2)本发明提供了一种底座六维力传感器误差补偿方案，由于底座六维力传感器不可避免地存在安装倾角、螺栓松紧不一带来的安装误差及力传感器长期受力、零点漂移等引起的测量误差，对此，该方案以底座六维力传感器预测值和测量值为基础，建立基于遗忘因子最小二乘算法的在线误差矩阵求解算法在线求解底座六维力传感器的补偿矩阵，修正底座六维力传感器测量输出值，提高传感器测量精度；

69、3)本发明建立了一种底座六维力传感器在线监测与误差补偿系统的数据筛选规则，在采集和利用机器人运行过程中的状态数据时，并没有使用采集的所有数据，而是通过设定筛选规则，如：a)机器人不带负载；b)只有机器人的末端关节处于加速状态，其它关节处于非零位的静止状态或匀速状态等条件下的机器人状态数据进行使用，提高数据的有效性，增加了底座六维力传感器在线监测与误差补偿的准确性；

70、4)本发明通过底座六维力传感器在线监测与测量误差补偿系统的实施，保障了底座六维力传感器持续、稳定的工作能力和测量精度，提高机器人的精确外力感知能力和控制精度，增强机器人的性能。