CN110509279A - 一种仿人机械臂的运动路径规划方法及系统 - Google Patents

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、采用偏向概率采样算法确定随机采样点P_rand；遍历所述随机树上的节点，确定距所述随机采样点P_rand最近的节点P_near；并判断所述随机采样点P_rand与最近节点P_near间的距离是否大于搜索步长stepsize；若是，则在距所述随机采样点P_rand最近的节点P_near到所述随机采样点P_rand的连线上以所述搜索步长stepsize的长度扩展节点P_tmp；若否，则将所述随机采样点P_rand直接作为扩展节点P_tmp；具体通过以下公式进行扩展：

S104、在对节点的扩展过程中，检测所扩展的节点是否位于障碍物边界的内部；若是，则删除所扩展的节点，反之则保留所扩展的节点；

S105、将保留的扩展节点P_tmp形成扩展随机树；所述扩展随机树是以所述待搜索空间的起始位置Gstart为源节点，以所述待搜索空间的目标位置Ggoal为终节点的随机树；

S106、遍历所述扩展随机树上的所有节点，连接所述扩展随机树上的所有节点；判断节点间相互连接的连接线是否与障碍物发生碰撞；若是则删除节点间的连线，若否则保留节点间的连线；

S107、根据所保留的节点间的连线，确定仿人机械臂从所述源节点到所述终节点的各路径；获取每一路径中各节点间连线的长度；以所述各节点间连线的长度作为子权重，计算得到每一路径的总权重；

S108、获取总权重最小的路径，将所述总权重最小的路径中所有的节点信息通过逆运动学解算为仿人机械臂运动的关节角信息，并使仿人机械臂按照所述关节角信息进行运动。其中扩展方向夹角公式如下：

α＝atan2(P_near(1)-P_temp(1)，P_near(2)-P_temp(2))

其中P_near(1)与P_near(2)分别表示扩展随机数上距离扩展节点P_tem最近的节点P_near的横坐标与纵坐标；P_tem(1)与P_tem(2)分别表示扩展节点P_tem的横坐标和纵坐标。

在S103中采用偏向概率采样算法确定随机采样点P_rand，具体包括：

采用偏向概率算法和均匀概率分布算法随机获取概率值P；

设定采样阈值t(0<t<1)；

将所述概率值P与所设定的采样阈值t进行比较，当所述采样阈值t大于所述概率值P时，将所述待搜索空间的起始位置Gstart确定为随机采样点P_rand，当所述采样阈值t不大于所述概率值P时，采用随机采样策略确定所述随机采样点P_rand。

在S104中对节点的扩展过程中，还包括：计算最近节点P_near与扩展节点P_tmp之间的方向向量，在此方向上以预先设定的扩展步长不断循环扩展，直到扩展距离与最近节点P_near到扩展节点P_tmp的距离相等。且在上述这一过程中在此过程中不断检测扩展点的坐标值是否存在于障碍物区域边界的内部。如果不存在碰撞则保留扩展点，否则删除此扩展点。

在S104之前还包括：

判断所扩展的节点是否位于所述扩展随机树的终节点的设定邻域范围内；若是，则停止扩展节点P_tmp；若否，则继续扩展节点P_tmp。

在S108中是采用Dijkstra算法确定各路径中各连线长度的总权重最小的路径。

在S104和S106中，在进行碰撞检测的过程中，具体进行快速检测碰撞公式如下：

Pathcheck＝P_n+r*[sin(α)cos(α)]

其中α表示两点之间的方向角；P_n表示快速搜索随机树上面的第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法建立机械臂坐标系，在考虑机械臂的运动学约束的情况下建立机械臂的运动学模型。图中坐标系(X₀,Y₀,Z₀)为基坐标系，坐标系(X_i,Y_i,Z_i)分别表示每个机械臂关节的坐标系。每个机械臂关节的运动方向围绕坐标系的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

