基于Apriltag和UWB的智能小车定位系统并和动捕真值进行实验对比

 

引言：

        本篇文章将详细介绍如何使用apriltags、UWB以及动捕设备获得智能小车的定位数据并进行对比实验。主要内容包括实验环境介绍、工具包的安装和测试、实验场景布置、数据采集和数据分析。进行后续实验的基础是已经装好了Ubuntu20.04以及ROS（noetic），并配置好了你的相机ros驱动。

一、实验环境介绍

        1.1 设备介绍

          1.1.1 turtlebot3_burger

           这是本次实验使用的实物机器人，它是一个ROS标准平台机器人。具体介绍可以查看之前的博客初识TurtleBot3 burger-CSDN博客。

          1.1.2 realsenseD435i

           实验使用的相机，用于识别Apriltags。这里默认你已经安装好相机的ROS驱动包了，就是能用roslaunch启动相机。

        按理来说实验之前需要进行相机标定获得相机内外参，我用的是kalibr标定的，但是结果不好，所以直接使用的原厂参数进行的实验。详细信息参考https://github.com/ethz-asl/kalibr/wiki/installation下面是个人安装kalibr的步骤以及对realsenseD435i的标定：

# 安装依赖
sudo apt-get install python3-catkin-tools python3-osrf-pycommon 
sudo apt-get install -y \
    git wget autoconf automake nano \
    libeigen3-dev libboost-all-dev libsuitesparse-dev \
    doxygen libopencv-dev \
libpoco-dev libtbb-dev libblas-dev liblapack-dev libv4l-dev
sudo apt-get install -y python3-dev python3-pip python3-scipy \
python3-matplotlib ipython3 python3-wxgtk4.0 python3-tk python3-igraph python3-pyx

# 创建工作目录并编译标定库
mkdir -p ~/kalibr_workspace/src
cd ~/kalibr_workspace
export ROS1_DISTRO=noetic 
source /opt/ros/$ROS1_DISTRO/setup.bash
catkin init
catkin config --extend /opt/ros/$ROS1_DISTRO
catkin config --merge-devel
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release
git clone https://github.com/ethz-asl/kalibr.git
catkin build -DCMAKE_BUILD_TYPE=Release -j4

        下图就是编译成功的效果：

        标定步骤：

source ~/kalibr_workspace/devel/setup.bash
# 生成二维码PDF，打印出来贴在墙上或者找个平板
rosrun kalibr kalibr_create_target_pdf --type apriltag --nx 6 --ny 6 --tsize 0.022 --tspace 0.3

新建april_6x6_A4.yaml 文件，里面写入下面内容
target_type: 'aprilgrid' #gridtype
tagCols: 6               #number of apriltags
tagRows: 6               #number of apriltags
tagSize: 0.0219           #size of apriltag, edge to edge [m]  要亲自拿尺子量一下
tagSpacing: 0.30          #ratio of space between tags to tagSize

# 关闭相机结构光
roslaunch realsense2_camera rs_camera.launch
rosrun rqt_reconfigure rqt_reconfigure

        关闭结构光：

        新终端运rviz

rviz

        软件打开之后点击add添加rgb和双目对应的topic，我这里没有插入相机所以没有话题，正常就可直接找到下面的话题，

/camera/color/image_raw、/camera/infra1/image_rect_raw、/camera/infra2/image_rect_raw

# 重新映射话题名称并修改相机帧数
rosrun topic_tools throttle messages /camera/color/image_raw 4.0 /color
rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 4.0 /infra_left
rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 4.0 /infra_right

# 录制ROS包
rosbag record -O multicameras_calibration /infra_left /infra_right /color

录制时的要求：通过rviz界面观察，手持相机对准标定板。俯仰角摆动3次；偏航角左右摆动3次；滚转角摆动3次；上下移动3次；左右移动3次；前后移动3次；然后自由移动一段时间，摆动幅度要大一点，让视角变化大一点，但是移动要缓慢一点，同时要保证标定板在3个相机视野内部，整个标定时间要在90s以上更好。下面是录制数据的视频教程

Kalibr相机及IMU校准教程(Tutorial: IMU-camera calibration)_哔哩哔哩_bilibili

