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

各位同学，欢迎来到《多传感器融合定位实战》。我是这门课的主讲，一个在定位领域摸爬滚打了十来年的老工程师。

说实话，我刚入行那会儿，多传感器融合还是个挺小众的方向。那时候做AGV导航，一个激光雷达加个码盘就觉得挺豪华了。现在呢？手机里都塞着GPS、IMU、磁力计、气压计，甚至还有视觉SLAM。时代变了，兄弟们。

这一章，咱们不急着写代码。先把地基打牢。我会带你看看融合定位到底在解决什么问题，以及后面要用到的数学工具——坐标系、四元数、旋转矩阵。这些东西，我当年学的时候也觉得枯燥，但后来发现，不懂它们，你连代码里的bug都找不到。

1.1 多传感器融合定位概述

什么叫多传感器融合定位？说白了，就是「用多个传感器的数据，互相补短板，算出一个更靠谱的位置」。

你想想看，单个传感器总有它的脾气：

• GPS：进隧道就歇菜，高楼底下飘得厉害
• IMU：短时间很准，但积分久了就漂到姥姥家
• 激光雷达：精度高，但怕雨怕雾怕灰尘
• 视觉相机：信息丰富，但光照一变就抓瞎

所以，聪明的做法是把它们组合起来。用IMU补GPS的掉线，用GPS修正IMU的漂移，用激光雷达做局部约束。这就是融合定位的核心思想。

我在做无人车项目时遇到过一个问题：车辆经过高架桥下，GPS信号瞬间丢失，纯IMU积分5秒后位置误差已经超过2米。后来加了轮速计和视觉里程计做融合，同样场景下误差控制在0.3米以内。嗯，这就是融合的价值。

1.2 应用场景

融合定位的应用场景，比你想象的要多。我随便列几个：

• 自动驾驶：L4级以上的车，必须有多传感器冗余。我记得某家Tier1的方案，用了GPS+IMU+激光雷达+摄像头+毫米波雷达，五重冗余。
• 机器人导航：仓储AGV、扫地机器人、送餐机器人。这些场景下，成本敏感，通常用IMU+轮速计+单线激光。
• 无人机飞控：GPS+IMU+气压计+视觉。我见过一个植保无人机，在果园里靠视觉+IMU做定位，GPS被树冠遮挡时依然能稳定飞行。
• AR/VR：手机或头显里的定位。IMU+视觉+磁力计，实现6DoF追踪。
• 手机导航：GPS+WiFi指纹+基站定位+IMU。你在地铁里还能看到自己的位置在动，就是融合的功劳。

1.3 课程大纲速览

这门课一共10章，我按「由浅入深、理论+实战」的思路来组织：

章节	内容	实战项目
第1章	课程导论、坐标系、四元数基础	无（打基础）
第2章	卡尔曼滤波与扩展卡尔曼滤波	一维位置估计
第3章	IMU传感器模型与预积分	IMU数据仿真
第4章	GPS+IMU松耦合融合	车载GPS+IMU融合定位
第5章	激光雷达+IMU紧耦合	LIO-SAM实战
第6章	视觉+IMU紧耦合	VINS-Mono实战
第7章	多传感器时空标定	外参标定工具使用
第8章	因子图优化基础	GTSAM入门
第9章	多传感器融合框架设计	自研融合定位系统
第10章	工程落地与性能调优	实车测试与调参

每一章我都会给一个可运行的代码示例。你跟着敲一遍，比看十遍书都管用。

1.4 坐标系基础

做定位，坐标系是绕不开的坎。我见过太多人因为坐标系搞混，调试了三天才发现是坐标轴方向反了。嗯，我自己也干过这种事。

1.4.1 WGS84坐标系

WGS84，全称World Geodetic System 1984。说白了，就是GPS用的那个坐标系。它用经纬度和海拔高度来描述一个点在地球上的位置。

• 纬度：-90°到+90°，北正南负
• 经度：-180°到+180°，东正西负
• 高度：椭球面以上的高度，单位米

注意，WGS84是个三维坐标系，但它的经纬度不是笛卡尔坐标。你不能直接用经纬度差来计算距离，因为地球是椭球。要算距离，得用Haversine公式或者投影到平面。

1.4.2 ENU坐标系

ENU，East-North-Up，东-北-天。这是一个局部笛卡尔坐标系，原点通常设在某个参考点（比如GPS基站或者车辆起点）。

• X轴：指向东
• Y轴：指向北
• Z轴：指向天（垂直于地球表面向上）

ENU坐标系的好处是，它是一个右手系，而且符合我们的直觉——东、北、天。在局部范围内（比如几公里内），你可以把它近似看作平面直角坐标系，计算起来很方便。

