坐标系与状态表示：全局坐标系与局部坐标系、状态向量、状态空间模型

各位工程师，咱们今天聊点硬核的——坐标系和状态表示。说实话，我刚入行那会儿，觉得坐标系不就是画个十字轴嘛，有什么好讲的？直到我在一个无人机编队项目里栽了跟头，才明白坐标系选错了，后面全白干。

一、全局坐标系 vs 局部坐标系

先说说这两个坐标系。全局坐标系，也叫世界坐标系，是固定不动的参考系。比如你建一个工厂地图，把原点定在大门口，那整个厂区的位置都相对于这个原点来描述。局部坐标系呢，是跟着运动物体走的，原点就在物体身上。

我举个例子你就明白了。你开车时，导航告诉你「前方500米右转」，这个500米是相对于道路的，属于全局坐标。但如果你车上的雷达说「前方2米有障碍物」，这个2米是相对于车头的，属于局部坐标。

为什么会这样？因为传感器安装的位置不同。GPS给的是全局坐标，IMU给的是局部坐标。你想想看，如果不用局部坐标系，光靠GPS做避障，那延迟早就撞上了。

二、状态向量：位置、速度、加速度

状态向量说白了就是描述物体「当前是什么状态」的一组数字。我习惯用 x 来表示状态向量，它通常包含位置、速度、加速度。

对于一个在二维平面上运动的点，状态向量长这样：

x = [x, y, vx, vy, ax, ay]^T

其中：

• x, y —— 位置（单位：米）
• vx, vy —— 速度（单位：米/秒）
• ax, ay —— 加速度（单位：米/秒²）

嗯，这里要注意：状态向量的维度取决于你的系统复杂程度。如果你只关心位置，那2维就够了。但如果你要做精确跟踪，速度和加速度必须带上。

我曾经犯过一个错误：只用了位置和速度做跟踪，结果目标突然急转弯，我的滤波器直接发散。后来加上加速度项，收敛速度快了不止一倍。

三、状态空间模型

状态空间模型，说白了就是用数学公式描述「状态怎么随时间变化」。它由两个方程组成：

1. 状态方程：描述状态如何演化
2. 观测方程：描述传感器如何测量状态

写成矩阵形式：

状态方程：x(k+1) = A * x(k) + B * u(k) + w(k)
观测方程：z(k)   = C * x(k) + v(k)

其中：

• A —— 状态转移矩阵，描述系统自身的动态
• B —— 控制输入矩阵，描述外部控制的影响
• C —— 观测矩阵，描述传感器如何测量
• w, v —— 过程噪声和观测噪声

你想想看，这个模型其实很直观。A矩阵告诉你「下一时刻的位置等于当前时刻的位置加上速度乘以时间」，B矩阵告诉你「踩油门会改变加速度」。就这么简单。

四、坐标系转换与状态表示的关系

这里有个关键点：状态向量是在哪个坐标系下定义的？

我建议的做法是：

• 在全局坐标系下定义状态方程（因为运动规律在惯性系下最简单）
• 在局部坐标系下定义观测方程（因为传感器数据天然是局部的）
• 两者之间通过旋转矩阵R和平移向量T进行转换

比如，全局坐标 p_global 和局部坐标 p_local 的关系：

p_global = R * p_local + T

这个R矩阵就是旋转矩阵，T是平移向量。嗯，这里要注意：R必须是正交矩阵，否则坐标转换会失真。

五、一个完整的例子

假设我们要跟踪一个在二维平面上运动的机器人。状态向量取6维：

x = [x, y, vx, vy, ax, ay]^T

状态转移矩阵A（假设匀速运动模型，加速度为随机游走）：

A = [1, 0, dt, 0, 0.5*dt^2, 0;
     0, 1, 0, dt, 0, 0.5*dt^2;
     0, 0, 1, 0, dt, 0;
     0, 0, 0, 1, 0, dt;
     0, 0, 0, 0, 1, 0;
     0, 0, 0, 0, 0, 1]

观测矩阵C（假设我们只能测量位置）：

C = [1, 0, 0, 0, 0, 0;
     0, 1, 0, 0, 0, 0]

这样，我们就建立了一个完整的状态空间模型。剩下的就是跑卡尔曼滤波或者粒子滤波了。

六、知识体系结构图

下面这张图展示了本章的核心逻辑，我手绘了一个SVG，你一看就明白：

这张图把本章的三个核心概念串起来了。你从左往右看：先确定用哪个坐标系，然后定义状态向量包含哪些量，最后用状态空间模型描述动态。右侧的坐标系转换是连接全局和局部的桥梁。

好了，坐标系和状态表示就讲到这里。记住：坐标系是骨架，状态向量是血肉，状态空间模型是灵魂。三者缺一不可。