# 数据录制结束后进行标定，记得更换命令里面的路径
source ~/kalibr_workspace/devel/setup.bash
rosrun kalibr kalibr_calibrate_cameras \
--target ~/kalibr_workspace/src/kalibr/calibration_space/april_6x6_A4.yaml \
--bag  ~/kalibr_workspace/src/kalibr/calibration_space/multicameras_calibration.bag \
--models pinhole-radtan pinhole-radtan pinhole-radtan \
--topics /infra_left /infra_right /color \
--bag-from-to 10 207 --show-extraction --approx-sync 0.04

         查看标定结果：重点关注camera_calibration_4Hz-results-cam.txt中的重投影误差reprojection error数值，理想范围是0.1-0.2。查看pdf可视化结果，三个相机重投影误差reprojection error，在1px以内就行。

        这是网上标定较好的参考图：

        这是我自己标定的结果：

          1.1.3 LinkTrack P-AS2

           LinkTrack P-AS2是一款基于UWB（超宽带）技术的定位系统，集测距、定位、数传功能于一体。它可能具备高精度的定位能力，典型二维定位精度可达±10厘米，数据更新频率可以根据需求设定，支持5HZ、10HZ、25HZ、50HZ。它还支持多种工作模式，如LP（局部定位）、DR（分布式测距）、DT（数传）等。这里使用的就是LP功能。下面是实物图：

        1.2 实验环境

                虚拟机：VMware® Workstation 16 Pro

                Ubuntu：20.04

                ROS：noetic

        1.3 实验场景和坐标系转换

            根据网上已经生成好的Apriltags然后打印在A4纸上面，我这里使用的是热心网友生成好的apriltag，我保存了一份在百度网盘，网盘下载链接: https://pan.baidu.com/s/1iMMeuqREA2TUYtUoWRD4ww?pwd=hy4y

             下面是贴好 apriltags的场景图，贴在天花板可以方便坐标系转换。也可以根据自己的实验场景布置二维码。

            建议apriltag坐标系和世界坐标系有明显的夹角，这样方便计算apriltag在世界坐标系里面的四元数。apriltag坐标系如下，世界坐标系根据自己需求定义。

         下面是世界坐标系、apriltag坐标系以及UWB坐标系的定义。UWB使用之前需要先设置好角色（Tag、anchor、console）并进行标定，详细内容查看2.2节。

        UWB搭建参考的是官方推荐，表6里面的第一种情况。

        坐标系变换：坐标系变换的目的是把定位设备的数据对齐到世界坐标系里面，这样方便对比定位精度。UWB坐标系和世界坐标系的X，Y，Z方向完全一致，所以把UWB对齐到世界坐标里面非常容易，每个分量直接减去补偿值即可，我这里的补偿值是(X,Y,Z) = (1.2 ,0.6 ,-1.15)，UWB采集到的数据直接减去这个补偿值即可对齐到世界坐标里面。重点说一下apriltag定位，apriltag_ros包输出的定位信息是相机坐标系下面apriltag中心的的位姿，我们需要做的就是把相机坐标系下的位置信息转换到世界坐标系下面。第一步转换很好实现，根据apriltag_ros输出的apriltag坐标和位姿信息可以很方便的把相机坐标系下apriltag位姿转换为apriltag坐标系下相机的位姿。只需要根据输出的四元数得到相机到apriltag的旋转矩阵，然后对旋转矩阵求逆（转置）得到apriltag到相机的旋转矩阵，然后使用旋转矩阵左乘输出的坐标即可把相机坐标系下apriltag的位姿转换为apriltag坐标系下相机的位姿。第二步转换就是把apriltag坐标系转到世界坐标系里面，apriltag坐标系X方向和世界坐标系一致，Y和Z的方向不同，但是绕着X轴旋转180度之后就会转到世界坐标里面，对应的四元数是(W,X,Y,Z) = (0,1,0,0)。后面还需要测得每一个apriltag中心点在世界坐标系里面的坐标，可以手动拉尺子测量，但是这样误差大也不方便，还有一种方式就是使用相机测量，这也意味着初始的时候就已经有1-2cm的误差了，所以我的测量方式是使用动捕软件测量，动捕误差是1-2mm，这也就是为啥会把动捕的数据作为真值的原因。有了apriltag在世界坐标系里面的坐标和四元数就可以把apriltag坐标系转换到世界坐标系里面，这也是使用apriltag定位的核心。

        使用上面的坐标转换方法就可以把UWB和apriltag的定位信息都转换到世界坐标系里面进行定位精度对比。也可以参考这个博客apriltags2_ros应用实践——如何用apriltags二维码输出相机位姿与轨迹_apriltag坐标系-CSDN博客，直接在源码里面进行修改，这样apriltag_ros输出就直接是相机在世界坐标里面的位姿。

二、工具包安装和测试

        Windows里面安装NAssistant和串口是为了方便后面设置UWB和一键标定，当然你也可以选择不在Windows里面安装，直接在Ubuntu里面测试。

     2.1 Windows安装串口驱动

        在空循环官网下载串口驱动：资料下载 - Nooploop空循环，Windows里面需要安装两个CP210X、CH343，安装方式也是直接下一步即可完成安装

     2.2 Windows安装软件NAssistant

        NAssistant软件也在空循环官网下载即可，安装方式也是直接下一步（根据自己情况修改安装路径）。下面是安装好串口驱动和软件后的测试界面，插上UWB之后就可以看到中间的数据区域一直在变。在就意味着串口通信没啥问题。

        然后根据空循环的用户手册设计对每一个UWB模块挨个设置角色，我这里需要四个基站（anchor）和一个标签（Tag）,出厂默认是Tag，所以需要把其余的四个UWB挨个插到电脑上打开软件设置角色，在设置过程确保每一个基站（anchor）的硬件版本和固件版本是否一致。下面是anchor和Tag的配置，配置好后点击写入参数即可。

 

        上述配置完之后所有的anchor上电进行一键标定，pc连接其中一个anchor，然后点击

左边会有一键标定的按钮，点击之后等十几秒就可以在软件的2D和3D视图里面看到每一个基站的位置，一键标定对新手很友好但是误差大，我试了一下一键标定误差能有15-30cm左右，这也是每个基站互相通信得到位置，肯定没有自己拉尺子的准，所以也可以自己写入坐标参数，就是标定结束后直接在标定的坐标上修改，然后写入参数即可。后续如果实验场景不变（UWB位置不动）的话就不需要重新标定了，保证连接电脑进行标定的基站上电即可读取上一次标定的坐标。

     2.3 Ubuntu安装Nassistant和串口驱动

        NAssiatant安装包也是在空循环官网下载，然后命令行打开文件位置，给它赋予执行权限后安装，安装过程和Windows一样，直接下一步完成安装。

# 先cd到安装包目录，然后加执行权限，最后安装
chmod +x nassistant_ubuntu_64bit
./nassistant_ubuntu_64bit

        Ubuntu串口驱动只需要安装一个CH343，这个安装包也是在空循环官网进行下载，或者使用下面的克隆方式进行安装

