定位系统：让无人机知道”我在哪”

本节学习目标

• 理解定位系统在无人机中的作用，掌握”我在哪”这一核心问题的技术解决方案

• 掌握IMU、GPS、UWB、动作捕捉等定位硬件的工作原理与核心参数

• 学会根据应用场景选择合适的定位方案

• 在Gazebo中配置定位传感器并读取ROS话题数据

为什么需要定位系统？

想象一下，你闭上眼睛在一个陌生的房间里走动——你很快就会撞到墙壁或家具。这是因为你失去了两种关键信息：位置（我在哪）和姿态（我朝向哪）。

无人机面临同样的问题。与地面机器人不同，无人机在三维空间中飞行，没有固定的参照物。如果不知道自己的位置和姿态，无人机就无法：

• 保持稳定悬停（会漂移）

• 按照规划路径飞行（会偏离）

• 安全降落到指定地点（会坠毁）

人体类比：定位系统相当于人的前庭系统和小脑。前庭系统感知头部的加速度和旋转，小脑整合这些信息来维持平衡。当你转圈后突然停下，会感到眩晕——这就是前庭系统暂时”校准失败”的结果。

定位系统需要回答的三个问题

问题	技术术语	数学表示	解决方案
我在哪？	位置（Position）	\((x, y, z)\)	GPS、UWB、视觉定位
我朝向哪？	姿态（Attitude）	\((\phi, \theta, \psi)\)（Roll, Pitch, Yaw）	IMU、磁力计
我在怎么动？	速度（Velocity）	\((v_x, v_y, v_z)\)	IMU积分、光流

知识拓展：姿态的三个角度

• 横滚角（Roll, :math:`phi`）：绕机体X轴旋转，控制无人机左右倾斜

• 俯仰角（Pitch, :math:`theta`）：绕机体Y轴旋转，控制无人机前后倾斜

• 偏航角（Yaw, :math:`psi`）：绕机体Z轴旋转，控制无人机航向

惯性测量单元（IMU）：感知自身运动

IMU（Inertial Measurement Unit，惯性测量单元）是无人机最基础、最重要的定位传感器。它不依赖外部信号，仅通过测量自身的加速度和角速度来推算位置和姿态。

IMU的组成

一个典型的IMU包含以下传感器：

传感器	测量物理量	单位	用途
三轴加速度计	线加速度 \(a_x, a_y, a_z\)	\(\text{m/s}^2\) 或 \(g\)	测量重力和运动加速度
三轴陀螺仪	角速度 \(\omega_x, \omega_y, \omega_z\)	\(°/\text{s}\) 或 \(\text{rad/s}\)	测量旋转速率
三轴磁力计（可选）	磁场强度 \(B_x, B_y, B_z\)	\(\mu\text{T}\)	提供绝对航向参考

IMU的工作原理

加速度计利用MEMS（微机电系统）技术，通过测量微小质量块在加速时产生的位移来计算加速度。其核心原理基于牛顿第二定律：

(2)\[F = ma\]

在静止状态下，加速度计测量的是重力加速度。当无人机运动时，测量值是重力加速度与运动加速度的叠加。

陀螺仪基于科里奥利效应（Coriolis Effect）。当振动质量块在旋转参考系中运动时，会产生与旋转角速度成正比的科里奥利力：

(3)\[F_c = 2m(\omega \times v)\]

其中 \(m\) 是质量，\(\omega\) 是角速度矢量，\(v\) 是振动速度矢量。

从IMU数据到位置：积分过程

IMU无法直接测量位置，需要通过积分运算：

(4)\[\text{角速度} \xrightarrow{\int} \text{姿态} \xrightarrow{} \text{加速度（机体系→世界系）} \xrightarrow{\int} \text{速度} \xrightarrow{\int} \text{位置}\]

用数学公式表示：

姿态更新（简化的欧拉角积分）：

(5)\[\begin{split}\begin{aligned} \phi(t) &= \phi(t-1) + \omega_x \cdot \Delta t \\ \theta(t) &= \theta(t-1) + \omega_y \cdot \Delta t \\ \psi(t) &= \psi(t-1) + \omega_z \cdot \Delta t \end{aligned}\end{split}\]

速度更新：

(6)\[v(t) = v(t-1) + a \cdot \Delta t\]

位置更新：

(7)\[ \begin{align}\begin{aligned}p(t) = p(t-1) + v \cdot \Delta t\\**警告**\ ：积分漂移问题\\由于传感器噪声和偏置误差的存在，每次积分都会累积误差。这就是著名的\ **积分漂移（Drift）**\ 问题。即使是高精度的IMU，纯惯性导航在几分钟后位置误差就可能达到数米甚至数十米。因此，IMU通常需要与GPS、视觉等外部传感器融合使用。\end{aligned}\end{align} \]

