[发明专利]机械臂避障路径规划方法、系统及存储介质

权利要求书

1.一种机械臂避障路径规划方法，其特征在于，包括以下步骤：

获取机械臂的初始参数；其中，所述初始参数包括所述机械臂的执行末端的初始位置；

根据所述初始参数和目标位置计算出所述执行末端的第一运动方向；

根据所述初始位置、预设步进长度和所述第一运动方向计算出所述执行末端的当前位置；

根据预设的障碍物模型、机械臂模型和所述当前位置确定所述机械臂的当前碰撞状态；其中，所述当前碰撞状态包括发生碰撞状态和未发生碰撞状态；

确定所述当前位置所处的当前区域；其中，所述当前区域包括安全区域；

若所述当前区域为所述安全区域且所述碰撞状态为未发生碰撞状态，则将预设步进长度与预设步进增益因子的乘积作为预设步进长度，将所述当前位置作为所述初始位置，并执行所述根据所述初始位置、预设步进长度和所述第一运动方向计算出所述执行末端的当前位置；

确定所述当前位置为目标位置，将得到的所述执行末端的轨迹作为规划路径；

还包括构建所述机械臂模型，所述机械臂模型包括连杆模型和关节模型，具体包括：

在三维空间将所述机械臂的连杆采用圆柱体包络法进行建模，形成圆柱体区域，得到连杆模型；

在空间将所述机械臂的关节采用球体包络法进行建模，形成球体区域，得到关节模型；

所述根据预设的障碍物模型、机械臂模型和所述当前位置确定所述机械臂的当前碰撞状态，包括：

根据预设的机器人模型获取所述机械臂的多个连杆之间的第一关系；

建立所述机械臂的运动学方程，并根据所述当前位置分别得到所述运动学方程的多个逆解，所述逆解为所述关节的角度；

根据所述当前位置、所述第一关系和多个所述关节的角度，得到所述连杆的位置和所述关节的位置；

根据所述连杆的位置、所述连杆模型、所述关节的位置、所述关节模型和所述障碍物模型，确定所述机械臂与所述障碍物的当前碰撞状态。

2.根据权利要求1所述的机械臂避障路径规划方法，其特征在于，所述当前区域还包括威胁区域，还包括以下步骤：

若所述当前区域为所述威胁区域且所述碰撞状态为未发生碰撞状态，则建立所述机械臂的执行末端在所述威胁区域的惩罚势函数和第二引力势函数，所述惩罚势函数与所述第二引力势函数之和为所述机械臂的执行末端在所述威胁区域的第一总势场强度；

采用模拟退火法搜索势场强度小于所述第一总势场强度的第一位置；

若所述第一位置处的惩罚势函数对应的值为0，则所述第一位置位于所述安全区域。

3.根据权利要求2所述的机械臂避障路径规划方法，其特征在于，还包括：

若所述第一位置处的惩罚势函数对应的值大于0，则将所述第一位置处的势场强度作为第一总势场强度，并将所述第一位置作为当前位置，执行所述采用模拟退火法搜索势场强度小于所述第一总势场强度的第一位置。

4.根据权利要求1所述的机械臂避障路径规划方法，其特征在于，还包括构建所述障碍物模型，具体包括：

在三维空间采用球体包络法对所述障碍物进行建模，并向外径向扩展第一距离，形成一个球体的范围，得到障碍物模型；

将三维空间区域划分为安全区域和威胁区域，所述威胁区域为所述球体包围的区域，所述安全区域为所述球体外部的区域。

5.根据权利要求1所述的机械臂避障路径规划方法，其特征在于，还包括：若所述碰撞状态为发生碰撞状态，则将所述当前位置的预设区域内的任意一位置作为目标位置，并执行所述根据所述初始参数和目标位置计算出所述执行末端的第一运动方向。

6.根据权利要求1至5任一项所述的机械臂避障路径规划方法，其特征在于，还包括：

采用最小二乘法对所述规划路径进行拟合，用于得到平滑的规划路径。

7.一种机械臂避障路径规划系统，其特征在于，包括：至少一个存储器、至少一个处理器及至少一个程序指令，所述程序指令存储在所述存储器上并可在所述处理器上运行，所述处理器用于执行权利要求1至6中任一项所述的机械臂避障路径规划方法。

8.一种存储介质，其特征在于：所述存储介质上存储有程序指令，所述程序指令用于执行权利要求1至6中任一项所述的机械臂避障路径规划方法。