表中的d_i表示机械臂的连杆距离；a_i-1表示连杆偏距，α_i-1为连杆扭角，θ_i为两连杆夹角。通过建立机械臂的关节参数后便可以进行机械臂的运动规划。

如图3为现有技术中的RRT算法示意图(RRT路径规划算法，rapidly exploringrandom tree，RRT)，其中未加入偏向概率采样算法，采样点随机产生，虽然具有概率完备性，但是大幅降低了RRT路径搜索树的速度。

首先在进行采样前需确定机械臂的随机树搜索初始参数，机械臂的起始位置作为机械臂待搜索空间的起始位置Gstart，机械臂的目标位置作为机械臂搜索空间的目标位置Ggoal，机械臂的搜索步长stepsize，机械臂的最大采样次数maxattempts，采样设定阈值t(0<t<1)。

其次针对图3中基本的RRT算法不足引入目标通过引入偏向概率采样算法对随机产生的随机采样点进行有限干预，具体采用上述本发明公开的仿人机械臂的运动路径规划方法进行机械臂的运动规划。

图4为本发明采用仿人机械臂的运动路径规划方法对扩展随机树扩展过程中进行碰撞检测的示意图。如图4所示，是通过检测随机树上面的最近的节点P_near与扩展节点P_tmp之间的连接路径是否与障碍物发生碰撞来确定路径可行性。

图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用于采用偏向概率采样算法确定随机采样点P_rand；遍历所述随机树上的节点，确定距所述随机采样点P_rand最近的节点P_near；并判断所述随机采样点P_rand与最近节点间的距离是否大于搜索步长stepsize；若是，则在距所述随机采样点P_rand最近的节点P_near到所述随机采样点P_rand的连线上以所述搜索步长stepsize的长度扩展节点P_tmp；若否，则将所述随机采样点P_rand直接作为扩展节点P_tmp；

第一检测模块5用于在对节点的扩展过程中，检测所扩展的节点是否位于障碍物边界的内部；若是，则删除所扩展的节点，反之则保留所扩展的节点；

扩展随机树生成模块6用于将保留的扩展节点P_tmp形成扩展随机树；所述扩展随机树是以所述待搜索空间的起始位置Gstart为源节点，以所述待搜索空间的目标位置Ggoal为终节点的随机树；

第二检测模块7用于遍历所述扩展随机树上的所有节点，连接所述扩展随机树上的所有节点；判断节点间相互连接的连接线是否与障碍物发生碰撞；若是则删除节点间的连线，若否则保留节点间的连线；

路径权重获取模块8用于根据所保留的节点间的连线，确定仿人机械臂从所述源节点到所述终节点的各路径；获取每一路径中各节点间连线的长度；以所述各节点间连线的长度作为子权重，计算得到每一路径的总权重；

关节角信息解算模块9用于获取总权重最小的路径，将所述总权重最小的路径中所有的节点信息通过逆运动学解算为仿人机械臂运动的关节角信息，并使仿人机械臂按照所述关节角信息进行运动。

其中，所述节点扩展模块4包括：概率值获取单元、采样阈值设定单元和随机采样点P_rand确定单元。

概率值获取单元用于采用偏向概率算法和均匀概率分布算法随机获取一个概率值；

采样阈值设定单元用于设定采样阈值；

随机采样点Prand确定单元用于将所述概率值与所设定的采样阈值进行比较，当所述采样阈值大于所述概率值时，将所述待搜索空间的目标位置Ggoal确定为随机采样点Prand，当所述采样阈值不大于所述概率值时，采用随机采样策略确定所述随机采样点P_rand。

所述系统还包括：扩展判断模块10。所述扩展判断模块10用于判断所扩展的节点是否位于所述扩展随机树的终节点的设定邻域范围内；若是，则停止扩展节点P_tmp；若否，则继续扩展节点P_tmp。

