文章目录
前言
我用最简单、最实用的方式讲清楚：怎么从视觉二维码 / 标记 → 算出机器人的位置和朝向（位姿）。
现在的场景和需求是：
- 机器人上有摄像头
- 环境里贴了 ArUco 视觉标记（Marker）
- 想通过摄像头看到 Marker → 解算出机器人在地图中的坐标和朝向
一句话解释如何得到：摄像头看到 Marker → 计算出摄像头相机相对于 Marker 的位姿 → 通过坐标变换（TF）得到机器人相对于地图 / 世界坐标系的位姿。
实现步骤
ROS/ROS2 中，通过视觉 Marker 解算机器人位姿，一般按四个标准步骤实现：
前期标定
首先完成两个关键标定：
- 相机内参标定
使用标定板得到相机内参矩阵和畸变系数，用于后续去畸变和位姿解算。 - 2、手眼标定（外参）
得到相机相对于机器人本体坐标系（base_link）的变换关系
T_{base}^{camera} 或 T_{camera}^{base}。
视觉 Marker 检测与位姿解算
使用 ArUco ：
1、启动相机驱动usb_cam ，发布图像话题。
2、运行 Marker 检测节点，识别图像中的 Marker ID 与角点。
3、基于 Marker 实际物理尺寸，通过 PnP 算法 解算相机相对于 Marker 的旋转矩阵和平移向量。
4、节点会输出 Marker 的位姿话题（pose），并发布 TF 坐标变换。
构建完整 TF 坐标树
关键是建立一条完整坐标变换链：
bash
map → marker → camera → base_link预先发布 Marker 在地图 / 世界坐标系下的固定位姿 TF
视觉节点发布 marker → camera
手眼标定结果发布 camera → base_link
TF 树会自动闭合，直接可查机器人在世界系下的位姿。获取并使用机器人位姿
1、 通过 TF 监听 map → base_link，直接得到机器人 6DoF 位姿。
2、 可将该位姿用于：
-
里程计漂移校正
-
全局重定位
-
精准停靠 / 抓取
工程上通常会与轮速里程计、IMU 进行 EKF/UKF 融合，提升平滑度和鲁棒性。
总结
整体流程就是：标定 → 检测解算 → TF 坐标变换 → 多传感器融合定位，最终得到机器人在世界坐标系下的精确位姿；
详细流程总结：
- 先做相机内参标定和相机与机器人的手眼标定，得到相机内参和坐标系外参。
- 摄像头识别 ArUco这类 Marker，用PnP 算法解算出相机相对于 Marker 的位姿。
- 已知 Marker 在世界 / 地图中的固定坐标，通过TF 坐标变换，把坐标系串起来： 世界→Marker→相机→机器人本体。
- 最终直接得到机器人在世界坐标系下的完整位姿，工程上还会和里程计、IMU 融合，让定位更稳定。
常见问题解答
1、 为什么要用 PnP？
因为已知 3D 物理点（Marker 角点）和 2D 图像点，
PnP 能唯一求解相机相对于 Marker 的旋转和平移，是视觉定位标准算法。
2、 只有一个 Marker 够吗？
单个 Marker 可定位，但容易受遮挡、抖动影响。工程上一般用多个 Marker 构成标签板，增加观测约束，提高精度和稳定性。
3、怎么和导航融合？
将视觉解算的位姿作为观测值，输入到 robot_localization 等滤波节点，与里程计、IMU 融合，输出平滑、连续、抗漂移的定位结果。
PnP 算法详解
- PnP 就是已知 2D点、3D点、内参 K，反解外参 R、t。
简述：PnP 是根据已知的 3D 空间点和对应 2D 图像点，求解相机位姿的算法。在 Marker 定位中，用 Marker 四个角的 3D 坐标和图像 2D 坐标，结合相机内参，通过 PnP 解算出相机相对于 Marker 的旋转和平移，最终推导出机器人位姿
作用：已知N个3D空间点，以及它们在图像上的 2D 投影点，求解相机在空间中的6DoF位姿（旋转 R + 平移 t）。
- 3D点：Marker 四个角点在Marker 自身坐标系下的坐标（已知）；
- 2D 点：这四个角在图像上的像素坐标（检测出来）；
- 求解：相机相对于 Marker 的 R、t；
2、 核心原理
根据小孔成像模型：
简单解释：
- 左边：图像 2D 像素点；
- 右边 K：相机内参（焦距、光心，已标定）；
- R\|t：相机外参 → 就是我们要求的位姿；
- XYZ：3D 空间点
- 需要多少个点？
P3P：最少 3 个点，可求出 4 组解，再用第 4 点验证
Marker 正好 4 个角点，非常适合 PnP，点越多，鲁棒性越强，通常配合 RANSAC 去除外点噪声； - 常用求解方法
1、P3P
几何解法，速度快，适合实时视觉定位。
2、EPnP（Efficient PnP）
精度高、速度快，OpenCV 默认用这个，是工程首选。
5、PnP 到底是怎么算出相机位姿的？
可以这么理解：我在空间里放了几个已知精确位置的点（就是 Marker 的四个角），相机拍下它们，得到图像上的像素点。因为相机的成像规律是固定的（小孔成像），同一个空间点，相机站在不同位置、不同角度，拍到的像素位置是不一样的。
PnP 就是反过来算：根据这几组 "空间点 ↔ 像素点" 的对应关系，反推出相机当时站在哪儿、朝哪个方向拍；
精简后总结一句话：PnP 就是利用已知的 3D 点和对应的 2D 像素点，根据成像几何关系，反算出相机在空间里的位置和朝向。
用Marker 如何实现自动回充
实现思路：
1、机器人在导航快接近充电桩时，启动视觉检测寻找充电桩上贴的Marker。
2、通过 PnP 算法解算相机相对于 Marker 的精确位姿得到相机到充电桩的距离、偏角、俯仰角。
3、通过坐标变换，得到机器人base_link 相对于充电桩的位姿知道机器人离充电座还有多远、偏了多少角度。
4、进入精细对准阶段
机器人根据视觉位姿，进行小范围平移、旋转，一步步调整姿态，精准对准充电电极。 完成对接后缓慢后退 / 前进，实现自动充电
整个过程就是：
全局导航粗略靠近 → 视觉 Marker 精定位 → 闭环控制对准 → 充电对接
以上讲解如果帮到您请点赞加收藏哦，如有错误欢迎指正！