基于四旋翼无人机离散建模与增量PID控制及轨迹跟踪研究，MATLAB代码

四旋翼无人机作为一类典型的强耦合、多输入多输出（MIMO）高度非线性欠驱动系统，凭借结构简洁、机动性能优异、悬停特性良好等优势，在科研、工业及军用领域得到广泛应用。此类系统的动力学特性复杂，仅通过经验调参难以实现鲁棒稳定的控制效果与可预测的动态响应，因此需通过严格的建模与控制器设计流程，在保证系统可控性与可分析性的前提下，完成控制律的设计与实现。本文以四旋翼无人机为研究对象，从六自由度刚体动力学出发，完成悬停点小扰动线性化、零阶保持（ZOH）离散化建模，进而设计增量式PID串级控制器，并通过MATLAB仿真验证该控制策略在悬停及圆形轨迹跟踪任务中的有效性，同时分析关键参数对系统控制性能的影响。

一、四旋翼无人机动力学建模

1.1 六自由度刚体动力学基础

将四旋翼无人机抽象为具有三个平动自由度（空间位置x,y,zx,y,zx,y,z）与三个转动自由度（滚转角ϕ\phiϕ、俯仰角θ\thetaθ、偏航角ψ\psiψ）的刚体系统，建立固连于无人机的机体坐标系BBB与地面固定的惯性坐标系III。定义惯性系下位置向量pI=x, y, z⊤\mathbf{p}_I = x,\\,y,\\,z^\toppI=x,y,z⊤、速度向量vI=p˙I\mathbf{v}_I = \dot{\mathbf{p}}_IvI=p˙I，采用Z-Y-X欧拉角描述机体姿态，机体坐标系下角速度向量为ω=p, q, r⊤\boldsymbol{\omega} = p,\\,q,\\,r^\topω=p,q,r⊤。

综合考虑机体所受重力、旋翼总推力TTT、平动阻尼作用，建立平动动力学受力平衡方程：
mv˙I=R(η)00T−mge3−DvvIm\dot{\mathbf{v}}_I = R(\eta) \begin{bmatrix}0\\0\\ T\end{bmatrix} - mg\mathbf{e}_3 - D_v\mathbf{v}_Imv˙I=R(η) 00T −mge3−DvvI

其中，mmm为无人机质量，R(η)R(\eta)R(η)为机体坐标系到惯性坐标系的旋转矩阵，e3\mathbf{e}_3e3为z轴单位向量，DvD_vDv为平动阻尼矩阵。

结合旋翼产生的控制力矩τ\boldsymbol{\tau}τ、转动惯量矩阵JJJ、陀螺力矩及转动阻尼作用，建立转动动力学方程：
Jω˙=τ−ω×(Jω)−DωωJ\dot{\boldsymbol{\omega}} = \boldsymbol{\tau} - \boldsymbol{\omega} \times (J\boldsymbol{\omega}) - D_\omega\boldsymbol{\omega}Jω˙=τ−ω×(Jω)−Dωω

其中，DωD_\omegaDω为转动阻尼矩阵。上述动力学方程为非线性形式，无法直接应用于线性控制器的设计与分析，需在工作点处进行线性化处理。

1.2 悬停工作点小扰动线性化

选取无人机悬停状态为平衡工作点，此时满足姿态角ϕ0=0,θ0=0,ψ0=ψ0\phi_0 = 0, \theta_0=0, \psi_0 = \psi_0ϕ0=0,θ0=0,ψ0=ψ0，惯性系速度vI0=0\mathbf{v}_{I0}=0vI0=0，机体角速度ω0=0\boldsymbol{\omega}_0 = 0ω0=0，旋翼总推力T0=mgT_0 = mgT0=mg。对系统状态与输入引入小扰动增量，定义状态增量向量：
δx=x, y, z, vx, vy, vz, ϕ, θ, ψ, p, q, r⊤\delta\mathbf{x} = x,\\,y,\\,z,\\,v_x,\\,v_y,\\,v_z,\\,\\phi,\\,\\theta,\\,\\psi,\\,p,\\,q,\\,r^\topδx=x,y,z,vx,vy,vz,ϕ,θ,ψ,p,q,r⊤

输入增量向量：
δu=δT, τϕ,τθ,τψ⊤\delta\mathbf{u} = \\delta T,\\,\\tau_\\phi,\\tau_\\theta,\\tau_\\psi^\topδu=δT,τϕ,τθ,τψ⊤

其中，δT\delta TδT为总推力增量，τϕ、τθ、τψ\tau_\phi、\tau_\theta、\tau_\psiτϕ、τθ、τψ分别为滚转、俯仰、偏航控制力矩。

