CN116652972B - 基于双向贪心搜索算法的串联机器人末端轨迹规划方法 - Google Patents
基于双向贪心搜索算法的串联机器人末端轨迹规划方法 Download PDFInfo
- Publication number
- CN116652972B CN116652972B CN202310948481.8A CN202310948481A CN116652972B CN 116652972 B CN116652972 B CN 116652972B CN 202310948481 A CN202310948481 A CN 202310948481A CN 116652972 B CN116652972 B CN 116652972B
- Authority
- CN
- China
- Prior art keywords
- robot
- point
- track
- feasible
- sequence
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 56
- 238000010845 search algorithm Methods 0.000 title claims abstract description 12
- 230000002457 bidirectional effect Effects 0.000 title claims abstract description 11
- 238000012163 sequencing technique Methods 0.000 claims abstract description 7
- 238000007689 inspection Methods 0.000 claims description 7
- 238000001514 detection method Methods 0.000 claims description 6
- 238000012545 processing Methods 0.000 claims description 5
- 238000005315 distribution function Methods 0.000 claims description 4
- 230000009191 jumping Effects 0.000 claims description 3
- 238000003860 storage Methods 0.000 description 12
- 239000011159 matrix material Substances 0.000 description 10
- 238000012546 transfer Methods 0.000 description 6
- 238000005259 measurement Methods 0.000 description 5
- 238000004590 computer program Methods 0.000 description 4
- 230000006870 function Effects 0.000 description 4
- 230000003287 optical effect Effects 0.000 description 3
- 238000006243 chemical reaction Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 238000009776 industrial production Methods 0.000 description 2
- 230000000644 propagated effect Effects 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 238000003466 welding Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 239000000835 fiber Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
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
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1671—Programme controls characterised by programming, planning systems for manipulators characterised by simulation, either to verify existing program or to create and verify new program, CAD/CAM oriented, graphic oriented programming systems
-
- 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
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
本发明涉及机器人控制技术领域，公开了基于双向贪心搜索算法的串联机器人末端轨迹规划方法，包括以下步骤：步骤S1.利用串联机器人的几何关系获取机器人的逆运动学模型；步骤S2.根据机器人及环境的模型获得机器人关节位姿的物理可行域；步骤S3.将空间点位按照距离关系进行排序，得到机器人的空间点位运行序列；步骤S4.将点位均分为两份，分别从起点和终点出发，将串联机器人空间点位依次输入机器人逆运动学模型中，规划机器人半轨迹，得到两条半轨迹；步骤S5.将所述两条半轨迹连接，最终获得机器人完整的轨迹。本发明降低了机器人末端轨迹规划所用的时间，减少了搜索空间，提高了机器人的运行效率，具有良好的工业应用价值。
Description
技术领域
本发明涉及机器人控制技术领域，尤其涉及机器人轨迹规划领域，更具体的说涉及一种基于双向贪心搜索算法的串联机器人末端轨迹规划方法。
背景技术
串联机器人因其工作空间灵活、可扩展性强被大量应用于工业生产中的测量、焊接以及装配等任务中。测量或装配等任务可以描述为，机器人不发生碰撞的情形下末端以一定顺序经过给定位置，在各个位置完成指定任务。由于机器人逆运动学具有多解的性质，每个末端位置不一定对应单一的关节空间位置，同时由于机器人碰撞限制，关节空间的直线轨迹不一定可达。
目前，机器人末端的轨迹规划常用的方法为基于随机搜索实现，但是其一般情况下得到的轨迹较长，机器人运行效率较低并且每次得到的结果重复性较差。
发明内容
为了解决上述现有技术中存在的问题和不足，本发明提出了一种基于双向贪心搜索算法的串联机器人末端轨迹规划方法，通过起点和终点的双向贪心搜索轨迹规划方法，利用二进程并行方法降低了机器人末端轨迹规划所用的时间，同时在串联机器人位姿选取时采用贪心方法，减少了搜索空间，提高了机器人的运行效率，具有良好的工业应用价值。
为了实现上述发明目的，本发明的技术方案具体如下：
一种基于双向贪心搜索算法的串联机器人末端轨迹规划方法，包括如下步骤：
步骤S1.利用串联机器人的几何关系获取机器人的逆运动学模型；
步骤S2.根据机器人及环境的模型获得机器人关节位姿的物理可行域；
步骤S3.将空间点位按照距离关系进行排序，得到机器人的空间点位运行序列；
步骤S4.将点位均分为两份，分别从起点和终点出发，将串联机器人空间点位依次输入机器人逆运动学模型中，规划机器人半轨迹，得到两条半轨迹；
步骤S5.将所述两条半轨迹连接，最终获得机器人完整的轨迹。
作为优选地，所述步骤S1具体包括：
步骤S11.通过CAD模型或机器人标定方法获取机器人几何关系；
步骤S12.利用所述机器人几何关系建立机器人运动学模型；
步骤S13.通过机器人运动学模型建立机器人逆运动学模型。
作为优选地，所述步骤S2具体包括：
步骤S21.在机器人关节位姿空间中随机选取大量关节位姿；
步骤S22.在选取的关节位姿中，计算关节位姿对应的机器人各个关节的位置和环境各个组件的位置，对CAD模型两两进行碰撞检测；若发生碰撞则为不可行关节位姿，反之为可行关节位姿；
步骤S23.通过插补的方式获取整个串联机器人的物理可行域。
作为优选地，所述步骤S3具体包括：
步骤S31.根据机器人任务获取机器人末端的起点、终点以及需要经过的中间空间点位；
步骤S32.将中间空间点位向所述起点和终点的连线投影，沿连线均分上述空间点位，将其顺序作为两条原始序列；
步骤S33.从原始序列中的第一点位作为出发点出发，将其加入输出序列，并在原始序列中删除该点位；
步骤S34.选取所述原始序列中与出发点在测量方向距离小于阈值的一系列点，获得局部序列，若局部序列长度小于等于1，则回到步骤S33；
步骤S35.将所述局部序列按照与出发点空间距离从小到大进行排序，遍历该局部序列，若遍历的当前点位不是原始序列的当前第一点，则将其加入输出序列，否则停止遍历；
步骤S36.循环步骤S33-S35至原始序列为空，此时输出序列即为规划好的点位运行序列。
作为优选地，所述步骤S4具体包括：
步骤S41.采用双进程分别处理两个点位运行序列，分别从起点和终点出发，将两个点位序列中的机器人空间点位依次输入至机器人逆运动学模型中，获得两个关节空间序列；
步骤S42.以两个关节空间序列的起点和终点作为第一点分别建立两条半轨迹，分别遍历两个可行空间序列；
步骤S43.循环取上一位置起点，在当前关节空间序列中未加入半轨迹的点中随机选取一定量的位姿为终点，在位姿空间中取起点和终点位姿的直线轨迹，进行插值，计算每个插值点是否发生碰撞，若所有点均不发生碰撞，则该直线轨迹称为直线可行轨迹，并将该轨迹作为机器人可行轨迹，在所有可行轨迹中选取运行时间最短的机器人轨迹，加入半轨迹，继续循环，否则跳转至步骤S44。
步骤S44.使用随机搜索树方法在关节位姿空间中搜索可行轨迹，再尝试跳跃连接优化轨迹，将所述轨迹作为机器人轨迹，加入半轨迹序列。
作为优选地，所述步骤S5具体包括：
步骤S51.选取步骤S4中两条半轨迹的末端点称为中间点；
步骤S52.在关节位姿空间中取中间点的直线轨迹，进行插值，计算每个插值点是否发生碰撞，若所有点均不发生碰撞，则将该轨迹作为机器人中间轨迹；
步骤S53.将起点半轨迹、中间轨迹、终点半轨迹依次连接，最终得到完整的机器人轨迹。
作为优选地，所述步骤S22具体包括：
步骤S22.1.获取机器人各个关节及环境的准确CAD模型；
步骤S22.2.将所述CAD模型三角化，获取三角化CAD模型的所有顶点和顶点法向；
步骤S22.3.将所述三角化CAD模型顶点的坐标沿顶点法向移动固定值，得到CAD模型外包络；
步骤S22.4.将所述三角化CAD模型顶点的坐标沿顶点法向反向移动固定值，得到CAD模型内包络；
步骤S22.5.将所述三角化CAD模型、CAD模型外包络以及CAD模型内包络组合得到扩展CAD模型；
步骤S22.6.计算所述扩展CAD模型的最小凸体包络；
步骤S22.7.将各个组件位置表示为齐次矩阵，将齐次矩阵与各个组件的最小凸体包络的其次坐标相乘，计算各个组件最小凸体包络的世界坐标；
步骤S22.8.使用世界坐标下各个模型的最小凸体包络两两进行相交检测，若相交则说明发生碰撞，反之说明未碰撞。
作为优选地，对于完全机器人，逆运动学模型的解为分离解，几何可行空间为空集或有限集；对于冗余机器人，逆运动型模型的解为连续解，通过空间范围表示。
作为优选地，所述步骤S43具体包括：
步骤S43.1.在所述关节空间序列中按照到起点欧式距离加权随机选取终点位姿，计算该位姿是否在所述物理可行域中，若满足，则将其加入可行点序列，否则在物理可行域中去除其邻域，循环选取直至可行终点序列长度达到预设值或物理可行域为空；
步骤S43.2.遍历可行终点序列，选取终点/>与起点进行线性插值，得到/>个中间点/>，计算所述/>个中间点是否在所述物理可行域中，若不满足则为发生碰撞，否则未碰撞；其中，为终点机器人的关节坐标，/>为起点机器人的关节坐标，/>表示中间点坐标。
作为优选地，所述步骤S44具体包括：
步骤S44.1.以起点为根建立随机搜索树；
步骤S44.2.在物理可行域中以一定的概率分布函数随机取点，以该随机取得的点为中心，将随机搜索树中所有节点按照到该点的欧式距离由小到大进行排序，得到相应的节点序列，遍历所述节点序列，使用步骤S43的插值和检验方法检测当前点到当前树的所有节点是否有步骤S43所述的直线可行轨迹，若存在则将其作为该节点的子节点，结束循环；
其中，所述距离计算为：设所述物理可行域中随机选取的点的坐标为，所述随机搜索树中的任意节点的坐标为/>，距离/>计算为：。
步骤S44.3.从根节点出发遍历搜索树，使用步骤S43的插值和检验方法尝试其他已搜索到的节点与当前轨迹上节点是否直线可行，若可行且距离更短，则将其代替当前轨迹。
本发明的有益效果：
（1）本发明针对现有技术机器人末端轨迹规划效率较低的问题，提出了从起点和终点双向贪心搜索的轨迹规划方法，通过二进程并行方法降低了机器人末端轨迹规划所用的时间，同时在串联机器人位姿选取时采用贪心方法，减少了搜索空间，提高了机器人的运行效率，具有良好的工业应用价值。
（2）本发明针对现有技术在机器人末端轨迹规划时，搜索轨迹较长、运行效率较低的问题，提出了直线轨迹搜索和随机搜索树搜索结合的方法。在直线随机搜索中提出通过设计分布函数尽可能选取较短轨迹的方法，在随机搜索树搜索中提出将随机搜索树与图优化方法结合优化轨迹的方法，减少了机器人的搜索轨迹，提高了运行效率。
附图说明
本发明的前述和下文具体描述在结合以下附图阅读时变得更清楚，附图中：
图1为本发明方法流程图。
具体实施方式
为了使本领域的技术人员更好地理解本发明中的技术方案，下面将通过几个具体的实施例来进一步说明实现本发明发明目的的技术方案，需要说明的是，本发明要求保护的技术方案包括但不限于以下实施例。基于本发明中的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都应当属于本发明保护的范围。
串联机器人因其工作空间灵活、可扩展性强被大量应用于工业生产中的测量、焊接以及装配等任务中。测量或装配等任务可以描述为，机器人不发生碰撞的情形下末端以一定顺序经过给定位置，在各个位置完成指定任务。由于机器人逆运动学具有多解的性质，每个末端位置不一定对应单一的关节空间位置，同时由于机器人碰撞限制，关节空间的直线轨迹不一定可达。
目前，机器人末端的轨迹规划常用的方法为基于随机搜索实现，但是其一般情况下得到的轨迹较长，机器人运行效率较低并且每次得到的结果重复性较差。
基于此，本发明的实施例提出了一种双向贪心搜索算法的串联机器人末端轨迹规划方法，从起点和终点双向贪心搜索的轨迹规划方法，通过二进程并行方法降低了机器人末端轨迹规划所用的时间，同时在串联机器人位姿选取时采用贪心方法，减少了搜索空间，提高了机器人的运行效率，具有良好的工业应用价值。
本实施例公开了一种基于双向贪心搜索算法的串联机器人末端轨迹规划方法，参照说明书附图1，该方法具体如下。
步骤S1.利用串联机器人的几何关系获取机器人的逆运动学模型。
在本实施例中，通过步骤S1可以将机器人的关节坐标用末端坐标进行表示，具体的步骤如下：
步骤S11.通过CAD模型或者机器人标定方法，获取机器人的几何关系。
在本实施例中，所述CAD模型为机器人关节和环境CAD模型，机器人关节指所有关节，包括工具等；环境则指固定工装、测量/操作对象等。
步骤S12.利用获取的机器人几何关系建立机器人运动学模型。
步骤S13.对所述机器人运动学模型进行反解，建立机器人逆运动学模型。
作为本发明的一个实施例，机器人为具有六个旋转关节的机器人，其运动学模型可以表示为：
；
进一步地，其逆运动学模型可表示为八个分支函数，即对于任意空间位置，机器人有八个可能的关节坐标（即有八个解）：
；
其中，为逆运动学模型；/>表示机器人第一个关节到基座的传递矩阵；/>表示机器人第二个关节到第一个关节的传递矩阵；/>表示机器人第三个关节到第二个关节的传递矩阵；/>表示机器人第四个关节到第三个关节的传递矩阵；/>表示机器人第五个关节到第四个关节的传递矩阵；/>表示机器人第六个关节到第五个关节的传递矩阵；/>表示机器人第一个关节的坐标；/>表示机器人第二个关节的坐标；/>表示机器人第三个关节的坐标；表示机器人第四个关节的坐标；/>表示机器人第五个关节的坐标；/>表示机器人第六个关节的坐标；/>表示机器人关节坐标的集合；/>表示对应不同解机器人的关节坐标；/>表示任意空间位置/>的坐标，/>代表位置，/>代表方向。
进一步地，当其有实数解时，说明该可行位置是存在的，否则说明该位置不存在。
作为本发明的另一个实施例，机器人为搭载在二自由度平动滑台上的具有六个旋转关节的机器人，那么此时其运动学模型可以表示为：
；
其中，表示上述二自由度平动滑台到世界坐标系的传递矩阵；
对于给定的滑台位置，其逆运动学解同样可以表示为八个分支函数，即对于任意给定空间位置/>和滑台位置/>，机器人有八个可能的关节坐标（即有八个解）：
；
当其有实数解时说明该可行位置存在，否则说明该位置不存在。通过给定不同的，结合插值方法可以获得机器人的逆运动学解系。与前一个发明实例不同，该解系为关节空间的一个子空间，不是分离解。
步骤S2.使用机器人关节和环境CAD模型，获得机器人关节位姿的物理可行域。
本实施例中，所述步骤S2具体包括以下内容。
步骤S21.在机器人关节位姿空间中随机选取大量关节位姿。
在本实施例中，需要说明的是，机器人有两个空间，一个是关节空间，一个是操作空间，操作空间可理解为世界坐标系下的三维坐标（位置、方向），关节空间就是机器人每个关节的坐标，机器人运动学模型简单说就是操作空间与关节空间的转换矩阵。也就是操作空间运动到某个位置，那么机器人各个关节怎么运动才能实现运动到这个位置。
步骤S22.在选取的关节位姿中，计算关节位姿对应的机器人各个关节位置和环境各个组件的位置，对CAD模型两两进行碰撞检测；若发生碰撞则为不可行关节位姿，反之为可行关节位姿。
在本实施例中，所述步骤S22进一步包括以下内容。
步骤S22.1.获取机器人各个关节及环境的准确CAD模型；
步骤S22.2.将所述CAD模型三角化，获取所述三角化CAD模型的所有顶点和顶点法向；
步骤S22.3.将所述三角化CAD模型顶点坐标沿顶点法向移动固定值，得到CAD模型外包络；
步骤S22.4.将所述三角化CAD模型顶点坐标沿顶点法向反向移动固定值，得到CAD模型内包络；
步骤S22.5.将所述三角化CAD模型、CAD模型外包络以及CAD模型内包络组合得到扩展CAD模型；
步骤S22.6.计算所述扩展CAD模型的最小凸体包络；
步骤S22.7.将各个组件位置表示为齐次矩阵，将齐次矩阵与各个组件的所述最小凸体包络的齐次坐标相乘，计算各个组件所述最小凸体包络的世界坐标；
步骤S22.8.使用世界坐标下各个模型的所述最小凸体包络两两进行相交检测，若相交则说明发生碰撞，反之说明未碰撞。
在本实施例中，需要说明的是，各个组件是指经过步骤S22.1-S22.6处理后的机器人关节及环境中的CAD模型。
步骤S23.通过插补的方式获取整个串联机器人的物理可行域。
在本实施例中，插补指的是运动学插补，为本领域技术人员公知的技术手段，在此不再赘述。
本发明的一个实施例中，机器人为具有六个旋转关节的机器人，环境为机架以及待加工工件。首先建立机器人六个关节以及工件的精确CAD模型，再使用所述方法获得各个模型的最小凸体包络（共八个）。八个最小凸体包络模型可以表示为一系列关键点的组合，即每个凸体包络模型都可以由一系列关键点来表示。在世界坐标系下，通过标定和配准获得机架到世界坐标系的转换矩阵/>以及工件到世界坐标系的转换矩阵，由此可以给出各个组件的变换关系，包括机器人：
；
；
；
；
；
；
机架：
；
工件：
；
其中，表示机器人的凸体包络模型的关键点在世界坐标系下的坐标；/>表示机架的凸体包络模型的关键点在世界坐标系下的坐标；/>表示工件的凸体包络模型的关键点在世界坐标系下的坐标；/>表示机器人凸体包络模型的关键点在自身坐标系下的坐标；/>表示机架的凸体包络模型的关键点在自身坐标系下的坐标；/>表示工件的凸体包络模型的关键点在自身坐标系下的坐标；之后使用所述方法计算获得物理可行域。
步骤S3.将空间点位按照距离关系进行排序，得到机器人的空间点位运行序列。
在本实施例中，所述步骤S3具体包括以下内容。
步骤S31.根据机器人任务获取机器人末端的起点、终点以及需要经过的中间空间点位；
步骤S32.将中间空间点位向所述起点和终点的连线投影，沿连线均分上述空间点位，将其顺序作为两条原始序列，称为起点原始序列和终点原始序列，两条原始序列分别执行下述步骤；
步骤S33.从原始序列中的第一点位作为出发点出发，将其加入输出序列，并在原始序列中删除该点位；
步骤S34.选取所述原始序列中与出发点在测量方向距离小于阈值的一系列点，获得局部序列，若局部序列长度小于等于1，则回到步骤S33；
步骤S35.将所述局部序列按照与出发点空间距离从小到大进行排序，遍历该局部序列，若遍历的当前点位不是原始序列的当前第一点，则将其加入输出序列，否则停止遍历；
步骤S36.循环步骤S33-S35至原始序列为空，此时输出序列即为规划好的点位运行序列。
在本实施例中，步骤S34的阈值一般和机器人大小以及工作空间大小有关，本实例中可取200mm。
在本实施例中，步骤S32的起点原始序列和终点原始序列分别对应起点运行序列和终点运行序列。
步骤S4.将点位均分为两份，分别从起点和终点出发，将串联机器人空间点位依次输入机器人逆运动学模型中，规划机器人半轨迹，得到两条半轨迹。
在本实施例中，所述步骤4具体包括以下内容。
步骤S41.将步骤S3获得的机器人空间点位序列均分为两份，采用双进程分别处理两个点位运行序列，分别从起点和终点出发，将两个点位序列中的机器人空间点位依次输入至机器人逆运动学模型中，获得两个关节空间序列；
步骤S42.以两个关节空间序列的起点和终点作为第一点分别建立两条半轨迹，分别遍历两个可行空间序列；
步骤S43.循环取上一位置起点，在当前关节空间序列中未加入半轨迹的点中随机选取一定量的位姿为终点在位姿空间中取起点和终点位姿的直线轨迹，进行插值，计算每个插值点是否发生碰撞，若所有点均不发生碰撞，则该直线轨迹称为直线可行轨迹，并将该轨迹作为机器人可行轨迹，在所有可行轨迹中选取运行时间最短的机器人轨迹，加入半轨迹，继续循环，否则跳转至步骤S44。
进一步地，所述步骤S43具体包括以下内容：
步骤S43.1.在所述关节空间序列中按照到起点欧式距离加权随机选取终点位姿，计算该位姿是否在步骤S2所述物理可行域中，若满足，则将其加入可行点序列，否则在物理可行域中去除其邻域，循环选取直至可行终点序列长度达到预设值或物理可行域为空；
步骤S43.2.遍历可行终点序列，选取终点/>与起点进行线性插值，得到/>个中间点/>，计算所述/>个中间点是否在所述步骤S2的物理可行域中，若不满足则为发生碰撞，否则未碰撞；其中，/>为终点机器人的关节坐标，/>为起点机器人的关节坐标，/>表示中间点的坐标。
步骤S44.使用随机搜索树方法在关节位姿空间中搜索可行轨迹，再尝试跳跃连接优化轨迹，将所述轨迹作为机器人轨迹，加入半轨迹序列。
进一步地，所述步骤S44具体包括以下内容。
步骤S44.1.以起点为根建立随机搜索树；
步骤S44.2.在物理可行域中以一定的概率分布函数随机取点，以该随机点为中心，将随机搜索树中所有节点按照到该点距离由小到大进行排序，并建立节点序列，遍历所述节点序列，使用步骤S43的插值和检验方法检测当前点到当前树的所有节点是否有步骤S43所述的直线可行轨迹，若存在则将其作为该节点的子节点，结束循环，并将其作为终点，其与起点的连线为当前轨迹；
步骤S44.3.从根节点出发遍历搜索树，使用步骤S43的插值和检验方法尝试其他已搜索到的节点与当前轨迹上节点是否直线可行，若可行且距离更短，则将其代替当前轨迹。
本发明的一个实施例中，机器人为具有六个旋转关节的机器人，所述逆运动学模型可以表示为：
；
可行物理域为八个函数的实数函数值集合。
每次搜索获得的点可以表示为；在随机搜索树的基础上建立伴生k-d树，取伴生k-d树中到/>距离小于给定值/>的节点建立节点序列。伴生k-d树中任意节点/>到/>的距离/>可以表示为：
；
其中，表示每次搜索获得的点/>的坐标；表示伴生k-d树中任意节点/>的坐标；
遍历所述节点序列，使用步骤S43的插值和检验方法检测当前点到当前树的所有节点是否有步骤S43所述的直线可行轨迹，若存在则将其作为该节点的子节点，结束循环，并将其作为终点，其与起点的连线为当前轨迹。
步骤S5.将所述两条半轨迹连接，最终获得机器人完整的轨迹。
在本实施例中，所述步骤S5具体包括以下内容。
步骤S51.选取步骤S4中两条半轨迹的末端点称为中间点。
步骤S52.在关节位姿空间中取中间点的直线轨迹，进行插值，计算每个插值点是否发生碰撞，若所有点均不发生碰撞，则将该轨迹作为机器人中间轨迹。
步骤S53.将起点半轨迹、中间轨迹、终点半轨迹依次连接，最终得到完整的机器人轨迹。
基于同一发明构思，本发明的实施例还提出了一种适用于上述双向贪心搜索算法的串联机器人装置，该机器人装置包括：
运动构件模块，包含六个以上的活动关节，末端有六个完全空间自由度，包括三个平动自由度和三个转动自由度；
动力驱动模块，每个活动关节具有一个对应的动力驱动模块，能够驱动该活动关节沿轴单自由度旋转或单自由度平动；
控制模块，能够以指令形式控制动力驱动模块以给定的速度运动到给定的位置。
为了实现上述实施例所述的轨迹规划方法，本发明的实施例又提出一种计算机可读存储介质，存储介质上存储有计算机程序，所述计算机程序被处理器执行时可实现上述实施例中所述的串联机器人末端轨迹规划方法的步骤。
需要说明的是，本发明上述的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本发明中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本发明中，计算机可读信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读信号介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：电线、光缆、RF(射频)等等，或者上述的任意合适的组合。
进一步地，本发明的实施例还提供了一种电子设备，该电子设备包括处理器、存储器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时，实现上述实施中所述的串联机器人末端轨迹规划方法的步骤。
所述电子设备可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。所述电子设备可包括，但不仅限于，处理器、存储器，例如所述电子设备还可以包括输入输出设备、网络接入设备、总线等。
所称处理器可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(FieldProgrammable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。所述存储器可以是所述电子设备的内部存储单元，例如终端设备的硬盘或内存。所述存储器也可以是所述电子设备的外部存储设备，例如所述电子设备上配备的插接式硬盘，智能存储卡 (Smart Media Card ,SMC) ，安全数字 (Secure Digital ,SD)卡，闪存卡(Flash Card)等。进一步地，所述存储器还可以既包括所述电子设备的内部存储单元也包括外部存储设备。所述存储器用于存储所述计算机程序以及所述电子设备所需的其他程序和数据。所述存储器还可以用于暂时地存储已经输出或者将要输出的数据。
上述计算机可读介质可以是上述电子设备中所包含的；也可以是单独存在，而未装配入该电子设备中。上述计算机可读介质承载有一个或者多个程序，当上述一个或者多个程序被该电子设备执行时，使得该电子设备执行上述实施例。
以上所述，仅是本发明的较佳实施例，并非对本发明做任何形式上的阻碍，凡是依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化，均落入本发明的保护范围之内。
Claims (3)
1.基于双向贪心搜索算法的串联机器人末端轨迹规划方法，其特征在于，包括如下步骤：
步骤S1.利用串联机器人的几何关系获取机器人的逆运动学模型；
步骤S2.根据机器人及环境的模型获得机器人关节位姿的物理可行域；
步骤S3.将空间点位按照距离关系进行排序，得到机器人的空间点位运行序列；
步骤S4.将点位均分为两份，分别从起点和终点出发，将串联机器人空间点位依次输入机器人逆运动学模型中，规划机器人半轨迹，得到两条半轨迹；
步骤S5.将所述两条半轨迹连接，最终获得机器人完整的轨迹；
所述步骤S1具体包括：
步骤S11.通过CAD模型或机器人标定方法获取机器人几何关系；
步骤S12.利用所述机器人几何关系建立机器人运动学模型；
步骤S13.通过机器人运动学模型建立机器人逆运动学模型；
所述步骤S2具体包括：
步骤S21.在机器人关节位姿空间中随机选取大量关节位姿；
步骤S22.在选取的关节位姿中，计算关节位姿对应的机器人各个关节的位置和环境各个组件的位置，对CAD模型两两进行碰撞检测；若发生碰撞则为不可行关节位姿，反之为可行关节位姿；
步骤S23.通过插补的方式获取整个串联机器人的物理可行域；
所述步骤S3具体包括：
步骤S31.根据机器人任务获取机器人末端的起点、终点以及需要经过的中间空间点位；
步骤S32.将中间空间点位向所述起点和终点的连线投影，沿连线均分上述空间点位，将其顺序作为两条原始序列；
步骤S33.从原始序列中的第一点位作为出发点出发，将其加入输出序列，并在原始序列中删除该点位；
步骤S34.选取所述原始序列中与出发点在测量方向距离小于阈值的一系列点，获得局部序列，若局部序列长度小于等于1，则回到步骤S33；
步骤S35.将所述局部序列按照与出发点空间距离从小到大进行排序，遍历该局部序列，若遍历的当前点位不是原始序列的当前第一点，则将其加入输出序列，否则停止遍历；
步骤S36.循环步骤S33-S35至原始序列为空，此时输出序列即为规划好的点位运行序列；
所述步骤S4具体包括：
步骤S41.采用双进程分别处理两个点位运行序列，分别从起点和终点出发，将两个点位序列中的机器人空间点位依次输入至机器人逆运动学模型中，获得两个关节空间序列；
步骤S42.以两个关节空间序列的起点和终点作为第一点分别建立两条半轨迹，分别遍历两个可行空间序列；
步骤S43.循环取上一位置起点，在当前关节空间序列中未加入半轨迹的点中随机选取一定量的位姿为终点，在位姿空间中取起点和终点位姿的直线轨迹，进行插值，计算每个插值点是否发生碰撞，若所有点均不发生碰撞，则该直线轨迹称为直线可行轨迹，并将该轨迹作为机器人可行轨迹，在所有可行轨迹中选取运行时间最短的机器人轨迹，加入半轨迹，继续循环，否则跳转至步骤S44；
步骤S44.使用随机搜索树方法在关节位姿空间中搜索可行轨迹，再尝试跳跃连接优化轨迹，将所述轨迹作为机器人轨迹，加入半轨迹序列；
所述步骤S5具体包括：
步骤S51.选取步骤S4中两条半轨迹的末端点称为中间点；
步骤S52.在关节位姿空间中取中间点的直线轨迹，进行插值，计算每个插值点是否发生碰撞，若所有点均不发生碰撞，则将该轨迹作为机器人中间轨迹；
步骤S53.将起点半轨迹、中间轨迹、终点半轨迹依次连接，最终得到完整的机器人轨迹；
所述步骤S43具体包括：
步骤S43.1.在所述关节空间序列中按照到起点欧式距离加权随机选取终点位姿，计算该位姿是否在所述物理可行域中，若满足，则将其加入可行点序列，否则在物理可行域中去除其邻域，循环选取直至可行终点序列长度达到预设值或物理可行域为空；
步骤S43.2.遍历可行终点序列，选取终点/>与起点/>进行线性插值，得到/>个中间点/>，计算所述/>个中间点是否在所述物理可行域中，若不满足则为发生碰撞，否则未碰撞；其中，/> 为终点机器人的关节坐标，/> 为起点机器人的关节坐标，/> 表示中间点坐标；
所述步骤S44具体包括：
步骤S44.1.以起点为根建立随机搜索树；
步骤S44.2.在物理可行域中以一定的概率分布函数随机取点，以该随机取得的点为中心，将随机搜索树中所有节点按照到该点的欧式距离由小到大进行排序，得到相应的节点序列，遍历所述节点序列，使用步骤S43的插值和检验方法检测当前点到当前树的所有节点是否有步骤S43所述的直线可行轨迹，若存在则将其作为该节点的子节点，结束循环；
步骤S44.3.从根节点出发遍历搜索树，使用步骤S43的插值和检验方法尝试其他已搜索到的节点与当前轨迹上节点是否直线可行，若可行且距离更短，则将其代替当前轨迹。
2.根据权利要求1所述的基于双向贪心搜索算法的串联机器人末端轨迹规划方法，其特征在于，所述步骤S22具体包括：
步骤S22.1.获取机器人各个关节及环境的准确CAD模型；
步骤S22.2.将所述CAD模型三角化，获取三角化CAD模型的所有顶点和顶点法向；
步骤S22.3.将所述三角化CAD模型顶点的坐标沿顶点法向移动固定值，得到CAD模型外包络；
步骤S22.4.将所述三角化CAD模型顶点的坐标沿顶点法向反向移动固定值，得到CAD模型内包络；
步骤S22.5.将所述三角化CAD模型、CAD模型外包络以及CAD模型内包络组合得到扩展CAD模型；
步骤S22.6.计算所述扩展CAD模型的最小凸体包络；
步骤S22.7.将各个组件位置表示为齐次矩阵，将齐次矩阵与各个组件的最小凸体包络的其次坐标相乘，计算各个组件最小凸体包络的世界坐标；
步骤S22.8.使用世界坐标下各个模型的最小凸体包络两两进行相交检测，若相交则说明发生碰撞，反之说明未碰撞。
3.根据权利要求1所述的基于双向贪心搜索算法的串联机器人末端轨迹规划方法，其特征在于，对于完全机器人，逆运动学模型的解为分离解，几何可行空间为空集或有限集；对于冗余机器人，逆运动型模型的解为连续解，通过空间范围表示。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310948481.8A CN116652972B (zh) | 2023-07-31 | 2023-07-31 | 基于双向贪心搜索算法的串联机器人末端轨迹规划方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310948481.8A CN116652972B (zh) | 2023-07-31 | 2023-07-31 | 基于双向贪心搜索算法的串联机器人末端轨迹规划方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116652972A CN116652972A (zh) | 2023-08-29 |
| CN116652972B true CN116652972B (zh) | 2023-11-10 |
Family
ID=87710134
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310948481.8A Active CN116652972B (zh) | 2023-07-31 | 2023-07-31 | 基于双向贪心搜索算法的串联机器人末端轨迹规划方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116652972B (zh) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110497403A (zh) * | 2019-08-05 | 2019-11-26 | 上海大学 | 一种改进双向rrt算法的机械臂运动规划方法 |
| CN113246142A (zh) * | 2021-06-25 | 2021-08-13 | 成都飞机工业（集团）有限责任公司 | 一种基于激光引导的测量路径规划方法 |
| US11254003B1 (en) * | 2019-04-18 | 2022-02-22 | Intrinsic Innovation Llc | Enhanced robot path planning |
| CN114199270A (zh) * | 2021-12-14 | 2022-03-18 | 安徽理工大学 | 融合双向搜索机制与改进a*算法的机器人路径规划方法 |
| CN114310904A (zh) * | 2022-01-19 | 2022-04-12 | 中南大学 | 一种适用于机械臂关节空间路径规划的新型双向rrt*方法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10521473B2 (en) * | 2012-05-21 | 2019-12-31 | Kent State University | Shortest path computation in large networks |
-
2023
- 2023-07-31 CN CN202310948481.8A patent/CN116652972B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US11254003B1 (en) * | 2019-04-18 | 2022-02-22 | Intrinsic Innovation Llc | Enhanced robot path planning |
| CN110497403A (zh) * | 2019-08-05 | 2019-11-26 | 上海大学 | 一种改进双向rrt算法的机械臂运动规划方法 |
| CN113246142A (zh) * | 2021-06-25 | 2021-08-13 | 成都飞机工业（集团）有限责任公司 | 一种基于激光引导的测量路径规划方法 |
| CN114199270A (zh) * | 2021-12-14 | 2022-03-18 | 安徽理工大学 | 融合双向搜索机制与改进a*算法的机器人路径规划方法 |
| CN114310904A (zh) * | 2022-01-19 | 2022-04-12 | 中南大学 | 一种适用于机械臂关节空间路径规划的新型双向rrt*方法 |
Non-Patent Citations (4)
| Title |
|---|
| A branch-and-bound approach for AGV dispatching and routing problems in automated container terminals;Wang, ZH;COMPUTERS&INDUSTRIAL ENGINEERING;第166卷;第1-25页 * |
| A Path Planning Method with a Bidirectional Potential Field Probabilistic Step Size RRT for a Dual Manipulator;Liu, YY;SENSORS;第23卷(第11期);第1-20页 * |
| 基于改进RRT的机械臂路径规划算法研究;赵惠;中国优秀硕士学位论文全文数据库(第1（2013）期);I140-1286 * |
| 机器人辅助飞机装配自动化钻孔技术;高文翔;组合机床与自动化加工技术(第11（2021）期);第134-138页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116652972A (zh) | 2023-08-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Zhuang et al. | Method for kinematic calibration of Stewart platforms | |
| Baizid et al. | IRoSim: Industrial Robotics Simulation Design Planning and Optimization platform based on CAD and knowledgeware technologies | |
| CN110370256B (zh) | 机器人及其路径规划方法、装置和控制器 | |
| Zacharia et al. | Task scheduling and motion planning for an industrial manipulator | |
| CN110640746B (zh) | 机器人的坐标系标定及定位方法、系统、设备、介质 | |
| CN112318506A (zh) | 机械臂自动标定方法、装置、设备、机械臂和介质 | |
| JP4905285B2 (ja) | 工具参照面を作成する装置と方法とプログラム | |
| JP2019084649A (ja) | 干渉判定方法、干渉判定システム及びコンピュータプログラム | |
| CN113119104B (zh) | 机械臂控制方法、机械臂控制装置、计算设备及系统 | |
| Kim et al. | A RRT-based motion planning of dual-arm robot for (Dis) assembly tasks | |
| US20210323154A1 (en) | Disassembly based assembly planning | |
| CN114800534A (zh) | 一种机械臂控制方法及装置 | |
| Kim et al. | GraphDistNet: A graph-based collision-distance estimator for gradient-based trajectory optimization | |
| CN116652972B (zh) | 基于双向贪心搜索算法的串联机器人末端轨迹规划方法 | |
| CN117428791B (zh) | 一种用于肩部四轴康复机器人的逆运动学求解方法及系统 | |
| CN117001663A (zh) | 机械臂运动路径规划方法及系统、存储介质 | |
| JP2017131990A (ja) | 干渉回避方法 | |
| CN114851190A (zh) | 面向低频驱控一体的机械臂轨迹规划方法及系统 | |
| Wu et al. | Trajectory Planning and Singularity Avoidance Algorithm for Robotic Arm Obstacle Avoidance Based on an Improved Fast Marching Tree | |
| Gueta et al. | Multiple-goal task realization utilizing redundant degrees of freedom of task and tool attachment optimization | |
| CN114378860B (zh) | 软体机器人操纵器的生成式设计技术 | |
| CN117207200B (zh) | 机械臂的工作空间生成方法、装置和计算机设备 | |
| TWI856570B (zh) | 加工路徑規劃模擬裝置以及方法 | |
| CN110555240A (zh) | 一种从机器人装配体模型到仿真模型的自动生成方法 | |
| CN115870976B (zh) | 一种机械臂的采样轨迹规划方法和装置、电子设备 |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |