CN110509279B - 一种仿人机械臂的运动路径规划方法及*** - Google Patents
一种仿人机械臂的运动路径规划方法及*** Download PDFInfo
- Publication number
- CN110509279B CN110509279B CN201910843685.9A CN201910843685A CN110509279B CN 110509279 B CN110509279 B CN 110509279B CN 201910843685 A CN201910843685 A CN 201910843685A CN 110509279 B CN110509279 B CN 110509279B
- Authority
- CN
- China
- Prior art keywords
- node
- mechanical arm
- nodes
- random
- path
- 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
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
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
- Feedback Control In General (AREA)
Abstract
本发明公开了一种仿人机械臂的运动路径规划方法及***。所述方法包括:依据仿人机械臂的D‑H模型确定仿人机械臂待运动空间的起始位置和目标位置;采用快速搜索随机树法对仿人机械臂待运动空间的采样点进行探索,在探索过程中采用偏向概率采样算法确定随机采样点;依据所确定的随机采样点扩展新的节点,并结合新的节点构建以起始位置为源节点,以目标位置为终节点的扩展随机树。最后在所构建的扩展树的各路径中找到总权重最小的路径,将总权重值最小的路径中所有节点的信息通过逆运动学解算为仿人机械臂运动的关节角信息,使仿人机械臂按照关节角信息运动。本发明提供的仿人机械臂的运动路径规划方法及***,能够节约搜索时间、加快搜索采样点的速度。
Description
技术领域
本发明涉及机械臂的运动规划技术领域,特别是涉及一种仿人机械臂的运动路径规划方法及***。
背景技术
近年来随着生产技术的不断发展,机械臂的应用更加广泛,对机械臂操作精度的要求也不断提高。而对于机械臂完成一个完整的操作流程,需要机械臂能够较快速、准确的到达预定位置后再进行操作。机械臂有很多可以行进的路径,但是找到一条既符合运动约束又满足衡量标准的路径一直是对机械臂进行运动规划的目标。而最优路径一般包括路径最短、使用时间最短、损耗能量最少等条件。
现有的机械臂运动规划算法例如遗传算法、人工势场法、蚁群算法等,均不能够很好找到一条真正意义上的最优路径,且传统的算法中一般还具有非完整约束问题,并且需要对环境进行精确的建模才能较为准确的寻找得到最优路径。但随着机械臂自由度数的增加计算复杂度呈指数性增长,传统的运动规划方法并不适合用于多自由度的机械臂的运动规划。
RRT算法与传统方法相比,它具有独特的优势,因为它避免了精确的环境模型。该算法效率高,适用于解决在高维空间中多自由度机器人的复杂约束下的运动规划问题。目前,RRT算法已广泛应用于机器人运动规划领域。值得注意的是,该算法不提供最优解,因为搜索路径的探索树是由随机采样点扩展的。此外,RRT算法采用全局均匀随机采样策略,收敛速度慢,时间消耗大。
发明内容
本发明提供的仿人机械臂的运动路径规划方法及***,能够节约搜索时间、加快搜索采样点的速度。
为实现上述目的,本发明提供了如下方案:
一种仿人机械臂的运动路径规划方法,包括:
采用D-H法描述仿人机械臂的位姿,建立仿人机械臂的连杆坐标系,确定仿人机械臂的D-H参数,并计算得到仿人机械臂的D-H模型;
根据所述仿人机械臂的D-H模型,确定初始参数;所述初始参数包括所述仿人机械臂的起始位置、所述仿人机械臂运动的目标位置、搜索步长和最大采样次数;
以所述仿人机械臂的起始位置为待搜索空间的起始位置,以所述仿人机械臂运动的目标位置为待搜索空间的目标位置,采用快速搜索随机树法根据所述搜索步长和最大采样次数搜索所述待搜索空间的采样点,并将搜索得到的所述采样点作为节点构建随机树;所述待搜索空间为所述仿人机械臂的工作空间;
采用偏向概率采样算法确定随机采样点;遍历所述随机树上的节点,确定距所述随机采样点最近的节点;并判断所述随机采样点与最近节点间的距离是否大于搜索步长;若是,则在距所述随机采样点最近的节点到所述随机采样点的连线上以所述搜索步长的长度扩展节点;若否,则将所述随机采样点直接作为扩展节点;
在对节点的扩展过程中,检测所扩展的节点是否位于障碍物边界的内部;若是,则删除所扩展的节点,反之则保留所扩展的节点;
将保留的扩展节点形成扩展随机树;所述扩展随机树是以所述待搜索空间的起始位置为源节点,以所述待搜索空间的目标位置为终节点的随机树;
遍历所述扩展随机树上的所有节点,连接所述扩展随机树上的所有节点;判断节点间相互连接的连接线是否与障碍物发生碰撞;若是则删除节点间的连线,若否则保留节点间的连线;
根据所保留的节点间的连线,确定仿人机械臂从所述源节点到所述终节点的各路径;获取每一路径中各节点间连线的长度;以所述各节点间连线的长度作为子权重,计算得到每一路径的总权重;
获取总权重最小的路径,将所述总权重最小的路径中所有的节点信息通过逆运动学解算为仿人机械臂运动的关节角信息,并使仿人机械臂按照所述关节角信息进行运动。
可选的,所述采用偏向概率采样算法确定随机采样点,包括:
采用偏向概率算法和均匀概率分布算法随机获取概率值;
设定采样阈值;
将所述概率值与所设定的采样阈值进行比较,当所述采样阈值大于所述概率值时,将所述待搜索空间的目标位置确定为随机采样点,当所述采样阈值不大于所述概率值时,采用随机采样策略确定所述随机采样点。
可选的,所述在对节点的扩展过程中,检测所扩展的节点是否位于障碍物边界的内部;若是,则删除所扩展的节点,反之则保留所扩展的节点之前还包括:
判断所扩展的节点是否位于所述扩展随机树的终节点的设定邻域范围内;若是,则停止扩展节点;若否,则继续扩展节点。
可选的,采用Dijkstra算法确定各路径中各连线长度的总权重最小的路径。
一种仿人机械臂的运动路径规划***,包括:
D-H模型建立模块,用于采用D-H法描述仿人机械臂的位姿,建立仿人机械臂的连杆坐标系,确定仿人机械臂的D-H参数,并计算得到仿人机械臂的D-H模型;
初始参数确定模块,用于根据所述仿人机械臂的D-H模型,确定初始参数;所述初始参数包括所述仿人机械臂的起始位置、所述仿人机械臂运动的目标位置、搜索步长和最大采样次数;
随机树构建模块,用于以所述仿人机械臂的起始位置为待搜索空间的起始位置,以所述仿人机械臂运动的目标位置为待搜索空间的目标位置,采用快速搜索随机树法根据所述搜索步长和最大采样次数搜索所述待搜索空间的采样点,并将搜索得到的所述采样点作为节点构建随机树;所述待搜索空间为所述仿人机械臂的工作空间;
节点扩展模块,用于采用偏向概率采样算法确定随机采样点;遍历所述随机树上的节点,确定距所述随机采样点最近的节点;并判断所述随机采样点与最近节点间的距离是否大于搜索步长;若是,则在距所述随机采样点最近的节点到所述随机采样点的连线上以所述搜索步长的长度扩展节点;若否,则将所述随机采样点直接作为扩展节点;
第一检测模块,用于在对节点的扩展过程中,检测所扩展的节点是否位于障碍物边界的内部;若是,则删除所扩展的节点,反之则保留所扩展的节点;
扩展随机树生成模块,用于将保留的扩展节点形成扩展随机树;所述扩展随机树是以所述待搜索空间的起始位置为源节点,以所述待搜索空间的目标位置为终节点的随机树;
第二检测模块,用于遍历所述扩展随机树上的所有节点,连接所述扩展随机树上的所有节点;判断节点间相互连接的连接线是否与障碍物发生碰撞;若是则删除节点间的连线,若否则保留节点间的连线;
路径权重获取模块,用于根据所保留的节点间的连线,确定仿人机械臂从所述源节点到所述终节点的各路径;获取每一路径中各节点间连线的长度;以所述各节点间连线的长度作为子权重,计算得到每一路径的总权重;
关节角信息解算模块,用于获取总权重最小的路径,将所述总权重最小的路径中所有的节点信息通过逆运动学解算为仿人机械臂运动的关节角信息,并使仿人机械臂按照所述关节角信息进行运动。
可选的,所述节点扩展模块包括:
概率值获取单元,用于采用偏向概率算法和均匀概率分布算法随机获取一个概率值;
采样阈值设定单元,用于设定采样阈值;
随机采样点确定单元,用于将所述概率值与所设定的采样阈值进行比较,当所述采样阈值大于所述概率值时,将所述待搜索空间的目标位置确定为随机采样点,当所述采样阈值不大于所述概率值时,采用随机采样策略确定所述随机采样点。
可选的,所述***还包括:
扩展判断模块,用于判断所扩展的节点是否位于所述扩展随机树的终节点的设定邻域范围内;若是,则停止扩展节点;若否,则继续扩展节点。
可选的,所述关节角信息解算模块包括:最小路径确定单元,用于采用Dijkstra算法确定各路径中各连线长度的总权重最小的路径。
根据本发明提供的具体实施例,本发明公开了以下技术效果:本发明提供的仿人机械臂的运动路径规划方法及***,在寻找仿人机械臂的最优运动路径的过程中,加入具有偏向概率采样算法的搜索方法,这就能够在保证采样完备性的同时,加快采样点向目标点搜索的速度,节约搜索时间。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获取其他的附图。
图1为本发明实施例仿人机械臂的运动路径规划方法的流程示意图;
图2为本发明实施例所采用的七自由度机械臂的结构简图;
图3为现有技术中的RRT算法示意图;
图4为本发明采用仿人机械臂的运动路径规划方法对扩展随机树扩展过程中进行碰撞检测的示意图;
图5为本发明采用仿人机械臂的运动路径规划方法生成一条无碰撞扩展随机树的示意图;
图6为本发明采用仿人机械臂的运动路径规划方法最终得到的路径图;
图7为本发明实施例仿人机械臂的运动路径规划方法的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获取的所有其他实施例,都属于本发明保护的范围。
本发明提供的仿人机械臂的运动路径规划方法及***,能够节约搜索时间、加快搜索采样点的速度。
为使本发明的上述目的、特征和优点能够更加明显易懂,下面结合附图和具体实施方式对本发明作进一步详细的说明。
图1为本发明实施例仿人机械臂的运动路径规划方法的流程示意图,如图1所示,一种仿人机械臂的运动路径规划方法,包括:
S100、采用D-H法描述仿人机械臂的位姿,建立仿人机械臂的连杆坐标系,确定仿人机械臂的D-H参数,并计算得到仿人机械臂的D-H模型;
S101、根据所述仿人机械臂的D-H模型,确定初始参数;所述初始参数包括所述仿人机械臂的起始位置Gstart、所述仿人机械臂运动的目标位置Ggoal、搜索步长stepsize和最大采样次数maxattempts;
S102、以所述仿人机械臂的起始位置Gstart为待搜索空间的起始位置Gstart,以所述仿人机械臂运动的目标位置Ggoal为待搜索空间的目标位置Ggoal,采用快速搜索随机树法根据所述搜索步长stepsize和最大采样次数maxattempts搜索所述待搜索空间的采样点,并将搜索得到的所述采样点作为节点构建随机树;所述待搜索空间为所述仿人机械臂的工作空间;
S103、采用偏向概率采样算法确定随机采样点Prand;遍历所述随机树上的节点,确定距所述随机采样点Prand最近的节点Pnear;并判断所述随机采样点Prand与最近节点Pnear间的距离是否大于搜索步长stepsize;若是,则在距所述随机采样点Prand最近的节点Pnear到所述随机采样点Prand的连线上以所述搜索步长stepsize的扩展节点Ptmp;若否,则将所述随机采样点Prand直接作为扩展节点Ptmp;具体通过以下公式进行扩展:
S104、在对节点的扩展过程中,检测所扩展的节点是否位于障碍物边界的内部;若是,则删除所扩展的节点,反之则保留所扩展的节点;
S105、将保留的扩展节点Ptmp形成扩展随机树;所述扩展随机树是以所述待搜索空间的起始位置Gstart为源节点,以所述待搜索空间的目标位置Ggoal为终节点的随机树;
S106、遍历所述扩展随机树上的所有节点,连接所述扩展随机树上的所有节点;判断节点间相互连接的连接线是否与障碍物发生碰撞;若是则删除节点间的连线,若否则保留节点间的连线;
S107、根据所保留的节点间的连线,确定仿人机械臂从所述源节点到所述终节点的各路径;获取每一路径中各节点间连线的长度;以所述各节点间连线的长度作为子权重,计算得到每一路径的总权重;
S108、获取总权重最小的路径,将所述总权重最小的路径中所有的节点信息通过逆运动学解算为仿人机械臂运动的关节角信息,并使仿人机械臂按照所述关节角信息进行运动。其中扩展方向夹角公式如下:
α=atan2(Pnear(1)-Ptemp(1),pnear(2)-ptemp(2))
其中Pnear(1)与Pnear(2)分别表示扩展随机数上距离扩展节点Ptmp最近的节点Pnear的横坐标与纵坐标;Ptemp(1)与Ptemp(2)分别表示扩展节点Ptmp的横坐标和纵坐标。
在S103中采用偏向概率采样算法确定随机采样点Prand,具体包括:
采用偏向概率算法和均匀概率分布算法随机获取概率值P;
设定采样阈值t(0<t<1);
将所述概率值P与所设定的采样阈值t进行比较,当所述采样阈值t大于所述概率值P时,将所述待搜索空间的起始位置Gstart确定为随机采样点Prand,当所述采样阈值t不大于所述概率值P时,采用随机采样策略确定所述随机采样点Prand。
在S104中对节点的扩展过程中,还包括:计算最近节点Pnear与扩展节点Ptmp之间的方向向量,在此方向上以预先设定的扩展步长不断循环扩展,直到扩展距离与最近节点Pnear到扩展节点Ptmp的距离相等。且在上述这一过程中在此过程中不断检测扩展点的坐标值是否存在于障碍物区域边界的内部。如果不存在碰撞则保留扩展点,否则删除此扩展点。
在S104之前还包括:
判断所扩展的节点是否位于所述扩展随机树的终节点的设定邻域范围内;若是,则停止扩展节点Ptmp;若否,则继续扩展节点Ptmp。
在S108中是采用Dijkstra算法确定各路径中各连线长度的总权重最小的路径。
在S104和S106中,在进行碰撞检测的过程中,具体进行快速检测碰撞公式如下:
Pathcheck=Pn+r*[sin(α)cos(α)]
其中α表示两点之间的方向角;Pn表示快速搜索随机树上面的第n个节点;r表示两点之间的不断以一定的搜索步长延长的检测长度。
在S107到S108确定最短路径的过程中,还具体包括:
计算S107中的各节点之间的路径长度,并将长度作为权重。将所有的扩展随机树上面的节点分成两部分,第一部分为已求出最短路径的集合S,在最开始的时候只有起始位置Gstart,后面每计算出一个最短路径点都加入到第一部分的集合S中去。第二部分为其余的待确定的节点的集合U。最后通过dijkstra算法对路径进行优化,具体通过以下步骤对路径寻优:
首先对第一部分集合S和第二部分集合U进行初始化D(v)记作为起始位置到某个终点v的路径的长度总和。同时L(w,v)记作为源点w到终点v的距离。
其次,遍历第二部分集合U中的所有节点并且计算min[D(v),D(v)+L(w,v)],在所有节点中寻找一个节点w,用min[D(v),D(v)+L(w,v)]中的最小值去替换D(v)的数值,并使其从第二集合U中移至第一集合S中,直至第二集合U成为空集。
最后,将上面得到的第一集合S中的扩展节点依次连接,就能得到从源节点Gstart到目标点Ggoal的扩展随机树的最短路径。
下面对本发明所提供的技术方案进行具体说明:
如图2所示,如图2所示为本发明实施例所采用的七自由度机械臂的结构简图,采用D-H法建立机械臂坐标系,在考虑机械臂的运动学约束的情况下建立机械臂的运动学模型。图中坐标系(X0,Y0,Z0)为基坐标系,坐标系(Xi,Yi,Zi)分别表示每个机械臂关节的坐标系。每个机械臂关节的运动方向围绕坐标系的z轴旋转。其中D-H坐标系的参数如下表1所示:
表一D-H坐标系参数表
连杆i | a<sub>i-1</sub>/m | α<sub>i-1</sub>/° | d<sub>i</sub>/m | θ<sub>i</sub>/° |
1 | 0 | 0 | 0.225 | -90 |
2 | 0 | -90 | 0 | -90 |
3 | 0 | 90 | 0.25 | 90 |
4 | 0 | 90 | 0 | 0 |
5 | 0 | -90 | 0.25 | 90 |
6 | 0 | 90 | 0 | 90 |
7 | 0 | 90 | 0 | -90 |
表中的di表示机械臂的连杆距离;ai-1表示连杆偏距,αi-1为连杆扭角,θi为两连杆夹角。通过建立机械臂的关节参数后便可以进行机械臂的运动规划。
如图3为现有技术中的RRT算法示意图(RRT路径规划算法,rapidlyexploringrandomtree,RRT),其中未加入偏向概率采样算法,采样点随机产生,虽然具有概率完备性,但是大幅降低了RRT路径搜索树的速度。
首先在进行采样前需确定机械臂的随机树搜索初始参数,机械臂的起始位置作为机械臂待搜索空间的起始位置Gstart,机械臂的目标位置作为机械臂搜索空间的目标位置Ggoal,机械臂的搜索步长stepsize,机械臂的最大采样次数maxattempts,采样设定阈值t(0<t<1)。
其次针对图3中基本的RRT算法不足引入目标通过引入偏向概率采样算法对随机产生的随机采样点进行有限干预,具体采用上述本发明公开的仿人机械臂的运动路径规划方法进行机械臂的运动规划。
图4为本发明采用仿人机械臂的运动路径规划方法对扩展随机树扩展过程中进行碰撞检测的示意图。如图4所示,是通过检测随机树上面的最近的节点Pnear与扩展节点Ptmp之间的连接路径是否与障碍物发生碰撞来确定路径可行性。
图5为本发明采用仿人机械臂的运动路径规划方法生成一条无碰撞扩展随机树的示意图,如图5所示,其中的实线表示快速搜索随机树的搜索路径;虚线表示通过加入偏向搜索算法后的快速搜索随机树的规划路径。
图6为本发明采用仿人机械臂的运动路径规划方法最终得到的路径图,其中实线为图5中加入偏向概率采样算法后快速搜索随机树生成的路径;虚线表示通过dijkstra算法对路径进行优化后生成的路径。进一步说明如下:
为验证改进RRT运动规划算法的有效性,通过图6实验结果中的虚线规划路径可以得出算法从起始点到达目标点的过程中大幅减少了采样点,节省了规划时间,同时通过规划路径可以直观的观察到优化后的路径更加的平滑,能够很好的减少机械臂在运动过程中的震动影响。
由于机械臂的运动规划采用的基础是基于随机采样的运动规划方法,所以机械臂在运动规划的路径结果上每次之间的效果是不相同的,为充分展现算法有效性,避免意外实验结果。在试验中共采集了50次运动规划实验的结果,从整体路径规划结果中可以得出基于改进的RRT路径规划算法所规划的路径总体长度较短且路径平滑。基于改进的RRT算法在50次的实验中的路径规划时间中平均规划时间为16.99s,改进后的路径长度为709.91;图6中虚线表示为起始点与终点之间的最短路径和避障后规划的路径图。
此外,本发明还提供了一种仿人机械臂的运动路径规划***。如图7所示,所述***包括:D-H模型建立模块1、初始参数确定模块2、随机树构建模块3、节点扩展模块4、第一检测模块5、扩展随机树生成模块6、第二检测模块7、路径权重获取模块8和关节角信息解算模块9。
其中,D-H模型建立模块1用于采用D-H法描述仿人机械臂的位姿,建立仿人机械臂的连杆坐标系,确定仿人机械臂的D-H参数,并计算得到仿人机械臂的D-H模型;
初始参数确定模块2用于根据所述仿人机械臂的D-H模型,确定初始参数;所述初始参数包括所述仿人机械臂的起始位置Gstart、所述仿人机械臂运动的目标位置Ggoal、搜索步长stepsize和最大采样次数maxattempts;
随机树构建模块3用于以所述仿人机械臂的起始位置Gstart为待搜索空间的起始位置Gstart,以所述仿人机械臂运动的目标位置Ggoal为待搜索空间的目标位置Ggoal,采用快速搜索随机树法根据所述搜索步长stepsize和最大采样次数maxattempts搜索所述待搜索空间的采样点,并将搜索得到的所述采样点作为节点构建随机树;所述待搜索空间为所述仿人机械臂的工作空间;
节点扩展模块4用于采用偏向概率采样算法确定随机采样点Prand;遍历所述随机树上的节点,确定距所述随机采样点Prand最近的节点Pnear;并判断所述随机采样点Prand与最近节点间的距离是否大于搜索步长stepsize;若是,则在距所述随机采样点Prand最近的节点Pnear到所述随机采样点Prand的连线上以所述搜索步长stepsize的扩展节点Ptmp;若否,则将所述随机采样点Prand直接作为扩展节点Ptmp;
第一检测模块5用于在对节点的扩展过程中,检测所扩展的节点是否位于障碍物边界的内部;若是,则删除所扩展的节点,反之则保留所扩展的节点;
扩展随机树生成模块6用于将保留的扩展节点Ptmp形成扩展随机树;所述扩展随机树是以所述待搜索空间的起始位置Gstart为源节点,以所述待搜索空间的目标位置Ggoal为终节点的随机树;
第二检测模块7用于遍历所述扩展随机树上的所有节点,连接所述扩展随机树上的所有节点;判断节点间相互连接的连接线是否与障碍物发生碰撞;若是则删除节点间的连线,若否则保留节点间的连线;
路径权重获取模块8用于根据所保留的节点间的连线,确定仿人机械臂从所述源节点到所述终节点的各路径;获取每一路径中各节点间连线的长度;以所述各节点间连线的长度作为子权重,计算得到每一路径的总权重;
关节角信息解算模块9用于获取总权重最小的路径,将所述总权重最小的路径中所有的节点信息通过逆运动学解算为仿人机械臂运动的关节角信息,并使仿人机械臂按照所述关节角信息进行运动。
其中,所述节点扩展模块4包括:概率值获取单元、采样阈值设定单元和随机采样点Prand确定单元。
概率值获取单元用于采用偏向概率算法和均匀概率分布算法随机获取一个概率值;
采样阈值设定单元用于设定采样阈值;
随机采样点Prand确定单元用于将所述概率值与所设定的采样阈值进行比较,当所述采样阈值大于所述概率值时,将所述待搜索空间的目标位置Ggoal确定为随机采样点Prand,当所述采样阈值不大于所述概率值时,采用随机采样策略确定所述随机采样点Prand。
所述***还包括:扩展判断模块10。所述扩展判断模块10用于判断所扩展的节点是否位于所述扩展随机树的终节点的设定邻域范围内;若是,则停止扩展节点Ptmp;若否,则继续扩展节点Ptmp。
所述关节角信息解算模块9包括:最小路径确定单元。所述最小路径确定单元用于采用Dijkstra算法确定各路径中各连线长度的总权重最小的路径。
根据本发明提供的具体实施例,本发明公开了以下技术效果:本发明提供的仿人机械臂的运动路径规划方法及***,在寻找仿人机械臂的最优运动路径的过程中,加入具有偏向概率采样算法的搜索方法,这就能够在保证采样完备性的同时,加快采样点向目标点搜索的速度,节约搜索时间。
此外,除上述技术效果外,与现有技术相比,本发明的有益效果还包括:
1、通过使用随机采样的方式对空间进行扩展,避免了对机械臂工作环境的精确建模,能够有效的减少建模所需的工作。
2、基于改进的RRT算法在引入基于偏向概率采样算法后,可以既保证RRT算法的完备性,同时也大幅缩减了运动规划所需的时间。
3、基于改进的RRT算法通过引入遍历路径比较分类的方式,有效的减少了RRT所规划路径的长度。
对于实施例公开的***而言,由于其与实施例公开的方法相对应,所以描述的比较简单,相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述,以上实施例的说明只是用于帮助理解本发明的方法及其核心思想;同时,对于本领域的一般技术人员,依据本发明的思想,在具体实施方式及应用范围上均会有改变之处。综上所述,本说明书内容不应理解为对本发明的限制。
Claims (6)
1.一种仿人机械臂的运动路径规划方法,其特征在于,包括:
采用D-H法描述仿人机械臂的位姿,建立仿人机械臂的连杆坐标系,确定仿人机械臂的D-H参数,并计算得到仿人机械臂的D-H模型;
根据所述仿人机械臂的D-H模型,确定初始参数;所述初始参数包括所述仿人机械臂的起始位置、所述仿人机械臂运动的目标位置、搜索步长和最大采样次数;
以所述仿人机械臂的起始位置为待搜索空间的起始位置,以所述仿人机械臂运动的目标位置为待搜索空间的目标位置,采用快速搜索随机树法根据所述搜索步长和最大采样次数搜索所述待搜索空间的采样点,并将搜索得到的所述采样点作为节点构建随机树;所述待搜索空间为所述仿人机械臂的工作空间;
采用偏向概率采样算法确定随机采样点;遍历所述随机树上的节点,确定距所述随机采样点最近的节点;并判断所述随机采样点与最近节点间的距离是否大于搜索步长;若是,则在距所述随机采样点最近的节点到所述随机采样点的连线上以所述搜索步长的长度扩展节点;若否,则将所述随机采样点直接作为扩展节点;其中,采用公式扩展节点;式中,Pnear为采样点最近的节点,Prand为随机采样点,stepsize为搜索步长,Ptmp为扩展节点;
采用偏向概率采样算法确定随机采样点,包括:
采用偏向概率算法和均匀概率分布算法随机获取概率值;
设定采样阈值;
将所述概率值与所设定的采样阈值进行比较,当所述采样阈值大于所述概率值时,将所述待搜索空间的目标位置确定为随机采样点,当所述采样阈值不大于所述概率值时,采用随机采样策略确定所述随机采样点;
在对节点的扩展过程中,检测所扩展的节点是否位于障碍物边界的内部;若是,则删除所扩展的节点,反之则保留所扩展的节点;
将保留的扩展节点形成扩展随机树;所述扩展随机树是以所述待搜索空间的起始位置为源节点,以所述待搜索空间的目标位置为终节点的随机树;
遍历所述扩展随机树上的所有节点,连接所述扩展随机树上的所有节点;判断节点间相互连接的连接线是否与障碍物发生碰撞;若是则删除节点间的连线,若否则保留节点间的连线;
根据所保留的节点间的连线,确定仿人机械臂从所述源节点到所述终节点的各路径;获取每一路径中各节点间连线的长度;以所述各节点间连线的长度作为子权重,计算得到每一路径的总权重;
获取总权重最小的路径,将所述总权重最小的路径中所有的节点信息通过逆运动学解算为仿人机械臂运动的关节角信息,并使仿人机械臂按照所述关节角信息进行运动;其中,扩展方向夹角公式如下:
α=atan2(Pnear(1)-Ptemp(1),Pnear(2)-Ptemp(2))
式中,Pnear(1)为扩展随机数上距离扩展节点Ptmp最近的节点Pnear的横坐标,Pnear(2)为扩展随机数上距离扩展节点Ptmp最近的节点Pnear的纵坐标,Ptemp(1)为扩展节点Ptmp的横坐标,Ptemp(2)为扩展节点Ptmp的纵坐标。
2.根据权利要求1所述的一种仿人机械臂的运动路径规划方法,其特征在于,所述在对节点的扩展过程中,检测所扩展的节点是否位于障碍物边界的内部;若是,则删除所扩展的节点,反之则保留所扩展的节点之前还包括:
判断所扩展的节点是否位于所述扩展随机树的终节点的阈值范围内;若是,则停止扩展节点;若否,则继续扩展节点。
3.根据权利要求1所述的一种仿人机械臂的运动路径规划方法,其特征在于,采用Dijkstra算法确定各路径中各连线长度的总权重最小的路径。
4.一种仿人机械臂的运动路径规划***,其特征在于,包括:
D-H模型建立模块,用于采用D-H法描述仿人机械臂的位姿,建立仿人机械臂的连杆坐标系,确定仿人机械臂的D-H参数,并计算得到仿人机械臂的D-H模型;
初始参数确定模块,用于根据所述仿人机械臂的D-H模型,确定初始参数;所述初始参数包括所述仿人机械臂的起始位置、所述仿人机械臂运动的目标位置、搜索步长和最大采样次数;
随机树构建模块,用于以所述仿人机械臂的起始位置为待搜索空间的起始位置,以所述仿人机械臂运动的目标位置为待搜索空间的目标位置,采用快速搜索随机树法根据所述搜索步长和最大采样次数搜索所述待搜索空间的采样点,并将搜索得到的所述采样点作为节点构建随机树;所述待搜索空间为所述仿人机械臂的工作空间;
节点扩展模块,用于采用偏向概率采样算法确定随机采样点;遍历所述随机树上的节点,确定距所述随机采样点最近的节点;并判断所述随机采样点与最近节点间的距离是否大于搜索步长;若是,则在距所述随机采样点最近的节点到所述随机采样点的连线上以所述搜索步长的长度扩展节点;若否,则将所述随机采样点直接作为扩展节点;其中,采用公式扩展节点;式中,Pnear为采样点最近的节点,Prand为随机采样点,stepsize为搜索步长,Ptmp为扩展节点;
所述节点扩展模块包括:
概率值获取单元,用于采用偏向概率算法和均匀概率分布算法随机获取一个概率值;
采样阈值设定单元,用于设定采样阈值;
随机采样点确定单元,用于将所述概率值与所设定的采样阈值进行比较,当所述采样阈值大于所述概率值时,将所述待搜索空间的目标位置确定为随机采样点,当所述采样阈值不大于所述概率值时,采用随机采样策略确定所述随机采样点;
第一检测模块,用于在对节点的扩展过程中,检测所扩展的节点是否位于障碍物边界的内部;若是,则删除所扩展的节点,反之则保留所扩展的节点;
扩展随机树生成模块,用于将保留的扩展节点形成扩展随机树;所述扩展随机树是以所述待搜索空间的起始位置为源节点,以所述待搜索空间的目标位置为终节点的随机树;
第二检测模块,用于遍历所述扩展随机树上的所有节点,连接所述扩展随机树上的所有节点;判断节点间相互连接的连接线是否与障碍物发生碰撞;若是则删除节点间的连线,若否则保留节点间的连线;
路径权重获取模块,用于根据所保留的节点间的连线,确定仿人机械臂从所述源节点到所述终节点的各路径;获取每一路径中各节点间连线的长度;以所述各节点间连线的长度作为子权重,计算得到每一路径的总权重;
关节角信息解算模块,用于获取总权重最小的路径,将所述总权重最小的路径中所有的节点信息通过逆运动学解算为仿人机械臂运动的关节角信息,并使仿人机械臂按照所述关节角信息进行运动;其中,扩展方向夹角公式如下:
α=atan2(Pnear(1)-Ptemp(1),Pnear(2)-Ptemp(2))
式中,Pnear(1)为扩展随机数上距离扩展节点Ptmp最近的节点Pnear的横坐标,Pnear(2)为扩展随机数上距离扩展节点Ptmp最近的节点Pnear的纵坐标,Ptemp(1)为扩展节点Ptmp的横坐标,Ptemp(2)为扩展节点Ptmp的纵坐标。
5.根据权利要求4所述的一种仿人机械臂的运动路径规划***,其特征在于,所述***还包括:
扩展判断模块,用于判断所扩展的节点是否位于所述扩展随机树的终节点的设定邻域范围内;若是,则停止扩展节点;若否,则继续扩展节点。
6.根据权利要求4所述的一种仿人机械臂的运动路径规划***,其特征在于,所述关节角信息解算模块包括:最小路径确定单元,用于采用Dijkstra算法确定各路径中各连线长度的总权重最小的路径。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910843685.9A CN110509279B (zh) | 2019-09-06 | 2019-09-06 | 一种仿人机械臂的运动路径规划方法及*** |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910843685.9A CN110509279B (zh) | 2019-09-06 | 2019-09-06 | 一种仿人机械臂的运动路径规划方法及*** |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110509279A CN110509279A (zh) | 2019-11-29 |
CN110509279B true CN110509279B (zh) | 2020-12-08 |
Family
ID=68631541
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910843685.9A Active CN110509279B (zh) | 2019-09-06 | 2019-09-06 | 一种仿人机械臂的运动路径规划方法及*** |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110509279B (zh) |
Families Citing this family (16)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110986953B (zh) * | 2019-12-13 | 2022-12-06 | 达闼机器人股份有限公司 | 路径规划方法、机器人及计算机可读存储介质 |
CN111015656A (zh) * | 2019-12-19 | 2020-04-17 | 佛山科学技术学院 | 一种机器人主动避障的控制方法、装置及存储介质 |
CN111230875B (zh) * | 2020-02-06 | 2023-05-12 | 北京凡川智能机器人科技有限公司 | 一种基于深度学习的双臂机器人仿人作业规划方法 |
CN111283686B (zh) * | 2020-03-05 | 2021-11-19 | 亿嘉和科技股份有限公司 | 一种用于带电作业机器人抓取支线场景下的抓取位姿计算方法 |
CN111251306B (zh) * | 2020-03-18 | 2022-11-29 | 广东省智能机器人研究院 | 一种带有底盘误差的机械臂路径规划方法 |
CN111546347B (zh) * | 2020-06-03 | 2021-09-03 | 中国人民解放军海军工程大学 | 一种适用于动态环境下的机械臂路径规划方法 |
CN112711256B (zh) * | 2020-12-23 | 2022-05-20 | 杭州电子科技大学 | 一种移动机器人的目标搜索观测点自动生成方法 |
CN113119115A (zh) * | 2021-03-22 | 2021-07-16 | 深圳市优必选科技股份有限公司 | 一种机械臂运动规划方法、装置、可读存储介质及机械臂 |
CN113341984A (zh) * | 2021-06-15 | 2021-09-03 | 桂林电子科技大学 | 基于改进rrt算法的机器人路径规划方法和装置 |
CN113359746A (zh) * | 2021-06-21 | 2021-09-07 | 桂林电子科技大学 | 基于改进双向RRT和Dijkstra融合算法的路径规划方法和装置 |
CN113618276B (zh) * | 2021-07-27 | 2022-04-26 | 华南理工大学 | 基于分层搜索树实现工件自动布置的变位机路径规划方法 |
CN114700937B (zh) * | 2022-01-13 | 2024-02-13 | 深圳市越疆科技有限公司 | 机械臂及其运动路径规划方法、控制***、介质及机器人 |
CN116652927A (zh) * | 2022-02-21 | 2023-08-29 | 中兴通讯股份有限公司 | 路径规划方法及装置、可读存储介质 |
CN114986524B (zh) * | 2022-08-08 | 2022-10-28 | 中国科学院自动化研究所 | 基于脑机接口的机械臂轨迹规划方法、装置以及电子设备 |
CN115723125B (zh) * | 2022-10-31 | 2024-06-21 | 北京工业大学 | 一种机械臂重复操作运动规划方法 |
CN115723129B (zh) * | 2022-11-17 | 2024-06-21 | 北京工业大学 | 一种机械臂连续操作运动规划方法 |
Family Cites Families (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR101667031B1 (ko) * | 2009-11-02 | 2016-10-17 | 삼성전자 주식회사 | 로봇의 경로 계획 장치 및 그 방법 |
JP6606442B2 (ja) * | 2016-02-24 | 2019-11-13 | 本田技研工業株式会社 | 移動体の経路計画生成装置 |
CN106737671B (zh) * | 2016-12-21 | 2019-04-19 | 西安科技大学 | 七自由度仿人机械臂的双层拟人运动规划方法 |
CN107943053A (zh) * | 2017-12-15 | 2018-04-20 | 陕西理工大学 | 一种移动机器人的路径规划方法 |
CN108444489A (zh) * | 2018-03-07 | 2018-08-24 | 北京工业大学 | 一种改进rrt算法的路径规划方法 |
CN109877836B (zh) * | 2019-03-13 | 2021-06-08 | 浙江大华技术股份有限公司 | 路径规划方法、装置、机械臂控制器和可读存储介质 |
-
2019
- 2019-09-06 CN CN201910843685.9A patent/CN110509279B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN110509279A (zh) | 2019-11-29 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110509279B (zh) | 一种仿人机械臂的运动路径规划方法及*** | |
CN111546347B (zh) | 一种适用于动态环境下的机械臂路径规划方法 | |
CN113219998B (zh) | 一种基于改进双向informed-RRT*的车辆路径规划方法 | |
CN109542117B (zh) | 基于改进rrt的水下航行器滚动规划算法 | |
CN109579854B (zh) | 基于快速扩展随机树的无人车避障方法 | |
CN112223291B (zh) | 一种基于三维任务空间约束的机械臂避障方法及装置 | |
CN113799120B (zh) | 冗余自由度机械臂的路径规划方法、装置及工程机械 | |
CN111251306B (zh) | 一种带有底盘误差的机械臂路径规划方法 | |
CN112683278B (zh) | 一种基于改进a*算法和贝塞尔曲线的全局路径规划方法 | |
CN112809665B (zh) | 一种基于改进rrt算法的机械臂运动规划方法 | |
CN112549016A (zh) | 一种机械臂运动规划方法 | |
CN110275528B (zh) | 针对rrt算法改进的路径优化方法 | |
CN113467476B (zh) | 考虑转角约束的无碰撞检测快速随机树全局路径规划方法 | |
CN114115271A (zh) | 一种改进rrt的机器人路径规划方法和*** | |
CN115958590A (zh) | 一种基于rrt的机械臂深框避障运动规划方法及装置 | |
Wang et al. | Dual-objective collision-free path optimization of arc welding robot | |
CN114939872B (zh) | 基于MIRRT*-Connect算法的智能仓储冗余机械臂动态避障运动规划方法 | |
CN113858210A (zh) | 基于混合算法的机械臂路径规划方法 | |
CN113771035B (zh) | 基于rrt*算法的冗余多自由度机械臂避障路径优化方法 | |
CN117124335B (zh) | 一种基于路径标记回溯策略的改进式rrt路径规划方法 | |
CN112828883B (zh) | 一种未知环境下的机器人环境探索方法及*** | |
CN113721622A (zh) | 一种机器人路径规划方法 | |
CN116698066A (zh) | 基于邻域扩展和边界点改进A-star算法的机器人路径规划方法及*** | |
CN115533912A (zh) | 一种基于快速拓展随机树改进的机械臂运动规划方法 | |
CN115922716A (zh) | 一种融合工艺知识的双向RRT-connect算法实现了工业机器人快速路径规划 |
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 |