一种新型的自主装配机器人路径规划自主导航系统及方法
技术领域
本发明涉及机器人技术领域，具体为一种新型的自主装配机器人路径规划自主导航系统及方法。
背景技术
机器人路径规划一直是制约机器人应用的难题，其路径规划的水平一定程度上决定了机器人的效率，寻找一条没有碰撞的最佳路径，同时要满足相应参数的优化，例如，最短路径，最短时间，最低能耗等，传统的人工势场起初是为了解决机器人在抓取物体的时候，其手臂可以不触及工作台，后来运用到机器人运动避障中，其主要思想是将机器人、目标点和障碍物简化成点，目标点对机器人产生引力，障碍物对机器人产生斥力，在整个环境中，通过合力的作用控制机器人运动，进行路径规划。
人工势场法是传统路径规划方法中较为成熟且简洁高效的算法，其主要思路是通过目标点对机器人产生引力，障碍物对机器人产生斥力，以及机器人之间相互作用力，形成一个智能的人工势场，对机器人进行实时的路径进行规划。
根据专利号为CN105867365B所述的基于改进人工势场法的路径规划导航系统及方法，但传统的人工势场存在局部极小点的问题，当障碍物在目标点附近时，还会出现机器人在目标点周围震荡，导致机器人无法到目标点的问题，在使用时仍然存在大量误差。
发明内容
(一)解决的技术问题
针对现有技术的不足，本发明提供了一种新型的自主装配机器人路径规划自主导航系统及方法，解决了当障碍物在目标点附近时，还会出现机器人在目标点周围震荡，导致机器人无法到目标点的问题。
(二)技术方案
为实现以上目的，本发明通过以下技术方案予以实现：一种新型的自主装配机器人路径规划自主导航系统，包括装配机器人模型建立单元、摄像单元、障碍物模型建立单元、点云模型建立单元、关节角获取单元、单片机、三维坐标系建立单元、计算单元、判断单元、移动路径检测单元、人工调整单元和路径导出单元，所述单片机分别与装配机器人模型建立单元、摄像单元、障碍物模型建立单元、点云模型建立单元、关节角获取单元、三维坐标系建立单元、计算单元、判断单元和移动路径检测单元实现双向连接，所述装配机器人模型建立单元的输出端与障碍物模型建立单元的输入端连接，所述障碍物模型建立单元的输出端与关节角获取单元的输入端连接，所述关节角获取单元的输出端与计算单元的输入端连接，所述计算单元的输出端与判断单元的输入端连接，所述判断单元的输出端与路径导出单元的输入端连接，所述人工调整单元的输出端与路径导出单元的输入端连接。
优选的，所述摄像单元的输出端与点云模型建立单元的输入端连接，所述点云模型建立单元的输出端与三维坐标系建立单元的输入端连接，所述三维坐标系建立单元的输出端与移动路径检测单元的输入端连接，所述移动路径检测单元的输出端与判断单元的输入端连接。
优选的，所述摄像单元包括第一摄像模块、第二摄像模块、位置标定模块和第三摄像模块，所述第一摄像模块、第二摄像模块和第三摄像模块的输出端均与位置标定模块的输入端连接。
优选的，所述计算单元包括固定控制点坐标获取模块、障碍点距离计算模块、末端执行器位姿获取模块、目标点距离计算模块、势能导入模块和碰撞虚拟检测模块。
优选的，所述固定控制点坐标获取模块和末端执行器位姿获取模块的输出端与障碍点距离计算模块的输入端连接，所述障碍点距离计算模块的输出端与目标点距离计算模块的输入端连接，所述目标点距离计算模块的输出端与势能导入模块的输入端连接，所述势能导入模块的输出端与碰撞虚拟检测模块的输入端连接。
优选的，所述三维坐标系建立单元包括基准点选择模块、位姿关系确认模块和三维坐标系生成模块，所述基准点选择模块的输出端与位姿关系确认模块的输入端连接，所述位姿关系确认模块的输出端与三维坐标系生成模块的输入端连接。
优选的，所述判断单元包括势能最小点判断模块、梯度下降填充模块和路径重新规划单元，所述势能最小点判断模块的输出端与梯度下降填充模块的输入端连接，所述梯度下降填充模块的输出端与路径重新规划单元的输入端连接。
本发明还公开了一种新型的自主装配机器人路径规划自主导航系统的使用方法，包括以下步骤：
步骤一、基本模型建立：分别通过装配机器人模型建立单元和障碍物模型建立单元分别建立装配机器人模型及障碍物数学模型，然后设置工作空间起始点、目标点后通过关节角获取单元分别获取当前的关节角，通过计算单元内部的固定控制点坐标获取模块和末端执行器位姿获取模块由运动学正解得到关节角对应的固定控制点位置坐标以及末端执行器的位姿，然后通过障碍点距离计算模块和目标点距离计算模块分别计算固定控制点与障碍物和目标点距离，并计算连杆上浮动控制点的坐标和与障碍物距离，然后通过势能导入模块计算引力和斥力势能，记录合势能最小的机器人末端位姿，通过计算单元计算移动路径，势场计算公式为：
Ftotal q Fatt,i q Frep,i q；
步骤二、三维坐标系模型的建立：通过摄像单元内的第一摄像模块、第二摄像模块和第三摄像模块分别进行成像，通过位置标定模块对机器人的位置进行标定，然后通过点云模型建立单元建立点云模型，通过三维坐标系建立单元建立出三维坐标系，然后通过移动路径检测单元测算移动路径；
步骤三、碰撞判断：通过判断单元内部的势能最小点判断模块判断势能最小时机器人末端位姿是不是工作空间目标点，若是，则避障规划结束，否则通过梯度下降填充模块让机器人向降低势能的方向运动，通过路径重新规划单元规划移动路径。
(三)有益效果
本发明提供了一种新型的自主装配机器人路径规划自主导航系统及方法。与现有技术相比，具备以下有益效果：
(1)、该新型的自主装配机器人路径规划自主导航系统及方法，通过在单片机分别与装配机器人模型建立单元、摄像单元、障碍物模型建立单元、点云模型建立单元、关节角获取单元、三维坐标系建立单元、计算单元、判断单元和移动路径检测单元实现双向连接，装配机器人模型建立单元的输出端与障碍物模型建立单元的输入端连接，障碍物模型建立单元的输出端与关节角获取单元的输入端连接，关节角获取单元的输出端与计算单元的输入端连接，计算单元的输出端与判断单元的输入端连接，判断单元的输出端与路径导出单元的输入端连接，人工调整单元的输出端与路径导出单元的输入端连接，摄像单元的输出端与点云模型建立单元的输入端连接，点云模型建立单元的输出端与三维坐标系建立单元的输入端连接，三维坐标系建立单元的输出端与移动路径检测单元的输入端连接，移动路径检测单元的输出端与判断单元的输入端连，在使用时，通过引用人工势场和建立三维坐标系模型的方式，进行路径规划，能适应装配工作环境的变化，可以有效的避开障碍物，实现了机器人在向目标点运动过程中的避障。
(2)、该新型的自主装配机器人路径规划自主导航系统及方法，通过在摄像单元包括第一摄像模块、第二摄像模块、位置标定模块和第三摄像模块，第一摄像模块、第二摄像模块和第三摄像模块的输出端均与位置标定模块的输入端连接，摄像单元内的第一摄像模块、第二摄像模块和第三摄像模块分别进行成像，通过位置标定模块对机器人的位置进行标定，多个摄像头对基准点进行精准的定位，建立出来的坐标系更加精准。
(3)、该新型的自主装配机器人路径规划自主导航系统及方法，通过在固定控制点坐标获取模块和末端执行器位姿获取模块的输出端与障碍点距离计算模块的输入端连接，障碍点距离计算模块的输出端与目标点距离计算模块的输入端连接，目标点距离计算模块的输出端与势能导入模块的输入端连接，势能导入模块的输出端与碰撞虚拟检测模块的输入端连接，梯度下降算法可以在装配机器人初始位形到最终目标位形进行优化，让机器人向降低势能的方向运动。
附图说明
图1为本发明系统的原理框图；
图2为本发明摄像单元的原理框图；
图3为本发明计算单元的原理框图；
图4为本发明三维坐标系建立单元的原理框图；
图5为本发明判断单元的原理框图。
图中，1-装配机器人模型建立单元、2-摄像单元、3-障碍物模型建立单元、4-点云模型建立单元、5-关节角获取单元、6-单片机、7-三维坐标系建立单元、8-计算单元、9-判断单元、10-移动路径检测单元、11-人工调整单元、12-路径导出单元、21-第一摄像模块、22-第二摄像模块、23-位置标定模块、24-第三摄像模块、71-基准点选择模块、72-位姿关系确认模块、73-三维坐标系生成模块、81-固定控制点坐标获取模块、82-碍点距离计算模块、83-末端执行器位姿获取模块、84-目标点距离计算模块、85-势能导入模块、86-碰撞虚拟检测模块、91-势能最小点判断模块、92-梯度下降填充模块、93-路径重新规划单元。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
请参阅图1-5，本发明实施例提供一种技术方案：一种新型的自主装配机器人路径规划自主导航系统，包括装配机器人模型建立单元1、摄像单元2、障碍物模型建立单元3、点云模型建立单元4、关节角获取单元5、单片机6、三维坐标系建立单元7、计算单元8、判断单元9、移动路径检测单元10、人工调整单元11和路径导出单元12，单片机6分别与装配机器人模型建立单元1、摄像单元2、障碍物模型建立单元3、点云模型建立单元4、关节角获取单元5、三维坐标系建立单元7、计算单元8、判断单元9和移动路径检测单元10实现双向连接，装配机器人模型建立单元1的输出端与障碍物模型建立单元3的输入端连接，障碍物模型建立单元3的输出端与关节角获取单元5的输入端连接，关节角获取单元5的输出端与计算单元8的输入端连接，计算单元8的输出端与判断单元9的输入端连接，判断单元9的输出端与路径导出单元12的输入端连接，人工调整单元11的输出端与路径导出单元12的输入端连接，摄像单元2的输出端与点云模型建立单元4的输入端连接，点云模型建立单元4的输出端与三维坐标系建立单元7的输入端连接，三维坐标系建立单元7的输出端与移动路径检测单元10的输入端连接，移动路径检测单元10的输出端与判断单元9的输入端连接，摄像单元2包括第一摄像模块21、第二摄像模块22、位置标定模块23和第三摄像模块24，第一摄像模块21、第二摄像模块22和第三摄像模块24的输出端均与位置标定模块23的输入端连接，摄像单元2内的第一摄像模块21、第二摄像模块22和第三摄像模块24分别进行成像，通过位置标定模块23对机器人的位置进行标定，多个摄像头对基准点进行精准的定位，建立出来的坐标系更加精准，计算单元8包括固定控制点坐标获取模块81、障碍点距离计算模块82、末端执行器位姿获取模块83、目标点距离计算模块84、势能导入模块85和碰撞虚拟检测模块86，固定控制点坐标获取模块81和末端执行器位姿获取模块83的输出端与障碍点距离计算模块82的输入端连接，障碍点距离计算模块82的输出端与目标点距离计算模块84的输入端连接，目标点距离计算模块84的输出端与势能导入模块85的输入端连接，势能导入模块85的输出端与碰撞虚拟检测模块86的输入端连接，三维坐标系建立单元7包括基准点选择模块71、位姿关系确认模块72和三维坐标系生成模块73，基准点选择模块71的输出端与位姿关系确认模块72的输入端连接，位姿关系确认模块72的输出端与三维坐标系生成模块73的输入端连接，判断单元9包括势能最小点判断模块91、梯度下降填充模块92和路径重新规划单元93，势能最小点判断模块91的输出端与梯度下降填充模块92的输入端连接，梯度下降填充模块92的输出端与路径重新规划单元93的输入端连接，在使用时，通过引用人工势场和建立三维坐标系模型的方式，进行路径规划，能适应装配工作环境的变化，可以有效的避开障碍物，实现了机器人在向目标点运动过程中的避障。
本发明还公开了一种新型的自主装配机器人路径规划自主导航系统的使用方法，包括以下步骤：
步骤一、基本模型建立：分别通过装配机器人模型建立单元1和障碍物模型建立单元3分别建立装配机器人模型及障碍物数学模型，然后设置工作空间起始点、目标点后通过关节角获取单元5分别获取当前的关节角，通过计算单元8内部的固定控制点坐标获取模块81和末端执行器位姿获取模块83由运动学正解得到关节角对应的固定控制点位置坐标以及末端执行器的位姿，然后通过障碍点距离计算模块82和目标点距离计算模块84分别计算固定控制点与障碍物和目标点距离，并计算连杆上浮动控制点的坐标和与障碍物距离，然后通过势能导入模块85计算引力和斥力势能，记录合势能最小的机器人末端位姿，通过计算单元8计算移动路径，势场计算公式为：
Ftotal q Fatt,i q Frep,i q；
步骤二、三维坐标系模型的建立：通过摄像单元2内的第一摄像模块21、第二摄像模块22和第三摄像模块24分别进行成像，通过位置标定模块23对机器人的位置进行标定，然后通过点云模型建立单元4建立点云模型，通过三维坐标系建立单元7建立出三维坐标系，然后通过移动路径检测单元10测算移动路径；
步骤三、碰撞判断：通过判断单元9内部的势能最小点判断模块91判断势能最小时机器人末端位姿是不是工作空间目标点，若是，则避障规划结束，否则通过梯度下降填充模块92让机器人向降低势能的方向运动，通过路径重新规划单元93规划移动路径，梯度下降算法可以在装配机器人初始位形到最终目标位形进行优化，让机器人向降低势能的方向运动。
同时本说明书中未作详细描述的内容均属于本领域技术人员公知的现有技术。
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。
尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。