所述关节角信息解算模块9包括：最小路径确定单元。所述最小路径确定单元用于采用Dijkstra算法确定各路径中各连线长度的总权重最小的路径。

根据本发明提供的具体实施例，本发明公开了以下技术效果：本发明提供的仿人机械臂的运动路径规划方法及系统，在寻找仿人机械臂的最优运动路径的过程中，加入具有偏向概率采样算法的搜索方法，这就能够在保证采样完备性的同时，加快采样点向目标点搜索的速度，节约搜索时间。

此外，除上述技术效果外，与现有技术相比，本发明的有益效果还包括：

1、通过使用随机采样的方式对空间进行扩展，避免了对机械臂工作环境的精确建模，能够有效的减少建模所需的工作。

2、基于改进的RRT算法在引入基于偏向概率采样算法后，可以既保证RRT算法的完备性，同时也大幅缩减了运动规划所需的时间。

3、基于改进的RRT算法通过引入遍历路径比较分类的方式，有效的减少了RRT所规划路径的长度。

对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (8)

1.一种仿人机械臂的运动路径规划方法，其特征在于，包括：

采用D-H法描述仿人机械臂的位姿，建立仿人机械臂的连杆坐标系，确定仿人机械臂的D-H参数，并计算得到仿人机械臂的D-H模型；

根据所述仿人机械臂的D-H模型，确定初始参数；所述初始参数包括所述仿人机械臂的起始位置、所述仿人机械臂运动的目标位置、搜索步长和最大采样次数；

以所述仿人机械臂的起始位置为待搜索空间的起始位置，以所述仿人机械臂运动的目标位置为待搜索空间的目标位置，采用快速搜索随机树法根据所述搜索步长和最大采样次数搜索所述待搜索空间的采样点，并将搜索得到的所述采样点作为节点构建随机树；所述待搜索空间为所述仿人机械臂的工作空间；

采用偏向概率采样算法确定随机采样点；遍历所述随机树上的节点，确定距所述随机采样点最近的节点；并判断所述随机采样点与最近节点间的距离是否大于搜索步长；若是，则在距所述随机采样点最近的节点到所述随机采样点的连线上以所述搜索步长的长度扩展节点；若否，则将所述随机采样点直接作为扩展节点；

在对节点的扩展过程中，检测所扩展的节点是否位于障碍物边界的内部；若是，则删除所扩展的节点，反之则保留所扩展的节点；

将保留的扩展节点形成扩展随机树；所述扩展随机树是以所述待搜索空间的起始位置为源节点，以所述待搜索空间的目标位置为终节点的随机树；

遍历所述扩展随机树上的所有节点，连接所述扩展随机树上的所有节点；判断节点间相互连接的连接线是否与障碍物发生碰撞；若是则删除节点间的连线，若否则保留节点间的连线；

根据所保留的节点间的连线，确定仿人机械臂从所述源节点到所述终节点的各路径；获取每一路径中各节点间连线的长度；以所述各节点间连线的长度作为子权重，计算得到每一路径的总权重；

获取总权重最小的路径，将所述总权重最小的路径中所有的节点信息通过逆运动学解算为仿人机械臂运动的关节角信息，并使仿人机械臂按照所述关节角信息进行运动。

2.根据权利要求1所述的一种仿人机械臂的运动路径规划方法，其特征在于，所述采用偏向概率采样算法确定随机采样点，包括：

采用偏向概率算法和均匀概率分布算法随机获取概率值；

设定采样阈值；

将所述概率值与所设定的采样阈值进行比较，当所述采样阈值大于所述概率值时，将所述待搜索空间的目标位置确定为随机采样点，当所述采样阈值不大于所述概率值时，采用随机采样策略确定所述随机采样点。

3.根据权利要求1所述的一种仿人机械臂的运动路径规划方法，其特征在于，所述在对节点的扩展过程中，检测所扩展的节点是否位于障碍物边界的内部；若是，则删除所扩展的节点，反之则保留所扩展的节点之前还包括：

