3、目标定位（视觉测距）

news2024/9/23 7:23:27

目标定位的目的：获取物品相对于视觉模块的三维坐标，并将其转换为物品相对于机械臂坐标原点的三维坐标。

要获取物品三维坐标，则首先要测量物品距离摄像头的距离，又因为摄像头安装在机械臂末端上方，所以获取物品相对于摄像头的实际坐标后，需要转换为物品相对于机械臂坐标系原点的坐标。

机械臂的初始状态如下，其中D0-D5分别为机械臂的6个舵机

第一步，用视觉模块测距，基于相似三角形的原理，我们可以这样描述计算物体到相机距离的过程：设想有一个具有已知宽度W的物体，它被安置在离相机D距离的位置上。当我们用相机对该物体进行拍摄时，我们可以测量得到物体在照片中的像素宽度P，相似三角形示意图如下。通过这些信息，我们可以推导出计算相机焦距的公式：

$F = (P\times D)/W$

再用相似三角形来计算出物体离相机的距离：

$D=(W\times F)/P$

第二步，根据位置关系确定视觉模块与机械臂坐标原点的横纵向距离，即视觉模块相对于机械臂坐标原点的三维坐标，本步骤只需要简单的三角函数关系求解。

第三步，获取物品相对于视觉模块的横纵向距离。由第一步可测得物品P距离视觉模块的距离，由于摄像头照射范围的角度即视场角（Field of View）是一定的，摄像头的照射边界之间的夹角即为视场角。则只需要测得物品中心点与边界线的像素差值，由比例关系则可得到物品方向与边界线的角度值，经数据转换可得到物品方向与竖直方向的角度，进而计算出物品相对于视觉模块的横纵向距离。

举例：第三步，假设物品在视觉模块中的位置如下，其中：灰色为视觉模块照射的中心线，黑色实心方块为物品在视觉模块中显示的位置。由于第一步我们已经获得了物品相对于视觉模块的实际距离，我们可以根据这个实际距离以及物品在视觉模块中显示的位置的像素值计算出物品相对于视觉模块的三维坐标。然后将此三维坐标与第二步测得的视觉模块相对于机械臂坐标原点的三维坐标相加，即可得到物品相对于机械臂坐标原点的三维坐标。