CN114986501A - 一种机械臂路径规划方法、系统及机械臂 - Google Patents
一种机械臂路径规划方法、系统及机械臂 Download PDFInfo
- Publication number
- CN114986501A CN114986501A CN202210604675.1A CN202210604675A CN114986501A CN 114986501 A CN114986501 A CN 114986501A CN 202210604675 A CN202210604675 A CN 202210604675A CN 114986501 A CN114986501 A CN 114986501A
- Authority
- CN
- China
- Prior art keywords
- node
- path
- collision
- point
- planning
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 76
- 238000010845 search algorithm Methods 0.000 claims abstract description 15
- 239000012636 effector Substances 0.000 claims abstract description 10
- 230000008569 process Effects 0.000 claims description 34
- 238000013138 pruning Methods 0.000 claims description 31
- 238000001514 detection method Methods 0.000 claims description 17
- 238000005457 optimization Methods 0.000 claims description 11
- 238000012216 screening Methods 0.000 claims description 9
- 230000009191 jumping Effects 0.000 claims description 6
- 230000008901 benefit Effects 0.000 abstract description 5
- 238000010586 diagram Methods 0.000 description 4
- 238000004891 communication Methods 0.000 description 3
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 230000006870 function Effects 0.000 description 3
- 238000009499 grossing Methods 0.000 description 3
- 238000004088 simulation Methods 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 2
- 238000007796 conventional method Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000001914 filtration Methods 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000003860 storage Methods 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- 230000002411 adverse Effects 0.000 description 1
- 230000004888 barrier function Effects 0.000 description 1
- 230000006399 behavior Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000004590 computer program Methods 0.000 description 1
- 230000036461 convulsion Effects 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 238000009826 distribution Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000003672 processing method Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000011895 specific detection Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
- Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
- Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- Artificial Intelligence (AREA)
- Evolutionary Computation (AREA)
- Fuzzy Systems (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Manipulator (AREA)
- Numerical Control (AREA)
Abstract
本申请涉及一种机械臂路径规划方法、系统及机械臂,其包括获取预设的栅格化地图中的障碍点位置信息;通过图搜索算法对机械臂的末端执行器规划无碰路径;检测机械臂在以无碰路径运动时是否会发生碰撞或具有非可达点;若有,则将该节点添加入障碍点位置信息中;若无,则将无碰路径进行轨迹规划以生成机械臂的运动轨迹;检测机械臂在以该运动轨迹运动时是否会发生碰撞或具有非可达点;若有,则将该节点添加入障碍点位置信息中;其中,若障碍点位置信息被更新,则重新规划无碰路径。本申请对JPS算法进行了三维场景的理论拓展。另外,根据JPS算法的搜索特点,开发了一套适用于机械臂的完备的规划方案,相比较传统的路径规划算法,规划效率显著提高。
Description
技术领域
本申请涉及自动化机器人的领域,尤其是涉及一种机械臂路径规划方法、系统及机械臂。
背景技术
工业机器人路径规划的主流方法仍局限于人工示教和离线编程。前者需要人工在现场进行示教点位,编写运动程序,其优点在于快速便捷,缺点是繁琐、效率低、精度低且难以找到最优解。后者则通过上层软件和仿真系统进行运动指令的生成,需要对环境进行精确建模。这两种办法需要较高的部署成本。而一些行业由于产品迭代周期较快,对自动化部署以及柔性生产提出了较高的要求,传统的部署方法并不适用于产品快速调整的需求,这导致了这些行业的自动化程度很低,极大限制了行业的生产能力。
针对传统方法存在的不足,相关研究也在尝试解决传统方法带来的不便性。在近年来,例如基于概率的随机采样RRT(Rapidly-exploring Random Trees)算法,迭代优化生成的蚁群算法,基于机器视觉的改进RRT规划算法等智能算法和优化策略被应用于相关研究中,但这些方法由于在完备性、效率等方面存在一定的缺陷,并不能满足工业现场对相关方面的要求。
发明内容
为了可以较为高效的对机械臂的路径进行规划,本申请提供一种机械臂路径规划方法、系统及机械臂。
第一方面,本申请提供的一种机械臂路径规划方法采用如下的技术方案:
一种机械臂路径规划方法,包括:
获取预设的栅格化地图中的障碍点位置信息;
根据所需的起点位置、终点位置以及所获取的障碍点位置信息,通过图搜索算法对机械臂的末端执行器规划无碰路径;
检测机械臂在以无碰路径运动时是否会发生碰撞或具有非可达点;
若机械臂在以无碰路径运动时会发生碰撞或具有非可达点,则将该节点添加入障碍点位置信息中;
若无,则将所规划的无碰路径进行轨迹规划以生成机械臂的运动轨迹;
检测机械臂在以该运动轨迹运动时是否会发生碰撞或具有非可达点;
若机械臂在以该运动轨迹运动时会发生碰撞或具有非可达点,则将该节点添加入障碍点位置信息中;
其中,若障碍点位置信息被更新,则通过图搜索算法重新规划无碰路径。
通过采用上述技术方案,通过图搜索算法的快速规划以及资源占用量较小的优势快速的对末端执行器的无碰路径进行规划,然后再通过对整体的机械臂的其它连杆是否会发生碰撞或可达而进行检测,再不断的在栅格化地图中添加障碍点信息,而通过迭代的方式来快速规划可供机械臂运行的无碰运动轨迹。
优选的,在图搜索算法规划无碰路径的过程中包括:
以预设的寻找顺序并依据跳跃节点寻找规则寻找跳跃节点;
在寻找跳跃节点的过程中通过预设的剪枝规则对邻居节点进行剪枝并标记强制邻居节点;根据所获取的跳跃节点以及强制邻居节点规划无碰路径。
通过采用上述技术方案,通过剪枝规则可以减少在跳跃节点寻找过程中那些不必要的节点拓展,进行了有效筛选,使得对于无碰路径的规划的效率较高。
优选的,预设的剪枝规则包括:
若父节点p(x)移动至当前节点x的方向为直线,则在从父节点p(x)移动至当前节点x的路径中,若len(〈p(x),…,n〉\x)≤len(〈p(x),x,n〉),则节点n为需要剪枝的邻居节点;
若父节点p(x)移动至当前节点x的方向为任一平面内的斜线,则在从父节点p(x)移动至当前节点x的路径中,若len(〈p(x),…,n〉\x)<len(<p(x),x,n>),则节点n为需要剪枝的邻居节点;
其中,从父节点p(x)抵达父节点p(x)非一步可达的格点区域,若从父节点p(x)抵达父节点p(x)需跨越平面,则计算路径长度时必须经过当前节点x;
若父节点p(x)移动至当前节点x的方向为任一三维斜线方向,则在从父节点p(x)移动至当前节点x的路径中,若len(<p(x),…,n>\x)<len(<p(x),x,n>),则该节点n为需要剪枝的邻居节点;
其中,从父节点p(x)抵达父节点p(x)非一步可达的格点区域,若抵达该格点的路径中经过xy,yz,zx平面相交的棱,且这些棱均会穿过父节点p(x),则计算路径长度时必须经过当前节点x;
式中,p(x)为父节点,n为判断的节点,x为当前节点,len(<p(x),…,n>\x)表示从父节点p(x)开始经过各点到节点n但不经过节点x的路径长度,len(<p(x),x,n>)表示从父节点p(x)开始经过节点x到达节点n的路径长度。
优选的,标记强制邻居节点的规则需要满足该节点不是当前节点x所需剪枝的邻居节点,且满足len(<p(x),…,n>\x)<len(<p(x),x,n>)。
节点y为目标节点;或者,
节点y至少有一个邻居节点为强制邻居节点;或者,
在斜线移动方向下,存在一个节点z,该节点z能够从为目标节点的节点y或是至少一个邻居节点为强制邻居节点的节点y沿同一直线方向移动若干步抵达。
优选的,在检测机械臂在以无碰路径运动时是否会发生碰撞或具有非可达点之前,所述方法还包括对生成的无碰路径进行路径优化;
所述路径优化的方法包括:
获取无碰路径上相邻两个节点之间的自由路径节点;
将各相邻自由路径节点之间相互连接以形成扩展无碰路径。
通过采用上述技术方案,受到前述无碰路径规划算法的影响,实际上构成无碰路径的节点数量较少,而较少的节点数量并不能较好的满足构成机械臂运动所需的路径点数量,并且过少的路径点会导致机械臂轨迹规划时的路径点拟合出现较大的偏差,因而通过这种扩展优化方式可以有效地在满足无碰的前提下提高节点的数量,以满足后续所需的要求。
优选的,在形成扩展无碰路径之后,所述方法还包括:
在扩展无碰路径的相邻两个节点之间依次选择三个相邻的自由路径节点进行筛选并迭代,三个相邻的自由路径节点依次定义为p1,p2和p3;
在筛选过程中,如果dist(p1,p3)<dist(p1,p2)+dist(p2,p3),则剔除p2点,并设定当前的p1点为前一次迭代的p3点进行迭代;
如果dist(p1,p3)≥dist(p1,p2)+dist(p2,p3),则设定当前的p1点为前一次迭代的p2点进行迭代;
将剩余的相邻自由路径节点之间相互连接并更新无碰路径;
其中,dist(pi,pj)表示pi点到pj点之间的距离。
通过采用上述技术方案,这种方式可以去掉在扩展无碰路径之间所形成的直角拐角,使得无碰路径变成一个较为平滑的路径。
优选的,在每次障碍点位置信息被更新的过程中,判断无碰路径是否达到最大规划阈值次数,若是,则停止无碰路径的规划。
通过采用上述技术方案,可以有效的避免无限循环而无法结束进程的情况存在。
第二方面,本申请提供的一种机械臂路径规划系统采用如下的技术方案:
一种机械臂路径规划系统,包括:
初始化模块,用于初始化栅格化地图并获取预设的栅格化地图中的障碍点位置信息;
无碰路径规划模块,用于根据所需的起点位置、终点位置以及所获取的障碍点位置信息,通过图搜索算法规划无碰路径;
无碰路径检测模块,用于检测机械臂在以无碰路径运动时是否会发生碰撞或具有非可达点;若机械臂在以无碰路径运动时会发生碰撞或具有非可达点,则将该节点添加入障碍点位置信息中;
若无,则通过机械臂轨迹规划模块以将所规划的无碰路径进行轨迹规划以生成机械臂的运动轨迹;
机械臂轨迹检测模块,用于检测机械臂在以该运动轨迹运动时是否会发生碰撞或具有非可达点;
若机械臂在以该运动轨迹运动时会发生碰撞或具有非可达点,则将该节点添加入障碍点位置信息中;
其中,若障碍点位置信息被更新,则通过图搜索算法重新规划无碰路径。
第三方面,本申请提供的一种机械臂采用如下的技术方案:
一种机械臂,用于执行通过上述方法所规划的运动轨迹。
综上所述,本申请对JPS算法进行了三维场景的理论拓展。另外,根据JPS算法的搜索特点,开发了一套适用于机械臂的完备的规划方案,相比较传统的路径规划的算法,规划效率显著提高。
附图说明
图1是本发明其中一实施例的机械臂路径规划方法的流程示意图。
图2是本发明其中一实施例中父节点移动至当前节点的方向为直线时的剪枝示意图,其中箭头所指方向为父节点至当前节点的移动方向。
图3是本发明其中一实施例的父节点移动至当前节点的方向为任一平面内的斜线时的剪枝示意图,其中箭头所指方向为父节点至当前节点的移动方向。
图4是本发明其中一实施例的父节点移动至当前节点的方向为任一三维斜线方向时的剪枝示意图,其中箭头所指方向为父节点至当前节点的移动方向。
图5是本发明其中一实施例的扩展无碰路径的示意图,其中,左侧为无碰路径,右侧为扩展无碰路径。
图6是对图5中所举例的扩展无碰路径去直角转折点后的示意图,其中,左侧为扩展无碰路径,右侧为去直角转折点后的无碰路径。
具体实施方式
以下结合附图1-6对本申请作进一步详细说明。
本申请实施例公开一种机械臂路径规划方法,其中,本方法中主要基于图搜索的算法实现机械臂的路径规划,并且在对末端执行器规划出一条无碰路径之后,再考虑对整体的机械臂是否会发生碰撞等情况进行判断,尽可能减少由于在笛卡尔空间规划而产生的机械臂的正逆运动学求解以及开销较大的碰撞检测的过程,迭代地进行有效路径搜索,在得到有效路径后再进行轨迹规划以生成机械臂可执行的文件。
为使本发明实施例的目的、技术方案和优点更加清楚,下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
另外,本文中术语“和/或”,仅仅是一种描述关联对象的关联关系,表示可以存在三种关系,例如,A和/或B,可以表示:单独存在A,同时存在A和B,单独存在B这三种情况。另外,本文中字符“/”,如无特殊说明,一般表示前后关联对象是一种“或”的关系。
下面结合说明书附图对本发明实施例作进一步详细描述。
参照图1,本发明实施例提供一种机械臂路径规划方法,所述方法的主要流程描述如下。
步骤S100:初始化栅格化地图。
其中,栅格化地图在本申请实施例中采用Octomap栅格化地图。初始化Octomap栅格化地图有三个前提:Gazebo仿真环境搭建,使用Intel RealSense获取环境点云数据,Octomap地图生成。依据现实工况场景搭建Gazebo仿真环境后,通过ROS与Gazebo进行通信,将URDF描述的机械臂模型以及Intel RealSense模型发布于Gazebo环境中,并建立Gazebo与Rviz通信进行可视化表达,获取实时点云数据,这些点云数据提供于Moveit的PlanningSense服务,将filter处理后的点云用于构建Octomap环境障碍物地图。Octomap地图被保存为.bt格式的本地文件。初始化Octomap地图是通过自编写的地图发布服务节点加载本地的.bt格式地图文件。
对于栅格化地图中的障碍点所在的位置,可以通过深度相机获取的方式来将环境中的障碍物生成点云数据,并通过Octomap对点云数据进行转换,以八叉树的形式记录在栅格化地图的节点中。
其中,Octomap采用了八叉树结构管理点云信息,并可以对观察环境进行动态更新,由于剪枝策略以及八叉树结构的存储优势,一个具备百万量级节点的栅格化地图内存使用仅仅不到1Mb的内存,节点的存储使用无序哈希表也使得节点的访问速度极快。使用Octomap构建栅格化环境地图也使得路径规划的效率大大提高。
步骤S200:获取预设的栅格化地图中的障碍点位置信息。
其中,障碍点位置信息即为栅格化地图中障碍点所在的具体的坐标的位置。此处,对于障碍点位置信息可以采用膨胀障碍物所在节点的方式来将障碍物所在的范围进行扩大化,此处,膨胀障碍物指的是将障碍物所在的边缘的额外的节点也定义为障碍点位置信息。通过这种处理后进行规划的无碰路径可以远离障碍物。
此处,可以使用Octomap相关的dynamicEDT3D函数库对原Octomap数据进行预处理,将障碍物边上的膨胀系数值内的格点均设定为障碍物点,重新生成规划所用的Octomap栅格化地图。
步骤S300:根据所需的起点位置、终点位置以及所获取的障碍点位置信息,通过图搜索算法对机械臂的末端执行器规划无碰路径。
其中,在此处,通过图搜索算法对机械臂的末端执行器进行规划无碰路径之前,需要先得到机械臂的初始位姿并设定目标位姿。但在该步骤中,仅仅只对末端执行器所在的位置规划无碰路径。
具体的,图搜索算法在本申请实施例中优选采用JPS算法实现,也可以采用任何已知的基于图搜索的算法进行路径规划。其中,JPS算法是一种在搜索跳跃节点的过程中不断通过预设的剪枝规则对无需扩展的节点进行剪枝以减小运算量的具体方式,对于具体的实现方式,其包括下述步骤:
步骤S310:以预设的寻找顺序并依据跳跃节点寻找规则寻找跳跃节点。
步骤S320:在寻找跳跃节点的过程中通过预设的剪枝规则对邻居节点进行剪枝并标记强制邻居节点。
步骤S330:根据所获取的跳跃节点以及强制邻居节点规划无碰路径。
其中,对于步骤S320,每次在搜寻跳跃节点的过程中,对邻居节点的剪枝都基于当前节点周围的节点、父节点(即前一节点)和当前节点的所在位置以及障碍物所在的障碍点位置信息相关联,而对应的剪枝是在搜寻跳跃节点的过程中除去无关的邻居节点,并对所需的强制邻居节点进行标记。跳跃节点的寻找是为了标记出末端执行器所必要经过的位置,而对于跳跃节点的寻找,每次都沿一个方向移动一个节点来判定当前节点是否为跳跃节点。预设的寻找顺序可以是先沿x轴、y轴、z轴、xy平面、yz平面或zx平面的方向进行寻找,当在寻找过程中都遇到障碍物时,再沿着xyz斜线方向移动一个节点继续沿着x轴、y轴、z轴、xy平面、yz平面或zx平面的方向进行寻找。
此处,强制邻居节点和跳跃节点都是步骤S330中进行无碰路径规划时所需经过的节点位置。对应于格栅化地图所构建的坐标系,每一节点都对应有一唯一的节点坐标,并且所相对应的,当前节点、父节点以及所标定的跳跃节点与强制邻居节点也对应有一唯一的节点坐标。
对于三维的栅格化坐标系中,每一节点至多会有26个邻居节点(即3x3x3的范围中除去中心节点的其它节点),并且对于父节点至当前节点的移动共有三种移动方式,一种是沿坐标系的方向进行水平移动至邻居节点,其每次移动的距离都为一;一种是在一个平面上的斜向运动并移动至邻居节点,其每次移动的距离都为另一种是每个维度均有位移的斜向移动并移动至邻居节点,其每次移动的距离都为
对于具体的JPS剪枝规则,其在三维的栅格化坐标的限定下供需包括:
若父节点p(x)移动至当前节点x的方向为直线,则在从父节点p(x)移动至当前节点x的路径中,若len(<p(x),…,n>\x)≤len(<p(x),x,n>),则节点n为需要剪枝的邻居节点。
式中,p(x)为父节点,n为判断的节点,x为当前节点,len(<p(x),…,n>\x)表示从父节点p(x)开始经过各点到节点n但不经过节点x的路径长度,len(<p(x),x,n>)表示从父节点p(x)开始经过节点x到达节点n的路径长度。
条件2:
若父节点p(x)移动至当前节点x的方向为任一平面内的斜线,则在从父节点p(x)移动至当前节点x的路径中,若len(<p(x),…,n>\x)<len(<p(x),x,n〉),则节点n为需要剪枝的邻居节点。
其中,基于条件2,其还需通过定义1进行进一步定义,定义1为:从父节点p(x)抵达父节点p(x)非一步可达的格点区域,若从父节点p(x)抵达父节点p(x)需跨越平面,则计算路径长度时必须经过当前节点x。
条件3:
若父节点p(x)移动至当前节点x的方向为任一三维斜线方向,则在从父节点p(x)移动至当前节点x的路径中,若len(<p(x),…,n>\x)<len(<p(x),x,n>),则该节点n为需要剪枝的邻居节点。
其中,基于条件3,其还需通过定义2进行进一步定义,定义2为:
从父节点p(x)抵达父节点p(x)非一步可达的格点区域,若抵达该格点的路径中经过xy,yz,zx平面相交的棱,且这些棱均会穿过父节点p(x),则计算路径长度时必须经过当前节点x。
定义1与定义2中跨越平面指的是:假设当前沿xy平面方向拓展且假设当前平面z轴坐标为0,假定3×3×3立方体的中间层z=0,最上层z=1,最底层z=-1,若计算路径长度时,路径中存在格点z=+1,则定义从父节点p(x)抵达父节点p(x)非一步可达格点时路径发生了跨平面行为。
对于强制邻居节点的限定,其需要满足定义3:
该节点不是当前节点所需剪枝的邻居节点,且满足len((p(x),…,n>\x)<len((p(x),x,n))。
参照图2,在满足条件1的移动条件下,假设当前节点x的坐标为(xi,yi,zi)那么其父节点p(x)的坐标为(xi-1,yi,zi),则可知两者之间的位差为(Δx=±1,Δy=0,Δz=0),因此剪枝沿x轴施行。如若在邻居节点内不存在任何障碍物节点,那么在条件1的限定下,将会被剪枝25个邻居节点而剩下(xi+1,yi,zi)的自然节点。如若在坐标为(xi,yi,zi-1)处存在一个障碍物节点,那么根据定义3中所述,可以得到坐标为(xi+1,yi,zi-1)处的节点为强制邻居节点。如若在坐标为(xi,yi-1,zi-1)处存在一个障碍物节点,那么根据定义3中所述,可以得到坐标为(xi+1,yi-1,zi-1)处的节点为强制邻居节点。如若在坐标为(xi,yi-1,zi-1)以及(xi,yi-1,zi)处均存在一个障碍物节点,那么根据定义3中所述,可以得到坐标为(xi+1,yi-1,zi-1)处以及坐标(xi+1,yi-1,zi)的节点为均为强制邻居节点。
参照图3,在满足条件2的移动条件下,假设当前节点x的坐标为(xi,yi,zi)那么其父节点p(x)的坐标为(xi-1,yi-1,zi),可知剪枝过程在xy平面方向上进行,那么在条件2的限定下,(xi+1,yi+1,zi)、(xi,yi+1,zi)、(xi+1,yi,zi)均为自然节点。如若在坐标为(xi,yi-1,zi)处存在一个障碍物节点,那么根据定义3中所述,可以得到坐标为(xi+1,yi-1,zi)处为一个强制邻居节点。如若在坐标为(xi,yi,zi+1)处存在一个障碍物节点,那么根据定义1以及定义3中所述,可以得到(xi+1,yi,zi+1),(xi,yi+1,zi+1),以及(xi+1,yi+1,zi+1)为强制邻居节点,此处,相对于xy平面方向上的剪枝,z轴的障碍物节点已经使其发生了跨越平面的行为。如若在坐标为(xi,yi-1,zi)以及(xi,yi-1,zi+1)处均存在一个障碍物节点,那么根据定义1以及定义3中所述,可以得到坐标为(xi+1,yi-1,zi)以及(xi+1,yi-1,zi+1)处为强制邻居节点;
参照图4,在满足条件3的移动条件下,假设当前节点x的坐标为(xi,yi,zi)那么其父节点p(x)的坐标为(xi-1,yi-1,zi-1),可知剪枝过程在xyz方向上进行,那么在条件3的限定下,(xi+1,yi+1,zi)、(xi,yi+1,zi)、(xi+1,yi,zi)、(xi+1,yi+1,zi+1)、(xi+1,yi,zi+1)、(xi,yi+1,zi+1)、(xi,yi,zi+1)均为自然节点。如若在坐标为(xi,yi-1,zi)处存在一个障碍物节点,那么根据定义2以及定义3中所述,可以得到(xi,yi-1,zi+1),(xi+1,yi-1,zi),以及(xi+1,yi-1,zi+1)处的格点为强制邻居节点。如若在坐标为(xi,yi-1,zi-1)处存在一个障碍物节点,那么根据定义2以及定义3中所述,可以得到(xi+1,yi-1,zi-1)处的格点为强制邻居节点。
因而,通过上述的举例论述可知,在三维的栅格化坐标路径中,通过定义1、2、3以及条件1、2、3可以在搜索跳点的过程中对周围的邻居节点进行剪枝,并且对存在的强制邻居节点进行标记。
节点y为目标节点;或者,
节点y至少有一个邻居节点为强制邻居节点;或者,
因而,如果在斜线移动方向上存在的节点,如果能通过直线移动若干个节点的距离到达目标节点或至少有一个邻居节点为强制邻居节点的位置,那么这个点也需要被判定为跳跃节点。此处,目标节点指的是跳跃节点搜索的终点。
在跳跃节点的搜索过程中,会沿前述规定的搜索方向的先后顺序进行搜索,如果这个方向是直线方向,那么搜索会沿该方向一直进行,如果搜索到跳跃节点则会将这个跳跃节点加入对应的存储库中,而如果在搜索过程中碰到了障碍物或者边界,则说明从当前节点沿这个方向进行直线扩展是无价值的,那么就在当前搜索到的最后一个跳跃节点的基础上更换移动方向继续进行跳跃节点的搜索。但如果这个方向是斜线方向,那么将斜向方向分解为垂直方向以及水平方向进行搜索,若这两个直线方向的搜索是无价值的,则继续向下一个斜线格点进行扩展。对于三维场景,斜线向量方向若是三维的,则分解为二维斜线向量方向与一维直线向量方向,二维斜线向量方向则会进一步分解为一维直线向量方向进行搜索。
其中,在经过完整的跳跃节点的搜索后,可以得到所有的跳跃节点和强制邻居节点所在的位置,根据先后的顺序将其连接起来即可得到一条适用于当前障碍点位置信息下的无碰路径。
步骤S400:对生成的无碰路径进行路径优化。
在此处,对于步骤S300中生成的无碰路径,由于两个节点(跳跃节点与跳跃节点之间,或强制邻居节点与跳跃节点之间,或是强制邻居节点与强制邻居节点之间)的跨度较大,并且由于经过无碰路径规划后的节点数量较小,会导致所选定的节点数量难以直接满足构成机械臂运动所需的路径点的数量要求。并且,过少的路径点也会导致后续在拟合过程中发生较大的偏差。因此需要在所选的节点之间增加节点来消除上述影响。但是,在理想条件下,也可以省略步骤S400。
因此,具体的优化方法包括:
步骤S410:获取无碰路径上相邻两个节点之间的自由路径节点。
其中,自由路径节点指的是位于无碰路径上两个节点之间,且相互连接不会穿过障碍物节点的非障碍物节点,非障碍物节点即障碍物点位信息以外的节点。
步骤S420:将各相邻自由路径节点之间相互连接以形成扩展无碰路径。
其中,由于在步骤S300之中,所获取的路径一定是无碰路径,因此在扩展为扩展无碰路径的过程中一定会存在一条路径以形成所需的扩展无碰路径。如图5所示为一种情况下扩展节点数量后,无碰路径和扩展无碰路径的对比示意图,在通过引入自由路径节点后,可以发现,满足无碰路径上的节点数量会被扩展,同时,由于自由路径节点的限定,导致在扩展无碰路径上存在较多的直角。但是这种直角路径的存在会导致轨迹运动始终处于加速减速的过程,这会导致机械臂的移动带有明显的顿挫感,对机器设备以及执行效率都存在不利影响。因此,需要对这些直角的部分进行平滑性处理。
但是在进行平滑性处理的过程中,又需要经过平滑性处理的无碰路径与原无碰路径差别较小。这是因为机械臂的运动是多关节多连杆的耦合运动,假若移除路径直角点后的路径在优化后变成了起始点和目标点的直接连线,即便起始点和目标点之间不存在障碍物,但机械臂的某个连杆可能会与空间内其他障碍物发生碰撞,或者机械臂在这条路径上的某个点处是不可达的。这可能会导致每次路径规划都生成同一条路径,虽然路径规划显示成功,但机械臂无法执行对应的路径动作。
因而,具体的处理方法包括:
步骤S430:在扩展无碰路径的相邻两个节点之间依次选择三个相邻的自由路径节点进行筛选并迭代,三个相邻的自由路径节点依次定义为p1,p2和p3。
其中,在第一次筛选的过程中,会从一端选择三个相邻的自由路径节点,在当第一次迭代完成后,会向后多选择一个自由路径节点,并将第一个自由路径节点舍弃。因而在多次迭代过程中,任意三个相邻的自由路径节点都会被依序进行步骤S440的筛选操作。
步骤S440:在筛选过程中,如果dist(p1,p3)<dist(p1,p2)+dist(p2,p3),则剔除p2点,并设定当前的p1点为前一次迭代的p3点进行迭代。
如果dist(p1,p3)≥dist(p1,p2)+dist(p2,p3),则设定当前的p1点为前一次迭代的p2点进行迭代。
其中,dist(pi,pj)表示pi点到pj点之间的距离。这种筛选方式运用了三角形的边长定理,当出现步骤S440中所述情况时,即代表p2点所在的位置是直角转折点,因此即将p2点进行剔除。而在当出现步骤S450中所述情况时,即代表p1,p2和p3可能处于在一条直线上。
步骤S460:将剩余的相邻自由路径节点之间相互连接并更新无碰路径。
在将直角转折点剔除之后,将此点相邻两点之间进行连接会得到两个拐角大于等于135°的转折点。
如图6所示,在以图5为基础的扩展无碰路径的基础上,三个直角转折点所在的位置被剔除,同时整体的轨迹也没有发生较大的变化,因此,这种筛选方式所得到的无碰路径可以较好的消除无碰路径中的直角转折点,从而利于在步骤S700中生成平滑的运动轨迹。
步骤S500:检测机械臂在以无碰路径运动时是否会发生碰撞或具有非可达点。
其中,虽然在步骤S400中规划了末端执行器的无碰路径,但是由于机械臂整体占用了较大的空间,会导致其其余连杆部分可能会与障碍物产生碰撞,或者存在机械臂无法达到的节点。因此需要通过对机械臂整体进行判断以检测机械臂在以该无碰路径运动时是否会发生碰撞或具有非可达点。
借助Moveit可生成自碰撞检测的ACM矩阵,简化自碰撞检测流程。利用Moveit集成的FCL库,可对机械臂进行AABB包围盒碰撞检测,包括机械臂与环境障碍物的碰撞,机械臂自碰撞检测。自碰撞检测需要输入拟定位置的机械臂关节角角度值,通过IKFast逆运动学求解可以获得这些需要的角度值。运动空间合理性检测通过对路径点的逆运动学求解结果的判断进行检测,若在该点处无有效的逆运动学求解结果,则该点为当前机械臂的非可达点,反之存在有效的解,则认为该点可达,若存在有效结果但发生碰撞,则该点同样为非可达点。
步骤S600:若机械臂在以无碰路径运动时会发生碰撞或具有非可达点,则将该节点添加入障碍点位置信息中。
其中,对于会发生碰撞或具有非可达点的节点,需要将这些点设定为障碍物点,并将这些节点添加到障碍点位置信息中。当障碍物点位置信息被更新后,随即该循环即重新回到步骤S300中以重新规划新的无碰路径。同样的,经过更新后的无碰路径仍需要进行步骤S500的检测。
步骤S700:若无,则将所规划的无碰路径进行轨迹规划以生成机械臂的运动轨迹。
其中,对于机械臂的运动轨迹的规划可以采用TOPP-RA轨迹规划算法实现。
步骤S800:检测机械臂在以该运动轨迹运动时是否会发生碰撞或具有非可达点。
同样的,在此处也需要对步骤S700中生成的机械臂的运动轨迹进行碰撞和非可达点检测。对于具体的检测方式可以与步骤S500中的检测方式相同。
如果在执行完步骤S800后,机械臂在以该运动轨迹运动时不会发生碰撞或不具有非可达点,则表明所规划的规划轨迹为所需的可供机械臂执行的轨迹。此处,将TOPP-RA生成的本地参数,包括时间,关节角度,关节角速度,关节角加速度利用ROS发布至Moveit轨迹控制器,驱动控制Gazebo中的机械臂运动。并在抵达目标点后,发布Vacuum Gripper吸盘的控制指令,对目标物体进行抓取。
步骤S900:若机械臂在以该运动轨迹运动时会发生碰撞或具有非可达点,则将该节点添加入障碍点位置信息中。
同样的,再进行完步骤S900后,当障碍物点位置信息被更新后,随即该循环即重新回到步骤S300中以重新规划新的无碰路径。同样的,经过更新后的无碰路径仍需要进行步骤S500的检测,并且需要在重新生成机械臂的运动轨迹后再次进行步骤S800的检测。
在本方法中,为了防止无限次迭代情况的发生,可以将步骤S300中每次规划无碰路径的次数进行记步,当无碰路径的规划此处达到最大规划阈值次数时,即强制停止循环。在一种实施例中,也可以采用对整体的规划时间进行计时的方式,来判断整体的规划时间是否达到最大的上限,当整体的规划时间达到所预设的最大值时,也强制停止循环。这种方式可以有效的避免无限循环而无法结束进程的情况存在。
此处,在结束循环之后,也可以输出最后一次规划的无碰轨迹,以方便后续的人工调试,实现通过设定多目标点绕过当前规划失败的路径的目的。
基于同一发明构思,本申请实施例还公开一种机械臂路径规划系统,其包括:
初始化模块,用于初始化栅格化地图并获取预设的栅格化地图中的障碍点位置信息。
无碰路径规划模块,用于根据所需的起点位置、终点位置以及所获取的障碍点位置信息,通过图搜索算法规划无碰路径。
无碰路径优化模块,用于对生成的无碰路径进行路径优化。
无碰路径检测模块,用于检测机械臂在以无碰路径运动时是否会发生碰撞或具有非可达点。
若机械臂在以无碰路径运动时会发生碰撞或具有非可达点,则将该节点添加入障碍点位置信息中。
若无,则通过机械臂轨迹规划模块以将所规划的无碰路径进行轨迹规划以生成机械臂的运动轨迹。
机械臂轨迹检测模块,用于检测机械臂在以该运动轨迹运动时是否会发生碰撞或具有非可达点。
若机械臂在以该运动轨迹运动时会发生碰撞或具有非可达点,则将该节点添加入障碍点位置信息中。
其中,若障碍点位置信息被更新,则通过无碰路径规划模块重新规划无碰路径。
基于同一发明构思,本发明实施例提供一种机械臂,用于执行如图1一种机械臂路径规划方法的计算机程序。
所属领域的技术人员可以清楚地了解到,为描述的方便和简洁,仅以上述各功能模块的划分进行举例说明,实际应用中,可以根据需要而将上述功能分配由不同的功能模块完成,即将装置的内部结构划分成不同的功能模块,以完成以上描述的全部或者部分功能。上述描述的系统,装置和单元的具体工作过程,可以参考前述方法实施例中的对应过程,在此不再赘述。
在本申请所提供的几个实施例中,应该理解到,所揭露的系统,装置和方法,可以通过其它的方式实现。例如,以上所描述的装置实施例仅仅是示意性的,例如,所述模块或单元的划分,仅仅为一种逻辑功能划分,实际实现时可以有另外的划分方式,例如多个单元或组件可以结合或者可以集成到另一个系统,或一些特征可以忽略,或不执行。另一点,所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口,装置或单元的间接耦合或通信连接,可以是电性,机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的,作为单元显示的部件可以是或者也可以不是物理单元,即可以位于一个地方,或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外,在本申请各个实施例中的各功能单元可以集成在一个处理单元中,也可以是各个单元单独物理存在,也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现,也可以采用软件功能单元的形式实现。
以上所述,以上实施例仅用以对本申请的技术方案进行了详细介绍,但以上实施例的说明只是用于帮助理解本发明的方法及其核心思想,不应理解为对本发明的限制。本技术领域的技术人员在本发明揭露的技术范围内,可轻易想到的变化或替换,都应涵盖在本发明的保护范围之内。
Claims (10)
1.一种机械臂路径规划方法,其特征在于,包括:
获取预设的栅格化地图中的障碍点位置信息;
根据所需的起点位置、终点位置以及所获取的障碍点位置信息,通过图搜索算法对机械臂的末端执行器规划无碰路径;
检测机械臂在以无碰路径运动时是否会发生碰撞或具有非可达点;
若机械臂在以无碰路径运动时会发生碰撞或具有非可达点,则将该节点添加入障碍点位置信息中;
若无,则将所规划的无碰路径进行轨迹规划以生成机械臂的运动轨迹;
检测机械臂在以该运动轨迹运动时是否会发生碰撞或具有非可达点;
若机械臂在以该运动轨迹运动时会发生碰撞或具有非可达点,则将该节点添加入障碍点位置信息中;
其中,若障碍点位置信息被更新,则通过图搜索算法重新规划无碰路径。
2.根据权利要求1所述的机械臂路径规划方法,其特征在于,在图搜索算法规划无碰路径的过程中包括:
以预设的寻找顺序并依据跳跃节点寻找规则寻找跳跃节点;
在寻找跳跃节点的过程中通过预设的剪枝规则对邻居节点进行剪枝并标记强制邻居节点;
根据所获取的跳跃节点以及强制邻居节点规划无碰路径。
3.根据权利要求2所述的机械臂路径规划方法,其特征在于,预设的剪枝规则包括:
若父节点p(x)移动至当前节点x的方向为直线,则在从父节点p(x)移动至当前节点x的路径中,若len(<p(x),…,n>\x)≤len(<p(x),x,n>),则节点n为需要剪枝的邻居节点;
若父节点p(x)移动至当前节点x的方向为任一平面内的斜线,则在从父节点p(x)移动至当前节点x的路径中,若len(<p(x),…,n>\x)<len(<p(x),x,n>),则节点n为需要剪枝的邻居节点;
其中,从父节点p(x)抵达父节点p(x)非一步可达的格点区域,若从父节点p(x)抵达父节点p(x)需跨越平面,则计算路径长度时必须经过当前节点x;
若父节点p(x)移动至当前节点x的方向为任一三维斜线方向,则在从父节点p(x)移动至当前节点x的路径中,若len(<p(x),…,n>\x)<len(<p(x),x,n>),则该节点n为需要剪枝的邻居节点;
其中,从父节点p(x)抵达父节点p(x)非一步可达的格点区域,若抵达该格点的路径中经过xy,yz,zx平面相交的棱,且这些棱均会穿过父节点p(x),则计算路径长度时必须经过当前节点x;
式中,p(x)为父节点,n为判断的节点,x为当前节点,len(<p(x),…,n>\x)表示从父节点p(x)开始经过各点到节点n但不经过节点x的路径长度,len(<p(x),x,n>)表示从父节点开始经过节点x到达节点n的路径长度。
4.根据权利要求3所述的机械臂路径规划方法,其特征在于,标记强制邻居节点的规则需要满足该节点不是当前节点x所需剪枝的邻居节点,且满足len((p(x),…,n>\x)<len(<p(x),x,n>)。
6.根据权利要求1所述的机械臂路径规划方法,其特征在于,在检测机械臂在以无碰路径运动时是否会发生碰撞或具有非可达点之前,所述方法还包括对生成的无碰路径进行路径优化;
所述路径优化的方法包括:
获取无碰路径上相邻两个节点之间的自由路径节点;
将各相邻自由路径节点之间相互连接以形成扩展无碰路径。
7.根据权利要求6所述的机械臂路径规划方法,其特征在于,在形成扩展无碰路径之后,所述方法还包括:
在扩展无碰路径的相邻两个节点之间依次选择三个相邻的自由路径节点进行筛选并迭代,三个相邻的自由路径节点依次定义为p1,p2和p3;
在筛选过程中,如果dist(p1,p3)<dist(p1,p2)+dist(p2,p3),则剔除p2点,并设定当前的p1点为前一次迭代的p3点进行迭代;
如果dist(p1,p3)≥dist(p1,p2)+dist(p2,p3),则设定当前的p1点为前一次迭代的p2点进行迭代;
将剩余的相邻自由路径节点之间相互连接并更新无碰路径;
其中,dist(pi,pj)表示点pi到pj点之间的距离。
8.根据权利要求1所述的机械臂路径规划方法,其特征在于,在每次障碍点位置信息被更新的过程中,判断无碰路径是否达到最大规划阈值次数,若是,则停止无碰路径的规划。
9.一种机械臂路径规划系统,其特征在于,包括:
初始化模块,用于初始化栅格化地图并获取预设的栅格化地图中的障碍点位置信息;
无碰路径规划模块,用于根据所需的起点位置、终点位置以及所获取的障碍点位置信息,通过图搜索算法规划无碰路径;
无碰路径检测模块,用于检测机械臂在以无碰路径运动时是否会发生碰撞或具有非可达点;
若机械臂在以无碰路径运动时会发生碰撞或具有非可达点,则将该节点添加入障碍点位置信息中;
若无,则通过机械臂轨迹规划模块以将所规划的无碰路径进行轨迹规划以生成机械臂的运动轨迹;
机械臂轨迹检测模块,用于检测机械臂在以该运动轨迹运动时是否会发生碰撞或具有非可达点;
若机械臂在以该运动轨迹运动时会发生碰撞或具有非可达点,则将该节点添加入障碍点位置信息中;
其中,若障碍点位置信息被更新,则通过无碰路径规划模块重新规划无碰路径。
10.一种机械臂,其特征在于,用于执行通过如权利要求1至8中任一种方法所规划的运动轨迹。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210604675.1A CN114986501A (zh) | 2022-05-28 | 2022-05-28 | 一种机械臂路径规划方法、系统及机械臂 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210604675.1A CN114986501A (zh) | 2022-05-28 | 2022-05-28 | 一种机械臂路径规划方法、系统及机械臂 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN114986501A true CN114986501A (zh) | 2022-09-02 |
Family
ID=83031020
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210604675.1A Pending CN114986501A (zh) | 2022-05-28 | 2022-05-28 | 一种机械臂路径规划方法、系统及机械臂 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114986501A (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115674195A (zh) * | 2022-10-20 | 2023-02-03 | 四川大学 | 柔性机械臂在线臂形规划方法 |
CN115674195B (zh) * | 2022-10-20 | 2024-06-04 | 四川大学 | 柔性机械臂在线臂形规划方法 |
-
2022
- 2022-05-28 CN CN202210604675.1A patent/CN114986501A/zh active Pending
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115674195A (zh) * | 2022-10-20 | 2023-02-03 | 四川大学 | 柔性机械臂在线臂形规划方法 |
CN115674195B (zh) * | 2022-10-20 | 2024-06-04 | 四川大学 | 柔性机械臂在线臂形规划方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Alatartsev et al. | Robotic task sequencing problem: A survey | |
US5835684A (en) | Method for planning/controlling robot motion | |
Akbaripour et al. | Semi-lazy probabilistic roadmap: a parameter-tuned, resilient and robust path planning method for manipulator robots | |
Sudhakara et al. | Trajectory planning of a mobile robot using enhanced A-star algorithm | |
CN110703768A (zh) | 一种改进型动态rrt*的移动机器人运动规划方法 | |
Cortés et al. | Probabilistic motion planning for parallel mechanisms | |
Shao et al. | Rrt-goalbias and path smoothing based motion planning of mobile manipulators with obstacle avoidance | |
US11813756B2 (en) | Disassembly based assembly planning | |
Le et al. | Cooperative multi-robot sampling-based motion planning with dynamics | |
CN113650011B (zh) | 一种机械臂拼接路径规划方法及装置 | |
Ramer et al. | A robot motion planner for 6-DOF industrial robots based on the cell decomposition of the workspace | |
Tipary et al. | Planning and optimization of robotic pick-and-place operations in highly constrained industrial environments | |
CN111709095A (zh) | 一种面向复杂曲面6d虚拟夹具构造方法 | |
Ananthanarayanan et al. | A fast converging optimal technique applied to path planning of hyper-redundant manipulators | |
CN114986501A (zh) | 一种机械臂路径规划方法、系统及机械臂 | |
Shome et al. | Improving the scalability of asymptotically optimal motion planning for humanoid dual-arm manipulators | |
Liang et al. | PR-RRT*: Motion Planning of 6-DOF Robotic Arm Based on Improved RRT Algorithm | |
Vahrenkamp et al. | Efficient motion planning for humanoid robots using lazy collision checking and enlarged robot models | |
Sanders | Real‐time geometric modeling using models in an actuator space and cartesian space | |
Deng et al. | Robotic manipulation planning using dynamic RRT | |
Seyboldt et al. | Sampling-based path planning to cartesian goal positions for a mobile manipulator exploiting kinematic redundancy | |
Henrich et al. | On-line path planning by heuristic hierarchical search | |
CN114488853A (zh) | 一种机器人路径规划的场景仿真方法及装置 | |
Zhu et al. | Online motion generation using accumulated swept volumes | |
Al-Hmouz et al. | Cellular automata based path planning algorithm for autonomous mobile robots |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination |