一种移动机械臂轨迹规划方法、系统和设备与流程

本发明涉及智能建造用机械臂施工路径规划。

背景技术：

1、如今，数字化建造作为国家工业智能化浪潮中的一个重要组成部分，正在进入加速发展阶段中。数字化建造技术中最重要的一个部分，是聚焦于利用数字化工业产线描述传统的建造系统，通过数字化智能装配技术，实施大尺度下的智能建筑建造，从而将数字化加工与现场总装配联系起来，实现高效率分步骤的数字化智能建造场景。

2、当前的数字化建造场景需要像搭积木一样将各个钢木结构匹配组装，而每一块“积木”的加工制造需要克服一系列的难点：制造尺度规模大，要求精度高，生产线装配成本高等。为解决上述问题，布局智能化建造工厂，可以借由机械臂实施精准定点加工，并通过实时在线规划加工路径，可以在短时间内低成本架设加工产线，为数字化建筑输送零件。

3、在智能化建造工厂中，有些柔性加工场景需要使机械臂能够在较大的场景范围中完成加工任务，而当前市面上的大臂展机械臂往往需要配合外部轴才能完成大范围加工任务，而外部轴的部署与移动都需要较为庞大的时间与人力成本。另外外部轴也无法实现机械臂纵向范围的大幅度伸展操作。本技术预期希望通过移动底盘搭载机械臂的方法来实现机械臂大范围多角度的持续加工建造。

4、智能化建造场景中预期可以通过多机调度的方法，将每一个搭载移动底盘的机械臂作为一个加工个体，从而可以对全局统筹优化，高效率的实现多智能体的高度协调配合加工。机械臂的末端云台模拟同时可以满足诸多加工工艺的外部轴需求。

技术实现思路

1、本发明的目的就是提供一种移动机械臂轨迹规划方法，使机械臂在施工现场给定场景下无碰撞追踪工件。

2、为达上述目的，本发明采取的技术方案如下：

3、一种移动机械臂轨迹规划方法，包括如下步骤：

4、s3，对包含三维位置信息的靶点位置，通过姿态散点算法获取一组合法的机械臂末端姿态值，再结合机械臂当前的位置与姿态信息，生成一个完整的目标位姿；

5、s4，对s3得到的目标位姿，通过避障路径规划算法得到一条无碰撞的路径，再对该路径进行最优时序插值，生成一条带时间戳的运动轨迹；

6、s5，当机械臂在执行s4得到的运动轨迹时，若伺服信号位置发生变化，则需要通过轨迹修正拼接算法生成一条新的指向当前定位位置的运动轨迹，并覆盖之前轨迹；

7、s6，在机械臂运动过程中，若目标靶点没有超过预设阈值的偏移，则通过轨迹滤波算法对相邻帧路径进行比较滤波，防止机械臂异常规划造成的转移。

8、进一步，s3所述的靶点位置，是通过机械臂末端的视觉传感器得到靶点的定位信息和位置信息，然后与动作指令结合得到包含三维位置信息的靶点位置。更进一步，所述的定位信息通过机械臂末端的深度相机捕捉待加工工件上的二维码阵列得到，所述的位置信息通过机械臂末端的点激光采集数据得到。

9、机械臂在保持末端针尖持续追踪靶点位置时，使机械臂整体发生较大幅度的姿态变化，即实现旋转追踪的效果。为达到该运动效果，让机械臂在位置插值追踪的同时，生成新的符合工作范围与逆运动学要求的轴空间解范围的末端姿态。当机械臂处于静止时，可以判断当前位置处于无效姿态，生成新姿态进行追踪。

10、s3所述的姿态散点算法是指，在尚未运动到目标姿态时，可以将目标姿态按照数量插值给待追踪靶点的新轨迹所包含的一组路点；姿态达到目标姿态后(或是进入范围后)，无论是否完成当前轨迹对目标靶点的追踪，散点生成一个新的姿态，并插值后赋予当前执行轨迹路点。

11、本技术的技术目标需要考虑机械臂末端针尖时刻追踪靶点的同时又不会碰触到目标工件造成碰撞损伤。

12、s4所述的避障路径规划算法是指，将障碍物与工具头在笛卡尔空间以几何立方体的方式建立障碍物几何体模型，对几何体进行球体分割膨胀后，以欧氏距离作为碰撞检测依据；然后利用rrt-connect双向连通树路径规划算法进行se(3)障碍物空间的路径搜索，生成以当前姿态为初始点，以目标靶点逆解所得轴空间姿态为终点的一条无碰撞的运动路径。

