一种基于DMP的复杂曲面打磨轨迹规划方法与流程

本发明涉及机器人打磨，尤其是一种基于dmp的复杂曲面打磨轨迹规划方法。

背景技术：

1、随着机器人技术的不断发展，以及机械制造成本、零部件成本的逐渐降低，机器人逐渐从工业领域渗透进了我们的日常生活中。

2、服务机器人的不断普及，渐渐提升了人们对机器人的接受程度，市场上出现的机器人也逐渐由单一的送餐、快递机器人，拓展到具有医疗护养、解说导览等丰富功能的机器人，这同时也对机器人的稳定性、功能性与实用性有了更高的要求。在机器人打磨领域中，打磨路径的规划仍然是一个难点，并且现有技术较少考虑打磨速度对打磨质量的影响。例如：

3、专利cn202011493670.3公开了一种基于机器视觉的打磨路径规划方法。其提出了一种基于打磨过程中的打磨参数对打磨路径进行优化的方法，然而其方法繁琐，且没有考虑打磨速度对打磨质量的影响。

4、专利cn202111384297.2公开了一种基于实时点云的大型复杂构件表面打磨路径规划方法。其提出了一种基于表面点云的迭代规划路径方法，考虑了表面曲率对打磨的影响。但是没有考虑打磨速度与表面粗糙度的影响。

技术实现思路

1、针对现有技术的不足，本发明提供一种基于dmp的复杂曲面打磨轨迹规划方法，本发明提出了一种自适应打磨速度的方法，大大减少了在复杂曲面打磨时人的工作量，提高了打磨效率与打磨质量。

2、本发明的技术方案为：一种基于dmp的复杂曲面打磨轨迹规划方法，包括以下步骤：

3、s1)、利用深度相机获取打磨区域内的点云信息，然后切割出带打磨工件部分的点云数据；

4、s2)、在分割的打磨工件的点云数据中规划出打磨路径；

5、s3)、计算打磨路径上各点的法向量和曲率，根据法向量计算打磨头在打磨点的姿态，并用四元数形式表示；

6、s4)、基于打磨路径各点的曲率将打磨路径进行分割，将整条打磨路径轨迹分割成多个片段轨迹；

7、s5)、对于每个片段轨迹，用位置dmp1、和方向dmp2对每个片段轨迹的位置和姿态进行建模，并对打磨路径的曲率用曲率dmp3进行建模；

8、s6)、打磨时，控制器根据dmp1和dmp2分别控制打磨头的位置和姿态，打磨的速度由dmp3和打磨给进方向摩擦力共同决定。

9、作为优选的，步骤s1)中，利用深度相机获取打磨区域内的点云信息后通过pcl库可视化。

10、作为优选的，步骤s1)中，所述的切割出带打磨工件部分的点云数据为：在点云中找出工件的边界，根据边界将工件部分点云切割出。

11、作为优选的，步骤s2)中，根据打磨头的工作半径，在工件的点云中规划出打磨路径，使得打磨头沿着这些轨迹打磨可以覆盖整个工件的表面。

12、作为优选的，步骤s3)中，所述的打磨路径上各点法向量采用pcl::normalestimation获得。

13、作为优选的，步骤s3)中，所述的打磨路径上各点曲率的计算方法如下：

14、s31)、设打磨路径由点集p＝{p1,p2......pn}，每个点pi＝{xi,qi,ki}，xi是该点的位置，qi表示该点的姿态，ki表示该点的曲率；点pi的曲率ki由点pi-1、pi、pi+1计算得到；

15、s32)、曲率ki的计算方法为：

16、设xi＝(xi,yi,zi)、xi-1＝(xi-1,yi-1,zi-1)、xi+1＝(xi+1,yi+1,zi+1)、并且以xo＝(x0,y0,z0)为pi-1、pi、pi+1三点所在圆的圆心；建立如下方程：

17、

18、联立(1)(2)消元得到：

19、

20、记为：

21、a2＝2×(xi-1-xi)；

22、b2＝2×(yi-1-yi)；

23、c2＝2×(zi-1-zi)；

24、

25、联立(1)(3)消元得到：

26、

27、记为：

28、a3＝2×(xi+1-xi)

29、b3＝2×(yi+1-yi)

30、c3＝2×(zi+1-zi)

31、

32、根据三点共面约束可确定平面方程：

33、

34、a1＝yi×zi-1-yi-1×zi-yi×zi+1+yi+1×zi+yi-1×zi+1-yi+1×zi-1

35、b1＝-(xi×zi-1-xi-1×zi-xi×zi+1+xi+1×zi+xi-1×zi+1-xi+1×zi-1)

36、c1＝xi×yi-1-xi-1×yi-xi×yi+1+xi+1×yi+xi-1×yi+1-xi+1×yi-1

37、d1＝-(xi×yi-1×zi+1-xi×yi+1×zi-1-xi-1×yi×zi+1+xi-1×yi+1×zi+xi+1×yi×zi-1-xi+1×yi-1×zi)

38、通过以上(4)(5)(6)获得a～d系数建立线性方程组，三个未知数三个方程即可求解圆心和半径：

39、

40、

41、

42、

43、式中，r为pi-1、pi、pi+1三点所在圆的半径。

44、作为优选的，步骤s3)中，所述的根据法向量计算打磨头在打磨点的姿态，并用四元数形式表示，具体为：

45、设深度相机和机械臂基座之间的旋转矩阵为a，机械臂在打磨p点时末端