采用小角度近似（sin⁡ϕ≈ϕ,cos⁡ϕ≈1\sin\phi\approx \phi, \cos\phi\approx 1sinϕ≈ϕ,cosϕ≈1，俯仰角同理）对非线性动力学方程进行泰勒展开，忽略高阶小项后，推导得到悬停点附近的连续线性化状态空间模型：
δx˙=Acδx+Bcδu\delta\dot{\mathbf{x}} = A_c\delta\mathbf{x} + B_c \delta\mathbf{u}δx˙=Acδx+Bcδu

其中，系统矩阵AcA_cAc与输入矩阵BcB_cBc具有如下分块结构：
Ac=03×3I3000−1mDvAmv0000I3000−J−1Dω,Amv=0g0−g00000A_c=\begin{bmatrix} 0_{3\times3} & I_3 & 0 & 0 \\ 0 & -\frac{1}{m}D_v & A_{mv} & 0 \\ 0&0&0&I_3\\ 0&0&0& -J^{-1}D_\omega \end{bmatrix}, \quad A_{mv}=\begin{bmatrix} 0 & g & 0 \\ -g & 0 & 0 \\ 0 & 0 & 0 \end{bmatrix}Ac= 03×3000I3−m1Dv000Amv0000I3−J−1Dω ,Amv= 0−g0g00000
Bc=001×31m01×3001×30J−1B_c= \begin{bmatrix} 0 & 0_{1\times3} \\ \frac{1}{m} & 0_{1\times3} \\ 0 &0_{1\times3}\\ 0 & J^{-1} \end{bmatrix}Bc= 0m10001×301×301×3J−1

该12维线性化模型可完整描述四旋翼无人机在悬停工作点附近的平动与姿态动态特性，为后续离散化与控制器设计奠定基础。

二、线性模型ZOH离散化与开环特性分析

2.1 基于零阶保持的精确离散化

为实现数字控制器的设计，采用零阶保持（ZOH）方法对连续线性化模型进行精确离散化，设定采样周期Ts=0.01 sT_s=0.01 \text{ s}Ts=0.01 s，假设输入信号在每个采样周期内保持恒定。离散化后系统的状态空间模型为：
δxk+1=Fδxk+Gδuk\delta\mathbf{x}k+1 = F\delta\mathbf{x}k + G\delta\mathbf{u}kδxk+1=Fδxk+Gδuk

其中，状态转移矩阵F=eAcTsF = e^{A_cT_s}F=eAcTs，输入矩阵G=∫0TseAcτBc dτG = \int_0^{T_s}e^{A_c\tau}B_c\,d\tauG=∫0TseAcτBcdτ。通过MATLAB的c2d函数完成离散矩阵的求解，所得F、GF、GF、G矩阵作为后续离散系统仿真与分析的基础。

2.2 特征值分析与数值稳定性

对离散状态转移矩阵FFF进行特征值求解，结果表明其所有特征值均分布于复平面单位圆内部或边界附近，计算得谱半径ρ(F)=max⁡∣λi(F)∣≈0.9996\rho(F) = \max |\lambda_i(F)| \approx 0.9996ρ(F)=max∣λi(F)∣≈0.9996。根据离散系统稳定性判据，谱半径小于等于1表明该离散线性模型处于数值稳定/临界稳定状态，满足开环系统无发散的基本要求。

进一步分析采样周期对系统稳定性的影响，当采样周期增大至Ts=0.05 sT_s=0.05 \text{ s}Ts=0.05 s时，矩阵FFF的部分特征值将超出单位圆，导致离散系统失稳。该结果表明采样周期是影响线性化离散系统稳定性的关键参数，在后续控制器设计中需结合系统动态特性合理选取，保证控制器频带与采样频率匹配。

2.3 可控性分析

根据离散系统可控性判据，构建可控性矩阵：
C=G,FG,F2G,...,F11G\mathcal{C} = G, FG, F\^2G, \\dots,F\^{11}GC=G,FG,F2G,...,F11G

对可控性矩阵进行秩求解，得rank(C)=12\text{rank}(\mathcal{C}) = 12rank(C)=12，与系统状态维数一致，表明该离散线性化模型为完全可控系统。尽管四旋翼无人机在物理层面属于欠驱动系统（平动自由度需通过姿态角间接控制），但在悬停工作点附近的线性化模型具备数学意义上的完全可控性，为后续全状态控制律的设计提供了理论依据。

2.4 输入通道单位脉冲响应特性

为分析各输入通道对系统状态的影响规律，对四个输入通道分别施加单采样周期的单位脉冲信号，通过MATLAB仿真得到开环系统的脉冲响应特性，各通道响应规律如下：