13、生成一条无碰撞的路径后，根据机械臂本体电机特性与动力学特性，得到机械臂轴关节的速度与加速度上限，对上述得到的路径进行最优时序插值，生成一条带时间戳的运动轨迹，从而使机械臂能够在最短时间内避开障碍物保证无碰撞追踪目标靶点。

14、当机械臂处于运动中时，如果伺服信号位置发生变化，机械臂需要生成一条新的指向当前定位位置的运动轨迹，并覆盖之前轨迹。在两条轨迹之间也要保证能够顺滑过度，其中轨迹修正需要相应算法完成。

15、s5所述的轨迹修正拼接算法，可选择的一种如下：

16、s501，如当前指向靶点t1的初始轨迹t1共包含100个路点：a0到a99，此时机械臂正以v10速度运动到a10点，视觉定位发生变化，机械臂根据视觉伺服算法流程计算并得到了一条以a10为起点的指向新的靶点t2的轨迹t2，此时在计算过程中机械臂依然沿t1轨迹运动到了点a15；

17、s502，机械臂获取当前a15点关节信息，根据正运动学计算机械臂末端工具头追踪点位置，与a10位置以及新靶点位置t2相比较，接下来轨迹拼接顺滑有以下两种方法可以完成：

18、s503-1，如果a15更靠近t2位置(欧氏距离)，则直接将新轨迹t2中与distance(a15,t2)相较更远的轨迹点删去，并将a15点作为轨迹起始点，根据样条函数结合关节控制器速度加速度上限完成插值，重新排布轨迹时间戳，完成轨迹顺滑过度。

19、s503-2，如果a10更靠近t2位置，则在新轨迹t2中添加一段旧轨迹t1中包含的由a15回到a10的插值路点，将轨迹修正回退到a10后再延续新轨迹t2运行，其中路径时间戳的排布同样根据样条函数结合关节控制器速度加速度上限完成插值，从而完成轨迹顺滑过度。

20、s6所述的相邻帧路径进行比较滤波是指，

21、s601，将连续以相同目标点规划出的机械臂修正轨迹进行笛卡尔空间的曲线拟合后，比较其重复度，

22、a.如果重复度低于设定的阈值，理解为是一条新的轨迹，执行修正；

23、b.如果重复度高于设定的阈值，理解为机械臂在这一通讯周期中进行了重复轨迹规划；

24、s602，在每一次执行新轨迹前，与前一帧轨迹分别计算正运动学，获取笛卡尔空间轨迹结果；

25、s603，将单个轨迹进行以x为横坐标的多项式插值，分别插值y、z、roll、pitch、yaw；

26、s604，将插值结果进行比较，计算两端轨迹的差异值，根据阈值判断是否采取新轨迹：若超出阈值，使用新轨迹；若未超出阈值，舍弃本帧轨迹。

27、一种移动机械臂轨迹规划系统，包括：

28、目标位姿生成模块300，用于：对包含三维位置信息的靶点位置，通过姿态散点算法获取一组合法的机械臂末端姿态值，再结合机械臂当前位置与姿态信息，生成一个完整的目标位姿；

29、路径规划模块400，用于：对模块300的目标位姿，通过避障路径规划算法得到一条无碰撞的路径，再对该路径进行最优时序插值，生成一条带时间戳的运动轨迹；

30、轨迹修正模块500，用于：当机械臂在执行模块400得到的运动轨迹时，若伺服信号位置发生变化，则通过轨迹修正拼接算法生成一条新的指向当前定位位置的运动轨迹，并覆盖之前轨迹；

31、滤波处理模块600，用于：在机械臂运动过程中，若目标靶点没有超过预设阈值的偏移，则通过轨迹滤波算法对相邻帧路径进行比较滤波，防止机械臂异常规划造成的转移。

32、本专利同时提供一种获取靶点位置功能模块200和采集信息模块100。所述的采集信息模块100，用于：通过机械臂末端的深度相机捕捉待加工工件上的二维码阵列得到定位信息，通过机械臂末端的点激光采集数据得到机械臂末端距离待加工工件的位置信息。所述的获取靶点位置模块200，用于：对模块100得到的定位信息和位置信息与动作指令结合，得到一个包含三维位置信息的靶点位置。

