一种用于自主喷涂的机械臂末端轨迹规划方法

本发明涉及机械臂末端轨迹规划领域，尤其涉及一种用于自主喷涂的机械臂末端轨迹规划方法。

背景技术：

1、现有的机械臂喷涂轨迹规划方法主要分为主动式规划和被动式跟踪两种，其中，主动式规划方法大多基于预先绘制的精确cad模型，不能针对非标准化目标做出自适应调整，故通用性较差，且针对复杂的异构目标，需要多台机械臂协作完成任务，消耗成本大，缺乏泛用性；被动式跟踪方法步骤如下：首先由人工引导机械臂末端实现喷涂轨迹，而后控制机械臂重复此轨迹以完成任务，但是该方法的喷涂质量很大程度上取决于人工的经验，存在品控难度大、喷涂精度低，且恶劣的工作环境有害于员工的身体健康等问题。

2、为解决上述问题，现有技术进一步研发了基于测量点云模型的机械臂喷涂轨迹规划方法，其借助实际测量得到的点云模型，可实现针对非标物体的自适应调整，大大扩展了喷涂目标范围。但其仍然存在以下不足：

3、(1)路径特征点采样不合理，不均匀或不具有代表性的特征点会大幅度降低喷涂质量；

4、(2)喷涂轨迹高度依赖物体表面点，未考虑机械臂末端的姿态问题，易导致机械臂的关节与目标碰撞；

5、(3)喷涂轨迹转弯处缺乏平滑处理，机械臂关节易发生角度跳变，进一步产生安全隐患。

技术实现思路

1、为解决上述问题，本发明提供一种用于自主喷涂的机械臂末端轨迹规划方法，可生成平滑且连续的机械臂末端轨迹，提高了实际喷涂过程中机械臂关节旋转的稳定性、灵巧性和笛卡尔刚度。

2、为实现上述目的，本发明提供了一种用于自主喷涂的机械臂末端轨迹规划方法，包括以下步骤：

3、s1、在目标模型点云的基础上，构建物体点云的aabb包围盒，以确定栅格起点；

4、s2、以步骤s1确定的栅格起点为基准，采样获得三维体素栅格，并基于三维体素栅格的重心确定点云中路径特征点；

5、s3、利用主成分分析法计算步骤s2确定的点云中路径特征点的法向量；

6、s4、基于步骤s3计算的点云中路径特征点的法向量，构建双向非均匀b样条曲线，并进行平滑处理；

7、s5、基于步骤s4构建的双向非均匀b样条曲线，生成机械臂末端轨迹。

8、优选的，步骤s2具体包括以下步骤：

9、s21、设定栅格边长为cell，将点云划分栅格个数nvoxel为：

10、

11、式中，xmin、ymin、zmin分别为点云在x、y、z方向上的最小极值；xmax、ymax、zmax分别为点云在x、y、z方向上的最大极值；

12、s22、假设每个三维体素栅格内包含点p的个数为h，且设定三维体素栅格内每一点的权重相同，计算三维体素栅格的重心：

13、

14、式中，pe为三维体素栅格的重心；pi为三维体素栅格内第i个点；

15、s23、计算三维体素栅格内的点云中路径特征点：

16、

17、式中，pf为三维体素栅格内的点云中路径特征点。

18、优选的，步骤s3具体包括以下步骤：

19、s31、假设三维体素栅格内的点云中路径特征点pf周围kf个邻近点的坐标分别为pj＝(xj,yj,zj)t,j＝1,2,…,kf，构建去中心化点集：

20、δpj＝pj-pf,j＝1,2,…,kf (4)

21、式中，δpj表示去中心化点集；

22、s32、计算协方差矩阵：

23、

24、式中，c表示协方差矩阵；

25、s33、对协方差矩阵c进行特征值分解，得到特征值λ1＞λ2＞λ3及其对应的单位特征向量v1,v2,v3，满足以下条件：

26、cvl＝λlvl,l＝1,2,3 (6)

27、s34、计算点云中路径特征点pf的法向量：

28、

29、式中，n为点云中路径特征点pf的法向量；

30、优选的，步骤s4具体包括以下步骤：

31、s41、假设点云中控制点ps＝{ps1,ps2,…,psn}，则b样条曲线表达式如下：

32、

33、其中

34、

