第一章：课程导论与预备知识

各位同学好，我是你们这门课的主讲。在嵌入式系统领域摸爬滚打了十几年，传感器融合这块我踩过的坑，比你们走过的路可能还多几条（笑）。今天咱们先不急着写代码，把地基打牢了再说。

传感器融合，说白了就是让多个传感器「合作」，取长补短。比如无人机上，GPS 告诉你大概位置，IMU 告诉你姿态变化，但单独用哪个都不靠谱。GPS 会飘，IMU 会累积误差。把它们的数据揉在一起，才能得到稳定可靠的估计。我在做农业植保无人机项目时，就遇到过 GPS 信号被树冠遮挡的情况，全靠 IMU 和视觉里程计撑着，那会儿才真正体会到融合的重要性。

1.1 传感器融合概述

传感器融合不是简单的「取平均值」。它是一门关于「不确定性」的学问。你想想看，每个传感器都有噪声，都有误差。融合算法的核心任务，就是根据每个传感器的置信度，动态地调整它们对最终结果的贡献。

常见的融合架构有两种：

• 集中式融合：所有原始数据送到一个中心处理器，统一处理。优点是精度高，缺点是计算量大、通信带宽要求高。
• 分布式融合：每个传感器先自己做局部处理，再把结果汇总。优点是实时性好、容错性强，缺点是可能存在信息冗余或冲突。

我个人习惯在嵌入式平台上用分布式融合。为什么呢？因为地面站的算力虽然比飞控强，但也要考虑多任务并行。把预处理分摊到各个传感器节点，主处理器只做高层融合，这样系统更健壮。

1.2 地面站系统架构

地面站是无人机的大脑在地面的延伸。我参与过好几个地面站项目，从早期的串口屏方案到现在的全图形化系统，架构上其实万变不离其宗。

一个典型的地面站系统包含以下几层：

1. 通信层：负责与飞行器交换数据。常用协议有 MAVLink、UDP、串口等。这里要注意，通信延迟和丢包是融合算法的大敌。
2. 数据预处理层：对原始传感器数据进行时间戳对齐、异常值剔除、单位转换等。嗯，这一步很多人会忽略，但我在项目中吃过亏——IMU 和 GPS 的时间戳没对齐，融合结果直接发散。
3. 融合引擎层：核心算法模块，运行卡尔曼滤波、姿态解算等。
4. 可视化与交互层：把融合结果显示在地图上、仪表盘上，供操作员决策。

下面这张图是我画的地面站核心数据流，你可以看到传感器数据是怎么一步步变成可用信息的。

1.3 常用坐标系与姿态表示方法

坐标系是传感器融合的「语言」。语言不通，数据就乱套了。我见过太多新手把机体坐标系和导航坐标系搞混，结果姿态解算出来的角度完全不对。

1.3.1 常用坐标系

坐标系	定义	典型用途
导航坐标系 (n系)	北-东-地 (NED) 或 东-北-天 (ENU)	GPS 定位、航点规划
机体坐标系 (b系)	前-右-下 (FRD) 或 前-左-上 (FLU)	IMU 测量、姿态控制
地心地固坐标系 (ECEF)	以地心为原点，X指向本初子午线	卫星导航、全球定位

你想想看，IMU 测量的是机体坐标系下的加速度和角速度，而 GPS 给出的是导航坐标系下的位置。要把它们融合，就必须做坐标系变换。这个变换矩阵，就是姿态矩阵。

1.3.2 欧拉角

欧拉角是最直观的姿态表示方法。三个角度：横滚 (Roll)、俯仰 (Pitch)、偏航 (Yaw)。

但欧拉角有个致命问题——万向锁。当俯仰角接近 ±90° 时，横滚和偏航会耦合，丢失一个自由度。我在做固定翼无人机时遇到过这个问题，飞机做大机动动作时姿态解算直接炸了。所以，如果你做的是全姿态飞行器（比如特技无人机），千万别只用欧拉角。

1.3.3 四元数

四元数是个好东西。它没有万向锁，计算效率高，插值平滑。一个四元数 q = [w, x, y, z] 表示绕某个轴旋转一定角度。

四元数和欧拉角的转换公式，我建议你直接背下来，或者存成代码片段。每次手写推导太容易出错了。

import numpy as np

def euler_to_quat(roll, pitch, yaw):
    """欧拉角转四元数 (弧度)"""
    cr = np.cos(roll * 0.5)
    sr = np.sin(roll * 0.5)
    cp = np.cos(pitch * 0.5)
    sp = np.sin(pitch * 0.5)
    cy = np.cos(yaw * 0.5)
    sy = np.sin(yaw * 0.5)
    
    w = cr * cp * cy + sr * sp * sy
    x = sr * cp * cy - cr * sp * sy
    y = cr * sp * cy + sr * cp * sy
    z = cr * cp * sy - sr * sp * cy
    return np.array([w, x, y, z])

def quat_to_euler(q):
    """四元数转欧拉角"""
    w, x, y, z = q
    roll = np.arctan2(2*(w*x + y*z), 1 - 2*(x**2 + y**2))
    pitch = np.arcsin(2*(w*y - z*x))
    yaw = np.arctan2(2*(w*z + x*y), 1 - 2*(y**2 + z**2))
    return roll, pitch, yaw

1.4 Python 科学计算环境搭建

咱们这门课的所有算法，都会用 Python 实现和验证。为什么选 Python？因为开发效率高，NumPy 和 SciPy 的底层是 C 和 Fortran，性能足够做原型验证。等算法成熟了，再移植到 C++ 或嵌入式平台。

环境搭建其实很简单，但有几个坑要注意：

1. Python 版本：建议用 3.8 以上，但别追最新版，有些库可能不兼容。我个人用 3.10，稳定且生态好。
2. 虚拟环境：每个项目建一个虚拟环境，别把依赖搞混了。我曾经因为全局环境装了一堆包，版本冲突导致调试了一整天。
3. 安装命令：

# 创建虚拟环境
python -m venv sensor_fusion_env

# 激活 (Windows)
sensor_fusion_env\Scripts\activate

# 激活 (Linux/Mac)
source sensor_fusion_env/bin/activate

# 安装核心库
pip install numpy scipy matplotlib

验证安装是否成功，跑个简单的测试：

import numpy as np
import scipy.linalg as la
import matplotlib.pyplot as plt

# 创建一个简单的旋转矩阵
theta = np.radians(30)
R = np.array([[np.cos(theta), -np.sin(theta)],
              [np.sin(theta),  np.cos(theta)]])
print("旋转矩阵:\n", R)

# 验证正交性
print("R^T * R 是否为单位阵:\n", R.T @ R)

如果输出正常，说明环境没问题。嗯，这里要注意，SciPy 的线性代数模块比 NumPy 的更全，做卡尔曼滤波时会用到 scipy.linalg.solve 和 scipy.linalg.cholesky，提前熟悉一下。

---

可可精品资料库