33、目标位姿生成模块300，所述的姿态散点算法是指，在尚未运动到目标姿态时，可以将目标姿态按照数量插值给待追踪靶点的新轨迹所包含的一组路点；姿态达到目标姿态后(或是进入范围后)，无论是否完成当前轨迹对目标靶点的追踪，散点生成一个新的姿态，并插值后赋予当前执行轨迹路点。

34、路径规划模块400所述的避障路径规划算法是指，将障碍物与工具头在笛卡尔空间以几何立方体的方式建立障碍物几何体模型，对几何体进行球体分割膨胀后，以欧氏距离作为碰撞检测依据；然后利用rrt-connect双向连通树路径规划算法进行se(3)障碍物空间的路径搜索，生成以当前姿态为初始点，以目标靶点逆解所得轴空间姿态为终点的一条无碰撞的运动路径。

35、轨迹修正模块500所述的轨迹修正拼接算法，可选择的一种如下：

36、u501，如当前指向靶点t1的初始轨迹t1共包含100个路点：a0到a99，此时机械臂正以v10速度运动到a10点，视觉定位发生变化，机械臂根据视觉伺服算法流程计算并得到了一条以a10为起点的指向新的靶点t2的轨迹t2，此时在计算过程中机械臂依然沿t1轨迹运动到了点a15；

37、u502，机械臂获取当前a15点关节信息，根据正运动学计算机械臂末端工具头追踪点位置，与a10位置以及新靶点位置t2相比较，接下来轨迹拼接顺滑有以下两种方法可以完成：

38、u503-1，如果a15更靠近t2位置(欧氏距离)，则直接将新轨迹t2中与distance(a15,t2)相较更远的轨迹点删去，并将a15点作为轨迹起始点，根据样条函数结合关节控制器速度加速度上限完成插值，重新排布轨迹时间戳，完成轨迹顺滑过度。

39、u503-2，如果a10更靠近t2位置，则在新轨迹t2中添加一段旧轨迹t1中包含的由a15回到a10的插值路点，将轨迹修正回退到a10后再延续新轨迹t2运行，其中路径时间戳的排布同样根据样条函数结合关节控制器速度加速度上限完成插值，从而完成轨迹顺滑过度。

40、滤波处理模块600所述的相邻帧路径进行比较滤波是指，

41、u601，将连续以相同目标点规划出的机械臂修正轨迹进行笛卡尔空间的曲线拟合后，比较其重复度，

42、a.如果重复度低于设定的阈值，理解为是一条新的轨迹，执行修正；

43、b.如果重复度高于设定的阈值，理解为机械臂在这一通讯周期中进行了重复轨迹规划；

44、u602，在每一次执行新轨迹前，与前一帧轨迹分别计算正运动学，获取笛卡尔空间轨迹结果；

45、u603，将单个轨迹进行以x为横坐标的多项式插值，分别插值y、z、roll、pitch、yaw；

46、u604，将插值结果进行比较，计算两端轨迹的差异值，根据阈值判断是否采取新轨迹：若超出阈值，使用新轨迹；若未超出阈值，舍弃本帧轨迹。

47、一种移动机械臂轨迹规划的装置或终端，包括一个或多个处理器、存储装置；存储装置，用于存储一个或多个程序；当所述的一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器实现本专利提供的任一一种移动机械臂轨迹规划方法。

48、一种计算机可读存储介质，其存储有计算机程序，该程序被处理器执行时实现本专利提供的任一一种移动机械臂轨迹规划方法。

49、本发明的有益效果：

50、本发明提供的移动机械臂轨迹规划方法，在机械臂获取到一个包含三维位置信息的靶点位置后，在允许范围内利用姿态散点算法，获取一组合法的机械臂末端姿态值，再结合之前获取的位置与姿态信息，生成一个完整的目标位姿。之后通过避障路径规划算法得到一条无碰撞的路径，再对该路径进行最优时序插值，生成一条带时间戳的运动轨迹，从而使机械臂能够在最短时间内避开障碍物保证无碰撞追踪目标靶点。

51、在机械臂运动过程中，若目标靶点没有超过预设阈值的偏移，则通过轨迹滤波与拼接算法对相邻帧路径进行比较滤波，完成轨迹的顺滑过渡，从而实现机械臂轨迹的快速修正拼接与覆盖。

52、下面结合附图对本发明的具体实施方式举例说明：