简介基于ROS与OpenCV的二维码SLAM系统源码面向机器人自主导航、视觉定位与地图构建方向的开发者与学生。项目以二维码为地标利用ARToolkitPlus库实现多二维码的稳健检测与跟踪并通过扩展卡尔曼滤波SLAM算法EKFSLAM融合里程计与视觉观测数据在ROS环境下完成地图构建与自主导航。压缩包共含2000个文件其中1725个png图像为测试与可视化素材74个cpp和52个h文件构成核心源码另有CMake构建脚本、相机标定参数及说明文档等整体仅5.79MB便于快速迁移与部署。目前已有51人学习浏览过。资源提供可直接参考的完整工程涵盖图像去畸变、灰度转换、二维码识别、数据关联与状态更新等关键模块并附有相机标定与去畸变处理示例既适合作为SLAM入门学习资料也可作为毕业设计或科研项目的二次开发基础。1. 一个塞满标定文件与 CMake 产物的 ROS 二维码 SLAM 源码包从哪下手打开压缩包最先看见的并不是整齐的main.cpp而是CMakeDetermineCompilerABI_CXX.bin、CMakeCCompilerId.c这类 CMake 探测期文件以及一堆.cal标定文件AR_cali.cal、no_distortion_small_camera.cal~、no_distortion_caliBoard.cal~。这个形态说明工程已经被 CMake 配置过某些二进制是中途产物真正有用的反而是看起来不起眼的标定参数和源码逻辑。整套东西的目标很直接摄像头识别二维码把二维码当作固定路标融合轮式里程计在 ROS 里用扩展卡尔曼滤波完成定位与建图。适合刚做完《视觉 SLAM 十四讲》基础篇、想在真实或者 Gazebo 仿真里跑一遍 slam 建图的读者也适合准备 slam 面试时用来解释“路标从哪来、观测怎么写进 EKF”的人。2. ARToolkitPlus 标记检测二维码 SLAM 的地标前端2.1 为什么选二维码当路标而不是 ORB 特征点特征点 SLAM 对纹理要求高白墙、走廊、货架容易丢特征。二维码标记是人工路标只要进入视野且满足最小分辨率就能给出一组稳定角点和唯一 ID。二维码的关键性质是“已知尺寸”也就是说可以从像素坐标直接推算距离和方位不需要三角化。这对低算力平台特别友好一个TrackerSingleMarker实例处理 640x480 图像CPU 占用比 ORB 提取小一个量级。ARToolkitPlus 检测的结果不是普通 QR 码里的字符串而是方形黑白矩阵的 ID 号。它内部会做二值化、连通域分析、四边形拟合和模板匹配。对 SLAM 而言ID 号就是地标主键状态向量里可以用“ID - 路标坐标”建立一对一的对应关系。坏处是环境需要人为布置标记所以它适合仓库、工厂、实验室等接受改造的场景不适合户外全开放环境。2.2 TrackerSingleMarker 的初始化与关键参数ROS 节点里创建跟踪器时我一般直接声明TrackerSingleMarkerImplARToolkitPlus::cv::RGB24Pixel因为cv::Mat的默认排列正好是 RGB 还是 BGR 取决于采集方式这里用 RGB24 能减少一次通道转换。以下代码是初始化片段#include artoolkitplus/TrackerSingleMarker.h ARToolkitPlus::TrackerSingleMarker* create_tracker(int width, int height) { // 模板参数是输入图像像素格式这里使用 RGB24 ARToolkitPlus::TrackerSingleMarker* tracker new ARToolkitPlus::TrackerSingleMarkerImplARToolkitPlus::cv::RGB24Pixel( width, height); tracker-setCameraFile(AR_cali.cal); tracker-setMarkerWidth(80.0); // 二维码物理宽度单位 mm tracker-setPatternWidth(80.0); tracker-setThreshold(120); // 二值化阈值 tracker-setBorderWidth(0.5); // 标记边框占整体比例 return tracker; }代码逻辑说明构造函数里的width和height必须与采集分辨率一致否则畸形校正后的坐标会错位setCameraFile把相机内参和畸变系数交给库它在内部完成像素到归一化坐标的变换setMarkerWidth决定物理尺度直接影响后面 EKF 里“二维码离机器人多远”的计算单位必须全局统一。setThreshold(120)是经验值光线变亮时调低变暗时调高调试时可以用rosrun rqt_reconfigure动态改。检测循环里取出 ID 和角点vectorint ids(1024, 0); vectorfloat conf(1024, 0.0f); int count tracker-calc(cv_img.data, ids.data(), conf.data()); for (int i 0; i count; i) { float corners[8]; tracker-getMarkerCorners2D(i, corners); // corners 顺序: 左上, 右上, 右下, 左下 int marker_id ids[i]; float x1 corners[0], y1 corners[1]; float x2 corners[2], y2 corners[3]; float x3 corners[4], y3 corners[5]; float x4 corners[6], y4 corners[7]; }这段代码常见的问题是角点顺序。ARToolkitPlus 返回的是屏幕坐标系下的浮点像素坐标原点在图像左上角x 向右y 向下。如果直接拿去做欧氏距离计算必须先转换到相机坐标系。多二维码同时出现时calc返回的 ID 顺序没有明确保证所以不要用数组下标当作路标 ID必须用ids[i]。2.3 二维码 ID 管理与位姿解算二维码作为路标ID 管理决定了 EKF 的观测函数能不能找到对应路标。推荐把 ID 和物理尺寸写进 YAML 配置而不是硬编码在代码里markers: 0: {size_mm: 80.0, z: 0} 1: {size_mm: 80.0, z: 0} 3: {size_mm: 50.0, z: 0}z表示标记中心相对地面的高度。机器人只能观测到与摄像头在同一平面附近的二维码存在高度差时需要把观测模型中的像素投影加一个固定平移量。下表汇总了用来调检测灵敏度的参数参数作用调大影响调小影响setThreshold二值化阈值标记内部空洞变小暗处易漏检明处误检增多setBorderWidth边框宽度占比远近视角更稳定小尺寸易丢失小距离易误识别setMarkerWidth标记物理宽度尺度估计偏大尺度估计偏小setLogger日志输出输出过多拉高耗时关闭后问题难排查一幅图像里识别到 10 个以上二维码时不要全丢给 EKF。只取距离最近或角度差异最大的 3 到 4 个否则矩阵求逆耗时上去了还容易因为远端标定误差引入错误约束。3. OpenCV 去畸变与相机标定文件解析3.1 从相机驱动到.cal格式.cal是 ARToolkit 体系下的文本标定文件里面存的是焦距、主点、畸变系数等参数。由于不同版本 ARToolkitPlus 对格式的解析有差异拿到AR_cali.cal后第一件事不是直接使用而是打印前几行。常见结构类似# camera parameters 640 480 1.2 0.0 320.0 0.0 1.2 240.0 -0.35 0.11 0.0 0.0前三行对应分辨率和内参矩阵第四行以后是径向与切向畸变。某些生成工具会把畸变放成一行也可能用空格分段。不要迷信文件后缀no_distortion_small_camera.cal~这种带波浪号的通常是临时文件可能是人工编辑时留下的备份。在 ROS 里做标定最省事的是使用camera_calibration它会输出 YAML然后需要手动翻译成 ARToolkitPlus 的.cal。我一般会写一个 Python 小工具把 YAML 的内参矩阵和畸变系数按 ARToolkitPlus 要求的顺序重排。这个过程最容易错的是畸变系数顺序OpenCV 是k1, k2, p1, p2, k3ARToolkitPlus 是k1, k2, p1, p2截断不要裁错。3.2 OpenCV 去畸变代码与调用效果检测二维码前先用 OpenCV 去畸变能明显提升远距离角点精度。下面是直接使用cv::undistort的做法#include opencv2/opencv.hpp cv::Mat K (cv::Mat_double(3, 3) 615.0, 0.0, 320.0, 0.0, 610.0, 240.0, 0.0, 0.0, 1.0); cv::Mat D (cv::Mat_double(4, 1) -0.32, 0.11, -0.001, 0.002); cv::Mat raw, undist; cap.read(raw); cv::undistort(raw, undist, K, D);代码逻辑说明K是相机内参矩阵(0,0)和(1,1)是焦距(0,2)和(1,2)是主点D存放畸变系数。cv::undistort内部会创建映射表对整幅图像逐像素重采样。缺点是对 1080p 实时流有一定开销。如果机器人控制器算力有限可以缩小分辨率或者用cv::initUndistortRectifyMap和cv::remap把映射表预先计算好运行时只做 remap我在实际工程里会把后者做成一个独立的 ROS 节点发布去畸变后的图像话题。另一个选择是跳过去畸变在 ARToolkitPlus 内部使用.cal自带的畸变参数。此时 OpenCV 只做灰度变换和颜色格式转换。这个方案省一次全图重采样对帧率提升明显前提是标定文件里的畸变系数足够准。3.3 no_distortion 系列标定文件的选择逻辑压缩包里出现no_distortion_small.cal、no_distortion_big.cal和no_distortion_caliBoard.cal我倾向于把它们看成同一套参数在不同标记尺寸下的变体。名字里的small和big指的是二维码在实际场景里占据的画面大小。标记大时角落像素更准对畸变不敏感可以用更激进的阈值标记小时畸变带来的角点偏移会被放大必须选择更保守的参数。实际测试时可以用一个快速脚本给同一帧图像做三次去畸变然后对比二维码角点像素差rosrun qr_slam undistort_test _image:/camera/image_raw _cal:AR_cali.cal如果输出角点坐标差超过 2 个像素就需要用带完整畸变模型的AR_cali.cal而不是 no_distortion 版本。下表是我在调试时使用的选择依据文件特征适用条件不适用情况AR_cali.cal广角镜头、近距离大标记无畸变成像、计算资源紧张no_distortion_small.cal窄视角、标记占图比例小标记边缘有明显桶形畸变no_distortion_big.cal标记占图比例大标记在画面边缘标定本身不值得反复手工操作。先让摄像头对准棋盘格或 asymmetric circles 板转动 5 到 10 组姿态用camera_calibration输出重投影误差小于 0.3 像素的标定结果再转成.cal。误差大于 0.5 时换掉标定板姿态不要强行使用。4. EKFSLAM 数据融合里程计与二维码观测的更新机制4.1 状态向量与协方差结构EKFSLAM 把机器人位姿和所有二维码位置放进同一个状态向量x [x_r, y_r, θ, x_l0, y_l0, x_l1, y_l1, ...]前三维是机器人在地图中的坐标和朝向后面每两维对应一个二维码中心坐标。协方差矩阵是方阵维度等于状态长度。初始时刻二维码未知可以把检测到首个二维码时的机器人位姿作为路标初始位置。注意状态向量一旦确定二维码顺序不要随意增删否则语义丢失。二维码观测本质上是“机器人相对路标的位置”函数。这里有两层投影世界坐标到机器人坐标的变换以及机器人坐标到像素平面的相机投影。EKF 只处理高斯噪声因此观测噪声和运动噪声都要建模成零均值高斯。ARToolkitPlus 返回的角点噪声不是完全对称的近距离时角点抖动小远距离角点抖动大简单做法是把观测协方差设成固定矩阵但精确做法是让协方差随距离缩放。4.2 预测与观测更新代码骨架运动预测使用里程计增量。通常机器人发布的 odom 话题包含线速度和角速度直接离散化即可#include Eigen/Dense void predict(Eigen::VectorXd mu, Eigen::MatrixXd sigma, double v, double w, double dt) { double theta mu(2); Eigen::Vector3d delta; delta v*dt*cos(theta), v*dt*sin(theta), w*dt; mu.head(3) delta; Eigen::Matrix3d G; G 1, 0, -v*dt*sin(theta), 0, 1, v*dt*cos(theta), 0, 0, 1; Eigen::Matrix3d Q Eigen::Matrix3d::Identity() * 1e-3; sigma.block3, 3(0, 0) G * sigma.block3, 3(0, 0) * G.transpose() Q; }代码逻辑说明G是运动模型对机器人状态的雅可比矩阵它把里程计噪声映射到状态协方差。Q是过程噪声取值不能过大否则二维码观测更新后机器人位姿仍然可能发散也不能过小否则跟踪速度慢。实际路由器的轮式里程计通常比视觉里程计噪声大Q的三维值可以设为[0.05, 0.05, 0.01]。观测更新部分假设摄像头形成二维像素观测则预测值来自路标位置和机器人位姿void update_landmark(Eigen::VectorXd mu, Eigen::MatrixXd sigma, int landmark_id, double zx, double zy) { int k 3 2 * landmark_id; double dx mu(k) - mu(0); double dy mu(k 1) - mu(1); double theta mu(2); // 将路标变换到机器人坐标系 double bx cos(theta) * dx sin(theta) * dy; double by -sin(theta) * dx cos(theta) * dy; Eigen::Vector2d h; h bx, by; // 这里省略相机内参投影实际项目中用 K 矩阵将 bx,by 映射到像素 Eigen::Matrixdouble, 2, 5 H; H.setZero(); // H 的前三列是对机器人位姿的偏导后两列是对路标位置的偏导 // 完整推导见源码包中的 ekf_core.cpp Eigen::Matrix2d R Eigen::Matrix2d::Identity() * 0.01; Eigen::Matrix2d S H * sigma * H.transpose() R; Eigen::Vector2d innovation; innovation zx - h(0), zy - h(1); Eigen::MatrixXd K sigma * H.transpose() * S.inverse(); mu K * innovation; sigma (Eigen::MatrixXd::Identity(mu.size(), mu.size()) - K * H) * sigma; }代码逻辑说明这里用机器人局部坐标作为观测值H是观测模型对全状态量的雅可比维度是2 x NN 为状态长度。实际项目里不要尝试手算所有偏导最稳妥的方式是先用 Ceres 或者 Eigen 的自动求导验证再把解析式填入矩阵。像素观测和局部坐标观测之间的区别是像素观测还必须让 H 乘上相机内参矩阵否则可能出现“二维码明明在图像中心EKF 却认为它在侧面”的不一致。4.3 EKF 调参与数据关联数据关联错误是二维码 SLAM 最典型的发散原因。某个二维码 ID 一旦被赋值给路标后续观测就不能复用。常见错误是把同一次calc返回的多个 ID 当成多个不同路标实际上如果相机连续帧都识别出同一个 ID应该先做马氏距离门控只有当新观测距离预测位置小于设定阈值时才更新。阈值一般是 3 倍标准差也就是高斯分布下 99.7% 的置信区间。参数位置经验值影响Q运动噪声预测函数1e-3 ~ 5e-3太大定位抖太小建图滞后R观测噪声更新函数0.01 ~ 0.05太大不信任二维码太小被错误观测带偏初始路标协方差新增路标时矩阵块 1e-2决定首帧路标误差数据关联阈值更新前2 ~ 4 倍标准差过小漏关联过大错误关联我调试的顺序是先固定二维码不动机器人原地旋转观察从第一个二维码建立的坐标是否来回漂移。如果漂移明显优先检查theta的协方差因为角度误差会通过坐标变换放大到所有路标。5. 把二维码 SLAM 包跑在 ROS 上launch、rviz 与排错5.1 编写 node 与 launch 文件源码包里的 ROS 节点应该拆成三部分图像采集、二维码检测、EKFSLAM。这样单独切换相机驱动不影响核心算法。launch 文件建议这样组织launch node namecamera_node pkgusb_cam typeusb_cam_node param namevideo_device value/dev/video0/ param namepixel_format valueyuyv/ param nameimage_width value640/ param nameimage_height value480/ /node node nameqr_slam_node pkgqr_slam typeqr_slam_node outputscreen param namecalibration_file value$(find qr_slam)/calib/AR_cali.cal/ param namemarker_size_mm value80.0/ param nameodom_topic value/odom/ /node /launch这个 launch 文件的价值在于把所有和环境相关的参数暴露在 XML 层不用为了换相机重编源码。marker_size_mm必须和真实打印的二维码一致否则地图尺度会整体缩放。odom_topic需要确认是真实的轮式里程计还是 gazebo 仿真里程计两者的噪声特性差别很大。5.2 在 rviz 里读图运行后先把摄像头图像话题加到 rviz确认二维码角点有没有被画出边框。然后添加TF面板观察map、odom、base_link三个坐标系是否成链。如果 EKF 节点正常会在base_link到二维码路标之间形成观测关联估计出的路标位置会显示为MarkerArray。检查rosrun tf view_frames生成的 pdf 图定位在map下的机器人位姿是否有跳变。也可以用命令行快速看二维码检测频率rostopic hz /qr_slam/marker_pose话题发布频率低于 10Hz 时优先怀疑 ARToolkitPlus 检测耗时或者去畸变节点阻塞。高于 30Hz 时要注意 ECKF 更新是否被跳过。5.3 定位发散时的具体检查项如果机器人不动而地图中路标位置来回跳先查时间戳。使用rosbag回放时图像和里程计时间不同步会导致观测更新延迟我遇到最多的问题是某条 ROS 消息时间戳比当前时刻晚 100ms 以上此时 EKF 的预测和更新顺序出现颠倒。解决方案是统一使用ros::Time并确保相机驱动发布了真实的采集时间。另一个高频错误是二维码 ID 重合。两套标定板如果共用相同 ID代码会把两个物理位置完全不同的路标当成一个协方差矩阵会被错误拉紧。排错时打印mu向量看距离很近的两个路标是否都对应同一个 ID并在地图上重新检查二维码摆放顺序。每次新增路标后建议把 EKF 状态协方差的最大特征值打印到日志只有它随时间下降且最终稳定才算建图成功。本文还有配套的精品资源点击获取 
长河君 · 2026/9/15 3:58:54 · 阅读 1.2k ROS二维码SLAM：ARToolkitPlus检测与EKF融合实践
简介基于ROS与OpenCV的二维码SLAM系统源码面向机器人自主导航、视觉定位与地图构建方向的开发者与学生。项目以二维码为地标利用ARToolkitPlus库实现多二维码的稳健检测与跟踪并通过扩展卡尔曼滤波SLAM算法EKFSLAM融合里程计与视觉观测数据在ROS环境下完成地图构建与自主导航。压缩包共含2000个文件其中1725个png图像为测试与可视化素材74个cpp和52个h文件构成核心源码另有CMake构建脚本、相机标定参数及说明文档等整体仅5.79MB便于快速迁移与部署。目前已有51人学习浏览过。资源提供可直接参考的完整工程涵盖图像去畸变、灰度转换、二维码识别、数据关联与状态更新等关键模块并附有相机标定与去畸变处理示例既适合作为SLAM入门学习资料也可作为毕业设计或科研项目的二次开发基础。1. 一个塞满标定文件与 CMake 产物的 ROS 二维码 SLAM 源码包从哪下手打开压缩包最先看见的并不是整齐的main.cpp而是CMakeDetermineCompilerABI_CXX.bin、CMakeCCompilerId.c这类 CMake 探测期文件以及一堆.cal标定文件AR_cali.cal、no_distortion_small_camera.cal~、no_distortion_caliBoard.cal~。这个形态说明工程已经被 CMake 配置过某些二进制是中途产物真正有用的反而是看起来不起眼的标定参数和源码逻辑。整套东西的目标很直接摄像头识别二维码把二维码当作固定路标融合轮式里程计在 ROS 里用扩展卡尔曼滤波完成定位与建图。适合刚做完《视觉 SLAM 十四讲》基础篇、想在真实或者 Gazebo 仿真里跑一遍 slam 建图的读者也适合准备 slam 面试时用来解释“路标从哪来、观测怎么写进 EKF”的人。2. ARToolkitPlus 标记检测二维码 SLAM 的地标前端2.1 为什么选二维码当路标而不是 ORB 特征点特征点 SLAM 对纹理要求高白墙、走廊、货架容易丢特征。二维码标记是人工路标只要进入视野且满足最小分辨率就能给出一组稳定角点和唯一 ID。二维码的关键性质是“已知尺寸”也就是说可以从像素坐标直接推算距离和方位不需要三角化。这对低算力平台特别友好一个TrackerSingleMarker实例处理 640x480 图像CPU 占用比 ORB 提取小一个量级。ARToolkitPlus 检测的结果不是普通 QR 码里的字符串而是方形黑白矩阵的 ID 号。它内部会做二值化、连通域分析、四边形拟合和模板匹配。对 SLAM 而言ID 号就是地标主键状态向量里可以用“ID - 路标坐标”建立一对一的对应关系。坏处是环境需要人为布置标记所以它适合仓库、工厂、实验室等接受改造的场景不适合户外全开放环境。2.2 TrackerSingleMarker 的初始化与关键参数ROS 节点里创建跟踪器时我一般直接声明TrackerSingleMarkerImplARToolkitPlus::cv::RGB24Pixel因为cv::Mat的默认排列正好是 RGB 还是 BGR 取决于采集方式这里用 RGB24 能减少一次通道转换。以下代码是初始化片段#include artoolkitplus/TrackerSingleMarker.h ARToolkitPlus::TrackerSingleMarker* create_tracker(int width, int height) { // 模板参数是输入图像像素格式这里使用 RGB24 ARToolkitPlus::TrackerSingleMarker* tracker new ARToolkitPlus::TrackerSingleMarkerImplARToolkitPlus::cv::RGB24Pixel( width, height); tracker-setCameraFile(AR_cali.cal); tracker-setMarkerWidth(80.0); // 二维码物理宽度单位 mm tracker-setPatternWidth(80.0); tracker-setThreshold(120); // 二值化阈值 tracker-setBorderWidth(0.5); // 标记边框占整体比例 return tracker; }代码逻辑说明构造函数里的width和height必须与采集分辨率一致否则畸形校正后的坐标会错位setCameraFile把相机内参和畸变系数交给库它在内部完成像素到归一化坐标的变换setMarkerWidth决定物理尺度直接影响后面 EKF 里“二维码离机器人多远”的计算单位必须全局统一。setThreshold(120)是经验值光线变亮时调低变暗时调高调试时可以用rosrun rqt_reconfigure动态改。检测循环里取出 ID 和角点vectorint ids(1024, 0); vectorfloat conf(1024, 0.0f); int count tracker-calc(cv_img.data, ids.data(), conf.data()); for (int i 0; i count; i) { float corners[8]; tracker-getMarkerCorners2D(i, corners); // corners 顺序: 左上, 右上, 右下, 左下 int marker_id ids[i]; float x1 corners[0], y1 corners[1]; float x2 corners[2], y2 corners[3]; float x3 corners[4], y3 corners[5]; float x4 corners[6], y4 corners[7]; }这段代码常见的问题是角点顺序。ARToolkitPlus 返回的是屏幕坐标系下的浮点像素坐标原点在图像左上角x 向右y 向下。如果直接拿去做欧氏距离计算必须先转换到相机坐标系。多二维码同时出现时calc返回的 ID 顺序没有明确保证所以不要用数组下标当作路标 ID必须用ids[i]。2.3 二维码 ID 管理与位姿解算二维码作为路标ID 管理决定了 EKF 的观测函数能不能找到对应路标。推荐把 ID 和物理尺寸写进 YAML 配置而不是硬编码在代码里markers: 0: {size_mm: 80.0, z: 0} 1: {size_mm: 80.0, z: 0} 3: {size_mm: 50.0, z: 0}z表示标记中心相对地面的高度。机器人只能观测到与摄像头在同一平面附近的二维码存在高度差时需要把观测模型中的像素投影加一个固定平移量。下表汇总了用来调检测灵敏度的参数参数作用调大影响调小影响setThreshold二值化阈值标记内部空洞变小暗处易漏检明处误检增多setBorderWidth边框宽度占比远近视角更稳定小尺寸易丢失小距离易误识别setMarkerWidth标记物理宽度尺度估计偏大尺度估计偏小setLogger日志输出输出过多拉高耗时关闭后问题难排查一幅图像里识别到 10 个以上二维码时不要全丢给 EKF。只取距离最近或角度差异最大的 3 到 4 个否则矩阵求逆耗时上去了还容易因为远端标定误差引入错误约束。3. OpenCV 去畸变与相机标定文件解析3.1 从相机驱动到.cal格式.cal是 ARToolkit 体系下的文本标定文件里面存的是焦距、主点、畸变系数等参数。由于不同版本 ARToolkitPlus 对格式的解析有差异拿到AR_cali.cal后第一件事不是直接使用而是打印前几行。常见结构类似# camera parameters 640 480 1.2 0.0 320.0 0.0 1.2 240.0 -0.35 0.11 0.0 0.0前三行对应分辨率和内参矩阵第四行以后是径向与切向畸变。某些生成工具会把畸变放成一行也可能用空格分段。不要迷信文件后缀no_distortion_small_camera.cal~这种带波浪号的通常是临时文件可能是人工编辑时留下的备份。在 ROS 里做标定最省事的是使用camera_calibration它会输出 YAML然后需要手动翻译成 ARToolkitPlus 的.cal。我一般会写一个 Python 小工具把 YAML 的内参矩阵和畸变系数按 ARToolkitPlus 要求的顺序重排。这个过程最容易错的是畸变系数顺序OpenCV 是k1, k2, p1, p2, k3ARToolkitPlus 是k1, k2, p1, p2截断不要裁错。3.2 OpenCV 去畸变代码与调用效果检测二维码前先用 OpenCV 去畸变能明显提升远距离角点精度。下面是直接使用cv::undistort的做法#include opencv2/opencv.hpp cv::Mat K (cv::Mat_double(3, 3) 615.0, 0.0, 320.0, 0.0, 610.0, 240.0, 0.0, 0.0, 1.0); cv::Mat D (cv::Mat_double(4, 1) -0.32, 0.11, -0.001, 0.002); cv::Mat raw, undist; cap.read(raw); cv::undistort(raw, undist, K, D);代码逻辑说明K是相机内参矩阵(0,0)和(1,1)是焦距(0,2)和(1,2)是主点D存放畸变系数。cv::undistort内部会创建映射表对整幅图像逐像素重采样。缺点是对 1080p 实时流有一定开销。如果机器人控制器算力有限可以缩小分辨率或者用cv::initUndistortRectifyMap和cv::remap把映射表预先计算好运行时只做 remap我在实际工程里会把后者做成一个独立的 ROS 节点发布去畸变后的图像话题。另一个选择是跳过去畸变在 ARToolkitPlus 内部使用.cal自带的畸变参数。此时 OpenCV 只做灰度变换和颜色格式转换。这个方案省一次全图重采样对帧率提升明显前提是标定文件里的畸变系数足够准。3.3 no_distortion 系列标定文件的选择逻辑压缩包里出现no_distortion_small.cal、no_distortion_big.cal和no_distortion_caliBoard.cal我倾向于把它们看成同一套参数在不同标记尺寸下的变体。名字里的small和big指的是二维码在实际场景里占据的画面大小。标记大时角落像素更准对畸变不敏感可以用更激进的阈值标记小时畸变带来的角点偏移会被放大必须选择更保守的参数。实际测试时可以用一个快速脚本给同一帧图像做三次去畸变然后对比二维码角点像素差rosrun qr_slam undistort_test _image:/camera/image_raw _cal:AR_cali.cal如果输出角点坐标差超过 2 个像素就需要用带完整畸变模型的AR_cali.cal而不是 no_distortion 版本。下表是我在调试时使用的选择依据文件特征适用条件不适用情况AR_cali.cal广角镜头、近距离大标记无畸变成像、计算资源紧张no_distortion_small.cal窄视角、标记占图比例小标记边缘有明显桶形畸变no_distortion_big.cal标记占图比例大标记在画面边缘标定本身不值得反复手工操作。先让摄像头对准棋盘格或 asymmetric circles 板转动 5 到 10 组姿态用camera_calibration输出重投影误差小于 0.3 像素的标定结果再转成.cal。误差大于 0.5 时换掉标定板姿态不要强行使用。4. EKFSLAM 数据融合里程计与二维码观测的更新机制4.1 状态向量与协方差结构EKFSLAM 把机器人位姿和所有二维码位置放进同一个状态向量x [x_r, y_r, θ, x_l0, y_l0, x_l1, y_l1, ...]前三维是机器人在地图中的坐标和朝向后面每两维对应一个二维码中心坐标。协方差矩阵是方阵维度等于状态长度。初始时刻二维码未知可以把检测到首个二维码时的机器人位姿作为路标初始位置。注意状态向量一旦确定二维码顺序不要随意增删否则语义丢失。二维码观测本质上是“机器人相对路标的位置”函数。这里有两层投影世界坐标到机器人坐标的变换以及机器人坐标到像素平面的相机投影。EKF 只处理高斯噪声因此观测噪声和运动噪声都要建模成零均值高斯。ARToolkitPlus 返回的角点噪声不是完全对称的近距离时角点抖动小远距离角点抖动大简单做法是把观测协方差设成固定矩阵但精确做法是让协方差随距离缩放。4.2 预测与观测更新代码骨架运动预测使用里程计增量。通常机器人发布的 odom 话题包含线速度和角速度直接离散化即可#include Eigen/Dense void predict(Eigen::VectorXd mu, Eigen::MatrixXd sigma, double v, double w, double dt) { double theta mu(2); Eigen::Vector3d delta; delta v*dt*cos(theta), v*dt*sin(theta), w*dt; mu.head(3) delta; Eigen::Matrix3d G; G 1, 0, -v*dt*sin(theta), 0, 1, v*dt*cos(theta), 0, 0, 1; Eigen::Matrix3d Q Eigen::Matrix3d::Identity() * 1e-3; sigma.block3, 3(0, 0) G * sigma.block3, 3(0, 0) * G.transpose() Q; }代码逻辑说明G是运动模型对机器人状态的雅可比矩阵它把里程计噪声映射到状态协方差。Q是过程噪声取值不能过大否则二维码观测更新后机器人位姿仍然可能发散也不能过小否则跟踪速度慢。实际路由器的轮式里程计通常比视觉里程计噪声大Q的三维值可以设为[0.05, 0.05, 0.01]。观测更新部分假设摄像头形成二维像素观测则预测值来自路标位置和机器人位姿void update_landmark(Eigen::VectorXd mu, Eigen::MatrixXd sigma, int landmark_id, double zx, double zy) { int k 3 2 * landmark_id; double dx mu(k) - mu(0); double dy mu(k 1) - mu(1); double theta mu(2); // 将路标变换到机器人坐标系 double bx cos(theta) * dx sin(theta) * dy; double by -sin(theta) * dx cos(theta) * dy; Eigen::Vector2d h; h bx, by; // 这里省略相机内参投影实际项目中用 K 矩阵将 bx,by 映射到像素 Eigen::Matrixdouble, 2, 5 H; H.setZero(); // H 的前三列是对机器人位姿的偏导后两列是对路标位置的偏导 // 完整推导见源码包中的 ekf_core.cpp Eigen::Matrix2d R Eigen::Matrix2d::Identity() * 0.01; Eigen::Matrix2d S H * sigma * H.transpose() R; Eigen::Vector2d innovation; innovation zx - h(0), zy - h(1); Eigen::MatrixXd K sigma * H.transpose() * S.inverse(); mu K * innovation; sigma (Eigen::MatrixXd::Identity(mu.size(), mu.size()) - K * H) * sigma; }代码逻辑说明这里用机器人局部坐标作为观测值H是观测模型对全状态量的雅可比维度是2 x NN 为状态长度。实际项目里不要尝试手算所有偏导最稳妥的方式是先用 Ceres 或者 Eigen 的自动求导验证再把解析式填入矩阵。像素观测和局部坐标观测之间的区别是像素观测还必须让 H 乘上相机内参矩阵否则可能出现“二维码明明在图像中心EKF 却认为它在侧面”的不一致。4.3 EKF 调参与数据关联数据关联错误是二维码 SLAM 最典型的发散原因。某个二维码 ID 一旦被赋值给路标后续观测就不能复用。常见错误是把同一次calc返回的多个 ID 当成多个不同路标实际上如果相机连续帧都识别出同一个 ID应该先做马氏距离门控只有当新观测距离预测位置小于设定阈值时才更新。阈值一般是 3 倍标准差也就是高斯分布下 99.7% 的置信区间。参数位置经验值影响Q运动噪声预测函数1e-3 ~ 5e-3太大定位抖太小建图滞后R观测噪声更新函数0.01 ~ 0.05太大不信任二维码太小被错误观测带偏初始路标协方差新增路标时矩阵块 1e-2决定首帧路标误差数据关联阈值更新前2 ~ 4 倍标准差过小漏关联过大错误关联我调试的顺序是先固定二维码不动机器人原地旋转观察从第一个二维码建立的坐标是否来回漂移。如果漂移明显优先检查theta的协方差因为角度误差会通过坐标变换放大到所有路标。5. 把二维码 SLAM 包跑在 ROS 上launch、rviz 与排错5.1 编写 node 与 launch 文件源码包里的 ROS 节点应该拆成三部分图像采集、二维码检测、EKFSLAM。这样单独切换相机驱动不影响核心算法。launch 文件建议这样组织launch node namecamera_node pkgusb_cam typeusb_cam_node param namevideo_device value/dev/video0/ param namepixel_format valueyuyv/ param nameimage_width value640/ param nameimage_height value480/ /node node nameqr_slam_node pkgqr_slam typeqr_slam_node outputscreen param namecalibration_file value$(find qr_slam)/calib/AR_cali.cal/ param namemarker_size_mm value80.0/ param nameodom_topic value/odom/ /node /launch这个 launch 文件的价值在于把所有和环境相关的参数暴露在 XML 层不用为了换相机重编源码。marker_size_mm必须和真实打印的二维码一致否则地图尺度会整体缩放。odom_topic需要确认是真实的轮式里程计还是 gazebo 仿真里程计两者的噪声特性差别很大。5.2 在 rviz 里读图运行后先把摄像头图像话题加到 rviz确认二维码角点有没有被画出边框。然后添加TF面板观察map、odom、base_link三个坐标系是否成链。如果 EKF 节点正常会在base_link到二维码路标之间形成观测关联估计出的路标位置会显示为MarkerArray。检查rosrun tf view_frames生成的 pdf 图定位在map下的机器人位姿是否有跳变。也可以用命令行快速看二维码检测频率rostopic hz /qr_slam/marker_pose话题发布频率低于 10Hz 时优先怀疑 ARToolkitPlus 检测耗时或者去畸变节点阻塞。高于 30Hz 时要注意 ECKF 更新是否被跳过。5.3 定位发散时的具体检查项如果机器人不动而地图中路标位置来回跳先查时间戳。使用rosbag回放时图像和里程计时间不同步会导致观测更新延迟我遇到最多的问题是某条 ROS 消息时间戳比当前时刻晚 100ms 以上此时 EKF 的预测和更新顺序出现颠倒。解决方案是统一使用ros::Time并确保相机驱动发布了真实的采集时间。另一个高频错误是二维码 ID 重合。两套标定板如果共用相同 ID代码会把两个物理位置完全不同的路标当成一个协方差矩阵会被错误拉紧。排错时打印mu向量看距离很近的两个路标是否都对应同一个 ID并在地图上重新检查二维码摆放顺序。每次新增路标后建议把 EKF 状态协方差的最大特征值打印到日志只有它随时间下降且最终稳定才算建图成功。本文还有配套的精品资源点击获取