机械臂运行轨迹修正装置和相关计算机程序的制作方法

本发明涉及工业机器人控制领域，涉及一种应用于3c产品制造产线上的机械臂运行轨迹修正装置和相关计算机程序。

背景技术：

1、轨迹规划是指根据作业任务要求，计算出机器人预期的运动轨迹。轨迹规划一般表达为位移、速度、加速度等运动变量关于时间的函数。在此函数中，输入是机器人的期望运动轨迹、运动学和动力学参数，输出是机器人各关节或末端执行器的运动变量。为满足实际工业生产要求，一个优秀的轨迹规划方法应当充分考虑机器人的关节磨损和运动平稳性。

2、与之相对的，轨迹修正是根据感知反馈和机器人实时状态，基于控制算法对机器人轨迹进行调整和修正，以保证其运动轨迹的安全性和准确性。可以说，轨迹修正是对轨迹规划在实践中的补充和优化。基于轨迹修正，机器人能够以更高效率、更高精度完成既定任务。

3、在现有技术中，对机器人或者机械臂的控制过程通常为单向的，即仅仅能够通过输入控制指令实施控制，因此在控制机器人或者机械臂运行之前需要经过多重校准和环境配置。然而，生产线运转速度极快，随时可能发生突发状态，这会使得校准和配置失去意义。为了确保机器人运行的安全性、高效性，业界亟需提出一种运行轨迹修正方案。

技术实现思路

1、本发明要解决的技术问题在于，针对现有技术的上述缺陷，提供一种机械臂运行轨迹修正方法、系统和存储介质，在输入指令对机械臂实施控制的过程中，还通过机械臂所反馈的状态进行轨迹修正，具有更高效率、更高精度和更高安全性。

2、本发明解决技术问题所采用的技术方案如下：

3、一种机械臂运行轨迹修正方法，所述方法基于机械臂运行轨迹修正装置，所述装置包括转角传感器、速度传感器、加速度传感器、激光雷达和工业机器人控制器；所述转角传感器、所述速度传感器、所述加速度传感器设置于机械臂上，所述转角传感器用于采集机械臂关节的转角数据、所述速度传感器用于采集机械臂关节的速度数据、所述加速度传感器用于采集机械臂关节的加速度数据；所述激光雷达设置于机械臂上方或者侧方，用于采集机械臂运动空间数据；所述工业机器人控制器用于分析处理转角数据、速度数据、加速度数据和机械臂运动空间数据，并控制机械臂运行；所述方法包括以下步骤：

4、s1.获取机械臂的关节空间规划数据，根据关节空间规划数据确定机械臂的关节的控制序列；

5、s2.根据机械臂的关节的控制序列，将机械臂的关节空间规划数据转换为机械臂的笛卡尔空间规划数据；

6、s3.基于机械臂的笛卡尔空间规划数据分析得到机械臂的初始规划轨迹数据，所述初始规划轨迹数据描述机械臂在笛卡尔空间中的规划轨迹；

7、s4.实时采集机械臂关节运动数据，采集机械臂运行过程中关节的转角数据、速度数据以及加速度数据，所述转角数据表示关节的偏转角度，所述速度数据表示关节的运行速度，所述加速度数据表示关节的运行速度变化速率；

8、s5.对初始规划轨迹数据进行平滑性评估，得到平滑性评估结果；

9、s6.对初始规划轨迹数据进行精确性评估，得到精确性评估结果；

10、s7.采集机械臂运行过程中机械臂运动空间内障碍物的三维点云数据，结合初始规划轨迹数据和障碍物的三维点云数据进行碰撞风险评估，得到碰撞风险评估结果；

11、s8.根据平滑性评估结果、精确性评估结果和碰撞风险评估结果，结合机械臂关节的转角数据、速度数据以及加速度数据，通过动态校正算法分析得到关节的轨迹优化数据，所述轨迹优化数据包括转角调整数据、速度调整数据以及加速度调整数据。

12、与现有技术相比，本技术方案的有益效果是：将关节空间规划数据转换为笛卡尔空间规划数据，实时采集转角数据、速度数据以及加速度数据，并进行平滑性、精确性和碰撞风险评估，根据机械臂反馈的状态对规划轨迹进行修正，确保运行过程的高效率、高精度和高安全性。