46、相对于基座的旋转矩阵h的计算方法为：

47、假设打磨点p的坐标为p＝(x,y,z)t，法向量为-a＝(-xf,-yf,-zf)t，以法向量a为z轴的单位向量建立坐标系，假设x轴和y轴的单位向量分别为b＝(x1,y1,y1)t，c＝(x2,y2,y2)t，其中x和y轴单位向量的选取有无数种可能，任意选择一种即可；当前所建立坐标系与相机坐标系之间的旋转矩阵h为：

48、

49、当前坐标系与机器人基座之间的旋转矩阵rt＝ah，旋转矩阵rt转四元数通过scipy.spatial.transform中的函数求得。

50、作为优选的，步骤s4)中，整条打磨路径轨迹点集为p＝{p1,p2......pn}，根据曲率将打磨路径分成多段，以提高dmp对轨迹的拟合精度，分割方法为：

51、通过设置分界曲率k*，然后找出满足(ki-k*)*(ki-1-k*)＜0的点集，以它们为分割点对打磨路径轨迹点集p进行切割；分割完后，片段轨迹集合表示为pl＝{p1,p2......pm}，其中，pj(1≤j≤m)表示第j个片段轨迹的点的集合。

52、作为优选的，步骤s5)中，对于分割后的每个片段轨迹pj，用dmp1对轨迹内的位置和刚度进行建模，用方向dmp2对姿态进行建模，用dmp3对曲率进行建模。

53、作为优选的，步骤s5)中，用dmp1对轨迹内的位置和刚度进行建模表示为：

54、

55、

56、式中，f(x)为拟合时的跟随项，该项用于控制实际轨迹的形状，由高斯核φ(x)的线性组合和来自于正则系统的时间衰减系数x组成；y为示教点轨迹，τ为控制使用时中机器人末端运动速度的时间常数，g定义为使用示教轨迹时定义的机器人末端位置，z、分别表示经过时间常数τ放缩后的示教轨迹速度与示教轨迹加速度，αz、βz为控制器的控制参数，表示示教轨迹速度；

57、学习示教轨迹y的过程为通过已知示教点拟合跟随项f(x)的调整参数ωi；轨迹开始生成时，跟随项f(x)通过修改机器人末端加速度从而改变轨迹方向，当时间趋于无穷时，强迫跟随项f(x)趋于0；轨迹回到指定点；所述的跟随项f(x)表示为：

58、

59、式中，ωi表示第i个调整参数，φi(x)表示第i个高斯核，调整参数与高斯核共有n个，n由用户指定，x为时间衰减系数；

60、第i个高斯核φi(x)的生成方式如下：

61、φi(x)＝exp(-hi(x-ci)2)；

62、其中，hi表示第i个高斯核的宽度、ci分表示第i个高斯核的中心，均由用户指定；x为时间衰减系数；

63、时间衰减系数x生成方式如下：

64、

65、式中，表示时间衰减项，αx为用户指定的常数；

66、调整参数ωi拟合方式如下：

67、

68、式中，ftarget为拟合时由已知的轨迹生成的目标跟随项，t表示矩阵的转置；

69、学习示教轨迹的过程为通过已知示教点拟合目标方程ftarget；通过更新调整参数ωi，使f(x)的输出靠近ftarget：

70、

71、示教轨迹生成的过程是由已知的示教轨迹y学习f(x)使其成为目标跟随项ftarget，而重现的过程为由已知跟随项ftarget生成重现轨迹yr；

72、式中，表示示教轨迹加速度。

73、作为优选的，步骤s5)中，用方向dmp对四元数进行建模表示为：

74、

75、

76、式中，q∈s3为单位四元数，ω∈r3、分别为角速度和加速度，τ为时间尺度因子，*表示两个四元数的乘积，e(,)表示两个四元数之间的误差，为角速度四元数，即为一个以标量部分为零，角速度为矢量部分的四元数；为q的导数，s3为三维空间中的单位球体、r3表示三维空间。

77、作为优选的，步骤s6)中，为了提高打磨的质量，在曲率大和粗糙度高的地方减慢打磨速率，通过控制dmp1和dmp2中的时间尺度因子τ对打磨速度进行控制，具体如下：

78、

79、

80、

81、v＝vt-vt-1；

82、其中，τ(t)适用于dmp1和dmp2、dmp3，为设置的曲率阈值，τmax为时间系数的最大值，f是打磨前进方向上的打磨力，f是设置参考摩擦力，k(t)是dmp3的输出，u＝(fx,fy,fz)是力传感器在三个方向的力组成的向量，vt、vt-1分别表示dmp1在当前时刻t和上一个时刻t-1的输出；v为dmp1在当前时刻t和上一个时刻t-1的输出之差，通过上述方式，可以在曲率大和粗糙度高的地方延长打磨的时间，提升打磨的质量。

83、本发明的有益效果为：

84、1、本发明能够实现集成了轨迹分割，轨迹建模，打磨速度自适应调整；利用点云和dmp规划打磨路径，大大减少了在复杂曲面打磨时人的工作量，提高了打磨效率与打磨质量；

85、2、本发明根据曲率将打磨轨迹进行切割，通过分段建模，从而提高了精确度，用位置和方向dmp对打磨位置和姿态进行建模，使位置和姿态同步控制，根据曲率、给进方向摩擦力对打磨速度进行控制，提升了打磨质量。