git glone https://github.com/WCHSoftGroup/ch343ser_linux
cd ch343ser_linux/drive
make #（编译成功会看到ch343.ko）
sudo make load 或 sudo insmod ch343.ko #（随便执行其中一条）
sudo make install #（永久安装驱动包）

 

        驱动安装完成后，将产品通过USB线或者Nooploop官方的NUTT（NUTT-B、NUTT-C）型号的USB转TTL模块连接Linux设备，然后在命令行执行ls -l /dev/ttyUSB*；如果没有找到对应的端口则执行ls -l /dev/ttyCH343USB* ；如果能找到ttyCH343USBx端口，说明产品使用的是CH343的芯片，在后续的修改launch文件步骤中要注意修改对应的端口号。如果执行ls -l /dev/ttyUSB*和ls -l /dev/ttyCH343USB*都找不到端口，但是执行ls -l /dev/ttyACM*可以找到端口如ttyACM0，则根据找到的端口号（以ttyACM0为例）执行sudo chmod 777 /dev/ttyACM0然后重启电脑，再执行ls -l /dev/ttyCH343USB*，一般可以找到对应CH343端口。

        第一次使用串口设备通常需要开启串口权限，参考下面的链接，其实就是把当前用户加入dialout组Fix serial port permission denied errors on Linux - Jesin's Blog

# 我的用户名是Ubuntu，根据自己情况进行修改
id -Gn ubuntu
sudo usermod -a -G dialout ubuntu 

​

     2.4 Ubuntu安装串口库

git clone https://github.com/nooploop-dev/serial.git
cd serial
make
sudo make test
sudo make install

​

     2.5 Ubuntu安装nlink_parser（UWB的ROS支持包）

        Github官网页面：https://github.com/nooploop-dev/nlink_parser，但是直接在官方ROS驱动页面下载zip压缩包是错误的下载方式，会缺少子模块（serial）并在后续编译的时候报错。安装严格按照下面命令进行

mkdir -p ~/nooploop_ws/src
cd ~/nooploop_ws/src
catkin_init_workspace
git clone --recursive https://github.com/nooploop-dev/nlink_parser.git
cd .. && catkin_make
echo "source ~/nooploop_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc

         产品连接NAssistant软件正常识别和输出数据后，才能连接ROS测试，否则后续步骤会出现问题。ros测试出现waiting for data是没有关掉NAssistant软件。 LinkTrack系列产品，需要按照官网下载的用户手册、数据手册等资料，LP模式下搭建好测试环境(连接console或anchor进行一键标定获得正确刷新的标签坐标，同时需要注意此时所有的控制台和基站都需要处于上电状态)，标签连接NAssistant软件成功识别并输出正确的坐标以后再关闭NAssistant软件然后连接ROS。  

        ROS里面测试

roscore
cd ~/nooploop_ws && catkin_make run_tests
ls -l /dev/ttyCH343USB*  # 设备是CH343芯片

# 记录多出来的这个ttyUSB（或ttyCH343USB）端口号然后修改、运行产品对应launch文件：
vim ~/nooploop_ws/src/nlink_parser/launch/linktrack.launch

         修改完启动文件的端口之后执行下述命令启动数据接收（必须关掉NAssistant软件）

roslaunch nlink_parser linktrack.launch

# 查看话题信息
rostopic echo /nlink_linktrack_nodeframe2

         红框标注的就是定位信息，到这儿UWB测试就结束了。

     2.6 Ubuntu安装apriltag_ros包安装

        后面需要修改配置文件，所以这里使用源码安装

mkdir -p ~/aprilTag/src    
cd ~/aprilTag/src                     
git clone https://github.com/AprilRobotics/apriltag.git 
git clone https://github.com/AprilRobotics/apriltag_ros.git
cd ~/aprilTag                          
rosdep install --from-paths src --ignore-src -r -y  
catkin build (或者使用catkin_make_isolated)
source devel/setup.bash
echo "source ~/aprilTag/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc

        安装成功后修改配置文件：对~/aprilTag/src/apriltag_ros/apriltag_ros/config文件夹下的配置文件(settings.yaml和tags.yaml)进行修改 

# settings.yaml配置文件
tag_family: 'tag36h11'      # 标签家族必须与实际打印的标签一致
max_hamming: 0              # 允许的最大汉明误差（0 表示严格模式）
decimate: 1.0               # 图像降采样因子（1.0 不降采样，2.0 分辨率减半）
blur: 0.0                   # 高斯模糊半径（0 表示不模糊）
refine_edges: 1             # 是否优化边缘检测（1 启用）
debug: 0                    # 调试模式（0 关闭，1 显示中间图像）
# 相机参数（如果使用真实相机，以下参数会被覆盖）
publish_tf: true            # 发布 TF 变换（关键！自定位必须启用）
camera_frame: 'camera_color_optical_frame'  # Realsense 相机的光学坐标系帧
publish_tag_detections_image: true  # 启用检测结果图像发布

# tags.yaml配置文件
standalone_tags:
  [
    {id: 0, size: 0.185},  # 标签物理边长（单位：米，需实际测量）
    {id: 1, size: 0.185},
    {id: 2, size: 0.185},
    {id: 3, size: 0.185},
    {id: 4, size: 0.185}
  ]
# 定义标签在三维空间中的坐标（贴好apriltag测得在世界坐标系里面的位置后写在此处）
tag_bundles: []

        修改启动文件 

# 修改启动文件，~/aprilTag/src/apriltag_ros/apriltag_ros/launch文件夹下面修改
continuous_detection.launch，根据自己的相机话题进行修改
# 如果有自己的相机标定文件就可以指定文件路径<param name="camera_info_url" value="file:///path/to/your/camera_info.yaml"/>