13、进一步地，在步骤s1中，获取机械臂的关节空间规划数据，根据关节空间规划数据确定机械臂的关节的控制序列具体包括以下步骤：

14、s101.获取机械臂的末端执行器的初始位置在笛卡尔空间中的初始位置坐标为a(x1,y1,z1)，获取机械臂的末端执行器目标位置在笛卡尔空间中的目标位置坐标为b(x2,y2,z2)；

15、s102.根据初始位置坐标为a(x1,y1,z1)和目标位置坐标为b(x2,y2,z2)，分析得到从初始关节角度到目标关节角度，机械臂的关节所对应的关节角度变化量δθ1，δθ2，δθ3；

16、s103.基于线性插值方法在关节空间中计算出一条控制机械臂从初始关节角度平滑过渡运行到目标关节角度的路径；

17、s104.通过三次多项式插值生成机械臂的关节在笛卡尔空间中的运行轨迹，得到关节轨迹数据θi(t)：

18、θi(t)＝a0+a1t+a2t2+a3t3；

19、其中，t是运行时间，θi(t)是关节i在t时刻的角度，a0、a1、a2、a3为多项式的系数；

20、s105.根据初始关节角度、目标关节角度和机械臂运行时间进行参数分析，计算得到a0、a1、a2、a3的对应数值；

21、s106.基于a0、a1、a2、a3和关节轨迹数据θi(t)得到关节的控制序列。

22、采用上述方案的有益效果是：机械臂由多个关节组成，步骤s1在确定对应的关节角度变化量后，通过关节轨迹数据即可明确控制过程中某个具体时间点所对应的关节角度，从而获取关节的控制序列，即可直接获取机械臂的控制指令。

23、进一步地，在步骤s2中，将机械臂的关节空间规划数据转换为机械臂的笛卡尔空间规划数据具体包括以下步骤：

24、s201.获取关节的dh矩阵参数，所述dh矩阵参数包括第一连杆长度al1、第二连杆长度al2、第一连杆偏距dl1、第二连杆偏距dl2、第一关节角度θl1、第二关节角度θl2、第一关节扭转和第二关节扭转

25、s202.基于所述dh矩阵参数建立正向运动学模型，计算第一变换矩阵a1(θl1)和第二变换矩阵a2(θl2)，所述第一变换矩阵用于描述a1(θl1)第一连杆的末端执行器在笛卡尔空间中的姿态，所述第二变换矩阵a2(θl2)用于描述第二连杆的末端执行器在笛卡尔空间中的姿态，其中：

26、

27、

28、s203.根据第一变换矩阵a1(θl1)和第二变换矩阵a2(θl2)计算整体变换矩阵t02(t)，其中，t02(t)＝a1(θl1)·a2(θl2)；

29、s204.根据整体变换矩阵t02(t)将机械臂的关节空间规划数据转换为机械臂的笛卡尔空间规划数据。

30、采用上述方案的有益效果是：通过第一变换矩阵和第二变换矩阵分析得到整体变换矩阵后，基于整体变换矩阵即可机械臂的关节空间规划数据转换为机械臂的笛卡尔空间规划数据，使得机械臂控制过程更加直观。

31、进一步地，在步骤s5中，对机械臂的初始规划轨迹数据进行平滑性评估得到平滑性评估结果具体包括以下步骤：

32、s501.采集机械臂在二维平面上的轨迹数据(xsi,ysi)和对应的时间戳tsi，其中，i＝1,2,…,n；

33、s502.获取机械臂在二维平面上三个连续点p1(xs1,ys1)、p2(xs2,ys2)和p3(xs3,ys3)；

34、s503.根据p1(xs1,ys1)、p2(xs2,ys2)和p3(xs3,ys3)计算轨迹曲率ki，其中，

35、

36、s504.根据轨迹曲率ki计算轨迹曲率变化率δki，其中

37、

38、s505.根据轨迹曲率ki和轨迹曲率变化率δki进行平滑性评估得到平滑性评估结果；

39、在步骤s6中，对机械臂的初始规划轨迹数据进行精确性评估得到精确性评估结果具体包括以下步骤：

40、s601.采集机械臂在二维平面上的轨迹数据(xsi,ysi)和对应的时间戳tsi，其中，i＝1,2,…,n；