Pixhawk 6C的IMU配置

Pixhawk 6C飞控板集成了冗余IMU设计，包含以下传感器：

IMU芯片	类型	加速度量程	陀螺仪量程	特点
ICM-42688-P	6轴	±2/±4/±8/±16 \(g\)	±250/±500/±1000/±2000 °/s	主IMU，高性能，低噪声
BMI055	6轴	±2/±4/±8/±16 \(g\)	±125/±250/±500/±1000/±2000 °/s	备份IMU，低噪声，冗余保障

为什么需要双IMU？ 冗余设计提高了系统可靠性。当一个IMU出现故障或数据异常时，飞控可以自动切换到备份IMU，确保飞行安全。此外，Pixhawk 6C的IMU模块还配备了温控加热电阻和振动隔离系统，以减少温度漂移和机械振动对测量精度的影响。

IMU选型的核心参数

参数	含义	典型值范围	对无人机的影响
噪声密度（Noise Density）	传感器输出中的随机噪声	加速度计：100-400 μg/√Hz；陀螺仪：0.004-0.01 °/s/√Hz	影响姿态估计的平滑度
零偏稳定性（Bias Stability）	零输入时输出的漂移	加速度计：10-50 μg；陀螺仪：1-10 °/h	影响长时间积分精度
量程（Full Scale Range）	可测量的最大值	加速度计：±2g±16g；陀螺仪：±250±2000 °/s	需覆盖无人机的机动范围
采样率（Sample Rate）	每秒输出数据的次数	100Hz-8kHz	影响控制响应速度

选型建议：对于消费级无人机，±8g加速度量程和±2000°/s陀螺仪量程是常见配置，可以覆盖大多数飞行机动。

全球卫星导航系统（GNSS）：室外绝对定位

GNSS（Global Navigation Satellite System，全球卫星导航系统）是室外无人机获取绝对位置的主要手段。常见的GNSS系统包括：

系统名称	所属国家/地区	卫星数量	覆盖范围
GPS	美国	31颗	全球
GLONASS	俄罗斯	24颗	全球
Galileo	欧盟	30颗	全球
北斗（BeiDou）	中国	55颗	全球（三代）

GNSS定位原理

GNSS定位基于到达时间测量（Time of Arrival, ToA）原理。接收机同时接收多颗卫星的信号，通过测量信号传播时间计算与每颗卫星的距离（伪距），再利用三边测量法确定位置。

伪距计算公式：

(8)\[\rho_i = c \cdot (t_r - t_s^i) = r_i + c \cdot \delta t_r + \epsilon_i\]

其中：

• \(\rho_i\) 是到第 \(i\) 颗卫星的伪距

• \(c\) 是光速（约 \(3 \times 10^8\) m/s）

• \(t_r\) 是接收机时间，\(t_s^i\) 是卫星发射时间

• \(r_i\) 是真实距离

• \(\delta t_r\) 是接收机钟差

• \(\epsilon_i\) 是测量误差

由于存在接收机钟差这个未知数，定位至少需要4颗卫星：3个位置坐标 + 1个时间偏差。

GPS定位精度等级

定位方式	水平精度	垂直精度	典型应用
单点定位（SPP）	1-5 m	2-10 m	消费级无人机导航
差分GPS（DGPS）	0.5-2 m	1-3 m	农业植保无人机
RTK GPS	1-3 cm	2-5 cm	测绘、精准农业
PPK GPS	1-3 cm	2-5 cm	航测、后处理

RTK GPS：厘米级定位

RTK（Real-Time Kinematic，实时动态差分）是目前无人机获取厘米级定位精度的主流方案。

RTK工作原理：

1. 基站：架设在已知精确坐标的位置，持续接收卫星信号

2. 流动站：安装在无人机上的GPS接收机

3. 数据链：基站通过电台或网络将校正数据实时发送给流动站

4. 差分解算：流动站利用校正数据消除大气延迟、卫星轨道误差等共性误差

RTK的两种工作模式：

模式	精度	解算时间	可靠性
浮点解（Float）	分米级	即时	较低
固定解（Fixed）	厘米级	10-60秒	高

应用场景：RTK GPS适用于测绘航拍、精准农业喷洒、电力巡线等需要高精度定位的场景。但其缺点是需要额外的基站设备，增加了系统复杂度和成本。

Pixhawk 6C常用GPS模块

