小型固定翼飞控：六自由度建模、状态估计与制导链路

简介《Small Unmanned Aircraft: Theory and Practice》英文原版PDF面向无人机飞控、导航制导与控制方向的高年级本科生、研究生及工程技术人员系统讲解小型无人机从建模到自主飞行的核心理论。全书围绕系统架构、坐标系与旋转矩阵、运动学与刚体动力学、气动力与力矩、线性设计模型展开并进一步讨论基于串级回路的自驾仪设计、MEMS传感器、状态估计与卡尔曼滤波、制导设计模型等主题各章配有设计项目便于把公式推导落到工程实现。压缩包内为1个PDF文件大小约4.89MB结构完整、目录清晰适合按章节检索与课程对照学习。已有309人学习可作为无人机控制入门与进阶的案头参考也适合需要补齐动力学建模、控制器整定与传感器融合知识的读者反复查阅。1. 从一架 5 英尺翼展的固定翼说起这本 PDF 解决的到底是什么问题很多人第一次碰固定翼飞控手里拿到的不是教材而是几份调参截图和一个开源固件仓库。于是就会出现一个很典型的翻车现场飞机能起飞但航线一进弯道就切不进去高度在 20 米上下反复抖X_Track 误差一直压不下去。这台机子的气动参数并没有错得离谱真正的问题出在脑子里缺一条从坐标系到状态估计、再到制导律的完整链条所以调参只能靠玄学试。这份《Small Unmanned Aircraft: Theory and Practice》PDF 属于那种不给你现成参数、但能把链条讲通的书。它面向的是翼展 510 英尺的小型固定翼和翼展 5 英尺以下的微小型飞行器围绕六自由度模型、连续环闭合自整定、传感器建模、卡尔曼滤波状态估计、直线与盘旋航迹跟随、Dubins 路径管理器、覆盖式路径规划一直讲到视觉引导与撞地时间估计。整本书按 13 章加 8 个附录组织每章配一个 Design Project把飞行仿真器的每一块拆开让你自己搭起来。适合谁读手上已经有飞控板、想搞明白内环和外环为什么这么分层的人做毕设或课题需要一份能跑通的固定翼仿真骨架的人从电控、计算机背景转进飞行控制、气动基础薄弱但信号与系统、反馈控制、机器人学学过的人。不适合谁想直接抄一组 PID 参数就上天的或者只做多旋翼悬停的。这本书的假设几乎全部建立在固定翼巡航飞行上直升机和多旋翼的悬停控制不在范围内。2. 坐标系与六自由度建模把飞行器的状态变量理清楚固定翼仿真的第一个坎不是控制是「你到底在哪个系里描述这架飞机」。书里第 2、3、4 章基本都在解决这件事惯性系给任务指令机体系给传感器和执行器气流系给气动力。三者混着用后面所有误差计算都是错的。第 5 章再把它收拢成一个 12 状态的非线性模型然后线性化成传递函数和状态空间模型供内环设计使用。2.1 三个坐标系与风三角的换算关系常见做法是把惯性系取为 NED北-东-地机体系取前-右-下气流系用空速方向定义。三者之间靠旋转矩阵串联。绕 Z、Y、X 依次转 ψ、θ、φ得到机体系到惯性系的旋转矩阵import numpy as np def rot_body_to_inertial(phi, theta, psi): 机体系(NED对齐的机头-右-下) - 惯性系(NED) cphi, sphi np.cos(phi), np.sin(phi) cth, sth np.cos(theta), np.sin(theta) cpsi, spsi np.cos(psi), np.sin(psi) R_x np.array([[1, 0, 0], [0, cphi, -sphi], [0, sphi, cphi]]) R_y np.array([[ cth, 0, sth], [0, 1, 0], [-sth, 0, cth]]) R_z np.array([[cpsi, -spsi, 0], [spsi, cpsi, 0], [0, 0, 1]]) # 注意顺序先滚转、再俯仰、最后偏航 return R_z R_y R_x # 验证纯偏航 90 度机头方向应转到东 R rot_body_to_inertial(0.0, 0.0, np.pi/2) print(R np.array([1, 0, 0])) # 期望 [0, 1, 0]逻辑说明旋转矩阵的乘法顺序决定结果这里的顺序是Rz Ry Rx对应 Z-Y-X 欧拉角约定和书里第 2.1 节的推导一致。如果写成Rx Ry Rz在小角度下差别不大姿态一大就会全乱。参数说明φ、θ、ψ 分别是滚转、俯仰、偏航单位弧度输入向量是机体系下的分量输出是惯性系下的分量。这段代码最关键的一行是乘法顺序不是矩阵本身。风三角是第二个容易错的地方。地速等于空速加风速三者构成矢量三角形。书里第 2.4 节专门推导了它因为后面做航迹跟随用的必须是地速方向航向角 χ而空速传感器测的是空速大小和气流角。巡航段如果风速估计偏了 2 m/s盘旋跟随的半径误差就会被放大到肉眼可见。常见做法是先用 GPS 地速减去空速矢量反解出风估计再做低通平滑不要每帧都更新。2.2 12 状态非线性模型与配平点六自由度模型的状态量一般这么取位置 p_n、p_e、p_d速度 u、v、w姿态 φ、θ、ψ角速度 p、q、r。共 12 个。动力学方程分平动和转动两组平动由气动力、推力、重力合成转动由气动力矩和舵面力矩合成。书里第 4 章分三块讲力和力矩重力、气动力与力矩、推进力与力矩再加上大气扰动。气动部分用的是无量纲系数形式升力系数、阻力系数、侧力系数随迎角和侧滑角变化力矩系数还要加上舵面偏转的线性项。附录 E 直接给了两套机体参数Zagi 飞翼和 Aerosonde 无人机前者是小型飞翼后者属于更大一级的机型。这是全书最有价值的附录之一因为它让你不必自己风洞也能跑起来。建模时真正要动脑的是配平。书里第 5.3 节讲 trim 条件简单说就是找一组状态量和舵面输入让加速度项全为零。固定翼的配平不是解一个方程而是解一个欠定的非线性方程组给定空速和高度滚转角定为零剩下的俯仰角、迎角、升降舵、油门要通过迭代求出来。from scipy.optimize import fsolve def trim_residuals(x, Va_target, alt_target, params): x [alpha, theta, delta_e, delta_t] alpha, theta, de, dt x # 升力、阻力、推力、俯仰力矩的残差全部除以质量或转动惯量后置零 f_lift lift_force(alpha, Va_target, params) - weight_component(theta, params) f_drag drag_force(alpha, Va_target, params) - thrust_force(dt, Va_target, params) f_pitch pitch_moment(alpha, de, Va_target, params) f_alt theta - alpha # 航迹角为零时的几何约束 return [f_lift, f_drag, f_pitch, f_alt] sol fsolve(trim_residuals, x0[0.05, 0.05, 0.0, 0.5], args(Va_target, alt_target, params))逻辑说明四个方程四个未知量lift_force等函数需要你自己按书里第 4 章的系数公式写出来。f_alt那一行是几何约束航迹角为零时俯仰角等于迎角。少了这个约束方程组就不闭合fsolve 要么发散要么给你一组物理上说不通的值。参数说明Va_target是目标空速单位 m/salt_target只在需要密度修正时用params是气动系数、质量、转动惯量、翼面积等。初值给得好不好直接决定收敛性经验做法是先按巡航迎角 35 度、油门 0.40.6 附近起步。提示配平结果必须回代进非线性模型验证一次看位置导数和速度导数是不是都在 1e-6 量级。很多人的飞控一上天就缓慢掉高度原因就是配平点算出来的俯仰角带了 0.5 度的系统偏差而这个偏差在方程残差里看不出来。2.3 线性化与降阶模态第 5 章的后半部分是全书的技术枢纽。把 12 状态模型在配平点附近做雅可比线性化得到 A、B 矩阵再拆成横航向和纵向两组。横航向包含侧滑、滚转、偏航三自由度纵向包含空速、迎角、俯仰角、俯仰角速度。然后算特征值你会发现几个典型的模态短周期、长周期浮沉、滚转收敛、螺旋、荷兰滚。这些模态的名字网上到处都是但书里给了它们的物理来源和参数敏感性。短周期模态的阻尼如果太小俯仰指令就会振荡螺旋模态如果发散太慢飞机会在无控状态下慢慢盘成螺旋。调参时看特征值比看阶跃响应更有效阻尼比目标一般取 0.60.8自然频率由你允许的响应带宽决定。降阶模态那一节5.6说明了什么条件下可以把 12 状态砍到 4 状态做初步设计这也是实际工程里最省时间的一条路。3. 连续环闭合自整定内环、外环到底怎么分层书里的自动驾驶仪设计用的是 successive loop closure直译是连续环闭合。它的思路并不复杂把多变量问题拆成一串单回路由内向外逐个闭合每一个外环把已经闭合的内环当成一个近似的一阶或二阶环节来处理。难点在于两个环的带宽到底差多少才合适。3.1 带宽分离的控制逻辑与参数换算典型的分层是这样的最内层是滚转角速率或俯仰角速率往上一层是滚转角与俯仰角再往外是航向角和高度最外是航迹跟随。内层带宽最高外层带宽最低一般要求相邻两层之间至少差 510 倍否则内环的动态会串到外环里表现为外环怎么调都不稳。书里第 6.2 节专门讲了饱和约束与性能之间的关系。这一点非常重要因为固定翼的舵面是有限幅的风扰下如果指令给得太大舵面饱和积分项持续累积就会出现典型的积分饱和现象飞机回到配平后又反向过冲一大截。处理办法是积分限幅加上反算补偿书里给的是反算形式。class PID: def __init__(self, kp, ki, kd, u_min, u_max, kt1.0): self.kp, self.ki, self.kd kp, ki, kd self.u_min, self.u_max u_min, u_max self.kt kt # 反算增益 self.integ 0.0 self.prev_error 0.0 def step(self, cmd, meas, dt): error cmd - meas p_term self.kp * error d_term self.kd * (error - self.prev_error) / dt u_unsat p_term self.integ d_term u_sat max(self.u_min, min(self.u_max, u_unsat)) # 反算用饱和差修正积分防止积分饱和 self.integ (self.ki * error self.kt * (u_sat - u_unsat)) * dt self.prev_error error return u_sat逻辑说明关键在那行self.kt * (u_sat - u_unsat)。当输出未饱和时这一项为零退化成普通 PID一旦饱和差值非零把积分往回拉。kt的取值一般取1/TtTt 常用积分时间或略大一点。参数说明u_min、u_max是舵面或油门限幅单位按你的执行机构定dt是控制周期固定翼内环一般 50100 Hz外环 1020 Hz。数字实现那一节6.5还提醒了微分项要加低通滤波否则陀螺噪声会被放大成舵面抖动。3.2 横航向与纵向回路的实现顺序实际的调试顺序建议是先纵向后横航向先角速率后角度。纵向回路的目标是保持空速和高度。俯仰角指令由高度误差通过 PI 产生油门指令由空速误差产生或者反过来——这取决于机型的推力配置。书里给的方案是油门控空速、俯仰控高度这对大多数小型固定翼都适用。如果你的机型推重比很低油门控空速会明显滞后那就得改成俯仰控空速、油门控高度。横航向回路的目标是保持航向和滚转。滚转角指令由航向误差产生副翼控滚转方向舵控侧滑或者协调转弯。协调转弯那一条约束第 5.2 节经常被忽略转弯时必须满足升力的垂直分量等于重力否则飞机会掉高度。换算下来就是滚转角与转弯半径、空速的关系参数符号典型取值范围影响巡航空速Va1225 m/s决定转弯半径下限最大滚转角φ_max3045 度大滚转提高机动性、损失升力转弯半径R30150 m与 Va²/tan(φ) 成正比航向环带宽ω_χ0.52 rad/s过高会激发荷兰滚调试时先用阶跃指令看滚转角的响应超调控制在 10% 以内再把航向环闭合。如果航向环一闭合就出现低频摆动八成是航向带宽给得太高把荷兰滚模态激出来了这时候要降带宽而不是加微分。3.3 从 Simulink 骨架到可运行仿真书配套的骨架文件是基于 Matlab/Simulink 的附录 C 讲了怎么用 handle graphics 做动画附录 D 讲了用 S-function 建模二阶微分方程附录 F 讲了 trim 和 linmod 两个命令的用法。我一般会先跑通一个最小闭环只留纵向通道注入一个高度阶跃确认俯仰角、迎角、空速的响应符合线性模型的预测再加横航向。一个容易忽略的点是仿真步长。S-function 里的连续状态如果用固定步长求解器步长超过 0.02 秒短周期模态就会被数值阻尼吃掉看起来「响应很好」实际是假的。建议定步长 0.01 秒起步或者干脆用变步长求解器跑对照。4. 状态估计为什么传感器不能直接信第 7、8 章是我认为全书最值得反复读的部分。原因很实际小型固定翼的载荷只有几十克到几公斤传感器都是低成本的 MEMS 器件加速度计有振动噪声陀螺有零偏漂移气压计受风和温度影响磁力计受电机电流干扰GPS 更新率只有 510 Hz 而且有跳点。你不能直接把传感器读数当状态用必须做估计。4.1 传感器误差模型与低通滤波的边界书里第 7 章逐个讲加速度计、速率陀螺、压力传感器、数字罗盘和 GPS 的工作原理第 8 章开头用一个 benchmark maneuver 来标定传感器。这个思路很好不是抽象地谈噪声而是让飞机做一组已知的机动用输出反推零偏和尺度因子。低通滤波是第一步。陀螺信号在 2050 Hz 有结构振动耦合直接积分会漂加速度计在机动时测的不是重力还叠加了线加速度。所以常见做法是陀螺用截止频率 3050 Hz 的低通加速度计在解算姿态前用截止频率 510 Hz 的低通。但低通会引入相位滞后截止频率给得太低姿态估计就跟不上机动。def lowpass_alpha(fc, fs): 一阶低通返回离散化系数 alpha dt 1.0 / fs rc 1.0 / (2 * np.pi * fc) return dt / (rc dt) alpha_gyro lowpass_alpha(fc40.0, fs200.0) # 陀螺通道 alpha_acc lowpass_alpha(fc8.0, fs200.0) # 加速度计通道 def lpf(x_prev, x_new, alpha): return x_prev alpha * (x_new - x_prev)逻辑说明这里用的是一阶 RC 离散化alpha越接近 1跟随越快、抑制噪声越弱。两个通道给不同的截止频率是因为它们的噪声特性和信号带宽完全不同用同一个系数是常见的偷懒做法。参数说明fc是截止频率Hzfs是采样率Hz。注意fs必须和你的实际控制周期一致如果传感器采样 200 Hz 而控制周期是 50 Hz要先降采样再滤波否则系数算出来是错的。4.2 卡尔曼滤波在姿态估计上的落地形态第 8.5 节推导了连续-离散卡尔曼滤波第 8.6 节专门讲姿态估计。实际工程里最常用的不是完整推导而是它的简化形态用陀螺做预测用加速度计和磁力计做观测修正。这就是互补滤波的由来也是很多开源飞控里姿态解算的骨架。书里的推导价值在于它告诉你协方差矩阵 Q 和 R 该怎么取。Q 反映过程噪声主要来自陀螺零偏不稳定性R 反映观测噪声来自加速度计在振动下的等效噪声。Q 取太大滤波器过度相信观测姿态会跟着振动抖R 取太大滤波器过度相信陀螺姿态会慢慢漂。经验做法是先离线跑一段静态数据用 Allan 方差估出陀螺的随机游走系数再反推 Q。GPS 平滑那一节8.7也值得看。GPS 的位置跳点在低速飞行时相对误差很大直接做差分算速度会得到一堆尖峰。书里的做法是把 GPS 位置和惯性导航的位置做融合而不是直接拿 GPS 差分当速度用。5. 制导与路径管理从航点飞到航迹跟随再到覆盖规划第 9 到 12 章是整本书最贴近实际作业的部分。很多人以为飞控的目标是「飞到某个航点」其实真正的目标是「沿着一条航迹飞并且横向误差收敛」。这两件事在控制上是不同的前者是点到点后者是跟踪一个几何对象。5.1 直线与盘旋跟随的横向误差收敛直线跟随的核心是构造一个横向误差然后让航向角指令与这个误差成比例。但直接比例控制有个问题误差大时航向指令会顶到限幅飞机绕着航迹来回摆。书里第 10.1 节给的形式带有前馈项和误差饱和处理本质上是让航向指令在误差大时趋近于一个固定角而不是无限增大。盘旋跟随同理只是误差定义换成了到圆心的距离减去半径。这里有个细节盘旋的方向顺时针或逆时针必须作为参数传进去否则误差符号会反飞机会往外飞。环节输入输出关键参数直线跟随位置、航迹起点终点航向角指令前馈角、误差增益盘旋跟随位置、圆心、半径、方向航向角指令半径、方向符号航向环航向角指令滚转角指令比例增益、限幅滚转环滚转角指令副翼指令比例、微分5.2 Dubins 路径与航点切换第 11 章的 path manager 解决的是航点之间的过渡。两个航点之间如果只连直线飞机在拐角处必然过冲。Dubins 路径用「圆弧-直线-圆弧」的组合在给定最小转弯半径的约束下生成一条可飞的平滑路径。六种基本组合LSL、LSR、RSL、RSR、RLR、LRL覆盖了所有情况。切换逻辑是另一个坑。书里第 11.1 节讲了几种切换条件到达半径、航迹夹角、以及基于时间的切换。只用到达半径做切换在侧风下会切早了或者切晚了。稳妥的做法是组合判断既要进入到达圆又要航向偏差小于阈值两个条件同时满足才切。覆盖率算法12.2是给区域扫描用的比如航拍测绘或者农业巡检。它的思路是把区域按扫描间距切成若干条平行线再用 Dubins 路径把它们串起来同时处理边界和转弯半径约束。扫描间距一般取相机幅宽的 70%80%留重叠是为了拼接。5.3 视觉引导与撞地时间估计第 13 章讲的是相机模型、万向节点指向、地理定位、目标在像平面上的运动估计、撞地时间和精确着陆。地理定位那部分的核心是把像素坐标反投影到地面平面前提是知道相机的姿态和地面高程。这里最容易出错的是相机内外参的标定和坐标系的约定图像坐标是左上角原点、y 轴向下而机体系是右下符号一错定位结果就会镜像。撞地时间估计用的是光流膨胀率公式本身不复杂但对图像噪声非常敏感。书里给的处理是先做区域光流平均再算膨胀率避免单个特征点失效导致估计跳变。6. 复现这份资源时的几个硬坑6.1 避坑清单五条血泪经验坑一配平点算出来能收敛但回代后速度导数不为零。现象是仿真一开始飞机就缓慢加速或减速高度却基本不变。原因是配平方程组里漏了推力随空速变化的项或者气动系数用的参考面积和实际不一致。解决办法是把配平残差打印出来逐项检查尤其是阻力项和推力项的平衡量级应该在 1e-3 以内。坑二线性化后的模型和仿真对不上。现象是线性模型预测的阶跃响应很干净非线性仿真却有过冲。原因是线性化的扰动幅度给得太大。书里的线性模型是「小偏差」模型扰动量超过配平空速的 10% 就开始失真。解决办法是把阶跃幅度降到 12 m/s 或 23 度再对比。坑三卡尔曼滤波在静态下收敛一机动就发散。现象是地面静止时姿态估计很准起飞后滚转角估计滞后甚至反号。原因是过程噪声 Q 给得太小滤波器过度相信陀螺而陀螺在振动下零偏会变。解决办法是把 Q 按实际振动水平调大并且给协方差矩阵加对称性保护避免数值累积导致非正定。坑四航点切换后飞机绕圈。现象是到达航点后不转向下一个航点而是绕着当前点转。原因是切换条件只用了距离而飞机在侧风下横向误差始终压不进到达圆或者 Dubins 路径的方向参数传反了。解决办法是切换条件里加上航向偏差判断并检查圆弧方向的符号。坑五Simulink 仿真跑得通代码生成后行为不同。现象是同一套控制律仿真里稳定烧进飞控后振荡。原因是仿真用了连续求解器而实际代码是离散执行积分项和微分项的离散化方式不一致等效增益变了。解决办法是在仿真里就用固定步长、按实际控制周期离散化别等到代码生成才发现。注意以上五条里前两条属于建模层面第三条属于估计层面后两条属于实现层面。出了问题时先定位在哪一层不要一上来就改 PID 增益那样只会把问题挪到另一个通道。6.2 用特征值验证闭环而不是靠肉眼最后一章落到一个具体技巧不要用阶跃响应判断闭环稳不稳用特征值。做法是把线性化后的 A、B 矩阵和你的控制增益组装成闭环 A_cl求特征值看实部是否全为负、阻尼比是否落在 0.60.8。import numpy as np def closed_loop_check(A, B, K): A,B 为线性化模型K 为状态反馈增益 A_cl A - B K eigvals np.linalg.eigvals(A_cl) print(特征值:, np.round(eigvals, 4)) print(最大实部:, np.max(eigvals.real)) # 阻尼比与自然频率 for lam in eigvals: if lam.imag ! 0: wn abs(lam) zeta -lam.real / wn print(fwn{wn:.3f} rad/s, zeta{zeta:.3f}) return eigvals # 示例纵向四状态状态反馈增益自行设计 closed_loop_check(A_lon, B_lon, K_lon)逻辑说明闭环矩阵的构造依赖状态反馈形式 u -Kx如果你的控制律带积分项或者前馈要先把它增广进状态空间再算。这一步比看曲线可靠得多因为曲线只能看到你激励出来的模态看不到那些没被激发但已经接近虚轴的危险模态。参数说明A_lon、B_lon是纵向线性化矩阵维度一般是 4×4 和 4×1K_lon是 1×4 的增益向量。判断标准最大实部必须严格小于零留的裕度建议不小于 0.1主导极点阻尼比低于 0.4 就要重新分配带宽。这个习惯我是被坑出来的。早年做一个小型固定翼的航线飞行地面站上看滚转响应漂亮得很结果一遇到侧风就进螺旋查了两天才发现螺旋模态的特征值实部是正的只是幅值很小短时间的阶跃激励根本看不出来侧风持续扰动才把它激起来。从那以后我每次改完增益都强制走一遍闭环特征值检查再去看时域响应。这一条比任何调参口诀都管用希望帮到你。本文还有配套的精品资源点击获取