[发明专利]一种仿人机械臂的运动路径规划方法及系统

权利要求书

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

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

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

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

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

采用偏向概率采样算法确定随机采样点，包括：

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

设定采样阈值；

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

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

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

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

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

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

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

式中，P_near(1)为扩展随机数上距离扩展节点P_tmp最近的节点P_near的横坐标，P_near(2)为扩展随机数上距离扩展节点P_tmp最近的节点P_near的纵坐标，P_temp(1)为扩展节点P_tmp的横坐标，P_temp(2)为扩展节点P_tmp的纵坐标。

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

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

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