模块	GNSS芯片	支持系统	定位精度	更新率
u-blox NEO-M8N	M8N	GPS/GLONASS/Galileo/北斗	2.5m CEP	10Hz
u-blox ZED-F9P	F9P	GPS/GLONASS/Galileo/北斗	1cm + 1ppm（RTK）	20Hz
HERE3	M8P	GPS/GLONASS	2.5m / 1cm（RTK）	8Hz

室内定位方案

GPS信号在室内环境中会被建筑物遮挡和反射，无法提供可靠的定位。室内无人机需要采用其他定位方案。

UWB超宽带定位

UWB（Ultra-Wideband，超宽带）是一种基于脉冲信号的无线定位技术，特别适合室内环境。

UWB定位原理：

UWB系统由固定的锚点（Anchor）和移动的标签（Tag）组成。标签发射超短脉冲信号，锚点接收后通过以下方法计算位置：

1. 双边测距（TWR, Two-Way Ranging）：标签与锚点之间进行信号往返，测量飞行时间

2. 到达时间差（TDoA, Time Difference of Arrival）：多个锚点同时接收信号，利用时间差定位

TWR测距公式：

(9)\[d = \frac{c \cdot (T_{round} - T_{reply})}{2}\]

其中 \(T_{round}\) 是往返时间，\(T_{reply}\) 是应答处理延迟。

UWB系统特点：

特点	说明
定位精度	10-30 cm（典型值）
更新率	10-100 Hz
覆盖范围	单锚点10-50m，系统可扩展
抗干扰能力	强，不受WiFi、蓝牙干扰
部署要求	需要至少4个锚点实现3D定位

常用UWB模块：

产品	芯片方案	定位精度	特点
Nooploop LinkTrack	DW1000	±10cm	支持伪GPS输出，可直接替代GPS
Pozyx	DW1000	±10cm	开源友好，ROS支持良好
Decawave DWM1001	DW1000	±10cm	开发套件丰富，性价比高

动作捕捉系统（Motion Capture）

动作捕捉系统是实验室环境下精度最高的定位方案，常用于无人机算法研究和室内集群飞行。

工作原理：

1. 在被跟踪物体上安装反光标记球（Marker）

2. 多台红外相机同时拍摄标记球

3. 通过多视角几何计算标记球的三维坐标

4. 根据标记球的空间配置识别和跟踪刚体

主流动作捕捉系统：

系统	定位精度	覆盖范围	更新率	参考价格
OptiTrack	< 0.3mm	可扩展至 \(10m \times 10m \times 5m\)	120-360Hz	$15,000+
Vicon	< 0.5mm	可扩展	100-330Hz	$50,000+
NOKOV	< 0.5mm	可扩展	100-340Hz	$20,000+

为什么研究人员喜欢使用动作捕捉？

• 亚毫米级精度：可以作为其他定位系统的”真值”（Ground Truth）

• 高更新率：适合高速运动跟踪

• 低延迟：通常 < 20ms，适合实时控制

• 多目标跟踪：可同时跟踪数十个无人机

定位方案对比

方案	精度	更新率	环境	成本	复杂度
GPS（单点）	1-5m	1-10Hz	室外	低	低
RTK GPS	1-3cm	10-20Hz	室外	中	中
UWB	10-30cm	10-100Hz	室内	中	中
动作捕捉	< 1mm	100-360Hz	室内（实验室）	高	高
视觉SLAM	1-10cm	10-30Hz	室内外	低	高

数据接口与ROS话题

定位传感器通过特定的接口与飞控或机载计算机通信。

硬件接口类型

传感器	常用接口	数据协议
IMU	SPI / I2C	寄存器读取
GPS	UART（串口）	NMEA-0183 / UBX
UWB	UART / USB	自定义协议
动作捕捉	以太网	VRPN / NatNet

ROS中的定位相关话题

在Prometheus仿真环境中，定位数据通过以下ROS话题发布：

话题名称	消息类型	内容	频率
/mavros/imu/data	sensor_msgs/Imu	IMU原始数据（加速度、角速度、姿态四元数）	50-200Hz
/mavros/global_position/global	sensor_msgs/NavSatFix	GPS全球坐标（经度、纬度、高度）	1-10Hz
/mavros/local_position/pose	geometry_msgs/PoseStamped	本地位置和姿态	30-50Hz
/mavros/local_position/velocity_local	geometry_msgs/TwistStamped	本地速度	30-50Hz

IMU消息结构解析

sensor_msgs/Imu 消息包含以下字段：