<launch>
  <arg name="camera_name" default="/camera/color" />    <!-- Realsense 默认话题前缀 -->
  <arg name="image_topic" default="image_raw" />        <!-- 图像话题 -->
  <arg name="info_topic" default="camera_info" />       <!-- 相机标定信息话题 -->
  <arg name="launch_prefix" default="" />
  <!-- 启动 AprilTag 检测节点 -->
  <node pkg="apriltag_ros" type="apriltag_ros_continuous_node" name="apriltag_detector" output="screen" launch-prefix="$(arg launch_prefix)">
    <remap from="image_rect" to="$(arg camera_name)/$(arg image_topic)" />
    <remap from="camera_info" to="$(arg camera_name)/$(arg info_topic)" />
    <!-- 加载参数文件 -->
    <rosparam command="load" file="$(find apriltag_ros)/config/settings.yaml" />
    <rosparam command="load" file="$(find apriltag_ros)/config/tags.yaml" />
  </node>
</launch>

        ROS里面进行测试

# 启动相机
roslaunch realsense2_camera rs_camera.launch align_depth:=false  # 关闭深度对齐（减少计算量）
# 启动apriltag_ros持续检测apriltag
roslaunch apriltag_ros continuous_detection.launch
# 查看检测效果
rqt_image_view

         查看话题输出

rostopic echo /tag_detections

        position：标签中心在相机坐标系下的坐标（apriltag_ros已转为相机坐标系）。

        z=0.32 表示标签中心距离相机镜头 0.32米。

        x=0.005 表示标签中心在相机坐标系右侧 0.005米。

        y=-0.03 表示标签中心在相机坐标系下方 0.03米

        也可以使用rviz可视化相对位姿

rosrun rviz rviz
# 在RViz中添加TF显示，然后再Fixed Frame里面修改为相机话题，然后就可以确认标签的坐标系（如tag_0、tag_1等）是否出现在相机坐标系下

     2.7 Ubuntu安装Vrpn（接收动捕数据的ROS支持包）

cd ~/catkin_ws/src
git clone https://github.com/ros-drivers/vrpn_client_ros.git
cd ~/catkin_ws
catkin_make -DCATKIN_WHITELIST_PACKAGES="vrpn_client_ros"
source devel/setup.bash
# 检测安装，可以查看是否安装好vrpn：
apt search vrpn

        测试连接 

roslaunch vrpn_client_ros sample.launch server:=10.1.1.198

        查看数据接收 

rostopic echo /vrpn_client_node/Tracker0/pose

         到这儿三个定位设备的数据都可以使用ROS话题接收了，后续只需要编写程序订阅这些话题就可以获得定位数据了。

三、采集数据

     3.1 创建ROS工作空间

mkdir -p ~/nokovtest/src
cd ~/nokovtest/src
catkin_init_workspace
cd ~/nokovtest
catkin_make
cd ~/nokovtest/src
catkin_create_pkg robot_trajectory rospy roscpp geometry_msgs nav_msgs
cd robot_trajectory
mkdir src
cd src
# 在下面的文件里进行编码实现自己的逻辑
touch trajectory.py   # Python版本
chmod +x trajectory.py
touch trajectory.cpp  # C++版本

     3.2 编码测试

# 下面是调AI得到的控制burger移动并进行采数的Python程序，而且只使用ID为0的apriltag数据，根据自己需求进行修改，这里只是参考

#!/usr/bin/env python3
import rospy
import csv
import numpy as np
import math
import tf  # 导入tf模块
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist, PoseStamped, PointStamped
from apriltag_ros.msg import AprilTagDetectionArray
from nlink_parser.msg import LinktrackNodeframe2

# 初始化变量
april_tag_data = []
uwb_data = []
nokov_data = []
start_time = None
initial_yaw = None  # 记录初始角度
last_camera_position_in_world = None  # 新增：存储上一帧相机在世界坐标系中的位置
initial_nokov_position = None
initial_uwb_position = None
tag_positions = {}
uwb_frames = []
nokov_frames = []
uwb_initialization_complete = False
nokov_initialization_complete = False


def get_yaw(orientation):
    """从四元数获取航向角（yaw）"""
    quaternion = (orientation.x, orientation.y, orientation.z, orientation.w)
    euler = tf.transformations.euler_from_quaternion(quaternion)
    return euler[2]  # 返回航向角（yaw）


def odom_callback(data):
    """订阅 /odom 数据，记录小车位置和航向"""
    global start_time, initial_yaw
    if initial_yaw is None:
        if start_time is None:
            start_time = rospy.Time.now()
        initial_yaw = get_yaw(data.pose.pose.orientation)
        rospy.loginfo(f"捕获到初始航向角: {initial_yaw:.2f} rad")


def load_apriltag_world_coords():
    """从ROS参数服务器读取AprilTag坐标（替代YAML文件）"""
    global tag_positions
    try:
        node_namespace = "/apriltag_ros_continuous_node"
        tag_bundles = rospy.get_param(f"{node_namespace}/tag_bundles", [])

        for bundle in tag_bundles:
            for tag in bundle.get('layout', []):
                tag_id = tag.get('id')
                x = tag.get('x', 0.0)
                y = tag.get('y', 0.0)
                tag_positions[tag_id] = np.array([x, y])

        standalone_tags = rospy.get_param(f"{node_namespace}/standalone_tags", [])
        for tag in standalone_tags:
            tag_id = tag.get('id')
            if tag_id not in tag_positions:
                tag_positions[tag_id] = np.array([0.0, 0.0])

        rospy.loginfo(f"从参数服务器加载{len(tag_positions)}个AprilTag坐标")
        return True

    except Exception as e:
        rospy.logerr(f"从参数服务器获取坐标失败: {e}")
        return False


def quaternion_to_rotation_matrix_2d(quat):
    """
    将四元数转换为二维旋转矩阵
    :param quat: 四元数，格式为 [x, y, z, w]
    :return: 2x2 旋转矩阵
    """
    x, y, z, w = quat
    # 计算二维旋转角度
    theta = np.arctan2(2 * (w * z + x * y), 1 - 2 * (y ** 2 + z ** 2))
    # 构造二维旋转矩阵
    rotation_matrix = np.array([
        [np.cos(theta), -np.sin(theta)],
        [np.sin(theta), np.cos(theta)]
    ])
    return rotation_matrix


