[发明专利]六轴机械臂避障控制方法、系统及计算机可读存储介质

权利要求书

1.一种六轴机械臂避障控制方法，其特征在于，所述方法包括以下步骤：

确定机械臂上的关键点集；其中，所述关键点集包括多个关键点，以各个关键点为球心的球均能满足在机械臂处于任意位姿时刚好覆盖整个机械臂；

控制机械臂朝目标运行，实时获取机械臂所在工作空间的点云数据，根据所述点云数据确定障碍物点云；

根据所述关键点集和障碍物点云实时确定机械臂和障碍物的最短距离，所述最短距离为最近关键点和最近障碍物点的距离；其中，所述最近关键点为所述关键点集中的关键点，所述最近障碍物点为障碍物点云中的障碍物点；

当所述最短距离小于设定的安全距离时，控制机械臂在朝目标运行的过程中实时避障；

其中，所述控制机械臂朝目标运行，包括：

实时确定机械臂末端与目标点的距离和位置向量，根据机械臂末端与目标点的距离和位置向量确定吸引速度；其中，所述目标点为控制机械臂到达的目标位置；

将吸引速度定义为第一笛卡尔速度，根据第一笛卡尔速度机械臂末端以及机械臂按全局路径朝目标运行；所述全局路径根据对机械臂末端以及机械臂进行全局路径规划得到；

所述实时确定机械臂末端与目标点的距离和位置向量，根据机械臂末端与目标点的距离和位置向量确定吸引速度，包括：

根据以下公式确定所述机械臂末端与目标点的距离：

其中，(P_endx,P_endy,P_endz)为机械臂末端在机械臂的基座坐标系下的坐标，(P_goalx,P_goaly,P_goalz)为目标点在机械臂的基座坐标系下的坐标，D_eg为机械臂末端与目标点的距离；

确定机械臂末端和目标点在机械臂的基座坐标系下的位置向量，并根据以下公式确定吸引速度：

其中，X(P_end,P_goal)为机械臂末端和目标点的位置向量，V_att为吸引速度，k_att1、k_att2为控制因子,D_a为设定的阈值；

其中，所述当所述最短距离小于设定的安全距离时，控制机械臂在朝目标运行的过程中实时避障，包括：

当确定机械臂和目标点之间存在障碍物时，实时确定机械臂和障碍物的最短距离；

若所述最短距离小于设定的安全距离，则将吸引速度降为零，并根据机械臂上的关键点和最近障碍物点确定排斥速度；

将排斥速度定义为第二笛卡尔速度，根据第二笛卡尔速度控制机械臂的最近关键点按局部路径朝目标运行；所述局部路径根据对最近关键点进行局部路径规划得到；

所述排斥速度的计算公式为：

其中，X(P_key,P_min)为机械臂上的关键点与最近障碍物点的位置向量，2-范数||X(P_key,P_min)||₂为位置向量的长度，D_s为设定的安全距离值，k_rep为控制因子，α为形变因子，v_rep为排斥速度的大小值，ρ为排斥速度影响范围的半径值，V_rep为排斥速度，V_endmax为机械臂的最大线速度。

2.根据权利要求1所述的一种六轴机械臂避障控制方法，其特征在于，所述实时获取机械臂所在工作空间的点云数据，根据所述点云数据确定障碍物点云，包括：

获取机械臂所在工作空间的点云数据，对所述点云数据进行融合处理，得到第一深度图；其中，所述第一深度图包括机械臂点云和障碍物点云；

将第一深度图中的机械臂点云进行过滤，得到第二深度图；

将第二深度图由相机坐标系转换为机械臂的基座坐标系，得到第一点云图；

滤除第一点云图中残留的机械臂点云，得到第二点云图。