在python中如何使用apriltag视觉定位技术

持续学习的程序员+1

于 2025-05-19 14:54:25 发布

阅读量718

点赞数 12

文章标签： python 数码相机开发语言

本文链接：https://blog.csdn.net/weixin_43915081/article/details/148063585

版权

前一段时间在研究强化学习，想用在机械臂相关的任务上，强化学习中有一个比较重要的部分就是reward的设计，例如机械臂抓取任务，机械臂需要较为精准的到达目标物体的上方，然后再抓取。在这个过程中，如何获得机械臂抓手的位姿，然后判断它与目标物体的相对位置关系就比较重要，我们可以根据抓手与目标物体的距离/IOU等指标判断机械臂是否到达一个合适的位置，如果位置合适的话，reward就高一些，否则reward就低。

在这里插入图片描述

由于我这边是一个纯视觉的环境，所以经过调研，AprilTag视觉定位技术是一个比较精准的技术，以前在项目中也测试过，在5米的位置，10cm以上的tag的大小，位置的准确度能达到<3cm。这次用在机械臂的环境中，由于机械臂大小限制，我用的是1.7cm大小的tag，在0.5米外放置相机，像素分辨率1280*720，也能达到<3cm的精度，还是比较准确的。当然建议在有条件的情况下，尽量选择tag size更大一些的，分辨率更高一些的，精准度更有保障。apriltag本身二维码大家可以检索“openmv apriltag”就可以找到生成方法。

１，坐标系

在apriltag使用中，理清楚坐标系非常重要，网上文章相互抄袭，讲得清楚的凤毛麟角。我经过实验后，下面比较严谨的apriltag坐标系讲得清楚一些。以下图为例，我直接将坐标系渲染在了图片上，其中蓝色为x坐标轴，绿色为y轴，红色为z轴。坐标原点在二维码的中心点（图片渲染的不太对，前几天截图的时候没有注意，后面也没有环境能再截了，所以大家将就一下~~）。
在这里插入图片描述

大家可能会有个疑惑，二维码是正方形的，正方形的哪个边是x，哪个边是y呢？以上面第一张图为例，这个二维码是默认打印出来的，正着放（文字在二维码正下方，代表是正着放）的话，横向就是x轴，正方向朝右，竖向就是y轴，正方向朝下。

apriltag检测结果是二维码坐标系在相机坐标系中的位置（旋转＋平移），所以在同一相机坐标系中检测出２个二维码的话，就可以获取这２个二维码之间的关系。

在我贴二维码的过程中，文字都在二维码的左侧，也就是让二维码躺平贴上去的。所有二维码我都是这样贴的，主要目的是人工可以更加方便的对结果进行检查，因为简化了二维码之间的坐标系的旋转关系（也说是２个二维码并排放在一起的话，它们之间的旋转角基本都为０），这点大家仅供参考，不是必须这样的。

2，结果呈现

下面的视频是我录的一个视频，把通过apriltag定位出来的信息放在了视频中。我在机械臂不会动的位置放了一个二维码，在桌面上的目标物体上放了一个二维码，下图中带颜色的box是我根据检测结果，在二维码坐标系原点位置，渲染了一个长宽高为5.6cm, 3cm, 3cm的box。图片中的tag_obj就是目标物体。

distance to tag_obj就是图片中两个二维码坐标系原点的物理距离，单位米。

coord in tag_obj:　以目标物体上贴的二维码坐标系（下面简称目标坐标系）为父坐标系，抓手上的二维码坐标系（下面简称抓手坐标系）为子坐标系，coord就是子坐标系在父坐标系中的坐标，这２个坐标系之间就有外参，coord也就是外参中的平移部分。

euler in tag_obj: 上述外参中的旋转角，本质上代表将父坐标系旋转到与子坐标系对齐，分别在xyz三个坐标轴上的旋转角度。

iou: 代表将抓手上面的box转换到目标坐标系后，在目标坐标系yz平面上与目标物体上的box之间的IOU。iou与reward与我研究的强化学习有关系，这点大家不感观兴趣的不用关注。

episode

3，代码讲解

我已经将apriltag的代码与lerobot环境打通了，代码地址：git@github.com:hxdoit/lerobot.git，在分支：rl上，文件名：lerobot/common/robot_devices/control_utils.py从３１０行开始。apriltag的代码相对比较独立，大家也可以单独使用。需要pip install apriltag就可以安装依赖。

apriltag需要相机的内参，我是用ros中的相关工具标定出的内参。注意一下，正常标定应该是用的原始相机输出的图像（畸变校正前）进行标定，但我们购买的相机一般内部应该都处理好了，输出的是无畸变的图像，所以标定出来的结果从理论上可能有一些不准，但不太影响，在使用时我这边发现标定出来的畸变参数不使用更加准确，只使用内参即可。

if camera_name == 'desktop':
        print('desktop')
        K = np.array([[1592.776294, 0.000000, 655.148278],
                      [0.000000, 1595.337130, 402.386737],
                      [0.000000, 0.000000, 1.000000]])
        distCoeffs = np.array([0.089836, 0.058097, 0.029668, 0.028918, 0.000000])
    else:
        print('other')
        K = np.array([[671.907962, 0.000000, 639.211904],
                      [0.000000, 674.667340, 360.929253],
                      [0.000000, 0.000000, 1.000000]])
        distCoeffs = np.array([0.109473, -0.127263, 0.000050, 0.003589, 0.000000])
    cameraparam = [K[0, 0], K[1, 1], K[0, 2], K[1, 2]]
    # 不用效果更好
    distCoeffs = None

下面代码用于计算２个二维码坐标系之间的外参:

tag_grasp_2_obj = compute_transform(result, tag_obj_result)

下面代码可以将抓手坐标系中的坐标，通过外参转换到目标坐标系：

temp = (rot_between_2tags @ virtual_obj_on_grasp_3d_corners.T).T + trans_between_2tags

其它代码比较容易理解，不多讲解。