def april_tag_callback(data):
    global last_camera_position_in_world,start_time

    if data.detections:
        for detection in data.detections:
            tag_id = detection.id[0]
            tag_pose = detection.pose.pose.pose
            # select Id ==0 and cameraToApriltag data
            if tag_id == 0 and tag_pose.position.z > 2.5:
                tag_position = np.array([tag_pose.position.x, tag_pose.position.y, tag_pose.position.z])
                tag_quaternion = np.array(
                    [tag_pose.orientation.x, tag_pose.orientation.y, tag_pose.orientation.z, tag_pose.orientation.w])
                tag_world_position = tag_positions.get(tag_id)

        quat = np.array([tag_quaternion[0], tag_quaternion[1], tag_quaternion[2], tag_quaternion[3]])
        R = quaternion_to_rotation_matrix_2d(quat)
        position = tag_position[:-1]
        camera_rotation_tag = R.T
        camera_position = np.dot(camera_rotation_tag, position)
        apriltag_rotation_world = quaternion_to_rotation_matrix_2d([1, 0, 0, 0])
        camera_position_in_world = tag_world_position - np.dot(apriltag_rotation_world, camera_position)
        last_camera_position_in_world = camera_position_in_world
        if start_time is None:
            start_time = rospy.Time.now()
        elapsed_time = (rospy.Time.now() - start_time).to_sec()
        sensor_timestamp = data.header.stamp.to_sec()
        april_tag_data.append([
            elapsed_time,
            sensor_timestamp,
            tag_id,
            camera_position_in_world[0],
            camera_position_in_world[1]
        ])
    elif last_camera_position_in_world is not None:
        camera_position_in_world = last_camera_position_in_world
        elapsed_time = (rospy.Time.now() - start_time).to_sec()
        sensor_timestamp = data.header.stamp.to_sec()
        april_tag_data.append([
            elapsed_time,
            sensor_timestamp,
            None,
            camera_position_in_world[0],
            camera_position_in_world[1]
        ])
    else:
        rospy.logwarn("未检测到AprilTag且无历史数据，跳过此帧")
        return


def uwb_callback(data):
    """订阅 UWB 数据"""
    global uwb_frames, uwb_initialization_complete, initial_uwb_position,start_time
    try:
        uwb_position = np.array(data.pos_3d)
    except IndexError:
        rospy.logwarn("UWB数据中没有有效的节点信息，跳过此帧")
        return

    # 在初始化阶段，丢弃前5帧数据
    if not uwb_initialization_complete:
        uwb_frames.append(uwb_position)
        if len(uwb_frames) > 5:  # 丢弃前5帧
            uwb_frames = uwb_frames[5:]
        if len(uwb_frames) >= 5:  # 使用接下来的5帧数据计算初始位置
            initial_uwb_position = np.mean(uwb_frames, axis=0)
            uwb_initialization_complete = True
            rospy.loginfo("UWB初始化完成")

    # 如果初始化完成并且允许运动，则开始记录数据
    if uwb_initialization_complete and initial_uwb_position is not None and start_time is not None:
        # 将UWB数据转换到NOKOV坐标系
        uwb_position_in_nokov = transform_uwb_to_nokov(uwb_position)
        elapsed_time = (rospy.Time.now() - start_time).to_sec()
        uwb_data.append([elapsed_time, data.system_time, uwb_position_in_nokov[0], uwb_position_in_nokov[1],
                         uwb_position_in_nokov[2]])


def nokov_callback(data):
    """订阅 NOKOV 数据"""
    global nokov_frames, nokov_initialization_complete, initial_nokov_position,start_time
    nokov_position = np.array([data.pose.position.x, data.pose.position.y, data.pose.position.z])

    # 在初始化阶段，丢弃前5帧数据
    if not nokov_initialization_complete:
        nokov_frames.append(nokov_position)
        if len(nokov_frames) > 5:  # 丢弃前5帧
            nokov_frames = nokov_frames[5:]
        if len(nokov_frames) >= 5:  # 使用接下来的5帧数据计算初始位置
            initial_nokov_position = np.mean(nokov_frames, axis=0)
            nokov_initialization_complete = True
            rospy.loginfo("NOKOV初始化完成")

    # 如果初始化完成并且允许运动，则开始记录数据
    if nokov_initialization_complete and initial_nokov_position is not None and start_time is not None:
        # 将NOKOV数据转换到世界坐标系
        #nokov_position -= initial_nokov_position
       
        elapsed_time = (rospy.Time.now() - start_time).to_sec()
        nokov_data.append([elapsed_time, data.header.stamp.to_sec(), nokov_position[0], nokov_position[1],nokov_position[2]])


def transform_uwb_to_nokov(uwb_position):
    """将UWB坐标转换到NOKOV坐标系"""
    global initial_uwb_position, initial_nokov_position
    if initial_uwb_position is None or initial_nokov_position is None:
        return uwb_position  # 如果初始位置未计算，直接返回原始数据

    # 手动指定UWB到nokov的坐标偏移
    #temp = [1.8, 1.24, -0.92]
    temp1 = [1.2, 0.6, -0.92]
    translation = initial_nokov_position - temp1
    #translation = initial_nokov_position - initial_uwb_position
    return uwb_position + translation


