沉默杀手HomeArchivesTagsCategories
使用AprilTag进行机器人导航（一）
|路径规划其他|
Word count: 1.4k|Reading time: 5 min

充电桩的导航，一般方案是先导航到充电桩附近的一个固定点，然后切换到另一种更精确的导航方式进行精确对接充电桩。 精确对接的定位技术有红外、蓝牙、激光特征、二维码、反光板

这里只谈Apriltag，如果是aruco二维码，使用aruco_ros

ROS官方提供了一个名为apriltags2_ros的包处理AprilTag，可以根据Tag进行定位导航，我使用的是april.launch。使用前需要先启动摄像头的节点，我用的是usb_cam.launch，如果摄像头程序重启，april.launch也要重启，否则无效。

节点会发布话题tag_detections和tag_detections_image，后者是带数字识别的图，前者是坐标关系的输出。默认的话题频率应该能达到10Hz，但是完全取决于计算机的性能，无法用参数配置。

settings.yaml中的参数如下，按默认即可:

  • Make sure tag_border corresponds to your tags’ black border bit width (typically 1 - each square in the tag is represents 1 bit);

  • Setting tag_threads allows certain parts of the core AprilTag 2 algorithm to run parallel computations. Typical multithreading advantages and limitations apply;

  • 增大tag_decimate>1 allows to decimate (reduce the resolution) of the image by that amount. This makes the core AprilTag 2 algorithm faster (there are less pixels to process) but means that smaller tags are less likely to be detected，甚至无法识别。Pose estimation and decoding still happens with the full-resolution image;

  • Setting tag_blur>0 blurs the image and tag_blur<0 sharpens the image;

  • Setting tag_refine_edges=1 improves the detection fit to the tag, thus the corner detection precision, thus the pose estimation accuracy. Not computationally expensive and recommended to be on (1);

  • Setting tag_refine_decode=1 reducecs the false negative detetion rate;

  • Setting tag_refine_pose=1 improves the estimated pose accuracy but is computationally expensive;

这个包有自定义的消息文件，不知道为什么，如果在自己的程序(我的叫tag_nav)中想包含其消息的头文件，就会报错：

解决方法：先编译好apriltag2_ros包，然后注释掉CMakeLists中的generate_messages部分，再编译tag_nav。之后没事不要再动apriltag2_ros了。如果先注释再编译apriltag2_ros包，会无法在tag_nav中包含前者的头文件。

我们使用36H11规格的ID为9的二维码进行测试。

注意事项:

  • 同一个tag ID不能对应不同的尺寸，这会造成检测的不确定。

  • 同一张图案里不要有相同的tag ID

  • standalone_tags 和 in tag_bundles 可以有相同ID的tag, 但是size也必须相同。

  • 打印出的tags周围要有一些白色空间，这是AprilTag 2 算法要求的，所以我们最好使用带黑框的图案

Apriltag2.jpg

两话题不同步的问题

运行后提示 image消息和camera_info消息没有同步

此时打开rqt，切换到主题tag_detections_image，发现没有结果

源码在continuous_detector.cpp，发现没有进入回调函数ContinuousDetector::imageCallback，它的注册在:

1
camera_image_subscriber_ =  it_->subscribeCamera("/usb_cam/image_raw",   1,  &ContinuousDetector::imageCallback, this);

函数解释：订阅一对同步化的image和camera info话题，This version assumes the standard topic naming scheme, where the info topic is named “camera_info” in the same namespace as the base image topic.

既然二者目前没有同步，当然没有进入回调函数，也就无法识别Tag了。最终发现原因就是在工控机上运行了roslaunch usb_cam usb_cam.launch，而在远程机运行了apriltag_ros包，而两台电脑通信不畅，解决方法就是将二者都在工控机上运行。

现在执行rqt就可以看到识别的结果了，二维码中心是ID,终端上也出现了二维码ID等信息:
识别Tag

Tag的位姿

若要查看检测结果，执行：

1
rostopic　echo　/tag_detections

得到这样的结果
tag_detections
很明显，我们需要的是position和orientation。在实际中，Tag应当是不动的，摄像头安装在机器人上，是运动的。所以描述二者关系时，以Tag为原点进行分析，如下图是Tag和摄像头（黄圈）

经过我实际的测试，这种环境中的坐标轴应当是这样的:

y代表了上下方向，由于Tag是固定的，而机器人不能飞起来，所以y可以不用考虑。当机器人在Tag右边时，x为负，在左边时为正。当机器人远离Tag时，z值变大，走近Tag时，z值变小。我所用的摄像头检测到Tag的最大距离接近5米，已经足够了。

总结一下，摄像头坐标系和tag坐标系的关系如下（普遍适用）：

坐标系转换关系问题

apriltag2_ros包存在tag_9坐标系向camera坐标系的转换，在TagDetector::detectTags函数的最后。

接下来我们要在camera和base_link之间建立联系，这样就将所有坐标系关联起来了，在launch文件中添加:

1
<node args="-0.24 0 0.295 1.571 0 -1.571 /base_link /camera 10" pkg="tf" name="base_link_to_camera" type="static_transform_publisher"/>

因为tf是树结构，所以一个坐标系只能有一个parent，但可以有多个child。因此这里不能让camera做parent，而是base_link，因为base_link的parent已经是odom了，不能再给它找个parent

启动所有launch文件后，使用rosrun tf view_frame命令生成pdf文件，查看得知，拥有全部坐标系的情况是这样的:

apriltags2_ros的bug

在apriltags2_ros启动一段时间后，会偶然的无法正常检测二维码，tag_detections话题没有结果，也没有图形识别的结果，但摄像头的节点正常。

多次观察tag_detections的发布情况，发现最快的一次是header.seq到1400的时候就出错了，有时是到3900的时候，始终不确定，难以找到原因。

因此尝试AprilTag3，同时把setting.yaml中的线程数改为4，再也没有发生这一bug

参考：
ROS image_transport
Using Apriltags with ROS
image_transport::CameraSubscriber Class