这门课我前后刷了两遍第一遍是在校期间跟着课程节奏走第二遍是工作后做飞控相关项目时回头补的。两次学习的感受完全不同第一次是被动接受知识点第二次才真正理解了四旋翼为什么这么飞、控制器为什么这么设计。如果你正在学这门课或者想系统入门空中机器人这份第3和第4周的学习记录和习题复盘应该能帮你少走不少弯路。第3周核心是动力学与运动规划第4周核心是状态估计与传感器融合。这两周内容正好是四旋翼从“能飞”到“会飞”的关键跨越——没有动力学模型控制器就是盲调没有状态估计控制精度就上不去。我把课程笔记、思考过程和课后习题的完整推导都整理在下面了顺带加了一些实验里的踩坑记录供你参考。1. 课程主线再梳理先弄懂四旋翼为什么能飞1.1 课程定位空中机器人的核心矛盾在进入具体内容之前我先说说整体感受。这门课的主线其实是围绕一个核心矛盾展开的四旋翼是一个欠驱动、强耦合、非线性系统但要在三维空间中完成精准的轨迹跟踪和姿态稳定这本身就是工程学里很典型的一类问题——约束比自由度多目标比直觉复杂。前两周内容集中在运动学、坐标系变换、旋转矩阵和基础控制思路而第3和第4周真正进入了“核心难点区”。如果把整门课比作搭积木那么前两周是认识零件的形状和编号第3周是弄清楚怎么把零件拼起来并且受力分析与参数化建模第4周则是给这套机构装上“眼睛”和“感觉器官”——用什么方式知道自己在哪、姿态是什么。很多初学者的问题是前面两周学会了旋转矩阵和期望姿态角的计算一到第3周就被“机体坐标系”“角速度传播”“电机推力与扭矩关系”这些物理量之间的相互耦合搞得晕头转向。我当时第一次学也有这个阶段回头想关键就是没有先建立“四旋翼是一个刚体而且是一个由四个螺旋桨驱动、力和力矩都从螺旋桨产生的刚体”这个大框架。你要是先把这句话刻在脑子里后面所有公式都只是给这句话做数学化表达。1.2 第34周在课程体系中的位置从课程地图来看第3周紧接运动学部分开始引入刚体动力学的重要概念。这部分内容的核心是让读者理解四旋翼不是一个质点而是一个有真实质量分布、转动惯量的复杂机械系统。你设计控制器时不能假设力只作用在质心上还要考虑力矩平衡、螺旋桨反扭矩等细节。第4周则是在动态模型基本建立之后把“状态感知”补充进来。这部分的重要性常常被低估因为很多学习者会觉得传感器数据接线读出来不就行了但实际上传感器的噪声、漂移、坐标对齐等问题是现实飞控开发中调试时间最长的环节。下面内容我会按照“先动力学、后状态估计”的顺序展开每个星期都包含课程核心知识点、我自己的思考过程、课后习题的思路与答案验证以及配套的实操建议。2. 第3周核心刚体动力学与控制建模2.1 刚体运动学与动力学方程的再梳理第3周课件在一开始就花了很大篇幅在刚体的运动学与动力学描述上这部分是整套控制架构的基础。刚体运动学解决的是“位置、速度、加速度”和“姿态、角速度”之间的关系。课程中特别强调了旋转矩阵$R$的导数与角速度$\omega$之间的关系也就是$$\dot{R} R [\omega]_\times$$这个公式看起来很简短但值得花点时间彻底想明白。[$\omega]_\times$表示角速度的反对称矩阵形式。我当时在这里卡了很久因为我一直在想为什么旋转矩阵的导数不是直接乘以角速度的标量倍数而是要引入一个矩阵。后来我意识到这是因为角速度本身是一个三维向量它对应的“叉乘操作”只能写成矩阵才能作用在列向量上。这个公式极重要后续姿态解算、旋转矩阵微分、卡尔曼滤波里的状态转移矩阵都会用到它。动力学方程则是从牛顿-欧拉方程出发得到四旋翼在机体坐标系下的力和力矩平衡关系。课程给出的形式是$$m\ddot{r} -mgz_W R F_B$$这里需要拆开理解。$m\ddot{r}$是惯性坐标系下的质量加速度$-mgz_W$是重力$RF_B$表示将机体坐标系下螺旋桨产生的合力转换到世界坐标系。很多初学者不理解为什么重力只在$z$方向实际上是因为重力的方向定义在世界坐标系中向下而推力的方向定义在机体坐标系中向上二者必须通过旋转矩阵$R$进行联系。课程对力矩部分的推导也不少典型的一条是$$I\dot{\omega} -\omega \times I\omega \tau$$这个公式其实就是欧拉方程在机体系下的表述其中$\tau$来自四个旋翼的差速或倾转作用。对于四旋翼而言横滚力矩由左右电机升力差生成俯仰力矩由前后电机升力差生成偏航力矩由旋翼反扭矩差生成也就是教材中说的“由于旋转方向不同导致的反扭矩差异”。我建议学习这部分时自己完整地把四旋翼的剖面图画一遍四个机臂、两个正转桨、两个反转桨、每个桨产生的升力和反扭矩、以及相应的控制量分配矩阵。一定要亲自画这一步能帮你建立三维空间的物理直觉比反复读公式有效得多。2.2 电机模型与控制量分配从指令到力的转换四旋翼的控制链路上核心环节是“期望加速度 → 期望姿态 → 期望力矩 → 电机转速”。课程在第3周引入了电机一阶响应模型$$T C_T \rho n^2 D^4$$其中$C_T$是推力系数$\rho$是空气密度$n$是螺旋桨转速$D$是螺旋桨直径。这个公式告诉我们升力与转速的平方成正比所以在实际控制中电机转速是一个非常敏感的控制输入。因此第3周引入了“电机的动态特性不可忽略”的观点。课程中一般用一阶惯性环节近似电机的响应速度$$\dot{n} \alpha (n_{des} - n)$$这个$\alpha$的大小决定了电机的响应速度实际飞控调试里这个参数的选取直接关系到系统稳定性。之前我在飞行平台上做半实物仿真时把$\alpha$设得很高结果电机转速变化过快反而在起飞阶段出现了机体振荡调大惯量参数后启动过程才平顺下来。你需要从物理意义上理解这种现象电机转速本质上是不能突变的物理量响应时间常数固然小但它是一个实际存在的惯性环节控制器设计时要留出稳定裕度。控制量分配矩阵是第3周毫无疑问的“题眼”。常见的形式是控制量电机分配公式简化形式总推力 $U_1$$\frac{F_1F_2F_3F_4}{4}$ 取归一化形式横滚力矩 $U_2$$l (F_1 - F_3)$俯仰力矩 $U_3$$l (F_2 - F_4)$偏航力矩 $U_4$$k_\tau (F_1 - F_2 - F_3 F_4)$ 的反扭矩组合在实际代码中这个矩阵往往直接写成一个4×4矩阵乘上各个电机的转速平方或转速本身。我当时犯过一个错误就是把 $l$机臂长度漏掉了——仿真里速度调得飞快跌落得也飞快。后来排查好一阵才发现推力到力矩换算时的比例因子少了机臂长度导致力矩计算整整小了一个数量级。2.3 第3周学习时要建立的计算直觉从“命中靶心”看控制器课程在控制部分强调了一个形象类比把四旋翼的控制看成是“命中靶心”的过程。你指挥它从当前位置飞到目标位置不能只看“当前误差”还得看误差的导数也就是误差变化率否则系统会持续振荡。这个思路直接对应到PD控制器上也是最常用的内环姿态控制形式$$\tau K_p e K_d \dot{e}$$其中$e$为姿态误差的向量表示$\dot{e}$为误差变化率。课程专门强调了“旋转矩阵的差”不能简单用三维向量差替代因为姿态并不是线性空间中的量。很多初学者在这个地方容易踩坑把欧拉角的差直接当作控制器输入。在完成小角度假设时这勉强成立但一旦机动幅度稍大这种简化方式就会导致控制器响应异常甚至翻转。我建议你在代码里用“期望旋转矩阵 $R_{des}$ 与当前旋转矩阵 $R_{cur}$ 之间的李代数误差”来计算姿态误差课程其实给出了相关方法通过 $e_R 0.5 (R_{des}^T R_{cur} - R_{cur}^T R_{des})^\vee$ 来转移为三维向量误差。尽管这个表达式在初学时看起来很吓人但它本质上是“把旋转矩阵差换成轴角形式的误差向量”可以看作目标与小球的偏差量在三维空间中的对应描述。在实验里我后来都是用这个方式去做姿态环效果比欧拉角误差稳很多。尤其在做大幅机动时姿态环超调小了将近一半这就是正确数学表达带来的工程收益。2.4 第3周课后习题思路从悬停控制到轨迹跟踪第3周课程后面有一道非常典型的习题设计一个控制器使四旋翼能够在空中悬停并在受到扰动后恢复原来位置。解题的路径通常分三步第一步确定期望加速度。也就是根据位置误差 $r_{des} - r$ 和速度误差 $\dot{r}{des} - \dot{r}$ 设计期望加速度 $\ddot{r}{des}$。如果你用的是PD控制那么期望加速度就有如下形式$$\ddot{r}{des} K_p (r{des} - r) K_d (\dot{r}_{des} - \dot{r}) g z_W$$很多习题答案只写了前两项把重力项落掉了。但四旋翼悬停时推力需要平衡重力来维持高度所以期望加速度必须包含重力补偿。这个细节考的就是“你是否真正理解$z$轴上的力平衡”。第二步根据期望加速度求期望姿态。由于四旋翼只能沿机体$z$轴产生推力要产生水平方向的加速度就必须倾斜机体。所以期望机体的$z$轴方向可以由期望加速度向量和重力向量联合求出$$z_B^{des} \frac{\ddot{r}{des} - g z_W}{|\ddot{r}{des} - g z_W|}$$这个式子揭示了一个重要事实你控制水平加速度的所有物理机制最终都转化为“如何让机体主轴朝某个方向倾斜”。第三步姿态环求解期望力矩并配合电机分配输出转速。到这一步整个控制链路就闭环了。我在做这道习题的时候最大的收获是意识到“位置控制与姿态控制不是两个独立系统而是一个级联闭环”。上层的位置控制器给出期望姿态指令下层的姿态控制器执行姿态调整。课程里说这种结构叫“嵌套控制回路”它虽然不如全状态反馈控制那样理论上统一但工程调试上非常稳定几乎所有商业飞控都采用类似架构。2.5 动力学与控制部分的实操建议在第3周课程学习中有两件事值得你额外投入时间第一亲手建立仿真环境实现四旋翼的动力学模型。我用的方式是逐步搭建实现旋转矩阵和角速度之间的传播关系加入重力与推力再把电机模型和控制分配矩阵接上最后把PD控制器接到输出端。这个过程最好用数值积分器来做常用的有欧拉积分和四阶龙格-库塔积分。在仿真里你会发现欧拉积分在步长稍大时会出现能量不守恒的奇异行为这也是很多模拟仿真“越飞越高”的根源。了解这些工具的边界是工程师的基本素养。第二把电机的推力饱和限制加入仿真。很多教材在推导时默认电机推力可以无限大但真实电机存在转速上限。课程在这个阶段虽然没有专门提饱和但习题里如果加入了饱和限制控制器就容易出现积分饱和或者输出冻结。我可以负责任地说凡是实际飞过的开发者都知道“不处理饱和的控制器根本没法上真机”所以提前在仿真里加上限幅是一个很好的习惯。3. 第4周核心状态估计与传感器融合3.1 状态估计为什么是空中机器人绕不开的一环第4周的主题完全集中在“状态估计与传感器融合”上。这部分从概念上讲和机器人学中的“定位问题”是一致的为了进行反馈控制系统必须实时知道当前的状态包括位置、速度、姿态和角速度。但在实际机器上没有任何一个传感器能直接给出所有状态量的精确值。比如GPS可以给出绝对位置但更新频率比较低而且高架桥或茂密植被下极易丢星惯性测量单元IMU更新频率很高但本身带有加速度偏置、陀螺漂移和集成噪声。超声波和气压计能测高度但容易受到环境扰动影响。课程的核心思路是利用不同传感器的优势用数学模型把它们加权融合得到比任何单一传感器都更精确的状态估计。这在控制工程中被称作“多传感器融合”。你可以把每个传感器想象成一个带噪声的测量者它们各自提供片面的信息而融合算法就像一个对各个测量者意见进行合理加权汇总的“决策委员会”。3.2 卡尔曼滤波在姿态估计中的应用逻辑第4周内容的重点之一是卡尔曼滤波Kalman FilterKF在线性系统中的应用。对于线性系统卡尔曼滤波被证明是最小均方误差意义上的最优估计器。课程推导了整个预测-更新流程预测阶段$$\hat{x}{k|k-1} A \hat{x}{k-1|k-1} B u_k$$$$P_{k|k-1} A P_{k-1|k-1} A^T Q$$更新阶段$$K_k P_{k|k-1} H^T (H P_{k|k-1} H^T R)^{-1}$$$$\hat{x}{k|k} \hat{x}{k|k-1} K_k (z_k - H \hat{x}_{k|k-1})$$$$P_{k|k} (I - K_k H) P_{k|k-1}$$很多学习者在看到这组公式时容易陷入符号海洋忘记背后的直觉。我建议你把卡尔曼滤波分解为两层来理解第一状态预测层。系统根据动力学模型推断“现在大概在哪里”。这个推断一定会受到模型误差和随机扰动的影响其不确定性由协方差矩阵 $P$ 描述。第二测量更新层。传感器给出一个观测值但这个观测值带有不确定性。此时需要通过卡尔曼增益 $K$ 来平衡“模型预测”和“测量修正”的权重。如果传感器噪声大$R$ 增大$K$ 变小滤波器更信预测如果模型不准确$Q$ 增大$K$ 变大滤波器更信测量。这个权衡过程非常直观用生活化的类比就是你预估自己走到某个位置每隔几秒睁开眼睛确认一次。如果眼睛清晰$R$小你会更多相信眼睛如果眼睛模糊$R$大你就更多依赖脚步的步数推算$Q$小。卡尔曼滤波做的事情就是通过数学方式把这个直觉变成最优决策。3.3 互补滤波一种更轻量的工程备选方案第4周课程中也介绍了互补滤波。这个名字听起来比卡尔曼滤波“简单很多”但实际上互补滤波在四旋翼飞控里的应用非常广泛尤其在小规模飞控和低成本传感器平台上。互补滤波的核心思想是加速度计在低频段表现较好可以提供姿态的长期参考但高频噪声很明显陀螺仪在高频段响应迅速、动态性能好但低频段漂移严重。互补滤波通过一个高通滤波器处理陀螺积分通过一个低通滤波器处理加速度计解算的姿态两者相加得到融合姿态。公式虽然简单但实现时要注意截止频率的选择。截止频率太低姿态响应慢截止频率太高加速度计的震动噪声会大量进入姿态解算。我在一个室内小型机上实验过当截止频率取0.5 Hz左右时姿态解算对机架震动比较敏感当截止频率取0.05 Hz时姿态响应滞后明显飞起来很“肉”。最终调到0.1 Hz附近的折中才感觉合适。所以第4周我建议你这样实操先把互补滤波调通让自己对“传感器噪声与响应速度的折中”有直观感觉然后再切换成扩展卡尔曼滤波或更复杂的算法这样你对融合算法的差异会有真实的体感而不是只看别人的博客。3.4 扩展卡尔曼滤波的必要性EKF处理非线性严格来说四旋翼的动力学和观测模型都是非线性的。卡尔曼滤波在线性系统上是最优的但到了强非线性系统直接用线性卡尔曼滤波效果不好。第4周因此引入了扩展卡尔曼滤波EKF核心思想是在每个时刻把非线性系统在当前状态估计附近做一阶线性化。EKF的核心变化是状态转移矩阵 $A$ 和观测矩阵 $H$ 不再固定而是变成“当前状态估计处的雅可比矩阵”。这个思路不难但实操时很容易出错尤其是建立雅可比矩阵的过程容易算错——矩阵偏导计算量很大一旦有个小错误滤波器可能在几秒内就发散。我在跑EKF仿真时遇到过这样的案例初始状态给了一个较大偏置滤波器很快就“飞走了”。排查很久后发现问题出在状态转移方程的离散化近似上——连续时间的微分方程到离散时间更新时如果采用最简单的欧拉离散化在时间步长较大时会产生较大的截断误差。换用零阶保持或半隐式离散化后稳定性立刻改善。第4周的习题中大部分也集中在对EKF更新公式中雅可比矩阵的推导。建议你在学习时不要跳过手算过程即使解题过程繁琐也要亲手推一边典型的旋转矩阵和向量观测的雅可比比如用加速度计观测姿态角时的观测矩阵。3.5 GPS与IMU融合的实际落地方案课程中提到的GPS与IMU融合是四旋翼自主飞行中很现实的方案也是第4周习题中经常出现的题型。GPS提供绝对位置和速度通过多普勒效应解算IMU提供高频的加速度和角速度。融合的基本思路是用IMU做高频状态传播用GPS做低频修正来抑制IMU积分发散漂移。如果你在实际飞控里用过GPS一定知道GPS的噪声并不是白噪声它存在比较明显的相关性也就是连续几分钟内会产生缓慢漂移。这种情况下单纯把GPS当测量源卡尔曼滤波容易对低频漂移“照单全收”导致高度和位置估计出现慢漂。工程上通常会用气压计做高度辅助或者加装更好的差分GPS这些都属于工程系统的折中方案。对于刚接触第4周内容的读者我的建议是先把“IMU数值积分GPS绝对位置修正”这个框架跑通不要一开始就追求高端传感器。我在早期做导航实验时试过只用IMU做积分一分钟内位置漂移到不可接受的程度加了GPS修正后误差能控制在可接受范围内这个对比会让你体会到融合算法的真正价值。4. 第34周课后习题详解从解题思路到易错点复盘4.1 典型习题类型与标准答案结构第34周课后题整体风格偏理论推导和简单计算但也不是死套公式就能过关它对物理直觉的考察非常明显。我把遇到的题目分成了三类类型一动力学与控制量计算。给定四旋翼参数质量、转动惯量、机臂长度等求悬停时四个电机的推力分配或者给定期望加速度向量求所需的期望姿态角和推力大小。类型二线性化与控制稳定性分析。将动力学模型在小角度扰动下线性化求传递函数或特征方程并分析控制增益对稳定性的影响。这类题目经常让人推导到一半发现代数越算越长其实很多时候需要你主动采用“小角度近似”把正弦余弦替换为角度本身。类型三状态估计与滤波器推导。给定一个简化的一维系统模型比如高度估计系统设计卡尔曼滤波器给出状态转移矩阵、观测矩阵、卡尔曼增益表达式并解释噪声协方差矩阵对滤波效果的影响。4.2 悬停控制环节中的典型答案推导对于一道典型的悬停控制题已知四旋翼质量 $m$、重力加速度 $g$、期望位置 $r_{des}$ 为原点初始位置与速度有扰动。使用PD控制器$$\ddot{r}{des} K_p (r{des} - r) K_d (\dot{r}_{des} - \dot{r}) g z_W$$代入数值或符号后得到期望加速度。然后通过关系$$z_B^{des} \parallel (\ddot{r}_{des} - g z_W)$$求期望机体$z$轴方向。保持偏航角不变的一个常见简便是设偏航角为零这样横滚角和俯仰角可以从期望$z$轴方向直接提取。我在第3周刚开始做这道题时习惯把横滚和俯仰分开算但后来发现直接从轴向量解出期望姿态矩阵更简洁高效。具体做法是先得到$z_B^{des}$再根据期望偏航角计算$x_B^{des}$最后用叉积得到$y_B^{des}$从而得到完整期望旋转矩阵$R_{des}$。这个过程在矩阵层面一步到位比拆成欧拉角好控制得多。容易犯错的地方有两个第一个是符号方向。重力加速度 $g$ 在惯性系中沿$-z$方向所以实际方程中会写成$-mg z_W$或$g z_W$取决于你如何定义 $g$ 的正负。很多同学会差一个符号导致方向反了控制器非但悬停不了还会直接往地上扎。第二个是增益选择。PD参数 $K_p$、$K_d$ 必须满足系统稳定条件。在仿真里如果 $K_p$ 太大而 $K_d$ 不够整个系统会出现持续振荡而且这种振荡会通过姿态耦合变得越来越严重。实际中你应该先用$K_d$ 阻尼把振荡压住再逐步提高 $K_p$ 即响应刚度。4.3 卡尔曼滤波习题中的几个常见误区和陷阱第4周卡尔曼滤波的习题里最容易出错的几个点我逐个说一下。第一没有区分“预测协方差”和“更新协方差”。二者对应不同的公式如果在代码中复用同一个变量而不加区分整个滤波器数值大概率发散。我的建议是严格按照教材区分 $P_{k|k-1}$ 和 $P_{k|k}$ 两个变量至少在代码层面不混用。第二将过程噪声协方差 $Q$ 设为零。这是非常灾难性的简化会导致滤波器的置信度无限偏向模型预测测量数据完全不参与修正最终滤波结果沿着动力学模型自由漂移传感器数据形同虚设。所以 $Q$ 应该根据模型的真实置信程度设置不能设成零。如果模型本身不精确就大胆把 $Q$ 调大让测量去修正预测。第三在更新方程里漏掉了矩阵维度匹配与转置操作尤其是 $H P H^T R$ 中 $H$ 的左右位置不能搞反。我曾经在一个四维状态、二维观测的模型里把 $H$ 的转置写反导致卡尔曼增益矩阵维数不匹配运行时报错后检查了整整一晚。这种低级问题在工程中非常常见但也是最浪费时间的所以建议写代码时先用最简一维模型验证滤波逻辑再拓展到高维。我在学习第4周习题时还注意到了一个额外的陷阱很多题目虽然假设测量噪声是高斯白噪声但真实传感器数据中往往存在“野值”异常大的跳变点。如果不做野值剔除直接送入卡尔曼滤波输出会发生瞬时突变。我通常的做法是加入“新息残差判断”逻辑如果 $(z_k - H\hat{x}_{k|k-1})$ 的模长超过某个阈值就降低甚至置零对应时刻的测量权重。这种在工程中的应用技巧课程里不专门教但实际项目里非常有用。4.4 从习题看控制与估计的闭环协同第34周习题有一个很好的设计它们并不是孤立的而是在不断提示“控制”和“估计”最终是要协同工作的。控制需要状态状态来自估计估计需要控制指令作为预测输入。这条因果链在习题中表现为控制章节的题目给出“真实”状态要求你设计控制律状态估计章节的题目给出“观测”数据要求你估计状态最后一部分题目则要求你用估计出的状态来驱动控制器评估闭环性能。这类综合题是我个人认为最有价值的练习。你在练习时可以尝试把第3周的控制器和第4周的卡尔曼滤波器结合起来跑一个完整仿真传感器数据加噪声 → EKF进行状态估计 → 控制器根据估计状态计算控制指令 → 动力学模型更新位置与姿态 → 输出新的观测数据。这样一个闭环仿真能够帮你把整个空中机器人系统完整地在“虚拟世界”里体验一遍。我当时做这个闭环仿真时印象最深的是用真值状态控制时轨迹非常平滑换成估计状态控制后轨迹一开始出现了轻微抖动。这让我意识到“控制精度上限是由状态估计精度决定的”而不是单纯调控制增益能补救的。随后我再调低了传感器噪声协方差轨迹立刻改善。这个试错过程给我建立了“控制与估计必须一体设计”的思维模式。5. 学习与实操建议如何扎实拿捏这一周内容5.1 从第34周内容延伸出的真实工程问题很多学习者在刷完课程、做完几道题之后会有一个错觉自己已经掌握四旋翼建模和状态估计了。但一到真实环境甚至稍微复杂一些的仿真里问题就接踵而至。下面这几个真实工程问题我认为是第34周内容的自然延伸建议提前有意识地去思考第一个是模型参数误差。四旋翼的真实质量、转动惯量、气动阻尼系数并不一定和标称值完全一致。控制律在模型准确时表现良好一旦模型参数偏差超过一定范围闭环系统可能出现低频振荡甚至不稳定。第3周所学内容中稳定性分析部分的价值正在于此——你要通过鲁棒性分析判断什么样的参数偏差是允许的。第二个是执行器饱和。电机转速存在物理上限和下限。当控制器给出的转速指令超出上限时实际执行并不会完全跟随。饱和不仅是简单的限幅问题它还会造成控制器积分饱和、姿态误差变大、甚至系统失稳。我在一个高机动飞行的项目中就遇到过横滚角误差持续增大导致的饱和累积现象后来不得不加入抗饱和策略才解决问题。第三个是传感器时间戳同步。GPS和IMU的数据频率不同而且传感器数据在传输过程中有延迟。如果不对齐时间戳就直接融合状态估计结果会带有隐含时间误差。专业飞控和导航系统中都有专门的时间同步模块这其实是对第4周课程的延伸和实战考验。5.2 环境搭建与代码调试工具推荐如果你第34周想动手实操我建议从以下几类工具入手仿真环境方面首推在Python里用数值积分实现简化四旋翼动力学。先不要一上来就用大型仿真平台因为大型平台封装得太好很多内部细节你看不透。手写动力学模型能帮助你理解每一步推导。调试工具方面可以准备一套数据可视化工具把四旋翼的姿态、角速度、电机指令、估计误差全部实时显示出来。我在调试EKF时如果只靠打印数据很难发现十几秒内的缓慢漂移画成曲线后信号的漂移趋势一目了然。任何能让你“肉眼看到状态随时间变化”的工具都是调试状态估计算法的利器。代码版本方面建议把每个实验步骤都写成可复现的脚本。特别是在调卡尔曼滤波时修改一个噪声协方差值就可能导致完全不同的输出只有文档齐全的脚本才能帮你定位到底哪一步改动导致行为变化。5.3 用“最小复现单元”验证每个知识点这是我学习这门课最受益的习惯每一个知识点都尽量用“最小复现单元”去验证。举例来说学旋转矩阵微分时就写一个几行代码的脚本随机生成角速度让旋转矩阵按公式积分再检查得到的矩阵是否始终满足单位正交性。学卡尔曼滤波时先设计一个一维问题设定一个简单的恒值状态加入高斯噪声让滤波收敛后查看估计值和真值的误差。这种验证方式的优势在于它逼着你手推公式也让代码运行结果成为“公式是否理解正确”的反馈信号。如果公式理解有偏差仿真输出很快会异常如果在理解完全正确的前提下你还能观察数值误差与积分步长之间的关系这会让你对数值方法本身的边界也有更深的认知。我在第3周做旋转矩阵积分验证时曾经发现一个有趣现象在离散积分中如果不做每步的正交化修正积分几十步后旋转矩阵就会偏离单位正交条件导致姿态矩阵“变形”甚至最终奇异。这个现象在理论推导中不明显但工程仿真中非常致命。正是通过最小复现单元我才真正理解了为什么很多姿态解算代码每步都要做重正交化处理。5.4 考试/作业刷题节奏安排考虑到第34周内容较多我通常按以下节奏来安排第1-3天通读课件和讲义只看概念和公式推导不看习题答案尝试理解每个公式的物理意义与推导来源。第4-6天动手推导每个核心公式把“旋转矩阵导数”“动力学方程”“控制分配矩阵”“卡尔曼滤波五个公式”等核心内容在纸上完整推导一遍务必独立完成。第7-10天做课后习题。如果遇到不会的题先卡在思路层面不回看答案尽量回忆课程中对应的概念和推导路径与技巧。第10-14天编写与控制、滤波相关的仿真代码与最小复现单元把习题答案在仿真中验证一遍。最后再挑一个控制估计的综合题做完整闭环仿真。这套节奏不一定适合所有人但核心思想是“先理解再推导后做题再代码验证”四个步骤相互支撑能避免“看着答案会了合上书又忘光”的假性学习。6. 避坑经验总结第34周的学习体验说白了就是一场“数学细节战”。我把自己在这两周里踩过的主要坑整理成一个速查表希望对你有实际帮助。易错环节典型表现正确做法旋转矩阵求导误以为 $\dot{R} \dot{\theta} R$必须使用 $\dot{R} R[\omega]_\times$重力项符号竖直方向力平衡少加重力补偿期望加速度必须包含 $g z_W$力矩计算分配矩阵中漏掉机臂长度 $l$每个力矩计算都要乘长度电机响应把电机当作理想无延迟执行器加入一阶惯性模型姿态误差计算直接用欧拉角差取代旋转矩阵误差使用 $e_R 0.5 (R_{des}^T R_{cur} - R_{cur}^T R_{des})^\vee$卡尔曼协方差$P_{k|k-1}$ 与 $P_{k|k}$ 混用区分预测与更新协方差过程噪声$Q$ 设置为零根据模型置信度设置合理 $Q$高频/低频融合陀螺与加速度计权重失衡用互补滤波/卡尔曼滤波合理加权传感器时序忽略GPS/IMU时间戳对齐做传感器时间同步后再融合野值干扰突变的测量直接进入滤波器加入残差判断与野值剔除我一直认为学习空中机器人最重要的不是背公式而是建立从“物理直觉”到“数学模型”再到“代码实现”的三层映射。第3周动力学部分是这层映射的起点第4周状态估计部分则是补全闭环的关键拼图。如果你能跟着这两周内容走并在仿真里亲手验证一遍你对四旋翼的理解会从“它会飞”进化到“我理解它怎么飞、为什么飞得稳”。后续再学路径规划、视觉导航、多机协同这些高阶方向时地基就会非常扎实。另外一个很实际的心得遇到数值仿真发散问题时别急着改控制参数先检查模型坐标定义和符号方向。很多发散问题的根源是坐标轴搞反或者旋转方向写反白白浪费了一晚上调试时间。我在那两次刷课经历中不少夜晚都在干这种“以为自己调参实际调的是符号”的傻事——把坐标系标注清楚、把符号约定写在代码注释最前面是降低这类无意义损耗的好习惯。希望这份学习记录能帮到正在啃第34周内容的你。如果你在推导或仿真里遇到具体问题也欢迎带着你的代码和公式来交流——一起把问题定位到“是物理直觉没建立还是数学推导出了岔子还是代码实现有疏漏”比单纯对答案有意义得多。 这门课我前后刷了两遍第一遍是在校期间跟着课程节奏走第二遍是工作后做飞控相关项目时回头补的。两次学习的感受完全不同第一次是被动接受知识点第二次才真正理解了四旋翼为什么这么飞、控制器为什么这么设计。如果你正在学这门课或者想系统入门空中机器人这份第3和第4周的学习记录和习题复盘应该能帮你少走不少弯路。第3周核心是动力学与运动规划第4周核心是状态估计与传感器融合。这两周内容正好是四旋翼从“能飞”到“会飞”的关键跨越——没有动力学模型控制器就是盲调没有状态估计控制精度就上不去。我把课程笔记、思考过程和课后习题的完整推导都整理在下面了顺带加了一些实验里的踩坑记录供你参考。1. 课程主线再梳理先弄懂四旋翼为什么能飞1.1 课程定位空中机器人的核心矛盾在进入具体内容之前我先说说整体感受。这门课的主线其实是围绕一个核心矛盾展开的四旋翼是一个欠驱动、强耦合、非线性系统但要在三维空间中完成精准的轨迹跟踪和姿态稳定这本身就是工程学里很典型的一类问题——约束比自由度多目标比直觉复杂。前两周内容集中在运动学、坐标系变换、旋转矩阵和基础控制思路而第3和第4周真正进入了“核心难点区”。如果把整门课比作搭积木那么前两周是认识零件的形状和编号第3周是弄清楚怎么把零件拼起来并且受力分析与参数化建模第4周则是给这套机构装上“眼睛”和“感觉器官”——用什么方式知道自己在哪、姿态是什么。很多初学者的问题是前面两周学会了旋转矩阵和期望姿态角的计算一到第3周就被“机体坐标系”“角速度传播”“电机推力与扭矩关系”这些物理量之间的相互耦合搞得晕头转向。我当时第一次学也有这个阶段回头想关键就是没有先建立“四旋翼是一个刚体而且是一个由四个螺旋桨驱动、力和力矩都从螺旋桨产生的刚体”这个大框架。你要是先把这句话刻在脑子里后面所有公式都只是给这句话做数学化表达。1.2 第34周在课程体系中的位置从课程地图来看第3周紧接运动学部分开始引入刚体动力学的重要概念。这部分内容的核心是让读者理解四旋翼不是一个质点而是一个有真实质量分布、转动惯量的复杂机械系统。你设计控制器时不能假设力只作用在质心上还要考虑力矩平衡、螺旋桨反扭矩等细节。第4周则是在动态模型基本建立之后把“状态感知”补充进来。这部分的重要性常常被低估因为很多学习者会觉得传感器数据接线读出来不就行了但实际上传感器的噪声、漂移、坐标对齐等问题是现实飞控开发中调试时间最长的环节。下面内容我会按照“先动力学、后状态估计”的顺序展开每个星期都包含课程核心知识点、我自己的思考过程、课后习题的思路与答案验证以及配套的实操建议。2. 第3周核心刚体动力学与控制建模2.1 刚体运动学与动力学方程的再梳理第3周课件在一开始就花了很大篇幅在刚体的运动学与动力学描述上这部分是整套控制架构的基础。刚体运动学解决的是“位置、速度、加速度”和“姿态、角速度”之间的关系。课程中特别强调了旋转矩阵$R$的导数与角速度$\omega$之间的关系也就是$$\dot{R} R [\omega]_\times$$这个公式看起来很简短但值得花点时间彻底想明白。[$\omega]_\times$表示角速度的反对称矩阵形式。我当时在这里卡了很久因为我一直在想为什么旋转矩阵的导数不是直接乘以角速度的标量倍数而是要引入一个矩阵。后来我意识到这是因为角速度本身是一个三维向量它对应的“叉乘操作”只能写成矩阵才能作用在列向量上。这个公式极重要后续姿态解算、旋转矩阵微分、卡尔曼滤波里的状态转移矩阵都会用到它。动力学方程则是从牛顿-欧拉方程出发得到四旋翼在机体坐标系下的力和力矩平衡关系。课程给出的形式是$$m\ddot{r} -mgz_W R F_B$$这里需要拆开理解。$m\ddot{r}$是惯性坐标系下的质量加速度$-mgz_W$是重力$RF_B$表示将机体坐标系下螺旋桨产生的合力转换到世界坐标系。很多初学者不理解为什么重力只在$z$方向实际上是因为重力的方向定义在世界坐标系中向下而推力的方向定义在机体坐标系中向上二者必须通过旋转矩阵$R$进行联系。课程对力矩部分的推导也不少典型的一条是$$I\dot{\omega} -\omega \times I\omega \tau$$这个公式其实就是欧拉方程在机体系下的表述其中$\tau$来自四个旋翼的差速或倾转作用。对于四旋翼而言横滚力矩由左右电机升力差生成俯仰力矩由前后电机升力差生成偏航力矩由旋翼反扭矩差生成也就是教材中说的“由于旋转方向不同导致的反扭矩差异”。我建议学习这部分时自己完整地把四旋翼的剖面图画一遍四个机臂、两个正转桨、两个反转桨、每个桨产生的升力和反扭矩、以及相应的控制量分配矩阵。一定要亲自画这一步能帮你建立三维空间的物理直觉比反复读公式有效得多。2.2 电机模型与控制量分配从指令到力的转换四旋翼的控制链路上核心环节是“期望加速度 → 期望姿态 → 期望力矩 → 电机转速”。课程在第3周引入了电机一阶响应模型$$T C_T \rho n^2 D^4$$其中$C_T$是推力系数$\rho$是空气密度$n$是螺旋桨转速$D$是螺旋桨直径。这个公式告诉我们升力与转速的平方成正比所以在实际控制中电机转速是一个非常敏感的控制输入。因此第3周引入了“电机的动态特性不可忽略”的观点。课程中一般用一阶惯性环节近似电机的响应速度$$\dot{n} \alpha (n_{des} - n)$$这个$\alpha$的大小决定了电机的响应速度实际飞控调试里这个参数的选取直接关系到系统稳定性。之前我在飞行平台上做半实物仿真时把$\alpha$设得很高结果电机转速变化过快反而在起飞阶段出现了机体振荡调大惯量参数后启动过程才平顺下来。你需要从物理意义上理解这种现象电机转速本质上是不能突变的物理量响应时间常数固然小但它是一个实际存在的惯性环节控制器设计时要留出稳定裕度。控制量分配矩阵是第3周毫无疑问的“题眼”。常见的形式是控制量电机分配公式简化形式总推力 $U_1$$\frac{F_1F_2F_3F_4}{4}$ 取归一化形式横滚力矩 $U_2$$l (F_1 - F_3)$俯仰力矩 $U_3$$l (F_2 - F_4)$偏航力矩 $U_4$$k_\tau (F_1 - F_2 - F_3 F_4)$ 的反扭矩组合在实际代码中这个矩阵往往直接写成一个4×4矩阵乘上各个电机的转速平方或转速本身。我当时犯过一个错误就是把 $l$机臂长度漏掉了——仿真里速度调得飞快跌落得也飞快。后来排查好一阵才发现推力到力矩换算时的比例因子少了机臂长度导致力矩计算整整小了一个数量级。2.3 第3周学习时要建立的计算直觉从“命中靶心”看控制器课程在控制部分强调了一个形象类比把四旋翼的控制看成是“命中靶心”的过程。你指挥它从当前位置飞到目标位置不能只看“当前误差”还得看误差的导数也就是误差变化率否则系统会持续振荡。这个思路直接对应到PD控制器上也是最常用的内环姿态控制形式$$\tau K_p e K_d \dot{e}$$其中$e$为姿态误差的向量表示$\dot{e}$为误差变化率。课程专门强调了“旋转矩阵的差”不能简单用三维向量差替代因为姿态并不是线性空间中的量。很多初学者在这个地方容易踩坑把欧拉角的差直接当作控制器输入。在完成小角度假设时这勉强成立但一旦机动幅度稍大这种简化方式就会导致控制器响应异常甚至翻转。我建议你在代码里用“期望旋转矩阵 $R_{des}$ 与当前旋转矩阵 $R_{cur}$ 之间的李代数误差”来计算姿态误差课程其实给出了相关方法通过 $e_R 0.5 (R_{des}^T R_{cur} - R_{cur}^T R_{des})^\vee$ 来转移为三维向量误差。尽管这个表达式在初学时看起来很吓人但它本质上是“把旋转矩阵差换成轴角形式的误差向量”可以看作目标与小球的偏差量在三维空间中的对应描述。在实验里我后来都是用这个方式去做姿态环效果比欧拉角误差稳很多。尤其在做大幅机动时姿态环超调小了将近一半这就是正确数学表达带来的工程收益。2.4 第3周课后习题思路从悬停控制到轨迹跟踪第3周课程后面有一道非常典型的习题设计一个控制器使四旋翼能够在空中悬停并在受到扰动后恢复原来位置。解题的路径通常分三步第一步确定期望加速度。也就是根据位置误差 $r_{des} - r$ 和速度误差 $\dot{r}{des} - \dot{r}$ 设计期望加速度 $\ddot{r}{des}$。如果你用的是PD控制那么期望加速度就有如下形式$$\ddot{r}{des} K_p (r{des} - r) K_d (\dot{r}_{des} - \dot{r}) g z_W$$很多习题答案只写了前两项把重力项落掉了。但四旋翼悬停时推力需要平衡重力来维持高度所以期望加速度必须包含重力补偿。这个细节考的就是“你是否真正理解$z$轴上的力平衡”。第二步根据期望加速度求期望姿态。由于四旋翼只能沿机体$z$轴产生推力要产生水平方向的加速度就必须倾斜机体。所以期望机体的$z$轴方向可以由期望加速度向量和重力向量联合求出$$z_B^{des} \frac{\ddot{r}{des} - g z_W}{|\ddot{r}{des} - g z_W|}$$这个式子揭示了一个重要事实你控制水平加速度的所有物理机制最终都转化为“如何让机体主轴朝某个方向倾斜”。第三步姿态环求解期望力矩并配合电机分配输出转速。到这一步整个控制链路就闭环了。我在做这道习题的时候最大的收获是意识到“位置控制与姿态控制不是两个独立系统而是一个级联闭环”。上层的位置控制器给出期望姿态指令下层的姿态控制器执行姿态调整。课程里说这种结构叫“嵌套控制回路”它虽然不如全状态反馈控制那样理论上统一但工程调试上非常稳定几乎所有商业飞控都采用类似架构。2.5 动力学与控制部分的实操建议在第3周课程学习中有两件事值得你额外投入时间第一亲手建立仿真环境实现四旋翼的动力学模型。我用的方式是逐步搭建实现旋转矩阵和角速度之间的传播关系加入重力与推力再把电机模型和控制分配矩阵接上最后把PD控制器接到输出端。这个过程最好用数值积分器来做常用的有欧拉积分和四阶龙格-库塔积分。在仿真里你会发现欧拉积分在步长稍大时会出现能量不守恒的奇异行为这也是很多模拟仿真“越飞越高”的根源。了解这些工具的边界是工程师的基本素养。第二把电机的推力饱和限制加入仿真。很多教材在推导时默认电机推力可以无限大但真实电机存在转速上限。课程在这个阶段虽然没有专门提饱和但习题里如果加入了饱和限制控制器就容易出现积分饱和或者输出冻结。我可以负责任地说凡是实际飞过的开发者都知道“不处理饱和的控制器根本没法上真机”所以提前在仿真里加上限幅是一个很好的习惯。3. 第4周核心状态估计与传感器融合3.1 状态估计为什么是空中机器人绕不开的一环第4周的主题完全集中在“状态估计与传感器融合”上。这部分从概念上讲和机器人学中的“定位问题”是一致的为了进行反馈控制系统必须实时知道当前的状态包括位置、速度、姿态和角速度。但在实际机器上没有任何一个传感器能直接给出所有状态量的精确值。比如GPS可以给出绝对位置但更新频率比较低而且高架桥或茂密植被下极易丢星惯性测量单元IMU更新频率很高但本身带有加速度偏置、陀螺漂移和集成噪声。超声波和气压计能测高度但容易受到环境扰动影响。课程的核心思路是利用不同传感器的优势用数学模型把它们加权融合得到比任何单一传感器都更精确的状态估计。这在控制工程中被称作“多传感器融合”。你可以把每个传感器想象成一个带噪声的测量者它们各自提供片面的信息而融合算法就像一个对各个测量者意见进行合理加权汇总的“决策委员会”。3.2 卡尔曼滤波在姿态估计中的应用逻辑第4周内容的重点之一是卡尔曼滤波Kalman FilterKF在线性系统中的应用。对于线性系统卡尔曼滤波被证明是最小均方误差意义上的最优估计器。课程推导了整个预测-更新流程预测阶段$$\hat{x}{k|k-1} A \hat{x}{k-1|k-1} B u_k$$$$P_{k|k-1} A P_{k-1|k-1} A^T Q$$更新阶段$$K_k P_{k|k-1} H^T (H P_{k|k-1} H^T R)^{-1}$$$$\hat{x}{k|k} \hat{x}{k|k-1} K_k (z_k - H \hat{x}_{k|k-1})$$$$P_{k|k} (I - K_k H) P_{k|k-1}$$很多学习者在看到这组公式时容易陷入符号海洋忘记背后的直觉。我建议你把卡尔曼滤波分解为两层来理解第一状态预测层。系统根据动力学模型推断“现在大概在哪里”。这个推断一定会受到模型误差和随机扰动的影响其不确定性由协方差矩阵 $P$ 描述。第二测量更新层。传感器给出一个观测值但这个观测值带有不确定性。此时需要通过卡尔曼增益 $K$ 来平衡“模型预测”和“测量修正”的权重。如果传感器噪声大$R$ 增大$K$ 变小滤波器更信预测如果模型不准确$Q$ 增大$K$ 变大滤波器更信测量。这个权衡过程非常直观用生活化的类比就是你预估自己走到某个位置每隔几秒睁开眼睛确认一次。如果眼睛清晰$R$小你会更多相信眼睛如果眼睛模糊$R$大你就更多依赖脚步的步数推算$Q$小。卡尔曼滤波做的事情就是通过数学方式把这个直觉变成最优决策。3.3 互补滤波一种更轻量的工程备选方案第4周课程中也介绍了互补滤波。这个名字听起来比卡尔曼滤波“简单很多”但实际上互补滤波在四旋翼飞控里的应用非常广泛尤其在小规模飞控和低成本传感器平台上。互补滤波的核心思想是加速度计在低频段表现较好可以提供姿态的长期参考但高频噪声很明显陀螺仪在高频段响应迅速、动态性能好但低频段漂移严重。互补滤波通过一个高通滤波器处理陀螺积分通过一个低通滤波器处理加速度计解算的姿态两者相加得到融合姿态。公式虽然简单但实现时要注意截止频率的选择。截止频率太低姿态响应慢截止频率太高加速度计的震动噪声会大量进入姿态解算。我在一个室内小型机上实验过当截止频率取0.5 Hz左右时姿态解算对机架震动比较敏感当截止频率取0.05 Hz时姿态响应滞后明显飞起来很“肉”。最终调到0.1 Hz附近的折中才感觉合适。所以第4周我建议你这样实操先把互补滤波调通让自己对“传感器噪声与响应速度的折中”有直观感觉然后再切换成扩展卡尔曼滤波或更复杂的算法这样你对融合算法的差异会有真实的体感而不是只看别人的博客。3.4 扩展卡尔曼滤波的必要性EKF处理非线性严格来说四旋翼的动力学和观测模型都是非线性的。卡尔曼滤波在线性系统上是最优的但到了强非线性系统直接用线性卡尔曼滤波效果不好。第4周因此引入了扩展卡尔曼滤波EKF核心思想是在每个时刻把非线性系统在当前状态估计附近做一阶线性化。EKF的核心变化是状态转移矩阵 $A$ 和观测矩阵 $H$ 不再固定而是变成“当前状态估计处的雅可比矩阵”。这个思路不难但实操时很容易出错尤其是建立雅可比矩阵的过程容易算错——矩阵偏导计算量很大一旦有个小错误滤波器可能在几秒内就发散。我在跑EKF仿真时遇到过这样的案例初始状态给了一个较大偏置滤波器很快就“飞走了”。排查很久后发现问题出在状态转移方程的离散化近似上——连续时间的微分方程到离散时间更新时如果采用最简单的欧拉离散化在时间步长较大时会产生较大的截断误差。换用零阶保持或半隐式离散化后稳定性立刻改善。第4周的习题中大部分也集中在对EKF更新公式中雅可比矩阵的推导。建议你在学习时不要跳过手算过程即使解题过程繁琐也要亲手推一边典型的旋转矩阵和向量观测的雅可比比如用加速度计观测姿态角时的观测矩阵。3.5 GPS与IMU融合的实际落地方案课程中提到的GPS与IMU融合是四旋翼自主飞行中很现实的方案也是第4周习题中经常出现的题型。GPS提供绝对位置和速度通过多普勒效应解算IMU提供高频的加速度和角速度。融合的基本思路是用IMU做高频状态传播用GPS做低频修正来抑制IMU积分发散漂移。如果你在实际飞控里用过GPS一定知道GPS的噪声并不是白噪声它存在比较明显的相关性也就是连续几分钟内会产生缓慢漂移。这种情况下单纯把GPS当测量源卡尔曼滤波容易对低频漂移“照单全收”导致高度和位置估计出现慢漂。工程上通常会用气压计做高度辅助或者加装更好的差分GPS这些都属于工程系统的折中方案。对于刚接触第4周内容的读者我的建议是先把“IMU数值积分GPS绝对位置修正”这个框架跑通不要一开始就追求高端传感器。我在早期做导航实验时试过只用IMU做积分一分钟内位置漂移到不可接受的程度加了GPS修正后误差能控制在可接受范围内这个对比会让你体会到融合算法的真正价值。4. 第34周课后习题详解从解题思路到易错点复盘4.1 典型习题类型与标准答案结构第34周课后题整体风格偏理论推导和简单计算但也不是死套公式就能过关它对物理直觉的考察非常明显。我把遇到的题目分成了三类类型一动力学与控制量计算。给定四旋翼参数质量、转动惯量、机臂长度等求悬停时四个电机的推力分配或者给定期望加速度向量求所需的期望姿态角和推力大小。类型二线性化与控制稳定性分析。将动力学模型在小角度扰动下线性化求传递函数或特征方程并分析控制增益对稳定性的影响。这类题目经常让人推导到一半发现代数越算越长其实很多时候需要你主动采用“小角度近似”把正弦余弦替换为角度本身。类型三状态估计与滤波器推导。给定一个简化的一维系统模型比如高度估计系统设计卡尔曼滤波器给出状态转移矩阵、观测矩阵、卡尔曼增益表达式并解释噪声协方差矩阵对滤波效果的影响。4.2 悬停控制环节中的典型答案推导对于一道典型的悬停控制题已知四旋翼质量 $m$、重力加速度 $g$、期望位置 $r_{des}$ 为原点初始位置与速度有扰动。使用PD控制器$$\ddot{r}{des} K_p (r{des} - r) K_d (\dot{r}_{des} - \dot{r}) g z_W$$代入数值或符号后得到期望加速度。然后通过关系$$z_B^{des} \parallel (\ddot{r}_{des} - g z_W)$$求期望机体$z$轴方向。保持偏航角不变的一个常见简便是设偏航角为零这样横滚角和俯仰角可以从期望$z$轴方向直接提取。我在第3周刚开始做这道题时习惯把横滚和俯仰分开算但后来发现直接从轴向量解出期望姿态矩阵更简洁高效。具体做法是先得到$z_B^{des}$再根据期望偏航角计算$x_B^{des}$最后用叉积得到$y_B^{des}$从而得到完整期望旋转矩阵$R_{des}$。这个过程在矩阵层面一步到位比拆成欧拉角好控制得多。容易犯错的地方有两个第一个是符号方向。重力加速度 $g$ 在惯性系中沿$-z$方向所以实际方程中会写成$-mg z_W$或$g z_W$取决于你如何定义 $g$ 的正负。很多同学会差一个符号导致方向反了控制器非但悬停不了还会直接往地上扎。第二个是增益选择。PD参数 $K_p$、$K_d$ 必须满足系统稳定条件。在仿真里如果 $K_p$ 太大而 $K_d$ 不够整个系统会出现持续振荡而且这种振荡会通过姿态耦合变得越来越严重。实际中你应该先用$K_d$ 阻尼把振荡压住再逐步提高 $K_p$ 即响应刚度。4.3 卡尔曼滤波习题中的几个常见误区和陷阱第4周卡尔曼滤波的习题里最容易出错的几个点我逐个说一下。第一没有区分“预测协方差”和“更新协方差”。二者对应不同的公式如果在代码中复用同一个变量而不加区分整个滤波器数值大概率发散。我的建议是严格按照教材区分 $P_{k|k-1}$ 和 $P_{k|k}$ 两个变量至少在代码层面不混用。第二将过程噪声协方差 $Q$ 设为零。这是非常灾难性的简化会导致滤波器的置信度无限偏向模型预测测量数据完全不参与修正最终滤波结果沿着动力学模型自由漂移传感器数据形同虚设。所以 $Q$ 应该根据模型的真实置信程度设置不能设成零。如果模型本身不精确就大胆把 $Q$ 调大让测量去修正预测。第三在更新方程里漏掉了矩阵维度匹配与转置操作尤其是 $H P H^T R$ 中 $H$ 的左右位置不能搞反。我曾经在一个四维状态、二维观测的模型里把 $H$ 的转置写反导致卡尔曼增益矩阵维数不匹配运行时报错后检查了整整一晚。这种低级问题在工程中非常常见但也是最浪费时间的所以建议写代码时先用最简一维模型验证滤波逻辑再拓展到高维。我在学习第4周习题时还注意到了一个额外的陷阱很多题目虽然假设测量噪声是高斯白噪声但真实传感器数据中往往存在“野值”异常大的跳变点。如果不做野值剔除直接送入卡尔曼滤波输出会发生瞬时突变。我通常的做法是加入“新息残差判断”逻辑如果 $(z_k - H\hat{x}_{k|k-1})$ 的模长超过某个阈值就降低甚至置零对应时刻的测量权重。这种在工程中的应用技巧课程里不专门教但实际项目里非常有用。4.4 从习题看控制与估计的闭环协同第34周习题有一个很好的设计它们并不是孤立的而是在不断提示“控制”和“估计”最终是要协同工作的。控制需要状态状态来自估计估计需要控制指令作为预测输入。这条因果链在习题中表现为控制章节的题目给出“真实”状态要求你设计控制律状态估计章节的题目给出“观测”数据要求你估计状态最后一部分题目则要求你用估计出的状态来驱动控制器评估闭环性能。这类综合题是我个人认为最有价值的练习。你在练习时可以尝试把第3周的控制器和第4周的卡尔曼滤波器结合起来跑一个完整仿真传感器数据加噪声 → EKF进行状态估计 → 控制器根据估计状态计算控制指令 → 动力学模型更新位置与姿态 → 输出新的观测数据。这样一个闭环仿真能够帮你把整个空中机器人系统完整地在“虚拟世界”里体验一遍。我当时做这个闭环仿真时印象最深的是用真值状态控制时轨迹非常平滑换成估计状态控制后轨迹一开始出现了轻微抖动。这让我意识到“控制精度上限是由状态估计精度决定的”而不是单纯调控制增益能补救的。随后我再调低了传感器噪声协方差轨迹立刻改善。这个试错过程给我建立了“控制与估计必须一体设计”的思维模式。5. 学习与实操建议如何扎实拿捏这一周内容5.1 从第34周内容延伸出的真实工程问题很多学习者在刷完课程、做完几道题之后会有一个错觉自己已经掌握四旋翼建模和状态估计了。但一到真实环境甚至稍微复杂一些的仿真里问题就接踵而至。下面这几个真实工程问题我认为是第34周内容的自然延伸建议提前有意识地去思考第一个是模型参数误差。四旋翼的真实质量、转动惯量、气动阻尼系数并不一定和标称值完全一致。控制律在模型准确时表现良好一旦模型参数偏差超过一定范围闭环系统可能出现低频振荡甚至不稳定。第3周所学内容中稳定性分析部分的价值正在于此——你要通过鲁棒性分析判断什么样的参数偏差是允许的。第二个是执行器饱和。电机转速存在物理上限和下限。当控制器给出的转速指令超出上限时实际执行并不会完全跟随。饱和不仅是简单的限幅问题它还会造成控制器积分饱和、姿态误差变大、甚至系统失稳。我在一个高机动飞行的项目中就遇到过横滚角误差持续增大导致的饱和累积现象后来不得不加入抗饱和策略才解决问题。第三个是传感器时间戳同步。GPS和IMU的数据频率不同而且传感器数据在传输过程中有延迟。如果不对齐时间戳就直接融合状态估计结果会带有隐含时间误差。专业飞控和导航系统中都有专门的时间同步模块这其实是对第4周课程的延伸和实战考验。5.2 环境搭建与代码调试工具推荐如果你第34周想动手实操我建议从以下几类工具入手仿真环境方面首推在Python里用数值积分实现简化四旋翼动力学。先不要一上来就用大型仿真平台因为大型平台封装得太好很多内部细节你看不透。手写动力学模型能帮助你理解每一步推导。调试工具方面可以准备一套数据可视化工具把四旋翼的姿态、角速度、电机指令、估计误差全部实时显示出来。我在调试EKF时如果只靠打印数据很难发现十几秒内的缓慢漂移画成曲线后信号的漂移趋势一目了然。任何能让你“肉眼看到状态随时间变化”的工具都是调试状态估计算法的利器。代码版本方面建议把每个实验步骤都写成可复现的脚本。特别是在调卡尔曼滤波时修改一个噪声协方差值就可能导致完全不同的输出只有文档齐全的脚本才能帮你定位到底哪一步改动导致行为变化。5.3 用“最小复现单元”验证每个知识点这是我学习这门课最受益的习惯每一个知识点都尽量用“最小复现单元”去验证。举例来说学旋转矩阵微分时就写一个几行代码的脚本随机生成角速度让旋转矩阵按公式积分再检查得到的矩阵是否始终满足单位正交性。学卡尔曼滤波时先设计一个一维问题设定一个简单的恒值状态加入高斯噪声让滤波收敛后查看估计值和真值的误差。这种验证方式的优势在于它逼着你手推公式也让代码运行结果成为“公式是否理解正确”的反馈信号。如果公式理解有偏差仿真输出很快会异常如果在理解完全正确的前提下你还能观察数值误差与积分步长之间的关系这会让你对数值方法本身的边界也有更深的认知。我在第3周做旋转矩阵积分验证时曾经发现一个有趣现象在离散积分中如果不做每步的正交化修正积分几十步后旋转矩阵就会偏离单位正交条件导致姿态矩阵“变形”甚至最终奇异。这个现象在理论推导中不明显但工程仿真中非常致命。正是通过最小复现单元我才真正理解了为什么很多姿态解算代码每步都要做重正交化处理。5.4 考试/作业刷题节奏安排考虑到第34周内容较多我通常按以下节奏来安排第1-3天通读课件和讲义只看概念和公式推导不看习题答案尝试理解每个公式的物理意义与推导来源。第4-6天动手推导每个核心公式把“旋转矩阵导数”“动力学方程”“控制分配矩阵”“卡尔曼滤波五个公式”等核心内容在纸上完整推导一遍务必独立完成。第7-10天做课后习题。如果遇到不会的题先卡在思路层面不回看答案尽量回忆课程中对应的概念和推导路径与技巧。第10-14天编写与控制、滤波相关的仿真代码与最小复现单元把习题答案在仿真中验证一遍。最后再挑一个控制估计的综合题做完整闭环仿真。这套节奏不一定适合所有人但核心思想是“先理解再推导后做题再代码验证”四个步骤相互支撑能避免“看着答案会了合上书又忘光”的假性学习。6. 避坑经验总结第34周的学习体验说白了就是一场“数学细节战”。我把自己在这两周里踩过的主要坑整理成一个速查表希望对你有实际帮助。易错环节典型表现正确做法旋转矩阵求导误以为 $\dot{R} \dot{\theta} R$必须使用 $\dot{R} R[\omega]_\times$重力项符号竖直方向力平衡少加重力补偿期望加速度必须包含 $g z_W$力矩计算分配矩阵中漏掉机臂长度 $l$每个力矩计算都要乘长度电机响应把电机当作理想无延迟执行器加入一阶惯性模型姿态误差计算直接用欧拉角差取代旋转矩阵误差使用 $e_R 0.5 (R_{des}^T R_{cur} - R_{cur}^T R_{des})^\vee$卡尔曼协方差$P_{k|k-1}$ 与 $P_{k|k}$ 混用区分预测与更新协方差过程噪声$Q$ 设置为零根据模型置信度设置合理 $Q$高频/低频融合陀螺与加速度计权重失衡用互补滤波/卡尔曼滤波合理加权传感器时序忽略GPS/IMU时间戳对齐做传感器时间同步后再融合野值干扰突变的测量直接进入滤波器加入残差判断与野值剔除我一直认为学习空中机器人最重要的不是背公式而是建立从“物理直觉”到“数学模型”再到“代码实现”的三层映射。第3周动力学部分是这层映射的起点第4周状态估计部分则是补全闭环的关键拼图。如果你能跟着这两周内容走并在仿真里亲手验证一遍你对四旋翼的理解会从“它会飞”进化到“我理解它怎么飞、为什么飞得稳”。后续再学路径规划、视觉导航、多机协同这些高阶方向时地基就会非常扎实。另外一个很实际的心得遇到数值仿真发散问题时别急着改控制参数先检查模型坐标定义和符号方向。很多发散问题的根源是坐标轴搞反或者旋转方向写反白白浪费了一晚上调试时间。我在那两次刷课经历中不少夜晚都在干这种“以为自己调参实际调的是符号”的傻事——把坐标系标注清楚、把符号约定写在代码注释最前面是降低这类无意义损耗的好习惯。希望这份学习记录能帮到正在啃第34周内容的你。如果你在推导或仿真里遇到具体问题也欢迎带着你的代码和公式来交流——一起把问题定位到“是物理直觉没建立还是数学推导出了岔子还是代码实现有疏漏”比单纯对答案有意义得多。 