def rotate_to(target_yaw, cmd_pub):
    """旋转小车到指定角度"""
    rospy.loginfo(f"调整朝向到 {target_yaw:.2f} rad")
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        try:
            odom = rospy.wait_for_message("/odom", Odometry, timeout=1.0)
        except rospy.ROSException:
            rospy.logwarn("等待里程计数据超时，继续尝试")
            continue
        current_yaw = get_yaw(odom.pose.pose.orientation)
        error = target_yaw - current_yaw

        if abs(error) < 0.05:
            break

        move_cmd = Twist()
        move_cmd.angular.z = 0.2 if error > 0 else -0.2
        cmd_pub.publish(move_cmd)
        rate.sleep()

    stop_robot(cmd_pub)


def move_straight(distance, speed, cmd_pub):
    """直线运动，修正角度误差"""
    global initial_yaw
    move_cmd = Twist()
    move_cmd.linear.x = speed

    start_time_move = rospy.Time.now()
    duration = distance / speed
    rate = rospy.Rate(10)

    while (rospy.Time.now() - start_time_move).to_sec() < duration:
        if initial_yaw is None:
            rospy.logwarn("等待初始航向角...")
            rate.sleep()
            continue
        try:
            odom = rospy.wait_for_message("/odom", Odometry, timeout=0.1)
        except rospy.ROSException:
            rospy.logwarn("等待里程计数据超时，使用上一帧")
            continue
        current_yaw = get_yaw(odom.pose.pose.orientation)
        yaw_error = initial_yaw - current_yaw
        move_cmd.angular.z = yaw_error * 0.5

        cmd_pub.publish(move_cmd)
        rate.sleep()

    stop_robot(cmd_pub)


def move_circle(cmd_pub, radius=0.5, angular_speed=0.5):
    twist = Twist()
    twist.linear.x = radius * angular_speed
    twist.angular.z = angular_speed

    total_angle = 0
    rate = rospy.Rate(20)
    time_step = 1.0 / 20

    while total_angle < 2 * 3.1415926535:
        cmd_pub.publish(twist)
        total_angle += angular_speed * time_step
        rate.sleep()

    stop_cmd = Twist()
    cmd_pub.publish(stop_cmd)


def stop_robot(cmd_pub):
    """停止小车"""
    move_cmd = Twist()
    move_cmd.linear.x = 0.0
    move_cmd.angular.z = 0.0
    cmd_pub.publish(move_cmd)


def save_to_csv(filename, data):
    """保存数据到CSV"""
    with open(filename, 'w', newline='') as file:
        writer = csv.writer(file)
        if 'apriltag' in filename:
            writer.writerow(["Elapsed Time (s)", "Sensor Timestamp (s)", "AprilTag ID", "X (m)", "Y (m)"])
        else:
            writer.writerow(["Elapsed Time (s)", "Sensor Timestamp (s)", "X (m)", "Y (m)", "Z (m)"])
        writer.writerows(data)

def move_linear_trajectory(cmd_pub):
    """执行带加减速的直线运动"""
    global initial_yaw
    
    # 加速阶段：0.6米，6秒加速到0.2m/s
    acceleration_time = 6.0
    start_time = rospy.Time.now()
    rate = rospy.Rate(10)
    for t in np.linspace(0, acceleration_time, int(acceleration_time * 10)):
        current_speed = 0.2 * (t / acceleration_time)
        move_cmd = Twist()
        move_cmd.linear.x = current_speed
        
        # 航向角修正
        if initial_yaw is not None:
            try:
                odom = rospy.wait_for_message("/odom", Odometry, timeout=0.1)
            except rospy.ROSException:
                rospy.logwarn("获取里程计超时")
                continue
            current_yaw = get_yaw(odom.pose.pose.orientation)
            yaw_error = initial_yaw - current_yaw
            move_cmd.angular.z = yaw_error * 0.5
        
        cmd_pub.publish(move_cmd)
        rate.sleep()

    # 匀速阶段：1米，0.2m/s持续5秒
    constant_speed = 0.2
    for _ in range(50):  # 5秒 @10Hz
        move_cmd = Twist()
        move_cmd.linear.x = constant_speed
        
        if initial_yaw is not None:
            try:
                odom = rospy.wait_for_message("/odom", Odometry, timeout=0.1)
            except rospy.ROSException:
                rospy.logwarn("获取里程计超时")
                continue
            current_yaw = get_yaw(odom.pose.pose.orientation)
            yaw_error = initial_yaw - current_yaw
            move_cmd.angular.z = yaw_error * 0.5
        
        cmd_pub.publish(move_cmd)
        rate.sleep()

    # 减速阶段：0.6米，6秒减速到0
    for t in np.linspace(0, 6.0, 60):
        current_speed = constant_speed - 0.2 * (t / 6.0)
        move_cmd = Twist()
        move_cmd.linear.x = current_speed
        
        if initial_yaw is not None:
            try:
                odom = rospy.wait_for_message("/odom", Odometry, timeout=0.1)
            except rospy.ROSException:
                rospy.logwarn("获取里程计超时")
                continue
            current_yaw = get_yaw(odom.pose.pose.orientation)
            yaw_error = initial_yaw - current_yaw
            move_cmd.angular.z = yaw_error * 0.5
        
        cmd_pub.publish(move_cmd)
        rate.sleep()

    # 最终停止
    stop_robot(cmd_pub)