41、s602.根据轨迹数据(xsi,ysi)和对应的时间戳tsi形成实际运行轨迹数据；

42、s603.通过对比实际运行轨迹数据和初始规划轨迹数据进行精确性评估得到精确性评估结果。

43、采用上述方案的有益效果是：根据轨迹曲率和轨迹曲率变化率进行平滑性评估，轨迹曲率和轨迹曲率变化率数值越小、平滑性越好；通过将实际运行轨迹数据和初始规划轨迹数据进行对比，重合程度越高，精确性越好。通过上述方案，将对初始规划轨迹数据进行量化评估，有利于提高控制科学性。

44、进一步地，在步骤s7中，结合初始规划轨迹数据和障碍物的三维点云数据进行碰撞风险评估，得到碰撞风险评估结果具体包括以下步骤：

45、s701.根据障碍物的三维点云数据在笛卡尔空间中建立障碍物模型；

46、s702.将初始规划轨迹数据分割成多个连续的规划轨迹片段，所述规划轨迹片段包括各自轨迹片段内的起始点、中间点和终止点；

47、s703.将规划轨迹片段匹配到障碍物模型中，判断障碍物模型和规划轨迹片段是否存在重叠部分，进而输出碰撞风险评估结果。

48、采用上述方案的有益效果是：根据机械臂运行空间中障碍物的三维点云数据建立障碍物模型，通过引入初始规划轨迹数据判断障碍物模型和规划轨迹片段是否存在重叠部分来进行碰撞风险评估，能够实时解除运行过程中的碰撞风险，提高安全性。

49、对应地，一种机械臂运行轨迹修正系统，所述系统基于机械臂运行轨迹修正装置，所述装置包括转角传感器、速度传感器、加速度传感器、激光雷达和工业机器人控制器；所述转角传感器、所述速度传感器、所述加速度传感器设置于机械臂上，所述转角传感器用于采集机械臂关节的转角数据、所述速度传感器用于采集机械臂关节的速度数据、所述加速度传感器用于采集机械臂关节的加速度数据；所述激光雷达设置于机械臂上方或者侧方，用于采集机械臂运动空间数据；所述工业机器人控制器用于分析处理转角数据、速度数据、加速度数据和机械臂运动空间数据，并控制机械臂运行；所述系统包括：

50、空间规划数据获取模块，用于获取机械臂的关节空间规划数据，根据关节空间规划数据确定机械臂的关节的控制序列；

51、坐标系数据转换模块，用于根据机械臂的关节的控制序列，将机械臂的关节空间规划数据转换为机械臂的笛卡尔空间规划数据；

52、规划轨迹分析模块，用于基于机械臂的笛卡尔空间规划数据分析得到机械臂的初始规划轨迹数据，所述初始规划轨迹数据描述机械臂在笛卡尔空间中的规划轨迹；

53、实时数据采集模块，用于实时采集机械臂关节运动数据，采集机械臂运行过程中关节的转角数据、速度数据以及加速度数据，所述转角数据表示关节的偏转角度，所述速度数据表示关节的运行速度，所述加速度数据表示关节的运行速度变化速率；

54、平滑性评估模块，用于对初始规划轨迹数据进行平滑性评估，得到平滑性评估结果；

55、精确性评估模块，用于对初始规划轨迹数据进行精确性评估，得到精确性评估结果；

56、碰撞风险评估模块，用于采集机械臂运行过程中机械臂运动空间内障碍物的三维点云数据，结合初始规划轨迹数据和障碍物的三维点云数据进行碰撞风险评估，得到碰撞风险评估结果；

57、轨迹优化模块，用于根据平滑性评估结果、精确性评估结果和碰撞风险评估结果，结合机械臂关节的转角数据、速度数据以及加速度数据，通过动态校正算法分析得到关节的轨迹优化数据，所述轨迹优化数据包括转角调整数据、速度调整数据以及加速度调整数据。

58、进一步地，所述空间规划数据获取模块包括：

59、坐标计算单元，用于获取机械臂的末端执行器的初始位置在笛卡尔空间中的初始位置坐标为a(x1,y1,z1)，获取机械臂的末端执行器目标位置在笛卡尔空间中的目标位置坐标为b(x2,y2,z2)；

