坐标系与运动学：地理坐标系与机体坐标系

各位同学，咱们今天聊聊坐标系。说实话，搞四旋翼避障，坐标系是绕不开的第一道坎。我刚开始做飞控时，就被坐标系搞晕过——明明代码逻辑没问题，飞机就是乱飞。后来才发现，是坐标系搞反了。

地理坐标系 vs 机体坐标系

先说说地理坐标系。说白了，就是咱们平时说的「东北天」坐标系。X轴指向东，Y轴指向北，Z轴指向天。这个坐标系是固定的，不随飞机转动而改变。

机体坐标系呢？它是绑在飞机身上的。X轴指向机头，Y轴指向右翼，Z轴指向下方。飞机怎么转，坐标系就怎么转。

为什么要搞两个坐标系？你想想看，传感器装在飞机上，测的是机体坐标系下的数据。但导航、路径规划，得用地理坐标系。所以，两个坐标系之间得来回转换。

我在项目中遇到过一个问题：用GPS定位时，位置信息是地理坐标系下的，但避障算法需要的是机体坐标系下的障碍物距离。如果不做转换，飞机就会往错误的方向躲。嗯，那次炸机让我印象深刻。

欧拉角与四元数

接下来聊聊姿态表示。欧拉角大家应该都听过——横滚角、俯仰角、偏航角。直观，好理解。但有个坑：万向锁。

那用什么？四元数。四元数是个数学工具，用四个数表示旋转。没有万向锁问题，计算效率也高。说白了，就是用一个三维向量加一个标量，描述绕某个轴转了多少角度。

四元数的形式：q = w + xi + yj + zk。其中w是标量部分，x、y、z是向量部分。别被这个形式吓到，实际用的时候，我们只需要知道怎么转换就行。

表示方法	优点	缺点
欧拉角	直观，容易理解	万向锁，计算复杂
四元数	无万向锁，计算高效	不够直观
旋转矩阵	通用性强	计算量大

我个人习惯用四元数做姿态解算，最后输出控制时再转成欧拉角。这样既避免了万向锁，又方便调试时观察。

姿态解算基础

姿态解算，说白了就是「猜」飞机当前是什么姿势。为什么用「猜」？因为传感器有噪声，有漂移，不能直接信。

常用的方法有互补滤波和卡尔曼滤波。互补滤波简单粗暴——加速度计测姿态，但高频噪声大；陀螺仪积分得姿态，但低频漂移严重。两者互补一下，效果还行。

卡尔曼滤波更高级，但计算量大。我建议初学者先从互补滤波入手，跑通了再换卡尔曼。

PX4姿态控制流程

最后说说PX4的姿态控制流程。这个流程我拆成三步：

1. 姿态估计：用传感器数据（加速度计、陀螺仪、磁力计）算出当前姿态
2. 姿态误差计算：目标姿态减去当前姿态，得到误差
3. 控制输出：用PID控制器把误差转成电机指令

你看，其实不复杂。但细节都在第二步——怎么算误差？用四元数算，还是用欧拉角算？PX4用的是四元数误差，然后转成角速度控制。

// PX4姿态控制核心代码（简化版）
// 1. 计算四元数误差
quat_error = target_quat * current_quat.inverse();

// 2. 提取角速度误差
angular_error = quat_to_angular(quat_error);

// 3. PID控制
output = pid_controller(angular_error);

这段代码看着简单，但实际调参时坑不少。我记得有一次，PID参数调得太大，飞机直接翻了个跟头。嗯，从那以后我调参都先绑在架子上试。

好了，坐标系和姿态控制就聊到这儿。这些是基础中的基础，后面讲避障路径规划时，你会反复用到这些概念。特别是坐标系转换，避障算法里到处都是。

这张图把今天讲的内容串起来了。地理坐标系和机体坐标系是基础，欧拉角和四元数是工具，姿态解算是方法，PX4控制是应用。一层层下来，逻辑很清楚。