std_msgs/Header header          # 时间戳和坐标系
geometry_msgs/Quaternion orientation    # 姿态四元数 (x, y, z, w)
float64[9] orientation_covariance       # 姿态协方差
geometry_msgs/Vector3 angular_velocity  # 角速度 (rad/s)
float64[9] angular_velocity_covariance  # 角速度协方差
geometry_msgs/Vector3 linear_acceleration  # 线加速度 (m/s²)
float64[9] linear_acceleration_covariance  # 加速度协方差

四元数到欧拉角的转换公式：

(10)\[\begin{split}\begin{aligned} \phi &= \arctan2(2(qw \cdot qx + qy \cdot qz), 1 - 2(qx^2 + qy^2)) \\ \theta &= \arcsin(2(qw \cdot qy - qz \cdot qx)) \\ \psi &= \arctan2(2(qw \cdot qz + qx \cdot qy), 1 - 2(qy^2 + qz^2)) \end{aligned}\end{split}\]

动手实践：读取Gazebo中的定位数据

现在让我们在Gazebo仿真环境中实际操作，读取和理解定位传感器的数据。

实验一：查看IMU原始数据

步骤1：启动仿真环境

roslaunch prometheus_gazebo sitl_indoor_1uav_P450.launch

步骤2：查看IMU话题列表

rostopic list | grep imu

预期输出：

/mavros/imu/data
/mavros/imu/data_raw
/mavros/imu/diff_pressure
/mavros/imu/mag
/mavros/imu/static_pressure
/mavros/imu/temperature_imu

步骤3：实时显示IMU数据

rostopic echo /mavros/imu/data

观察输出中的以下字段：

• orientation：姿态四元数

• angular_velocity：角速度（rad/s）

• linear_acceleration：线加速度（m/s²）

思考：当无人机静止在地面时，linear_acceleration.z 的值约为多少？为什么？

实验二：将四元数转换为欧拉角

创建一个Python脚本来实时显示无人机的姿态角：