1. 总推力增量δT\delta TδT至高度zzz通道：呈现典型的二阶系统响应特性，初始阶段加速度最大，随平动阻尼作用逐渐衰减，反映了无人机垂向平动的惯性与阻尼特性；
2. 滚转力矩τϕ\tau_\phiτϕ至滚转角ϕ\phiϕ通道：滚转角随控制力矩呈一阶上升并逐渐衰减，响应速度显著快于平动通道，表明姿态内环为高带宽子系统；
3. 俯仰力矩τθ\tau_\thetaτθ至俯仰角θ\thetaθ与水平速度vxv_xvx通道：存在明显的耦合特性，俯仰角的快速变化将直接导致水平速度vxv_xvx的耦合加速，该特性直观体现了线性化模型中重力-姿态耦合矩阵AmvA_{mv}Amv的动态作用，也是无人机水平运动的核心物理机制；
4. 偏航力矩τψ\tau_\psiτψ至偏航角ψ\psiψ通道：偏航角响应速度显著慢于滚转、俯仰通道，主要原因是无人机偏航方向的转动惯量通常更大，动态响应特性更迟缓。

三、增量式PID控制器的离散设计与实现

3.1 增量式PID离散公式推导

离散PID控制是数字控制系统中经典的控制策略，相较于位置式PID，增量式PID具有数值稳定性好、抗偏置能力强、可有效避免绝对值饱和导致的积分累积问题等优势，更适用于四旋翼无人机的离散控制。离散PID的传递函数形式为：
C(z)=Kp+KiTs1−z−1+Kd1−z−1TsC(z)=K_p+\frac{K_i T_s}{1-z^{-1}}+K_d\frac{1-z^{-1}}{T_s}C(z)=Kp+1−z−1KiTs+KdTs1−z−1

其中，Kp、Ki、KdK_p、K_i、K_dKp、Ki、Kd分别为比例、积分、微分增益，TsT_sTs为采样周期。对传递函数进行反z变换，推导得到增量式PID的递推计算公式：
Δuk=Kp(ek−ek−1)+KiTsek+KdTs(ek−2ek−1+ek−2)\Delta uk = K_p(ek-ek-1) + K_iT_sek + \frac{K_d}{T_s}(ek-2ek-1+ek-2)Δuk=Kp(ek−ek−1)+KiTsek+TsKd(ek−2ek−1+ek−2)

系统控制量的更新公式为：
uk=uk−1+Δukuk = uk-1+\Delta ukuk=uk−1+Δuk

其中，ek、ek−1、ek−2ek、ek-1、ek-2ek、ek−1、ek−2分别为当前时刻、前一时刻、前两时刻的控制误差，Δuk\Delta ukΔuk为控制量增量。

3.2 简化抗积分饱和策略设计

当控制器输出达到执行机构物理限幅umin⁡≤u≤umax⁡u_{\min}\le u\le u_{\max}umin≤u≤umax时，若积分项继续累积，将产生积分饱和（windup）现象，导致系统响应出现拖尾、超调增大甚至震荡等问题。针对该问题，本文采用简化的抗积分饱和策略：当控制器输出超出限幅范围时，将控制量钳位至饱和值uk=usatuk = u_{\text{sat}}uk=usat，并以饱和后的控制量更新PID控制器的内部状态，使积分项停止累积，直至控制误差方向发生变化，有效抑制积分饱和现象对系统性能的影响。

3.3 位置-姿态串级PID控制结构设计

结合四旋翼无人机的动态特性，其姿态通道（滚转、俯仰）动态响应快、带宽高，位置通道（x、y、z）动态响应慢、带宽低，具有天然的快慢环特性。基于此，设计位置-姿态串级增量式PID控制结构 ，将姿态控制作为内环，位置控制作为外环，同时将高度zzz与偏航角ψ\psiψ设计为独立单环控制通道，具体控制逻辑如下：

1. 位置外环：以位置参考值与实际值的误差为输入，通过增量式PID控制器输出虚拟加速度 ax∗,ay∗,az∗a_x^*,a_y^*,a_z^*ax∗,ay∗,az∗，其中水平方向虚拟加速度通过重力-姿态耦合关系映射为期望姿态角，映射关系为ax=gθ，ay=−gϕa_x = g\theta，a_y = -g\phiax=gθ，ay=−gϕ；
2. 姿态内环：以位置外环输出的期望姿态角为参考值，通过增量式PID控制器快速跟踪姿态误差，输出滚转、俯仰、偏航控制力矩，实现姿态的高精度快速跟踪；
3. 独立单环控制：高度zzz与偏航角ψ\psiψ分别通过独立的增量式PID控制器实现单环调节，其中高度环输出总推力增量，偏航环输出偏航控制力矩，避免多通道间的耦合干扰。