实际项目中，我们通常把GPS的WGS84坐标转换到ENU坐标系下，再做融合。转换步骤是：

1. 选一个参考点（比如起始位置的经纬高）
2. 用地理坐标转换公式，把WGS84转成地心地固坐标系（ECEF）
3. 再从ECEF旋转平移得到ENU

代码实现我会在后面的章节给出，这里先理解概念。

1.4.3 载体坐标系

载体坐标系，也叫Body Frame。它固定在传感器或车辆上，随载体运动而运动。

• X轴：通常指向载体前进方向（车头）
• Y轴：指向载体左侧或右侧（取决于定义）
• Z轴：指向载体上方

IMU测量的加速度和角速度，就是在载体坐标系下的。而GPS给出的位置，是在WGS84或ENU坐标系下的。融合定位要做的，就是在这两个坐标系之间来回转换。

1.5 四元数与旋转矩阵基础

旋转的表示，是定位算法里最基础也最容易出错的地方。常用的有三种：旋转矩阵、欧拉角、四元数。我重点讲四元数和旋转矩阵，因为欧拉角有万向锁问题，我一般只在可视化时用。

1.5.1 旋转矩阵

旋转矩阵是一个3x3的正交矩阵，行列式为+1。它描述了一个坐标系相对于另一个坐标系的旋转关系。

假设有一个点P，在坐标系A中的坐标为p_A，在坐标系B中的坐标为p_B，那么：

p_B = R_AB * p_A

其中R_AB表示从A系到B系的旋转矩阵。

旋转矩阵的性质：

• RT = R-1（转置等于逆）
• 多个旋转可以连乘：R_AC = R_AB * R_BC
• 旋转矩阵有9个元素，但只有3个自由度，所以存在冗余

我在做标定时，经常用旋转矩阵来验证外参的一致性。如果两个旋转矩阵的乘积不是单位阵，那肯定有地方搞错了。

1.5.2 四元数

四元数，说白了就是「用四个数表示旋转」。它没有万向锁问题，而且插值平滑，所以在SLAM和融合定位里用得最多。

一个四元数可以写成：

q = w + x*i + y*j + z*k

其中w是实部，x, y, z是虚部。单位四元数（模长为1）表示旋转。

四元数和旋转矩阵可以互相转换。比如，给定一个四元数q = [w, x, y, z]，对应的旋转矩阵是：

R = [[1-2y²-2z²,   2xy-2wz,     2xz+2wy],
     [2xy+2wz,     1-2x²-2z²,   2yz-2wx],
     [2xz-2wy,     2yz+2wx,     1-2x²-2y²]]

看着复杂，但别怕。实际编程时，我们直接用Eigen或Sophus库，一行代码就搞定转换。

1.5.3 常用转换代码片段

这里给一个C++的示例，用Eigen库实现四元数和旋转矩阵的互转：

#include <Eigen/Dense>

// 旋转矩阵 -> 四元数
Eigen::Matrix3d R;
R = Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitX())
  * Eigen::AngleAxisd(0.2, Eigen::Vector3d::UnitY())
  * Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ());

Eigen::Quaterniond q(R);
std::cout << "Quaternion: " << q.coeffs().transpose() << std::endl;

// 四元数 -> 旋转矩阵
Eigen::Quaterniond q2(0.707, 0.0, 0.707, 0.0);
Eigen::Matrix3d R2 = q2.toRotationMatrix();
std::cout << "Rotation Matrix:\n" << R2 << std::endl;

这段代码我几乎每个项目都会用到。你把它存成代码片段，以后直接复用。

1.6 本章知识体系

下面这张图，是我手绘的知识结构。你可以把它当作本章的「地图」：

这张图把本章的知识点串起来了。你可以看到，所有内容都围绕着「多传感器融合定位」这个核心展开。坐标系和旋转是工具，概述和应用场景是背景。后面的每一章，都会用到这些基础。

1.7 本章小结

这一章，我们聊了：

• 多传感器融合定位的核心思想——取长补短
• 几个典型的应用场景——自动驾驶、机器人、无人机等
• 课程的整体结构——10章，从理论到实战
• 三种坐标系——WGS84、ENU、载体坐标系
• 旋转的两种表示——旋转矩阵和四元数

这些东西，看起来是基础，但也是后面所有算法的基石。我建议你花点时间，把四元数和旋转矩阵的互转代码自己写一遍。写熟了，后面学卡尔曼滤波、因子图优化的时候，你会感谢现在的自己。

好，第一章就到这里。下一章，我们开始进入真正的算法——卡尔曼滤波。那是融合定位的「心脏」。

---