35、式中，c(u)为b样条曲线表达函数；n为控制点数；ni,1(u)为第i个1阶基函数；u为节点向量；ui为第i个节点；ui+1为第i+1个节点；ui+k-1为第i+k-1个节点；ui+k为第i+k个节点；ni,k-1(u)为第i个k-1阶基函数；ni+1,k-1(u)为第i+1个k-1阶基函数；ni,k(u)为第i个k阶基函数；k为b样条曲线阶数；

36、s42、计算节点向量：

37、u＝{u0,u1,…,um}, u0≤u1≤u2≤…≤um (10)

38、式中，u0,u1,…,um分别为第1,2,……,m个节点；

39、s43、将式(8)映射至高维空间，计算齐次坐标得到双向非均匀b样条曲线：

40、

41、式中，cps(u)为控制点ps的双向非均匀b样条曲线；ωi为控制点权重值，k为双向非均匀b样条曲线的阶数；

42、s44、基于点云中控制点ps＝{ps1,ps2,…,psn}的法向量n＝{n1,n2,…,nn}的正方向偏置距离b，得到控制点qs＝{qs1,qs2,…,qsn}，其中qsi＝psi+b，且偏置距离b被定义为末端执行器超出机械臂末端法兰的距离，对qs进行双向非均匀b样条曲线拟合，得到：

43、

44、式中，cqs(u)为控制点qs的双向非均匀b样条曲线表达函数；

45、s45、同步设置cqs(u)和cps(u)的参数u。

46、优选的，在步骤s43和步骤s44中双向非均匀b样条曲线的阶数k≥3。

47、优选的，步骤s5具体包括以下步骤：

48、s51、对于cps(u)中的每个路径点，在cqs(u)中搜索对应的路径点，并以cqs(u)作为机械臂末端平移向量，以cqs(u)指向cps(u)的方向向量作为末端姿态；

49、s52、根据四元数的旋转轴与轴角定义，将cqs(u)指向cps(u)的方向向量映射为四元数，并将四元数扩展至旋转矩阵；

50、s53、基于旋转矩阵生成机械臂末端轨迹。

51、优选的，步骤s52具体包括以下步骤：

52、s521、设法向量为a＝(x,y,z)，定义单位向量i＝(1,0,0)，且对法向量a与单位向量i归一化后，由向量点积公式计算法向量与单位向量之间的旋转角度θ：

53、

54、s522、由向量叉积公式计算法向量与单位向量之间的旋转轴n：

55、n＝(nx,ny,nz)＝a×i＝(0,-z,y) (14)

56、式中，nx,ny,nz分别为向量n在三个方向上的分量；

57、s523、由四元数定义得到：

58、

59、式中，q表示四元数；

60、将四元数扩展至旋转矩阵er(u)：

61、

62、式中，(q1,q2,q3,q4)分别为四元数q的四个分量。

63、优选的，在步骤s53中，生成的机械臂末端轨迹表达式为：

64、e(u)＝et(u)+er(u) (17)

65、式中，e(u)为机械臂末端姿态矩阵；et(u)为平移向量，且

66、本发明具有以下有益效果：

67、(1)通过引入双向非均匀b样条曲线，能生成一条覆盖率高、具有平滑性和连续性的喷涂轨迹；

68、(2)在点云的基础上，以确定栅格起点，采用基于重心的体素栅格采样路径特征点，使特征点的分布更加均匀合理，保证了栅格在点云中均匀生成，进而确保清洁轨迹的均匀性；

69、(3)使用pca(主成分分析方法)计算其法向量，将其映射至双向非均匀b样条曲线，进一步基于曲线点对计算末端姿态，以实现机械臂末端始终垂直于物体表面；

70、(4)引入双向非均匀b样条曲线，在双向非均匀b样条曲线的基础上，以逐一对应的路径点对计算得到机械臂的末端姿态，不仅确定了轨迹中机械臂末端的位置信息，还规划了机械臂末端的姿态信息，保证了轨迹的连续性和平滑性，以实现更好的贴合目标表面；

71、(5)拓展了非均匀b样条曲线的应用场景，首先根据路径特征点构建一条非均匀b样条曲线，其次再以法向量方向偏置距离点，构建第二条曲线，使特征点的法向量有了更具象化的表示。

72、综上，本发明能够基于目标物体的点云，根据栅格重心合理采样路径特征点，借助pca计算特征点的法向量，接着引入非均匀b样条曲线，构建基于特征点及其法向量的双向非均匀b样条曲线，以路径点对确定末端姿态，最终生成机械臂末端轨迹，能生成平滑且连续的机械臂末端轨迹，提高了实际喷涂过程中机械臂关节旋转的稳定性、灵巧性和笛卡尔刚度。

73、下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。