def main():
    global start_time
    rospy.init_node('robot_trajectory', anonymous=True)
    cmd_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
    rospy.Subscriber('/odom', Odometry, odom_callback)
    rospy.Subscriber('/tag_detections', AprilTagDetectionArray, april_tag_callback)
    rospy.Subscriber('/nlink_linktrack_nodeframe2', LinktrackNodeframe2, uwb_callback)
    rospy.Subscriber('/vrpn_client_node/turtlebot3/pose', PoseStamped, nokov_callback)

    if not load_apriltag_world_coords():
        rospy.logfatal("无法加载AprilTag坐标，程序终止")
        return

    rospy.loginfo("等待初始里程计数据...")
    try:
        rospy.wait_for_message("/odom", Odometry, timeout=10.0)
    except rospy.ROSException:
        rospy.logfatal("无法获取/odom数据，程序终止")
        return

    rospy.sleep(1)
    start_time = rospy.Time.now()

    # 初始化阶段，等待小车静止
    rospy.loginfo("初始化阶段，等待小车静止")
    max_wait_time = 10  # 最大等待时间（秒）
    start_wait_time = rospy.Time.now().to_sec()
    while not (uwb_initialization_complete and nokov_initialization_complete):
        if rospy.Time.now().to_sec() - start_wait_time > max_wait_time:
            rospy.logerr("初始化超时，可能是传感器数据未正常发布，请检查。")
            return
        rospy.sleep(0.1)
    rospy.loginfo("等待初始化完成......")

    # 直线加速减速逻辑
    # rospy.loginfo("开始直线运动")
    # move_linear_trajectory(cmd_pub)
    # rospy.loginfo("直线运动结束")

    rospy.loginfo("开始直线运动")
    move_straight(0.3, 0.2, cmd_pub)
    rospy.sleep(0.1)
    rospy.loginfo("直线运动结束")

    rospy.loginfo("开始圆周运动")
    move_circle(cmd_pub, 0.8, 0.3)
    rospy.sleep(0.1)
    rospy.loginfo("圆周运动结束")

    rospy.loginfo("开始调整方向")
    rotate_to(initial_yaw, cmd_pub)
    rospy.sleep(0.1)

    rospy.loginfo("开始最后一段直线运动")
    move_straight(0.3, 0.2, cmd_pub)
    rospy.loginfo("直线运动结束")

    if rospy.Time.now().to_sec() - start_time >= 15.0:
        rospy.loginfo("Finish")

    stop_robot(cmd_pub)
    rospy.loginfo("运动结束，小车已停止")

    save_to_csv('apriltag_data.csv', april_tag_data)
    save_to_csv('uwb_data.csv', uwb_data)
    save_to_csv('nokov_data.csv', nokov_data)
    rospy.loginfo("数据已保存到CSV文件")


if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        print("程序已中断运行")

        上述编码结束后启动实验采数据： 

# 快速启动实验
PC：roscore
PC：roslaunch vrpn_client_ros sample.launch server:=172.30.89.16
小车：roslaunch realsense2_camera rs_camera.launch enable_infra:=false
PC：roslaunch apriltag_ros continuous_detection.launch
小车：roslaunch turtlebot3_bringup turtlebot3_robot.launch
小车：roslaunch nlink_parser linktrack.launch
# 检测是否有数据
PC：rostopic echo /tag_detections
PC：rostopic echo /vrpn_client_node/turtlebot3/pose
PC：rostopic echo /nlink_linktrack_nodeframe2
# 如果三个话题都有数据就可进行后续操作
PC：rosbag record /tag_detections /vrpn_client_node/turtlebot3/pose /nlink_linktrack_nodeframe2
PC：rosrun robot_trajectory trajectory.py

四、分析数据

     4.1 导出rosbag录制的原始数据

        其实第3节的程序结束后会保存定位数据，那是已经处理好（对齐之后）的数据，不过apriltag定位只使用ID为0的进行，所以这里可以直接导出原始数据根据自己需求进行后处理。

#!/usr/bin/env python3
import rospy
import csv
import numpy as np
from apriltag_ros.msg import AprilTagDetectionArray
from geometry_msgs.msg import PoseStamped
from nlink_parser.msg import LinktrackNodeframe2

# 初始化变量
april_tag_data = []
nokov_data = []
uwb_data = []
start_time = None
last_april_tag_time = None
last_nokov_time = None
last_uwb_time = None
timeout_duration = 3  # 3 秒的超时时间


def april_tag_callback(data):
    global start_time, last_april_tag_time
    if start_time is None:
        start_time = rospy.Time.now()
    last_april_tag_time = rospy.Time.now()

    if data.detections:
        for detection in data.detections:
            tag_id = detection.id[0]
            tag_pose = detection.pose.pose.pose
            tag_position = np.array([tag_pose.position.x, tag_pose.position.y, tag_pose.position.z])
            tag_quaternion = np.array([tag_pose.orientation.x, tag_pose.orientation.y, tag_pose.orientation.z,
                                       tag_pose.orientation.w])

            elapsed_time = (rospy.Time.now() - start_time).to_sec()
            sensor_timestamp = data.header.stamp.to_sec()

            # 保存三维坐标和四元数
            april_tag_data.append([
                elapsed_time,
                sensor_timestamp,
                tag_id,
                tag_position[0],
                tag_position[1],
                tag_position[2],
                tag_quaternion[0],
                tag_quaternion[1],
                tag_quaternion[2],
                tag_quaternion[3]
            ])
    else:
        rospy.logwarn("未检测到AprilTag，跳过此帧")


def nokov_callback(data):
    global start_time, last_nokov_time
    if start_time is None:
        start_time = rospy.Time.now()
    last_nokov_time = rospy.Time.now()

    nokov_position = np.array([data.pose.position.x, data.pose.position.y, data.pose.position.z])
    elapsed_time = (rospy.Time.now() - start_time).to_sec()
    sensor_timestamp = data.header.stamp.to_sec()
    nokov_data.append([elapsed_time, sensor_timestamp, nokov_position[0], nokov_position[1], nokov_position[2]])


def uwb_callback(data):
    global start_time, last_uwb_time
    if start_time is None:
        start_time = rospy.Time.now()
    last_uwb_time = rospy.Time.now()

    try:
        uwb_position = np.array(data.pos_3d)
        elapsed_time = (rospy.Time.now() - start_time).to_sec()
        sensor_timestamp = data.system_time
        uwb_data.append([elapsed_time, sensor_timestamp, uwb_position[0], uwb_position[1], uwb_position[2]])
    except IndexError:
        rospy.logwarn("UWB数据中没有有效的节点信息，跳过此帧")


