[发明专利]四轴机械臂的位置确定方法和装置

权利要求书

1.一种四轴机械臂的位置确定方法，其特征在于，包括：

获取四轴机械臂上关节的关节值；

根据所述关节值、所述四轴机械臂的正运动学模型和所述四轴机械臂上手抓的平面几何模型，获得所述手抓的平面几何模型的位置信息；

根据所述手抓的平面几何模型、所述手抓的平面几何模型的位置信息、障碍物的平面几何模型和所述障碍物的平面几何模型的位置信息，获得所述手抓相对于所述障碍物的距离和方向；其中，所述方向指示了所述手抓远离所述障碍物的方向；

所述手抓的平面几何模型为圆形和多边形中的一个，所述障碍物的平面几何模型为所述圆形和所述多边形中的另一个，所述根据所述手抓的平面几何模型、所述手抓的平面几何模型的位置信息、障碍物的平面几何模型和所述障碍物的平面几何模型的位置信息，获得所述手抓相对于所述障碍物的距离和方向，包括：

将所述圆形的圆心向基准线投影获得参考点，将所述多边形向所述基准线投影获得第一参考线段；其中，所述基准线为所述多边形的一条边的中垂线；

将所述第一参考线段分别向两端扩展R后获得第二参考线段；其中，R为所述圆形的半径；

将所述参考点与所述第二参考线段的两个端点之间的距离的最小值确定为投影距离，将所述参考点相对于所述两个端点中与所述参考点距离较近的端点的方向确定为投影方向，所述投影方向指示了所述手抓远离所述障碍物的方向；

对于所述多边形的N条基准线中的每条基准线，重复执行上述步骤获得N个投影距离和N个投影方向；其中，N为大于1的整数；

将所述N个投影距离中的最小值确定为所述手抓相对于所述障碍物的距离，将所述N个投影距离中的最小值对应的投影方向确定为所述手抓相对于所述障碍物的方向；或者，

所述手抓的平面几何模型为第一多边形，所述障碍物的平面几何模型为第二多边形，所述根据所述手抓的平面几何模型、所述手抓的平面几何模型的位置信息、障碍物的平面几何模型和所述障碍物的平面几何模型的位置信息，获得所述手抓相对于所述障碍物的距离和方向，包括：

将所述第一多边形向基准线投影获得第三参考线段，将所述第二多边形向所述基准线投影获得第四参考线段；其中，所述基准线为所述第一多边形的一条边的中垂线或者所述第二多边形的一条边的中垂线；

将所述第三参考线段的两个端点与所述第四参考线段的两个端点中任意两个端点之间的距离的最小值确定为投影距离，根据所述投影距离对应的两个端点确定投影方向，所述投影方向指示了所述手抓远离所述障碍物的方向；

对于所述第一多边形的P条基准线和所述第二多边形的Q条基准线中的每条基准线，重复执行上述步骤获得P+Q个投影距离和P+Q个投影方向；其中，P、Q均为大于1的整数；

将所述P+Q个投影距离中的最小值确定为所述手抓相对于所述障碍物的距离，将所述P+Q个投影距离中的最小值对应的投影方向确定为所述手抓相对于所述障碍物的方向。

2.根据权利要求1所述的方法，其特征在于，还包括：

根据所述关节值、所述手抓相对于所述障碍物的方向和所述四轴机械臂的逆运动学模型，获得所述关节相对于所述障碍物的梯度方向。

3.根据权利要求1所述的方法，其特征在于，所述根据所述关节值、所述四轴机械臂的正运动学模型和所述四轴机械臂上手抓的平面几何模型，获得所述手抓的平面几何模型的位置信息，包括：

根据所述关节值和所述正运动学模型获得所述手抓的位姿信息，所述位姿信息包括所述手抓的中心点在机械臂平面坐标系中的坐标和所述手抓绕着所述中心点转过的角度；其中，所述机械臂平面坐标系的原点为所述四轴机械臂上第一个旋转关节的旋转轴轴线与水平面的交点；

根据所述手抓的位姿信息和所述手抓的平面几何模型获得所述手抓的平面几何模型的位置信息。

4.根据权利要求3所述的方法，其特征在于，所述手抓的平面几何模型为圆形，所述手抓的平面几何模型的位置信息包括：所述手抓的中心点在所述机械臂平面坐标系中的坐标和所述圆形的半径。