基于MEMS传感器的工业机器人控制系统的工作方法与流程

本发明属于工业机器人控制，具体涉及一种基于mems传感器的工业机器人控制系统的工作方法。

背景技术：

1、工业机器人作为“机器换人”的一个重要环节，其在使用前需要操作者观察机器人及其加持工具相对于作业对象的位姿，通过对示教器的反复操作，调整示教点处机器人的作业位姿、运动参数和工艺参数，然后将满足作业要求的这些数据保存起来，再转入下一个示教点，这种方式需要操作者为专业的机器人技术人员，且需要操作者拥有丰富的经验。

2、因此，亟需提供一种能够将体感动作与机器人动作相映射从而来实现控制机器人动作的方法。

技术实现思路

1、本发明的目的是提供一种基于mems传感器的工业机器人控制系统的工作方法，以解决传统工艺机器人适配难度高的技术问题。

2、为了解决上述技术问题，本发明提供了一种基于mems传感器的工业机器人控制系统的工作方法，包括：通过传感器获取人体手部动作的加速度数据和角速度数据；通过加速度数据和角速度数据获得人体手部动作的姿态角数据；根据获得的人体手部动作姿态角数据控制机器人动作。

3、进一步，所述获取人体手部动作的加速度数据和角速度数据的方法包括：

4、建立地理坐标系，即

5、采用北天东坐标系oxyz，坐标的原点o位于载体的重心，z轴与重力方向相反，x指向北，y指向东；

6、建立载体坐标系，即

7、载体坐标系与传感器固连，坐标的原点o′位于传感器的重心，y′轴与y轴间的夹角为俯仰角θ；x′轴与x轴间的夹角为滚转角以及

8、获取加速度数据和角速度数据在载体坐标系中各轴上的分量数据；

9、校准获取的分量数据；

10、根据校准后的分量数据计算出人体手部动作的加速度和角速度分别在三维坐标系中与各轴的夹角数据。

11、进一步，所述获取加速度数据和角速度数据在载体坐标系中各轴上的分量数据包括：

12、读取传感器中的加速度和旋转角速度的分量值数据；其中

13、加速度计的x轴分量为acc_x，加速度计的y轴分量为acc_y，加速度计的z轴分量为acc_z；

14、绕x轴旋转的角速度为gyr_x，绕y轴旋转的角速度为gyr_y，绕z轴旋转的角速度为gyr_z；

15、将获取的加速度和旋转角速度在各轴上的分量值数据表示为矩阵形式；其中

16、加速度倍率选择为4g，角速度量程选取为2000度/秒，即

17、[ax0，ay0，az0]t＝[(4g*acc_x)/32768，(4g*acc_y)/32768，(4g*acc_z)/32768]t；

18、[gx0，gy0，gz0]t＝[2000*gyr_x/32768，2000*gyr_y/32768,2000*gyr_z/32768]t；其中ax0为x轴加速度实测值，ay0为y轴加速度实测值，az0为z轴加速度实测值；gx0为x轴角速度实测值，gy0为y轴角速度实测值，gz0为z轴角速度实测值。

19、进一步，所述校准获取的分量数据的方法包括：

20、在静止时获取传感器在各轴方向上的加速度值、角速度值，并通过求均值获取偏移量，即

21、加速度在各轴上的偏移量为：

22、其中

23、axoff为x轴加速度偏移量，ayoff为y轴加速度偏移量，azoff为z轴加速度偏移量，ax[n]为静止时x轴加速度实测值，ay[n]为静止时y轴加速度实测值，az[n]为静止时z轴加速度实测值，n为所测组数；

24、角速度在各轴上的偏移量为：

25、其中

26、gxoff为x轴角速度偏移量，gyoff为y轴角速度偏移量，gzoff为z轴角速度偏移量，gx[n]为静止时x轴角速度实测值，gy[n]为静止时y轴角速度实测值，gz[n]为静止时z轴角速度实测值；

27、通过偏移量校准加速度和旋转角速度在各轴上的分量值数据矩阵，即

28、校准后的加速度在各轴上的分量值数据矩阵为：

29、

30、校准后的角速度在各轴上的分量值数据矩阵为：

31、其中

32、ax为x轴加速度校准值，ay为y轴加速度校准值，az为z轴加速度校准值；

33、gx为x轴角速度校准值，gy为y轴角速度校准值，gz为z轴角速度校准值。

34、进一步，所述根据校准后的分量数据计算出人体手部动作的加速度和角速度分别在三维坐标系中与各轴的夹角数据的方法包括：

35、通过三角函数获取加速度的欧拉角，即

36、加速度夹角滚转角为

37、加速度夹角俯仰角θj＝arctan(ax/az)；以及

38、通过积分运算获取角速度的欧拉角，即

39、其中

40、θ′g为前一步计算得到的积分值，初始值均为0。

41、进一步，根据人体手部动作的加速度和角速度在各轴方向的夹角数据获取人体手部动作在各轴方向的姿态角度的方法包括：

42、通过互补滤波法将加速度欧拉角数据和角速度欧拉角数据进行融合，即

43、融合滚转角为

44、融合俯仰角为θt＝k*θj+(1-k)*(θt-1+θg)；其中

45、k取值为0.6，为上一次采样时计算得到的θt-1为上一次采样时计算得到的θt。

46、进一步，所述根据获得的人体手部动作姿态角数据控制机器人动作的方法包括：

47、左手、右手分别佩戴第一载体、第二载体；

48、通过上位机接收第一载体、第二载体发送的姿态角度数据，并选择相应运动模式发出动作指令信号；

49、通过机器人接收上位机发送的动作指令信号并动作。

50、进一步，所述通过上位机接收第一载体、第二载体发送的姿态角度数据，并选择相应运动模式发出动作指令信号的方法包括：

51、与第一载体、第二载体建立串口连接，并与机器人通过socket通信；

52、选择机器人运动模式；

53、将接收的串口信号进行数据分析后向机器人发送动作指令；其中

54、所述机器人运动模式包括：线性运动和关节运动。

55、进一步，所述将接收的串口信号进行数据分析后向机器人发送动作指令的方法包括：

56、接收第一载体、第二载体获取的动作姿态角度；

57、通过接收的姿态角度数据，并根据选择的机器人运动模式，向机器人发出动作指令。

58、本发明的有益效果是，本发明的基于mems传感器的工业机器人控制系统的工作方法通过采集人体手部动作，并将手部动作与机器人动作映射，实现机器人按照手部姿态控制运动，并且通过将获取数据进行校准降噪，从而提高数据的准确度与操控的精准度。

59、本发明的其他特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点在说明书以及附图中所特别指出的结构来实现和获得。

60、为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。