串级控制结构通过内外环的分层控制，有效解耦了位置与姿态的动态特性，内环快速抑制姿态扰动，外环实现位置的精准跟踪，提升了系统的控制精度与抗干扰能力。

四、圆形轨迹跟踪仿真实验与结果分析

4.1 仿真实验设置

基于上述离散线性化模型与增量式PID串级控制结构，在MATLAB环境下搭建仿真平台，设置系统核心参数：无人机质量m=1.20 kgm=1.20\text{ kg}m=1.20 kg，采样周期Ts=0.01 sT_s=0.01\text{ s}Ts=0.01 s，并通过经验整定与微调确定各通道PID增益。

仿真任务分为两个阶段：前5秒实现无人机从地面原点垂直起飞并悬停于空间点(0,0,2) m(0,0,2)\text{ m}(0,0,2) m；从第5秒开始，切换为水平圆形轨迹跟踪任务，参考轨迹设置为半径2 m2\text{ m}2 m、角速度0.4 rad/s0.4\text{ rad/s}0.4 rad/s的水平圆，高度保持2 m2\text{ m}2 m恒定。对仿真过程中的系统状态、控制输入、轨迹跟踪误差进行全程记录，并设置控制量与姿态角限幅，结合抗积分饱和策略保证仿真的物理合理性。

4.2 轨迹跟踪结果

3D轨迹仿真结果表明，四旋翼无人机的实际运动轨迹与参考轨迹高度吻合，仅在圆形轨迹起始时刻（第5秒）存在小幅加速段偏差。该偏差为物理合理现象，原因是参考轨迹速度从0突然跃迁至RωR\omegaRω，导致位置外环输出较大的虚拟加速度，使机体产生短时的动态调整。在动态调整完成后，实际轨迹快速收敛至参考轨迹，实现稳定的圆形轨迹跟踪。

系统状态响应曲线显示，各位置通道（x、y、z）与姿态通道（ϕ、θ、ψ\phi、\theta、\psiϕ、θ、ψ）的实际值均能快速跟踪参考值，无明显超调与震荡，姿态内环的响应速度显著快于位置外环，符合串级控制的快慢环设计要求，控制输入增量平稳无剧烈波动，表明控制器输出具有良好的平滑性。

4.3 稳态误差分析

为定量评价轨迹跟踪精度，选取仿真第10秒后的稳态阶段数据，计算水平平面（x-y）轨迹跟踪的均方误差（MSE）：
MSExy=1N∑k=N10Nend(xref\[k−xactk)2+(yrefk−yactk)2]\text{MSE}{xy} = \frac{1}{N}\sum{k=N_{10}}^{N_{\text{end}}}\left(x_{\\text{ref}}\[k-x_{\text{act}}k)^2 + (y_{\text{ref}}k-y_{\text{act}}k)^2\right]MSExy=N1k=N10∑Nend(xref\[k−xactk)2+(yrefk−yactk)2]

计算结果表明MSExy≈O(10−2) m2\text{MSE}_{xy} \approx O(10^{-2})\text{ m}^2MSExy≈O(10−2) m2，即无人机实际轨迹与参考轨迹的稳态偏差为厘米级，验证了所设计的增量式PID串级控制器具有较高的轨迹跟踪精度。

五、研究结论

本文针对四旋翼无人机的非线性欠驱动特性，完成了从悬停点线性化建模到离散增量式PID控制器设计及轨迹跟踪仿真的全流程研究，主要研究结论如下：

1. 基于六自由度刚体动力学的悬停点小扰动线性化方法有效，所建立的12维连续线性化模型可准确描述无人机悬停附近的动态特性，通过ZOH方法实现的精确离散化模型满足数值稳定与完全可控的要求，为数字控制器设计提供了可靠的模型基础；
2. 所设计的位置-姿态串级增量式PID控制结构适配四旋翼无人机的快慢环动态特性，通过内外环分层控制实现了位置与姿态的解耦控制，结合输出限幅与简化抗积分饱和策略，有效避免了积分饱和现象，提升了系统的控制稳定性与鲁棒性；
3. MATLAB仿真结果验证了该控制策略的有效性，在悬停与半径2m的圆形轨迹跟踪任务中，系统实现了高精度的轨迹跟踪与姿态调节，水平平面稳态跟踪误差为厘米级，控制输入平滑无剧烈波动，满足四旋翼无人机的控制要求；
4. 采样周期、PID增益整定及模型线性化假设是影响系统控制性能的关键因素，采样周期过大会导致离散系统失稳，增益整定需匹配内外环的带宽特性，线性化模型的有效性仅局限于悬停附近的小扰动范围，大机动工况下需结合非线性控制方法进行优化。