判断所扩展的节点是否位于所述扩展随机树的终节点的阈值范围内；若是，则停止扩展节点；若否，则继续扩展节点。

4.根据权利要求1所述的一种仿人机械臂的运动路径规划方法，其特征在于，采用Dijkstra算法确定各路径中各连线长度的总权重最小的路径。

5.一种仿人机械臂的运动路径规划系统，其特征在于，包括：

D-H模型建立模块，用于采用D-H法描述仿人机械臂的位姿，建立仿人机械臂的连杆坐标系，确定仿人机械臂的D-H参数，并计算得到仿人机械臂的D-H模型；

初始参数确定模块，用于根据所述仿人机械臂的D-H模型，确定初始参数；所述初始参数包括所述仿人机械臂的起始位置、所述仿人机械臂运动的目标位置、搜索步长和最大采样次数；

随机树构建模块，用于以所述仿人机械臂的起始位置为待搜索空间的起始位置，以所述仿人机械臂运动的目标位置为待搜索空间的目标位置，采用快速搜索随机树法根据所述搜索步长和最大采样次数搜索所述待搜索空间的采样点，并将搜索得到的所述采样点作为节点构建随机树；所述待搜索空间为所述仿人机械臂的工作空间；

节点扩展模块，用于采用偏向概率采样算法确定随机采样点；遍历所述随机树上的节点，确定距所述随机采样点最近的节点；并判断所述随机采样点与最近节点间的距离是否大于搜索步长；若是，则在距所述随机采样点最近的节点到所述随机采样点的连线上以所述搜索步长的长度扩展节点；若否，则将所述随机采样点直接作为扩展节点；

第一检测模块，用于在对节点的扩展过程中，检测所扩展的节点是否位于障碍物边界的内部；若是，则删除所扩展的节点，反之则保留所扩展的节点；

扩展随机树生成模块，用于将保留的扩展节点形成扩展随机树；所述扩展随机树是以所述待搜索空间的起始位置为源节点，以所述待搜索空间的目标位置为终节点的随机树；

第二检测模块，用于遍历所述扩展随机树上的所有节点，连接所述扩展随机树上的所有节点；判断节点间相互连接的连接线是否与障碍物发生碰撞；若是则删除节点间的连线，若否则保留节点间的连线；

路径权重获取模块，用于根据所保留的节点间的连线，确定仿人机械臂从所述源节点到所述终节点的各路径；获取每一路径中各节点间连线的长度；以所述各节点间连线的长度作为子权重，计算得到每一路径的总权重；

关节角信息解算模块，用于获取总权重最小的路径，将所述总权重最小的路径中所有的节点信息通过逆运动学解算为仿人机械臂运动的关节角信息，并使仿人机械臂按照所述关节角信息进行运动。

6.根据权利要求5所述的一种仿人机械臂的运动路径规划系统，其特征在于，所述节点扩展模块包括：

概率值获取单元，用于采用偏向概率算法和均匀概率分布算法随机获取一个概率值；

采样阈值设定单元，用于设定采样阈值；

随机采样点确定单元，用于将所述概率值与所设定的采样阈值进行比较，当所述采样阈值大于所述概率值时，将所述待搜索空间的目标位置确定为随机采样点，当所述采样阈值不大于所述概率值时，采用随机采样策略确定所述随机采样点。

7.根据权利要求5所述的一种仿人机械臂的运动路径规划系统，其特征在于，所述系统还包括：

扩展判断模块，用于判断所扩展的节点是否位于所述扩展随机树的终节点的设定邻域范围内；若是，则停止扩展节点；若否，则继续扩展节点。

8.根据权利要求5所述的一种仿人机械臂的运动路径规划系统，其特征在于，所述关节角信息解算模块包括：最小路径确定单元，用于采用Dijkstra算法确定各路径中各连线长度的总权重最小的路径。