60、角度计算单元，用于根据初始位置坐标为a(x1,y1,z1)和目标位置坐标为b(x2,y2,z2)，分析得到从初始关节角度到目标关节角度，机械臂的关节所对应的关节角度变化量δθ1，δθ2，δθ3；

61、第一轨迹计算单元，用于基于线性插值方法在关节空间中计算出一条控制机械臂从初始关节角度平滑过渡运行到目标关节角度的路径；

62、第二轨迹计算单元，用于通过三次多项式插值生成机械臂的关节在笛卡尔空间中的运行轨迹，得到关节轨迹数据θi(t)：

63、θi(t)＝a0+a1t+a2t2+a3t3；

64、其中，t是运行时间，θi(t)是关节i在t时刻的角度，a0、a1、a2、a3为多项式的系数；

65、参数分析单元，用于根据初始关节角度、目标关节角度和机械臂运行时间进行参数分析，计算得到a0、a1、a2、a3的对应数值；

66、控制序列分析单元，用于基于a0、a1、a2、a3和关节轨迹数据θi(t)得到关节的控制序列。

67、进一步地，所述坐标系数据转换模块包括：

68、参数获取单元，用于获取关节的dh矩阵参数，所述dh矩阵参数包括第一连杆长度al1、第二连杆长度al2、第一连杆偏距dl1、第二连杆偏距dl2、第一关节角度θl1、第二关节角度θl2、第一关节扭转和第二关节扭转

69、第一变换矩阵计算单元，用于基于所述dh矩阵参数建立正向运动学模型，计算第一变换矩阵a1(θl1)和第二变换矩阵a2(θl2)，所述第一变换矩阵用于描述a1(θl1)第一连杆的末端执行器在笛卡尔空间中的姿态，所述第二变换矩阵a2(θl2)用于描述第二连杆的末端执行器在笛卡尔空间中的姿态，其中：

70、

71、

72、第二变换矩阵计算单元，根据第一变换矩阵a1(θl1)和第二变换矩阵a2(θl2)计算整体变换矩阵t02(t)，其中，t02(t)＝a1(θl1)·a2(θl2)；

73、坐标系数据转换单元，用于根据整体变换矩阵t02(t)将机械臂的关节空间规划数据转换为机械臂的笛卡尔空间规划数据。

74、进一步地，所述平滑性评估模块包括：

75、第一二维轨迹采集单元，用于采集机械臂在二维平面上的轨迹数据(xsi,ysi)和对应的时间戳tsi，其中，i＝1,2,…,n；

76、二维坐标获取采集单元，用于获取机械臂在二维平面上三个连续点p1(xs1,ys1)、p2(xs2,ys2)和p3(xs3,ys3)；

77、轨迹曲率计算单元，用于根据p1(xs1,ys1)、p2(xs2,ys2)和p3(xs3,ys3)计算轨迹曲率ki，其中，

78、

79、轨迹曲率变化率计算单元，用于根据轨迹曲率ki计算轨迹曲率变化率δki，其中

80、

81、平滑性评估单元，用于根据轨迹曲率ki和轨迹曲率变化率δki进行平滑性评估得到平滑性评估结果；

82、所述精确性评估模块包括：

83、第二二维轨迹采集单元，用于采集机械臂在二维平面上的轨迹数据(xsi,ysi)和对应的时间戳tsi，其中，i＝1,2,…,n；

84、运行轨迹计算单元，用于根据轨迹数据(xsi,ysi)和对应的时间戳tsi形成实际运行轨迹数据；

85、精确性评估单元，用于通过对比实际运行轨迹数据和初始规划轨迹数据进行精确性评估得到精确性评估结果；

86、所述碰撞风险评估模块包括：

87、模型建立单元，用于根据障碍物的三维点云数据在笛卡尔空间中建立障碍物模型；

88、轨迹分割单元，用于将初始规划轨迹数据分割成多个连续的规划轨迹片段，所述规划轨迹片段包括各自轨迹片段内的起始点、中间点和终止点；

89、碰撞风险评估单元，用于将规划轨迹片段匹配到障碍物模型中，判断障碍物模型和规划轨迹片段是否存在重叠部分，进而输出碰撞风险评估结果。

90、对应地，一种存储介质，所述存储介质存储有计算机程序，所述的计算机程序包括程序指令，当所述程序指令被处理器执行时，处理器执行如上所述的机械臂运行轨迹修正方法。