在建立无人机数学模型时，通常需要做出一些假设，以简化问题并使模型更易于分析和求解。首先，假设无人机是一个刚体，即无人机的形状和大小在运动过程中保持不变。这意味着无人机的各个部分之间没有相对运动，并且无人机的质量是均匀分布的，整体运动可以用一个点来代表。并且无人机的运动是在一个平面内进行的，这就使无人机的运动可以用二维坐标来描述，但在某些情况下，例如当无人机进行高度变化的运动时，可能需要使用三维坐标来描述其运动。同时，在理想状态下，无人机的空气阻力可不予考虑。
基于无人机刚体特性的假设，其运动能通过六个自由度来阐释，也就是三个平移自由度，即x、y、z方向上的位移，以及三个旋转自由度，包括偏航角Ψ、俯仰角θ、滚转角ζ。由此，我们构建起无人机的六自由度运动学方程。考虑到无人机的运动特点及控制需求，我们选取地面坐标系和机体坐标系。地面坐标系是以地面为基准，而机体坐标系则是以无人机自身为基准。
在无人机六自由度运动学方程建立时，我们通常需要使用牛顿第二定律和欧拉方程。牛顿第二定律描述了物体在力的作用下的运动，而欧拉方程则描述了物体在力矩的作用下的旋转。对于无人机来说，我们需要考虑它在空间中的平移和旋转，因此需要同时使用牛顿第二定律和欧拉方程。
根据牛顿第二定律，无人机的运动状态方程式：
(2-1)
(2-2)
(2-3)
其中，m是无人机的质量，、、分别是无人机在 x、y、z 方向上的合力。依据欧拉方程，无人机的旋转状态方程：
(2-4)
(2-5)
(2-6)
其中，、、分别是无人机绕x、y、z轴的转动惯量，、、分别是无人机在x、y、z方向上受到的力矩。
基于牛顿第二定律和欧拉方程中，选取原点位于飞行器质心的某一坐标系 Oxyz，相对于地面坐标系（，，）有转动角速度ω，质心的绝对直线速度为 V，可得到矩阵形式：
（2-7）
由于飞行器在飞行中受到的空气动力在速度坐标系三个轴上的分量分别为D，Y，L，飞行器的推力 Fp 与机体坐标系的 Ox2 轴重合，而重力 G=mg 则沿着地球坐标系 Oz1 轴的正方向。，故飞行器所受合力沿机体坐标系各轴的分量Fx2，Fy2，Fz2满足以下关系：
=（2-9）
在机体坐标系（x2，y2，z2）中构建飞行器绕质心旋转的动力学方程：
假设飞行器关于x-z平面对称，惯性积，并假设，则飞行器绕质心运动转动的动力学模型的表达式为：
（2-11）
为了确定飞行器的运动轨迹，必须对其进行运动学分析。对于机体坐标系来说，可通过机体对地面坐标系的转换矩阵来获得飞行器质心速度的方程：
（2-12）
利用角速度方程式，导出了围绕飞行器质心旋转的运动学方程。从地面坐标系转换到机体坐标系的过程中可知，ζ 角是由沿 Oz1 轴的角速度 dζ/dt 形成的，需要用到地面坐标系到机体坐标系的转换矩阵 Tb/e 来进行转换；θ 角是由沿 Oy 轴的角速度 dθ/dt 形成的，需要使用沿 Ox 轴旋转矩阵 Tx 将其沿机体坐标系进行投影；φ 角则是直接由沿 Ox2 轴的角速度 dφ/dt 形成的。由此可以写出旋转角速度投影到机体坐标系上的表达式为：
（2-13）
通过求解式（2-13），可得到围绕飞行器质心旋转的运动方程式：
= p+(q sin ɸ+r cos ɸ)tan ɸ
= q cos ɸ - r sin ɸ （2-14）
= (q sin ɸ +r cos ɸ)/cos θ
2.2 无人机数学模型的建立
在 simulink 中构建无人机数学模型时，首先要明确无人机的飞行姿态、动力系统、控制系统等，收集有关无人机的各种参数和数据[4]。其次，使用aerospace blocket的模块库创建无人机模型，根据收集到的数据，定义模型中的各种参数。最后，将模块中的各个模块连接起来，形成一个完整的无人机系统。此次无人机数学模型主要划分为 pilot 输入、environment 环境输入及飞行器本体这三个部分的建模。考虑到控制系统的自动性，pilot输入可忽略不计，无人机总体模型如图2-1。
图2-1 无人机总体模型
重力g是无人机所受到的地球引力，它会影响无人机所受到的地球引力和无人机的加速度和飞行高度。外界风场u是无人机在飞行过程中还需考虑外部气流对无人机的影响。风场的大小及方向会对无人机的飞行速度和方向产生影响。从而影响其飞行轨迹和控制性能。推动力是p无人机的动力源，它会影响无人机的飞行速度和方向。在无人机环境输入模型中，外界风场u和推动力p通常作为一个变量来考虑，而重力g通常作为常数来考虑，重力g根据无人机的地理位置和飞行高度来确定，而且，外界风场u由气象数据或传感器测量结果确定。同时，推动力p由无人机的发动机或电机的输出功率和控制信号来决定。在无人机环境输入模型的各模块中，环境模块用于模拟无人机所处的环境，如大气压力、温度、湿度、风速等，可以根据无人机的位置和时间来计算环境参数。传感器模块用于测量无人机的姿态、速度、位置等信息，而控制器模块则依据传感器信息来做出相应决策。测量的信息和环境参数来计算控制信号，以控制无人机的姿态和速度。执行器模块根据控制器计算的控制信号来控制无人机的运动。最后，模型输入输出模块将模型的传感器测量值输入到传感器和将控制信号输出到控制器中。
搭建无人机环境输入模型后，我们开始构建无人机飞行器本土建模。飞行器本体建模分为力、力矩建模和动力学建模，首先，我们先进行力、力矩建模无人机被视为刚体，四个电机固定在机臂末端，当电机转动时，四个电机均会产生向上的力，这四个力的总和即为总拉力。该拉力不仅能为机体提供升力，还会产生一个导致机体旋转的力矩。在产生拉力和力矩的过程中，无人机还产生了影响飞行性能和稳定性的气动力和气动力矩。因此，在进行力、力矩建模时，我们主要搭建无人机气动力和气动力矩计算模型，判断无人机的气动力，如图2-3所示。
图2-3 气动力和气动力矩计算模型
无人机气动力和气动力矩计算模型主要由气流速度和方向、空气密度、气动力系数、气动力矩系数及质量等组成。气动力系数和气动力矩系数是该计算模型的核心部分。它们根据气流的速度和方向来计算无人机的升力、阻力、滚动力矩和俯仰力矩等，这些系数直接关系到无人机的动力性能和操纵性能。气流的速度和方向包括来流速度、攻角、侧滑角等，将决定无人机与空气之间的相互作用。同时，在建立无人机气动力和气动力矩计算模型时，还需考虑无人机的外形、飞行状态、流场等因素。
2.3无人机数学模型的配平及线性化
无人机的数学模型是一个复杂的非线性系统，为了便于设计飞行控制律和高度保持控制律，需要进行配平和线性化处理。根据无人机的任务需求，我们需要确定无人机在平衡点处的飞行状态。在确定平衡点时，我们需要考虑无人机的稳定性和可控性，即无人机在平衡点附近的运动应是收敛的并且能够被控制器控制。在进行线性化时，我们需要选择合适的平衡点，通常，我们选择平衡点处的速度、加速度和姿态角作为线性化的参考点。然后，对数学模型进行泰勒展开并忽略高阶项来实现，将非线性模型转化为线性模型。根据线性化后的模型，我们计算控制系统的参数，如增益、积分时间和微分时间等。这些参数可以用于PID控制，使无人机能够稳定地保持在平衡点附近[5]。
2.4本章小结
这一章详细阐述了无人机数学模型的构建过程，首先基于牛顿第二定律和欧拉方程建立了无人机的六自由度运动学方程，描述了无人机在三维空间中运动和姿态变化的物理规律。接着，利用Simulink软件和aerospace blockset工具箱，对无人机的飞行控制系统、环境输入和传感器等进行了系统建模，创建了一个全面的无人机系统模型。此外，为了便于控制算法的设计，对非线性的无人机数学模型执行了配平和线性化处理，选择了平衡点并应用泰勒展开方法简化模型。本章的总结强调了数学模型在无人机自动驾驶控制系统设计中的核心作用，为后续的控制算法开发和仿真测试提供了理论基础和技术支持。
第3章 无人机自动驾驶控制算法设计
四旋翼飞行器由于其自身所具有的非线性以及高度耦合的特点，给控制策略带来了非常独特且具有挑战性的难题。为了能够有效地实现空中轨迹的跟踪，在本章中，我们将介绍一种采用内外环结构的控制策略，以此来避免直接去设计四旋翼飞行器的欠驱动控制律，首先需要建立起四旋翼飞行器的动力学模型，然后再对其进行解耦处理，将整个系统分解为一些相互独立的子系统，比如位置子系统和姿态子系统。
3.1 位置控制律设计
对于四旋翼飞行器的二阶控制系统，根据 Hurwitz 判据，二阶系统 的稳定性条件如下：
(3-1)
(3-3)
在控制过程中，我们选用PID控制算法。PID 是一种很常见的反馈回路控制系统，它主要是通过将目标设定值（给定值）与实际值进行比较。和系统的实际输出值来计算偏差。这种偏差，即采样偏差，是系统当前状态与期望状态之间的差距。控制器根据这个偏差调整控制信号，实时地对系统进行调节，以使系统输出尽可能接近给定值[16]。为了加快系统的响应速度并提高调节效率，分别在外环和内环加入PD（比例-微分）控制器。PD控制器通过比例和微分两种控制作用，能迅速减少系统偏差，并有效地抑制系统的超调和振荡为了加速系统的调节过程，提高控制精度，我们采用 PD 控制方法来设计 x 轴位置子系统的控制律。
(3-4)
(3-6)
z：
(3-8)
3.2姿态控制律设计
而姿态控制则是通过内环来实现的，其目标是对飞行器的俯仰角（θ）、偏航角（ψ）和滚转角（φ）进行调整，以保证飞行器能稳定地飞行并精确地调整其朝向。姿态控制率的设计也采用了PD控制器，加强了飞行器对姿态变化的快速响应和稳定性。结合等式
（3-9）
和与上文求出的即可得到
(3-10)
(3-12)
(3-13)
(3-14)
3.3本章小结
本章详细介绍了针对四旋翼飞行器的控制系统设计，着重于解决其非线性和高度耦合的特性所带来的控制挑战。通过采用内外环结构的控制策略，本研究有效地避免了直接设计四旋翼的欠驱动控制律，实现了系统的有效解耦，同时，我们还对位置子系统和姿态子系统分别进行了非常详细的控制律设计。
位置控制通过引入PD控制器，利用其比例-微分的控制作用快速减少系统偏差，并有效抑制系统的超调和振荡，提高了控制的精确性和响应速度[6]。特别是，通过分析x轴、y轴和z轴的控制律，并且验证了这些控制律符合Hurwitz稳定性条件，确保了系统在各个方向上的稳定操作。
姿态控制策略也采用了PD控制器，着重于调整飞行器的俯仰角（θ）、偏航角（ψ）和滚转角（φ）。设计中不仅包括了基本的PD控制律，还加入了前馈补偿，进一步优化了控制效果，使得飞行器能够在各种飞行状态下保持高度稳定。
通过综合使用位置控制和姿态控制的PD控制策略，本章构建了一个强大且灵活的控制框架，为四旋翼飞行器提供了可靠的飞行性能。这种控制框架不仅适应了四旋翼飞行器的动态特性，也为处理其他复杂非线性系统提供了参考[15]。
第4章 无人机自动驾驶控制系统仿真验证
为了验证本文所提出控制策略的效果和实际应用价值，本章将通过在 Simulink 环境中开展一系列仿真实验来展现四旋翼飞行器的性能表现。仿真实验主要包括轨迹跟踪和定高两个部分，旨在展示控制系统在实际操作中的响应性和精确性。
本实验所用的控制系统结合了位置控制和姿态控制的PD控制策略，通过Simulink的仿真模块精确实现[7]。这些控制策略不仅提高了系统的响应速度，还增强了系统对环境变化和内在动态的适应能力。仿真参数的设定，如飞行器的质量、阻力系数、转动惯量等，都是基于实际飞行条件精心选择的，确保了仿真结果的实用性和可靠性。
4.1 实验设置
使用MATLAB/Simulink软件构建了一个对四旋翼飞行器进行详细仿真的模型。该模型全面且真实地再现了四旋翼飞行器的动力学特性以及控制逻辑。包括其非线性特性和多变量耦合效应[13]。在仿真设置中，所有的控制算法均已实现并通过适当的模块进行模拟，以确保测试结果的准确性和可重复性[8]。
在实验中，设定了基本的垂直升降、复杂的三维路径跟踪和定高飞行等测试场景。按照上一章节控制算法的叙述采用如下的控制框架。
图4-1 系统控制框架图
表4-1 无人机的仿真参数
参数 | 值 | 参数 | 值 | ||
飞行器质量m/kg | 2 | 阻力系数 | 0.012 | ||
旋翼至中心长度l/m | 0.2 | 阻力系数 | 0.012 | ||
重力加速度 | 9.8 | 阻力系数 | 0.012 | ||
阻力系数 | 0.01 | 转动惯量 | 1.25 | ||
阻力系数 | 0.01 | 转动惯量 | 1.25 | ||
阻力系数 | 0.01 | 转动惯量 | 1.25 | ||
位置控制和姿态控制PD控制率在simulink建立仿真模型如下：
图4-2 无人机x轴PD控制
图4-2对无人机的X轴进行PD（比例-微分）控制是一种旨在维持无人机在X轴方向上稳定位置的策略。PD控制器通过调整无人机的控制输入来响应其在X轴方向上的偏差，其中P参数和D参数分别对控制器的性能和响应特性产生影响。具体来说，当P参数设定为0.03，这意味着控制器对X轴位置偏差的比例部分较为敏感，但响应强度适中。较小的P值有助于避免系统过激反应，减少超调，并提供较为平缓的控制响应，这在需要精细位置控制的场合非常重要。另一方面，D参数设定为0.8，表明微分控制部分在控制器中占有较大的权重。微分控制利用误差的变化率，即偏差的导数，来预测误差的趋势，从而在系统出现振荡或不稳定趋势时快速做出反应。较高的D值可以增加系统的稳定性，减少因滞后引起的振荡，提高系统的动态响应速度。
图4-3 无人机y轴PD控制
图4-3对无人机y轴进行PD控制，比例增益P设置为0.03，它负责根据无人机x轴的位置误差产生一个修正信号，这个信号与位置误差成正比。这意味着无人机偏离目标位置越远，控制器产生的修正作用就越大，从而帮助无人机返回预定轨迹。
微分增益D设置为0.8，其作用是考虑到位置误差的变化率，即无人机在x轴上的速度误差。通过这一参数，控制器可以对无人机的运动速度进行调整，尤其是在快速动态变化时，微分控制有助于减少超调和振荡，使无人机更平滑地达到或维持在目标位置。
图4-4 无人机z轴PD控制
综合P参数和D参数的设定，图4-4无人机的z轴PD控制器将展现出以下显著特点：P参数的适中设定确保了系统对X轴位置偏差的响应既迅速又平稳，避免了激烈的控制动作和潜在的超调现象。D参数的高权重赋予了控制器出色的预测和调节能力，使其能够快速响应并抑制系统中的振荡，进一步增强了飞行的稳定性。通过精心调整P和D参数，无人机在z轴方向上的动态性能得到了显著提升，能够更加精准地跟踪预定的飞行轨迹。
图4-5 角度控制在simulink中的仿真构建
在内外环控制中，内环的控制性能影响外环的稳定性，由此对整个闭环控制系统的稳定性产生作用。为达到收敛速度较快的控制，图4-5角度控制采用较大的PD增益，确保内环收敛速率大于外环收敛速率，比例增益设为0.5，微分增益设为0.2，并设置了无人机的三个不同角度，实际姿态（phi，pasi，thete），参考角度（phid，psaid，thetad），将实际姿态与参考角度进行比较，为定高实验提高数据。
综上，在实际应用中，PD控制参数的设定需要根据无人机的动力学特性、具体飞行任务的要求以及外部环境的变化进行细致的调整和优化。通过仿真测试和实际飞行验证，可以进一步校准P和D参数，以实现最佳的控制效果，确保无人机在方向上实现高精度、高稳定性的控制。
4.2定高实验
无人机起始位置设定为，给定高度为10m，仿真结果如图4-6所示：
图4-6 无人机实际姿态与参考角度对比
图4-6显示了无人机在三个不同角度（phi，psai，theta）上的实际姿态与其期望姿态的对比。从图中我们可以看出：
对于 phi 和 theta 角，无人机的实际姿态（phi，theta）与设定的参考角度（phid，）完全吻合，这意味着在这两个角度上无人机的姿态控制非常准确，没有偏差。在 psai 角的控制中，我们可以看到实际角度（psai）在初始时刻有一小段时间内与设定的参考角度（psaid）存在明显的偏差，但很快之后实际角度迅速调整，与参考角度吻合，如图4-7为无人机实际位置与参考位置对比。
图4-7 无人机实际位置与参考位置对比
图4-7展示了无人机在三个空间坐标轴（X、Y、Z）上的实际位置与其参考位置的对比。从图中我们可以分析出：X轴和Y轴位置：无人机的实际位置（实际值 Fx，Fy）与参考位置（参考值 Fxd，Fyd）完全吻合，表明无人机在这两个方向的位置控制准确，没有偏差。Z轴位置：无人机的实际位置（实际值 Fz）在开始时与参考位置（参考值 Fzd）存在较大偏差。尽管实际位置最终快速调整至参考位置，但这种调整表现出较大的动态响应，这可能是控制系统在垂直方向的响应性与调整机制导致的。
4.3轨迹跟踪实验
参考轨迹设定如下：
(4-1)
无人机起始位置设定为，实验结果如下图4-8所示。图4-8展示了无人机在执行轨迹跟踪任务时，其实际位置与参考位置在三个坐标轴（X、Y、Z）上的对比。我们可以从中得到以下观察：
X轴和Y轴位置：无人机的实际位置（实际值 Fx，Fy）与参考轨迹（参考值 Fxd，Fyd）基本一致，表现出良好的轨迹跟踪能力。虽然存在轻微的波动和偏离，但整体上非常接近，这显示了无人机在横向和纵向的控制系统响应良好，能有效跟随复杂的正弦波形轨迹。
Z轴位置：实际位置（实际值 Fz）与参考位置（参考值 Fzd）在开始时基本保持一致，随后实际位置逐渐线性增长，与参考位置逐步拉开差距。这表明在垂直方向上的控制系统可能需要进一步调整，以确保无人机能够更精确地遵循预定的高度变化。
图4-8 无人机在轨迹跟踪任务下实际位置与参考位置对比
如图4-9为无人机在轨迹跟踪任务下实际姿态与参考角度对比：
图4-9 无人机在轨迹跟踪任务下实际姿态与参考角度对比
图4-9表明无人机在执行轨迹跟踪任务时，其实际姿态角（phi，psai，theta）与参考角度的对比。我们可以从中得到以下分析：
Phi角：实际姿态角（phi）与参考角度（phid）之间存在一定的波动，尤其是在图中可见的轻微的超调现象。然而，波动范围较小，显示出系统能较好地控制该角度。
Psai角：图4-9显示，起始时实际姿态角（psai）与参考角度（psaid）之间有显著的偏差，但很快实际角度调整到与参考角度一致，此后维持良好的一致性。这显示了初始阶段可能存在的响应延迟或超调，但整体调节效果良好。
Theta角：实际姿态角（theta）与参考角度（thetad）之间的对比显示了非常好的跟踪效果，两者几乎完美重叠，表明无人机在此角度的控制精确性非常高。
4.4本章小结
本章通过在Simulink环境中对四旋翼飞行器进行一系列详尽的仿真实验，成功验证了本文提出的控制策略的有效性和实用性。这些实验主要围绕轨迹跟踪和定高飞行两个核心任务，展示了控制系统在实际操作中的高响应性和精确性。
通过定高实验和轨迹跟踪实验，观察到以下关键成果：
1.定高实验：无人机能够在设定的高度（10米）上保持稳定飞行，几乎没有明显的偏离。实验中，无人机的实际位置与预设的参考位置之间的偏差极小，显示出系统在垂直控制方面的高效稳定性。
2.轨迹跟踪实验：无人机精确跟踪了预定的复杂三维路径。尤其是在动态变化的轨迹上，控制系统能够快速响应路径变动，有效地调整飞行器的位置和姿态，以精确匹配参考轨迹。这表明了控制策略在处理复杂动态环境中的高度适应性和准确性。
通过系统控制框架图和Simulink中的具体实现，本文成功展示了一个整合了理论与实践的控制框架。实验结果不仅支持了理论分析的正确性，也展示了控制策略在实际应用中的高度可靠性和实用性。特别是在处理实际飞行中可能遇到的非线性和多变量耦合问题时，本文的控制策略表现出了显著的优势。