#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
文件名: imu_euler_display.py
功能: 订阅IMU数据，将四元数转换为欧拉角并显示
"""

import rospy
from sensor_msgs.msg import Imu
import math

def quaternion_to_euler(q):
    """
    将四元数转换为欧拉角（ZYX顺序）

    参数:
        q: 四元数对象，包含x, y, z, w属性

    返回:
        roll, pitch, yaw: 欧拉角（单位：度）
    """
    # Roll (x-axis rotation)
    sinr_cosp = 2 * (q.w * q.x + q.y * q.z)
    cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y)
    roll = math.atan2(sinr_cosp, cosr_cosp)

    # Pitch (y-axis rotation)
    sinp = 2 * (q.w * q.y - q.z * q.x)
    if abs(sinp) >= 1:
        pitch = math.copysign(math.pi / 2, sinp)  # 万向锁处理
    else:
        pitch = math.asin(sinp)

    # Yaw (z-axis rotation)
    siny_cosp = 2 * (q.w * q.z + q.x * q.y)
    cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z)
    yaw = math.atan2(siny_cosp, cosy_cosp)

    # 转换为度
    return math.degrees(roll), math.degrees(pitch), math.degrees(yaw)

def imu_callback(msg):
    """IMU数据回调函数"""
    roll, pitch, yaw = quaternion_to_euler(msg.orientation)

    # 清屏并显示
    print("\033[2J\033[H")  # ANSI转义码：清屏并移动光标到左上角
    print("=" * 50)
    print("       无人机姿态实时显示")
    print("=" * 50)
    print(f"横滚角 (Roll) : {roll:8.2f}°")
    print(f"俯仰角 (Pitch): {pitch:8.2f}°")
    print(f"偏航角 (Yaw)  : {yaw:8.2f}°")
    print("-" * 50)
    print(f"角速度 X: {math.degrees(msg.angular_velocity.x):8.2f} °/s")
    print(f"角速度 Y: {math.degrees(msg.angular_velocity.y):8.2f} °/s")
    print(f"角速度 Z: {math.degrees(msg.angular_velocity.z):8.2f} °/s")
    print("-" * 50)
    print(f"加速度 X: {msg.linear_acceleration.x:8.2f} m/s²")
    print(f"加速度 Y: {msg.linear_acceleration.y:8.2f} m/s²")
    print(f"加速度 Z: {msg.linear_acceleration.z:8.2f} m/s²")
    print("=" * 50)
    print("按 Ctrl+C 退出")

def main():
    rospy.init_node('imu_euler_display', anonymous=True)
    rospy.Subscriber('/mavros/imu/data', Imu, imu_callback)
    rospy.spin()

if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass

运行脚本：

chmod +x imu_euler_display.py
python imu_euler_display.py

实验三：查看GPS/本地位置数据

查看GPS全球坐标：

rostopic echo /mavros/global_position/global

输出示例：

latitude: 47.3977421
longitude: 8.5455933
altitude: 535.4

查看本地位置（ENU坐标系）：

rostopic echo /mavros/local_position/pose

输出示例：

pose:
  position:
    x: 0.0234
    y: -0.0156
    z: 0.0892
  orientation:
    x: 0.0012
    y: -0.0008
    z: 0.0034
    w: 0.9999

实验四：可视化位置轨迹

使用rqt_plot实时绘制位置曲线：

rqt_plot /mavros/local_position/pose/pose/position/x \
         /mavros/local_position/pose/pose/position/y \
         /mavros/local_position/pose/pose/position/z

或者使用RViz进行三维可视化：

rviz

在RViz中： 1. 设置Fixed Frame为map 2. 添加PoseStamped显示，话题选择/mavros/local_position/pose 3. 添加TF显示以查看坐标系关系

Gazebo中的IMU模型配置（选读）

在Gazebo仿真中，IMU通过SDF/URDF文件配置。以下是P450模型中IMU的典型配置：

<gazebo>
  <plugin name="imu_plugin" filename="libgazebo_ros_imu.so">
    <robotNamespace>/</robotNamespace>
    <topicName>mavros/imu/data</topicName>
    <bodyName>base_link</bodyName>
    <updateRateHZ>200.0</updateRateHZ>
    <gaussianNoise>0.0</gaussianNoise>
    <xyzOffset>0 0 0</xyzOffset>
    <rpyOffset>0 0 0</rpyOffset>
  </plugin>
</gazebo>

关键参数说明：

参数	含义	建议值
updateRateHZ	数据发布频率	50-400 Hz
gaussianNoise	高斯噪声标准差	0-0.01（仿真可设为0）
xyzOffset	传感器安装位置偏移	根据实际安装位置设置
rpyOffset	传感器安装角度偏移	根据实际安装角度设置

如果要模拟真实IMU的噪声特性，可以添加以下配置：

<sensor name="imu_sensor" type="imu">
  <always_on>true</always_on>
  <update_rate>200</update_rate>
  <imu>
    <angular_velocity>
      <x>
        <noise type="gaussian">
          <mean>0.0</mean>
          <stddev>0.0002</stddev>  <!-- 陀螺仪噪声 -->
        </noise>
      </x>
      <!-- y, z 类似配置 -->
    </angular_velocity>
    <linear_acceleration>
      <x>
        <noise type="gaussian">
          <mean>0.0</mean>
          <stddev>0.017</stddev>  <!-- 加速度计噪声 -->
        </noise>
      </x>
      <!-- y, z 类似配置 -->
    </linear_acceleration>
  </imu>
</sensor>

小结

本节我们学习了无人机定位系统的核心内容：

1. 定位系统的作用：回答”我在哪”、“我朝向哪”、“我怎么动”三个问题

2. IMU：通过加速度计和陀螺仪感知自身运动，是姿态估计的核心，但存在积分漂移问题

3. GNSS/GPS：室外绝对定位的主要手段，RTK可达厘米级精度

4. 室内定位：UWB适合通用室内场景，动作捕捉适合实验室研究

5. 数据接口：通过ROS话题获取定位数据，理解消息结构

核心要点：

• 单一定位方案都有局限性，实际系统通常融合多种传感器

• 精度与成本、复杂度之间需要权衡

• 理解传感器原理和参数有助于故障诊断和系统优化

练习题

基础题

1. 判断题：IMU可以直接测量无人机的位置。（ ）

2. 填空题：Pixhawk 6C配备了双IMU冗余设计，主IMU型号是______，备份IMU型号是______。

3. 选择题：以下哪种定位方案精度最高？

   • 1. 单点GPS

   • 2. RTK GPS

   • 3. UWB

   • 4. 动作捕捉系统

4. 计算题：某GPS接收机测得到卫星A的信号传播时间为0.07秒，计算该接收机到卫星A的伪距。（光速 \(c = 3 \times 10^8\) m/s）

进阶题

5. 实践题：编写一个ROS节点，订阅/mavros/local_position/pose话题，计算并打印无人机相对于起飞点的水平距离：

   (11)\[d_{horizontal} = \sqrt{x^2 + y^2}\]

6. 分析题：为什么高精度的IMU仍然需要与GPS融合使用？请从误差累积的角度分析。

思考题

7. 如果你需要设计一套用于仓库盘点的无人机定位系统，应该选择什么方案？请说明理由。

8. 动作捕捉系统虽然精度极高，但为什么不适合实际的无人机产品部署？