def save_to_csv(filename, data):
    """保存数据到CSV"""
    with open(filename, 'w', newline='') as file:
        writer = csv.writer(file)
        if 'apriltag' in filename:
            writer.writerow(
                ["Elapsed Time (s)", "Sensor Timestamp (s)", "AprilTag ID", "X (m)", "Y (m)", "Z (m)", "Quaternion X",
                 "Quaternion Y", "Quaternion Z", "Quaternion W"])
        elif 'uwb' in filename:
            writer.writerow(["Elapsed Time (s)", "Sensor Timestamp (s)", "X (m)", "Y (m)", "Z (m)"])
        else:
            writer.writerow(["Elapsed Time (s)", "Sensor Timestamp (s)", "X (m)", "Y (m)", "Z (m)"])
        writer.writerows(data)


def check_timeout(event):
    global last_april_tag_time, last_nokov_time, last_uwb_time
    current_time = rospy.Time.now()
    if last_april_tag_time is not None and last_nokov_time is not None and last_uwb_time is not None:
        april_tag_idle = (current_time - last_april_tag_time).to_sec() > timeout_duration
        nokov_idle = (current_time - last_nokov_time).to_sec() > timeout_duration
        uwb_idle = (current_time - last_uwb_time).to_sec() > timeout_duration
        if april_tag_idle and nokov_idle and uwb_idle:
            rospy.loginfo("3 秒内未接收到新数据，程序结束")
            save_data_and_shutdown()


def save_data_and_shutdown():
    # 保存数据
    save_to_csv('apriltag_data.csv', april_tag_data)
    save_to_csv('nokov_data.csv', nokov_data)
    save_to_csv('uwb_data.csv', uwb_data)
    rospy.loginfo("数据已保存到CSV文件")
    rospy.signal_shutdown("数据保存完成，程序退出")


def main():
    global start_time
    rospy.init_node('data_saver', anonymous=True)
    rospy.Subscriber('/tag_detections', AprilTagDetectionArray, april_tag_callback)
    rospy.Subscriber('/vrpn_client_node/turtlebot3/pose', PoseStamped, nokov_callback)
    rospy.Subscriber('/nlink_linktrack_nodeframe2', LinktrackNodeframe2, uwb_callback)

    # 启动定时器，每 1 秒检查一次是否超时
    rospy.Timer(rospy.Duration(1), check_timeout)

    try:
        rospy.spin()
    except KeyboardInterrupt:
        save_data_and_shutdown()


if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        print("程序已中断运行")

     4.2 Python可视化轨迹

        4.1导出数据后根据自己的要求处理完数据，这个代码我就不粘了，下面是Python可视化轨迹结果图

import pandas as pd
import numpy as np
import matplotlib.pyplot as plt

# 设置字体以支持中文显示
plt.rcParams['font.sans-serif'] = ['SimHei']
plt.rcParams['axes.unicode_minus'] = False

apriltag_data = pd.read_csv(r'C:\Users\remote\Desktop\processed\circle\apriltag_processed_circle.csv')
nokov_data = pd.read_csv(r'C:\Users\remote\Desktop\processed\circle\nokov_processed_circle.csv')
uwb_data = pd.read_csv(r'C:\Users\remote\Desktop\processed\circle\uwb_processed_circle.csv')


# 8. 绘制轨迹图
plt.figure(figsize=(10, 6))
plt.plot(apriltag_data['X (m)'], apriltag_data['Y (m)'], 'g-', label='apriltag Trajectory')
plt.plot(nokov_data['X (m)'], nokov_data['Y (m)'], 'r-', label='True Trajectory')
plt.plot(uwb_data['X (m)'], uwb_data['Y (m)'], label='UWB Trajectory', color='blue')
plt.xlabel('X (m)')
plt.ylabel('Y (m)')
plt.legend()
plt.title('Trajectory Comparison')
plt.show()

        红色的是动捕的轨迹图，误差是1-2mm，所以直接作为真值，绿色的是相机识别apriltag的轨迹，也是比较光滑的误差4-6cm。UWB本身就很抖动，定位结果不是很好，误差达到10cm 

五、知识点客串

     5.1 四元数计算

        下面是计算四元数的一个例子（红色框标注的就是绕某一个向量旋转的四元数计算公式）：

​

        因此最终的四元数(W,X,Y,Z) = (0,1,0,0) 

       需要注意的绕一个轴或者向量进行旋转的时候是逆时针转的，下面是顺时针旋转的一个例子：

​

        最终的四元数(W,X,Y,Z) = (0.707,0,0,-0.707) 

        5.2 四元数转旋转矩阵

        这里就直接使用现成的公式计算即可：

​

六、总结

        本文比较详细的介绍了如何使用apriltags和UWB以及动捕设备同时对智能小车进行定位，实验结果发现使用Apriltags和相机定位比较准确，轨迹比较光滑，UWB本身抖动就很严重，技术文档上说的是二维定位精度6-10CM，三维30CM，但实际得到的定位效果却比较差，这也是和实验场景布置以及UWB的摆放以及初始的标定息息相关的。 

        最后我想说的是文章注重实验流程和步骤操作，原理部分有欠缺，网上这方面的实验参考太少了，自己也是花了好长时间试错试出来的，初衷还是旨在给大家一个参考。另外本文属于实验参考性质的文章，技术也有欠缺，作为初始使用这项技术的新人难免会有理解不到位或者理解有偏差的地方，文中有问题的地方还请大家赐教，后面会及时修改有问题的地方。