四旋翼欠驱动控制与轨迹规划难点解析：建模、级联控制到状态估计

空中机器人这门课我是在学完基础控制和刚体运动学之后才碰的前后拖了三周才把那两周的作业完整啃完。第34周表面上看是一个“建模—控制—规划”的推进过程真正难的地方却在于所有公式背后都默认你已经理解了“四旋翼是一个欠驱动系统”这件事。这篇文章围绕第34周课程内容把知识主线、推导思路、习题解答以及我踩过的坑一起整理出来适合正在刷课、或者准备做四旋翼DIY项目的同学参考。下面直接进入正题。1. 这两周的知识骨架飞行器控制为什么不能靠试错1.1 先承认一个事实无人机不是在地面上调出来的大多数初学者第一次拿到开源飞控最喜欢干的事情就是改PID参数。改完发现自己机子抖、飘、掉高然后继续改。我刚开始搞四旋翼项目的时候也是这么干的结果在一个室内小桨机上烧坏过两个电调。直到后来回到基础动力学推导才意识到很多“飞得不好”的问题根本不是PID参数的问题而是你对飞行器的输入方式理解错了。第34周的课程恰恰就是从这一点切入的。它把四旋翼看成一个严格意义上的动力学系统先把电机转速对应到推力和力矩再把推力和力矩对应到线性加速度和角加速度最后才谈控制。也就是说这两周不是教你“怎么调”而是教你“为什么这样设计、这样调才有意义”。1.2 第三周和第四周的逻辑主线从我自己的学习笔记看第三周的核心是动力学建模与轨迹生成的基础第四周的核心是级联控制结构与最小加加速度轨迹的工程实现。两个部分连起来是一条完整的链路电机转速 → 推力、力矩推力、力矩 → 机体的线加速度、角加速度期望位置 → 期望加速度 → 期望姿态角 → 期望力矩 → 电机转速约束条件时间最优、平滑性、可行性这条链路在后来的作业中反复出现。凡是感觉自己跟不上课程节奏的同学我建议先把这条链从头到尾默写一遍再往下走会顺畅得多。1.3 需要的数学基础很多人被这一周的数学符号吓退其实核心只有三块旋转矩阵、刚体牛顿-欧拉方程、状态空间线性化。我个人的建议是不用先把《矩阵分析》读完再来看课但至少要会做旋转矩阵乘法、会求向量的叉乘、能理解惯性系与机体系之间的坐标变换。我在看第三周第一节的时候就被那个坐标变换搞晕过。当时我拿一个纸盒子当机体自己比划着转来转去总算是把“机体坐标系下的z轴经过旋转矩阵R变到世界系”这个关系理顺了。这个理顺之后后面所有公式都顺了。2. “四个输入打六个输出”四旋翼动力学的欠驱动本质2.1 为什么说四旋翼是欠驱动系统先看一个最基础的事实四旋翼有4个电机也就是4个独立的输入但它的位形是三维位置加三维姿态一共6个自由度。输入比自由度少所以叫欠驱动系统。这个问题初看只是“自由度”和“输入”的数量差异但它的后果是深刻的你没办法同时独立控制位置和姿态。这一点至关重要。很多初学者上来就想着“我要让飞机保持某姿态同时平移到某个位置”这在四旋翼上是做不到的。因为平移需要水平方向的力而四旋翼的推力方向始终沿机体z轴你只有先把机体倾斜一点才能产生水平分量。也就是说位置的变化是靠姿态诱导出来的姿态不是独立于位置的。2.2 从牛顿-欧拉方程看“推力方向”这个小细节四旋翼常用的简化模型可以写成下面两组方程线运动方程世界坐标系下m * r_ddot -m * g * e3 f * R * e3其中 m 是质量g 是重力加速度e3 是竖直向上的单位方向f 是总推力大小R 是从机体系到世界系的旋转矩阵。注意这里的推力向量就是f * R * e3它的方向永远取机体坐标系的z轴而不是你自己想要的推力方向。角运动方程机体坐标系下I * omega_dot -omega × (I * omega) M这里的 I 是转动惯量矩阵omega 是机体角速度M 是作用在机体上的总力矩。从四旋翼构型来看M 主要由四个旋翼的差动推力产生用于控制滚转、俯仰、偏航三个通道。2.3 电机转速到推力和力矩的映射关系第3周课程里反复出现的一组表达式是f k * (w1^2 w2^2 w3^2 w4^2) tau_phi k * L * (w4^2 - w2^2) tau_theta k * L * (w3^2 - w1^2) tau_psi d * (w1^2 - w2^2 w3^2 - w4^2)这里的 k 是升力系数L 是机臂长度d 是反扭矩系数w 是电机转速。滚转力矩由左右两个电机的差动推力产生俯仰力矩由前后两个电机的差动推力产生偏航力矩则由对角电机与另一对对角电机的反扭矩差异产生。想明白这件事之后再去看级联控制器里每个输出通道对应的电机组合就不会觉得那一堆“Mixer表”是玄学了。2.4 我在这里踩过的符号坑一个特别容易出错的地方是方向约定。例如某门课的作业里可能会出现“机体系z轴向上”还是“z轴向下”的差异如果推导时没注意你的推力方向搞反控制效果会直接变成“越推越往下掉”。我后来习惯的做法是在每道推导题、每个仿真脚本里都先用一个最小模型验证“重力方向与推力方向的关系”再继续往下。这一招极大的减少了我后续返工的时间。3. 级联控制外环算角度内环算力矩3.1 为什么不能直接对位置来一个“粗暴的PID”接触过地面机器人控制的同学可能下意识想既然位置能反馈能不能直接用三个PID通道给出水平方向的分力再加上一个垂直通道补偿重力这条思路听起来很顺但实际上忽略了“推力方向有限制”这个物理约束。你想让机子在 x 方向加速就必须让推力往 x 方向偏一点而推力的方向又是由姿态决定的姿态本身也需要控制。所以正确的做法是把控制系统拆成内外两个环外环位置环根据当前位置与期望位置的偏差计算出期望加速度内环姿态环根据期望姿态角与实际姿态角的偏差计算出期望力矩最后再通过力矩和总推力反解出四个电机的转速。3.2 外环一从期望加速度反推期望姿态课程中给了一个很漂亮的关系。设期望加速度为 a_d重力方向为竖直向下那么你仅靠机体z轴方向的推力要想得到水平方向加速度唯一办法是让机体z轴指向期望加速度方向反重力补偿后的方向。具体做法是z_b_des a_d / ||a_d||这里要求 a_d 不能为0所以至少要保留一个底限推力作为重力补偿。再给定期望的偏航角 psi就可以唯一确定期望的旋转矩阵 R_des进而算出期望的俯仰角与滚转角。这个推导过程也是后面习题里的常客建议亲手推一遍不要只背公式。3.3 外环二位置误差到期望加速度位置环通常采用比例微分控制。比如a_d -Kp_pos * (pos - pos_des) - Kd_pos * (vel - vel_des) g * e3注意这里的符号误差项和重力补偿项分开写。如果你在仿真里把 g 的方向弄反悬停以下的结果就是机子以1g加速度冲向地面。我在调一个模拟器时犯过这个错当时日志里速度曲线一路向下排查了很久才发现是重力补偿符号问题。3.4 内环姿态角误差到力矩姿态环的处理方式在第四周有一个很重要的简化把期望推力方向对应的期望四元数、期望角速度算出来再与当前四元数、当前角速度求差通过PD控制得到力矩期望值。公式的常见形式M_des I * (Kp_att * e_rot Kd_att * e_omega)其中 e_rot 是姿态误差的向量形式e_omega 是角速度误差。这里我建议用四元数误差而不是欧拉角误差因为欧拉角会在接近 ±90° 时出现奇异。3.5 关于内环和外环带宽的工程直觉这门课没有直接讲“带宽分离”但后面做仿真实验时你会发现一个非常现实的问题如果位置环跑得比姿态环还快系统很容易震荡。我自己的经验是姿态环的响应速度要保持内环很快外环才可能有稳定的性能。换言之位置环的P增益、D增益不要在内环还不稳的时候去调。先调内环把阶跃响应调到不振荡再调外环。这一点比任何教科书上的稳定性证明都更贴近真机调试。3.6 关于调参我用仿真验证的一件事我起初对“内环快、外环慢”这句话无感直到我自己在仿真里把外环的比例增益拉到很大的值。结果位置没有变稳反而姿态角出现持续振荡整个飞行轨迹变成了耗散的正弦。后来我把外环增益调低、内环带宽提高位置曲线立即变得平滑。这个现象让我对级联控制有了感性认识每个环都有自己的响应速度不能越级干预。4. 轨迹规划为什么使用“微分平坦”而不是直接规划角度4.1 微分平坦到底是什么意思用尽可能直白的话说微分平坦是指系统的所有状态量、输入量都可以表达为某几个“平坦输出”及其有限阶导数的函数。在四旋翼问题里平坦输出通常选为位置 (x, y, z) 和偏航角 psi一共4个量。只要给出这条4维曲线的足够高阶导数任何人都能唯一反推推力大小、姿态角、角速度等全部状态。这意味着路径规划可以完全在位置和偏航角空间里进行而不必先去规划一串姿态角。这对轨迹规划来说是巨大的简化你不需要考虑“这个姿态角是否能由某个合法电机转速产生”因为平坦性保证了一定能产生。我一开始很难理解这个性质为什么值得单独提出来。后来自己写了一个简单的二维轨迹生成器如果直接按姿态角规划我得额外做一步“反向校验”防着姿态角速度超限如果按平坦输出规划只要检查生成的加速度、加加速度是否在约束范围内就行流程简单得多。4.2 最小加加速度minimum jerk的分段多项式解法课程中反复提到用多项式来生成平滑轨迹。假设我们只考虑一维坐标 x(t)希望它经过若干路径点。这可以建模成一个带边界条件的最优化问题minimize integral ( dddx )^2 dt subject to x(t0) x0, xdot(t0) v0, xdd(t0) a0 x(t1) x1, xdot(t1) v1, xdd(t1) a1对一段轨迹来说这里有6个边界条件正好可以用一个5次多项式描述6个系数。为什么是5次因为一个n次多项式有n1个系数5次多项式对应6个系数刚好匹配“位置、速度、加速度”这6个参数。更高阶的项不是必需的。把系数列出来解一个6×6线性方程组就能得到满足平滑性约束的唯一轨迹。如果中间有多个路径点则需要分段多项式并保证段与段之间位置、速度、加速度连续。课堂上给出的思路是把这些约束写成等式然后用优化库一次求解所有系数。我自己复现的时候发现如果每段时间长度固定问题退化为线性最小二乘如果允许时间可变则变成非线性优化计算代价会大很多。因此很多工程实现中先固定各段时间再做优化速度非常快。4.3 一个直观对比jerk约束带来的飞行品质差异为了理解“为什么jerk小就代表飞行平滑”我做过一次实验用梯形速度曲线生成一条往返轨迹再用五阶多项式生成一条同端点约束的轨迹把两类命令分别送进同一个四旋翼仿真模型。梯形速度曲线的问题在于速度拐点处加速度突变四旋翼需要瞬间改变姿态角度仿真结果显示速率曲线勉强能跟上但电机转速出现明显的高频振荡。而五阶多项式因为加加速度连续姿态角和电机转速都平滑得多。这个实验让我彻底理解了课程反复强调的“连续”为什么重要飞行器控制追求的是“指令的导数连续”不是“曲线好看”。电机转速变化率受到电机机电时间常数的限制物理上不可能瞬间改变巨大的转速差。所以轨迹规划考虑平滑性本质上是在匹配电机物理极限。4.4 还要注意的一点加加速度与用户期望的矛盾当然过于平滑的轨迹往往不是时间最优的也就是“飞得没那么快”。这引出另一个问题如果某个任务需要尽可能快的从A到B该怎么办实际的工程方案是用“时间最优平滑约束”的多目标优化或者先求粗的快速路径再加平滑修正。第34周课程只覆盖了基本的生成逻辑但它给后续章节留下了很大一个“待办事项”。5. 状态估计没有GPS的房间里飞行器怎么定位5.1 为什么GPS在室内失效、IMU又为什么会漂可能有人觉得既然有IMU惯性测量单元测加速度并积分两次就能得到位置。课程里专门指出这种做法的致命问题IMU的加速度计和陀螺仪都存在缓慢漂移而位置是速度的积分速度是加速度的积分误差以二次方速度累积。换句话说放任IMU自己算位置3秒钟之后就已经漂到不可信了。这也是为什么室内机器人都要额外依赖视觉等手段。GPS在室外能用但在室内基本被墙体挡掉信号精度降到无法接收。这门课程在此处转而讨论了一种更现实的定位方式使用视觉传感器如向下摄像头跟踪地面特征同时用IMU提供高频的姿态和加速度信息两者融合得到较为可靠的位置估计。融合的核心思想是“用不同传感器互补的误差特性”视觉缓慢但绝对信息可靠IMU短期精确但长期漂移不可忽视。5.2 视觉特征与IMU融合的最小思路课程在习题里强调过这类系统的一般做法是设计一个扩展卡尔曼滤波器EKF状态里包含位置、速度、姿态、传感器偏置。预测阶段由IMU的角速度和加速度驱动修正阶段由视觉的观测来校正。视觉观测通常不能直接给出GPS式的位置而是给出“两帧图像之间特征点的移动”再把这种移动转换为机体位移的约束。我在这里的学习体会是不要一上来就去抠EKF里每个矩阵怎么更新先把“什么样的传感器信息可以约束系统的哪个状态”这件事想清楚。比如向下摄像头能够约束水平位置但对偏航角无能为力磁力计能够约束偏航角但容易受金属干扰。先清楚每个传感器能修正什么再去看滤波方程里的观测矩阵你会觉得很自然。5.3 估计器退化时我观察到的现象如果视觉特征丢失估计器会很快回到纯积分模式位置反馈会以肉眼可见的速度漂走。我自己在模拟环境中故意关闭视觉测量只留IMU结果是飞机“以为”自己在一个方向不停加速实际位置并没有明显偏差但控制环基于错误位置持续给出指令最终导致机体晃动失控。这个现象让我彻底记住了“估计器质量决定了控制器下限”这句话。如果你的项目里飞行器一到某个角度就偏离航线不要先怀疑PID先看特征跟踪是否稳定。6. 课后习题解答四道题的完整推演与易错点6.1 第一题判断欠驱动特性与角加速度的关系这道题常以“四旋翼有几个输入、几个输出角加速度方程为什么这样写”的形式出现。解答思路是先写出线运动和角运动方程参考前文第2章再指出总推力 f 只影响线运动方程中的推力大小力矩 M 只影响角运动方程两者加在一起正好4个输入对应4个电机无法独立控制6个输出。我见过很多人把这道题答成“因为电机共4个”但这只是表面原因。真正的本质是方程右侧的自由度数量线运动方程里只有1个推力大小和3个姿态角自由度角运动方程里只有3个力矩量4个量刚好等于输入数量。答题时最好把自由度拆开写比单说“电机数量”更有解释力。6.2 第二题悬停点的线性化与自然频率悬停点的推导属于高频考题。基本思路是把机体的滚转角、俯仰角视为小量在悬停状态下令推力约等于重力。水平方向的小扰动方程可以写成x_ddot g * theta y_ddot -g * phi这组式子看起来很简单但有两个关键概念很容易搞混。第一x方向的加速度是由俯仰角决定的不是由滚转角决定的第二符号方向由坐标系的选取决定不同教材可能差一个负号。如果算出来的期望角度符号和实际飞行方向相反飞行器会做出“往反方向打球”的奇怪控制效果。进阶一点的版本会要求在悬停点做线性化并计算闭环系统的自然频率和阻尼比。做法是先把位置环、姿态环的PD方程代入小扰动方程得到形如x_ddot 2*zeta*omega_n*x_dot omega_n^2*x 0的二阶系统再反解omega_n和zeta。每次作业里出现这道题都建议完整写出这一步因为它是后续理解“频域整定”的重要铺垫。6.3 第三题最小jerk轨迹边界条件的推演这类题通常会给你初始时刻和终止时刻的位置、速度、加速度让你求五阶多项式系数。设轨迹为p(t) a0 a1*t a2*t^2 a3*t^3 a4*t^4 a5*t^5代入初始边界条件 p(0)p0、pdot(0)v0、pddot(0)a0可以直接确定前三项代入末端边界条件后得到一个关于 a3、a4、a5 的3×3线性方程组。真正的易错点在于很多人忘了加速度条件也是边界条件只给了位置和速度结果多项式的阶数判断错误。还有一种变体是要求jerk在边界处也为0这时候需要7次多项式。做题时建议先把“边界条件数量”和“多项式系数数量”对齐再开始代入。6.4 第四题给定期望加速度求期望姿态角这道题是级联控制里最经典的计算题。给定期望加速度向量 a_d 和期望偏航角 psi先计算z_b_des a_d / ||a_d||然后按航向角构造出 x_c、y_c 轴再计算机体坐标系的 x 轴x_b_des y_c × z_b_des 叉积后归一化 y_b_des z_b_des × x_b_des最后从这三个向量组装出期望旋转矩阵 R_des再从中提取期望的欧拉角或四元数。答错的常见原因是直接把 psi 默认为0或者忘记z_b_des要先归一化。另外注意在处理叉积时如果不是单位向量结果长度会影响后面的角度提取最好每一步都归一化。这个计算过程在真实飞控里的姿态期望生成模块也会出现理解它相当于听懂一半的姿态控制器代码。6.5 习题里两个容易错的概念题概念题常见的有两类。第一类问“为什么四旋翼在悬停时不能通过改变四个电机转速的绝对大小来改变水平位置”。答案是同时提高四个电机转速只会增加总推力让机子上升水平位置由姿态差动决定。很多人想当然认为“推力大就往前冲”忘了推力方向始终在机体z轴上。第二类问“为什么需要状态估计器直接用加速度计两次积分不行吗”。答案是加速度积分得到的位移会在几秒内发散必须用视觉或其它绝对传感器做周期性修正。答题时可以提一下加速度计的常值偏置引起的二次误差增长并说明EKF的作用是融合短期精确与长期无漂移的信息。7. 学习建议与踩坑清单写给准备动手做项目的人7.1 先仿真再真机但仿真也别追求“画质”我推荐把课程配套的仿真环境用起来但建议先做“最简单的悬停任务”而不是一上来就规划一个走廊穿越。我的仿真经验是先让机子稳定悬停1分钟不漂移再做定点往返最后才加轨迹生成。每步都记录位置误差、姿态角、电机转速三条曲线对照着看能很快定位问题。不要嫌这个过程无聊它能帮你建立“信号链”的感觉。7.2 根据自己的项目选择深入程度如果只是想快速飞一个穿越机可能不用掌握轨迹生成的所有细节但如果想做一个自主室内搜索或物流配送车机轨迹优化和状态估计这两个专题一定得啃透。我的建议是把第34周当作一个路口——想深耕控制的同学可以多花时间在姿态环调参和线性化上想走规划方向的则把重点放在分段多项式与数值优化上。7.3 整理一份自己的“符号约定表”这门课涉及世界系、机体系、旋转矩阵、角速度、力矩符号各种教材和代码库的约定并不一致。我吃过最大的亏就是把机体坐标系的x轴和世界坐标系的x轴直接画混了。建议大家维护一个“符号约定表”列清楚每个符号的意义、单位、从哪个坐标系到哪个坐标系包括重力是正方向还是反方向。这个表的价值在你真正调试飞控时会无限放大。7.4 关于作业的一些实在话很多作业的难点不是公式本身而是“把公式转成方程求解”。比如五阶多项式求系数手推很痛苦建议直接写一小段求解代码把矩阵形式写对之后一切交给工具。还有作业里经常出现的“符号判断”题本质上都在测试你是否理清了机体坐标系的朝向不要跳着读题。第34周内容在整门课程里属于“地基中的地基”。如果只是刷完视频、抄完公式后面学起来会很吃力。我自己的经历是第一遍做作业时全凭记忆错误率很高第二遍静下心来重新推草稿并把所有物理量标注清楚后之前不太理解的状态估计章节也突然顺了。希望你也能用类似的方法走一遍收获应该比单纯看答案要大得多。