本发明提供一种针对降雨干扰的四旋翼无人机分层抗干扰控制方法，以解决无人机在降雨环境下执行器效率损失导致的升力不足以及轨迹飞行精度降低的问题。首先针对不同强度降雨的冲击力构建基于无人机的降雨干扰模型，然后在此基础上结合无人机的基本动力学模型构建基于无人机的降雨动力学模型。其次在无人机控制方面，利用干扰观测器、滑模控制相关理念与经验，合理地选取非线性干扰观测器的参数，并在此基础上设计针对降雨干扰的滑模控制律。本发明基于复合分层抗干扰控制方法，能够显著改善无人机在降雨环境下的飞行品质以及轨迹飞行精度，可用于风雨恶劣环境下的应急救援、交通指挥、航拍测绘等作业任务。
1.一种针对降雨干扰的四旋翼无人机分层抗干扰控制方法，其特征在于，包括如下步骤：第一步，建立针对不同降雨强度的雨水冲击载荷模型，包括：针对降雨终速、雨滴谱与降雨强度的关系，结合雨滴运动特点，构建以下雨水冲击载荷模型：（1） 假定降雨强度R，单位为mm/h；
（2） 计算雨滴谱，即单位体积空气中雨滴的数量随雨滴大小的分布，即M‑P分布N(D)：，
其中， 个/m /mm， 为斜率因子， ，D表示雨滴直径，单位为mm；
（5）计算单位体积空气内所有雨滴冲击至平面产生的冲击力 ：，
（7） 计算单位时间内雨滴的冲击力，进而计算雨滴的冲击载荷，需要计算单位时间内的冲击体积流量 ：，
其中，S表示等效受冲击平面的面积，取无人机的水平投影面积；
（9）计算在某一时刻既定受冲击面所受冲击载荷 ：，
（10）由不同降雨强度与其冲击载荷的对应关系，通过函数拟合得到取决于降雨强度的冲击载荷函数；
第二步，根据降雨冲击载荷模型建立基于无人机的降雨动力学模型，包括：已知四旋翼无人机动力学方程为：，
其中， 为机体坐标系， 为地理坐标系；m为无人机质量， 表示地理坐标系下无人机速度的一阶导数， 表示重力加速度， 表示由无人机机体坐标系坐标转化为地理坐标系坐标所需的旋转矩阵， 表示无人机在机体坐标系下产生的推力， 表示机体坐标系下的无人机角速度， 为 的一阶导数； 为无人机转动惯量，为控制力矩；
在地理坐标系下，令 表示四旋翼无人机飞行速度， 表示 在 平面投影，其与 方向夹角为 ， 表示 在 平面的投影，其与 方向夹角为 ；
在无人机飞行速度影响下，降雨自然条件下平均速度 对于无人机的相对速度分解为方向的速度 和垂直于 方向的速度 ：，
由基于无人机的降雨干扰模型得到雨水在 平面与 平面的冲击载荷分别与 、 有关，则定义冲击载荷因子 、 ，得在考虑无人机飞行速度时降雨冲击载荷 与无人机悬停时冲击载荷的关系：，
其中， 表示降雨在 平面的冲击载荷， 表示降雨在 平面的冲击载荷， 为无人机悬停时的冲击载荷；
无人机在以 速度飞行时，假设滚转角为 ，俯仰角为 ，得到无人机在飞行时所受到的降雨冲击力：，
其中， 表示无人机在垂直于 平面方向受到的冲击力， 表示无人机在垂直于 平面方向受到的冲击力， 表示无人机的水平投影面积；
由此，得到考虑无人机受到降雨冲击影响的无人机的降雨动力学模型：，
其中，， ，分别表示无人机在地理坐标系下三轴方向位置信息的二阶导数，，，分别表示无人机的三个欧拉角；
第三步，根据微分平坦方法生成无人机能够实现的平滑轨迹，包括：期望机体坐标系的z轴表示为： ,其中 表示欧几里得范数；
借助与 相关的中间单位向量 ，得到其它两个轴的表示：，
其中， 表示期望的偏航角， 与 分别表示 与 ， ， ， 分别表示无人机期望机体坐标系的 ， ，轴在地理坐标系下的表示， 表示地理坐标系的轴旋转 角度后的表示；
其中， 表示期望的机体坐标系三轴坐标， 为期望的旋转矩阵，代表的 个元素， 与 分别表示期望的滚转角与俯仰角；
第四步，根据建立的降雨动力学模型设计位置环与姿态环干扰观测器；
第五步，基于观测出的干扰值设计滑模控制律抑制扰动。
2.根据权利要求1所述的一种针对降雨干扰的四旋翼无人机分层抗干扰控制方法，其特征在于，所述第四步包括：首先进行位置环干扰观测器的设计，借助辅助变量 ，设计观测器增益矩阵，即生成以 为对角线元素的3×3方阵：，
其中， 表示与 有关的地理坐标系下的加速度， 表示地理坐标系下无人机位置的一阶导数， ， 表示观测器估计的干扰值， 为在地理坐标系下的期望升力，为 的一阶导数，并且 ；
其次进行姿态环干扰观测器的设计，借助辅助变量 ，设计观测器增益矩阵，即生成以 为对角线元素的3×3方阵：，
其中， 表示机体角速度， 表示 的一阶导数， 表示无人机转动惯量， 表示期望力矩， 表示干扰力矩， 表示观测器估计的干扰力矩值，表示 的一阶导数。
3.根据权利要求2所述的一种针对降雨干扰的四旋翼无人机分层抗干扰控制方法，其特征在于，所述第五步包括：进行滑模控制律设计，分为位置环和姿态环两部分：对 轴控制律进行设计并对干扰进行补偿，首先给出 轴方向动力学方程：，
其中，表示无人机位置 的二阶导数，为控制输入期望加速度， 为干扰力；
定义位置误差量 ，设计滑模面 ，并且采用指数趋近律，得到 轴方向的运动学控制律：，
其中， 与 分别表示地理坐标系下的期望 轴位置与实际位置， 均为设计参数，sgn表示符号函数，上标 表示一阶导数，上标 表示二阶导数；
选取Lyapunov函数 ，其中s为函数符号，证明只需满足 即满足位置环的稳定：
其中，上标 表示一阶导数；由上式得到，当 成立时， 恒成立，无人机位置环控制系统稳定；
对旋转运动进行滑模控制律设计，定义 ，其中 表示机体坐标系相对与地理坐标系旋转角度， 为其三个元素， ，其中 为 一阶导数， 为 的三个元素，则将原动力学方程 转化为如下形式：
其中， ，表示第 个元素， ， ，， ， 表示无人机的三个转动惯量， 表示力矩 的三个分量， 表示 的三个分量， 表示地理坐标系下的无人机角速度，diag表示生成对角线矩阵，上标 表示一阶导数，上标 表示二阶导数；
定义 ，其中 表示欧拉角的三个元素，为期望欧拉角的三个元素，设计滑模面 ，其中 ， 表示的一阶导数，并且采用指数趋近律 ，得到旋转运动的控制律：，
其中， 表示期望角度 的二阶导数， 表示 的第i个元素；
选取Lyapunov函数 ，证明只需满足 即满足姿态环的稳定；
滑模控制律消除颤振现象采用 替换为 的方法，其中：，
一种针对降雨干扰的四旋翼无人机分层抗干扰控制方法
技术领域
[0001] 本发明属于飞行机器人控制领域，具体涉及一种针对降雨干扰的四旋翼无人机分层抗干扰控制方法，适用于需要在雨天执行高精度飞行任务且需要实现稳定安全控制的无人机控制系统。
背景技术
[0002] 近年来，以多旋翼作为驱动机构的小型无人机在灾害预警、地质勘察、应急救援等国防、电力、航拍等领域得到了深入广泛的应用，但大多无人机并不能在雨天执行任务，致使其应用受到极大的限制。目前随着机械设计与加工技术的发展，无人机的电机、电路板、线路等部件已经可以具有一定的防水能力，但是在降雨环境中无人机仍然会受到降雨冲击、升力不足以及气动特性变差等方面的困扰。
[0003] 由于无人机抗雨相关的控制理论起步较晚，目前已经得到实验验证的控制方法很少。中国专利申请CN202111262360.5提出了一种考虑降雨影响的无人机稳定飞行控制方法，中国专利申请CN202111637133.6提出了一种考虑降雨影响的无人机动力学建模方法，但这些方法针对固定翼无人机，而非本发明中所针对的旋翼无人机。其余也有部分科研工作者从气动领域研究了降雨对旋翼无人机的影响或从旋翼无人机防水设计方面做出了贡献，但是这些工作均未能针对降雨干扰设计旋翼无人机抗雨控制律。
发明内容
[0004] 为克服现有控制方法的缺陷与不足，对于以多旋翼无人机为基础的飞行机器人系统，本发明提供一种针对降雨干扰的四旋翼无人机分层抗干扰控制方法，解决无人机在降雨环境下执行器效率损失导致的升力不足以及轨迹飞行精度降低的问题，本发明首先针对不同强度降雨的冲击载荷构建基于无人机的降雨干扰模型，然后在此基础上结合无人机的基本动力学模型构建基于无人机的降雨动力学模型。其次在无人机控制方面，利用干扰观测器、滑模控制相关理念与经验，合理地选取非线性干扰观测器的参数，并在此基础上设计针对降雨干扰的滑模控制律。能够保证飞行器在降雨干扰下的高精度飞行与稳定，以完成各类作业任务。
[0005] 为达到上述目的，本发明采用如下技术方案：
[0006] 一种针对降雨干扰的四旋翼无人机分层抗干扰控制方法，包括如下步骤：
[0007] 第一步，建立针对不同降雨强度的雨水冲击载荷模型；
[0008] 第二步，根据降雨冲击载荷模型建立基于无人机的降雨动力学模型；
[0009] 第三步，根据微分平坦方法生成无人机能够实现的平滑轨迹；
[0010] 第四步，根据建立的降雨动力学模型设计位置环与姿态环干扰观测器；
[0011] 第五步，基于观测出的干扰值设计滑模控制律抑制扰动。
[0012] 进一步地，所述第一步中，针对降雨终速、雨滴谱与降雨强度的关系，结合雨滴运动特点，构建以下雨水冲击载荷模型：
[0013] （1）假定降雨强度 ，单位为mm/h；
[0014] （2） 计算雨滴谱，即单位体积空气中雨滴的数量随雨滴大小的分布，即M‑P分布N(D)：
[0015] ；
[0016] 其中， 个/m3/mm， 为斜率因子， ，D表示雨滴直径，单位为mm；
[0017] （3）计算不同直径大小雨滴的终速 ：
[0018] ，
[0019] （4）计算每一直径雨滴的单滴冲击力 ：
[0020] ，
[0021] 其中， 为雨滴密度，表示降雨速度；
[0022] （5）计算单位体积空气内所有雨滴冲击至平面产生的冲击力 ：
[0023] ，
[0024] （6）计算单位体积空气内雨滴的平均速度 ：
[0025] ，
[0026] 雨滴的直径大小为0‑2.5mm；
[0027] （7）计算单位时间内雨滴的冲击力，进而计算雨滴的冲击载荷，需要计算单位时间内的冲击体积流量 ：
[0028] ，
[0029] 其中，S表示等效受冲击平面的面积，取无人机的水平投影面积；
[0030] （8）计算单位时间内既定受冲击面所受冲击力 ：
[0031] ，
[0032] （9）计算在某一时刻既定受冲击面所受冲击载荷 ：
[0033] ，
[0034] （10）由不同降雨强度与其冲击载荷的对应关系，通过函数拟合得到取决于降雨强度的冲击载荷函数。
[0035] 进一步地，所述第二步包括：
[0036] 已知四旋翼无人机动力学方程为：
[0037] ，
[0038] 其中， 为机体坐标系， 为地理坐标系；m为无人机质量， 表示地理坐标系下无人机速度的一阶导数， 表示重力加速度， 表示由无人机机体坐标系坐标转化为地理坐标系坐标所需的旋转矩阵， 表示无人机在机体坐标系下产生的推力， 表示机体坐标系下的无人机角速度， 为 的一阶导数； 为控制力矩， 为无人机转动惯量；
[0039] 在地理坐标系下，令 表示四旋翼无人机飞行速度， 表示 在 平面投影，其与 方向夹角为 ， 表示 在 平面的投影，其与 方向夹角为 ； 平面垂直于 ；
[0040] 在无人机飞行速度影响下，降雨自然条件下平均速度 对于无人机的相对速度分解为 方向的速度 和垂直于 方向的速度 ：
[0041] ，
[0042] ，
[0043] 由基于无人机的降雨干扰模型得到雨水在 平面与 平面的冲击载荷分别与 、 有关，则定义冲击载荷因子 、 ，得在考虑
无人机飞行速度时降雨冲击载荷 与无人机悬停时冲击载荷的关系：
[0044] ，
[0045] ，
[0046] 其中， 表示降雨在 平面的冲击载荷， 表示降雨在 平面的冲击载荷， 为无人机悬停时的冲击载荷；
[0047] 无人机在以 速度飞行时，假设滚转角为 ，俯仰角为 ，得到无人机在飞行时所受到的降雨冲击力：
[0048] ，
[0049] ，
[0050] 其中， 表示无人机在垂直于 平面方向受到的冲击力， 表示无人机在垂直于 平面方向受到的冲击力， 表示无人机的水平投影面积；
[0051] 由此，得到考虑无人机受到降雨冲击影响的无人机的降雨动力学模型：
[0052] ，
[0053] 其中， ， ，分别表示无人机在地理坐标系下三轴方向位置信息的二阶导数， ，， 分别表示无人机的三个欧拉角。
[0054] 进一步地，所述第三步包括：
[0055] 期望机体坐标系的z轴表示为： ,其中 表示欧几里得范数；
[0056] 借助与 相关的中间单位向量 ， ，得到其它两个轴的表示：
[0057] ,
[0058] 其中， 表示期望的偏航角， 与 分别表示 与 ， ， ， 分别表示无人机期望机体坐标系的 ， ，轴在地理坐标系下的表示， 表示地理坐标系的 轴旋转 角度后的表示；
[0059] 由于 ,因此得到期望的欧拉角：
[0060] ,
[0061] 其中， 表示期望的机体坐标系三轴坐标， 为期望的旋转矩阵， 代表 的 个元素， 与 分别表示期望的滚转角与俯仰角。
[0062] 进一步地，所述第四步包括：
[0063] 首先进行位置环干扰观测器的设计，借助辅助变量 ，设计观测器增益矩阵 ，即生成以 为对角线元素的3×3方阵：
[0064] ,
[0065] 其中， 表示与 有关的地理坐标系下的加速度， 表示地理坐标系下无人机位置的一阶导数， ， 表示观测器估计的干扰值， 为在地理坐标系下的期望升力，为 的一阶导数，并且 ；
[0066] 其次进行姿态环干扰观测器的设计，借助辅助变量 ，设计观测器增益矩阵 ，即生成以 为对角线元素的3×3方阵：
[0067] ,
[0068] 其中， 表示机体角速度， 表示 的一阶导数， 表示无人机转动惯量， 表示期望力矩， 表示干扰力矩， 表示观测器估计的干扰力矩值，表示 的一阶导数。
[0069] 进一步地，所述第五步包括：进行滑模控制律设计，分为位置环和姿态环两部分：
[0070] 对 轴控制律进行设计并对干扰进行补偿，首先给出 轴方向动力学方程：
[0071] ,
[0072] 其中， 表示无人机位置 的二阶导数， 为控制输入期望加速度， 为干扰力；
[0073] 定义位置误差量 ，设计滑模面 ，并且采用指数趋近律，得到 轴方向的运动学控制律：
[0074] ,
[0075] 其中， 与 分别表示地理坐标系下的期望 轴位置与实际位置， ， ，均为设计参数，sgn表示符号函数，上标 表示一阶导数，上标 表示二阶导数。
[0076] 选取Lyapunov函数 ，其中s为函数符号，证明只需满足即满足位置环的稳定：
[0077] ,
[0078] 其中，上标 表示一阶导数。由上式得到，当 成立时， 恒成立，无人机位置环控制系统稳定；
[0079] 对旋转运动进行滑模控制律设计，定义 ，其中 表示机体坐标系相对与地理坐标系旋转角度， 为其三个元素， ，其中为 一阶导数， 为 的三个元素，则将原动力学方程
转化为如下形式：
[0080] ,
[0081] 其 中 ， ，表示 第 个 元素 ， ，， ， ， 表示无
人机的三个转动惯量， 表示力矩 的三个分量， 表示 的三
个分量， 表示地理坐标系下的无人机角速度，diag表示生成对角线矩阵,上标 表示一阶导数，上标 表示二阶导数。
[0082] 定义 ，其中 表示欧拉角的三个元素，为期望欧拉角的三个元素，设计滑模面 ，其中 ， 表示
的一阶导数，并且采用指数趋近律 ，得到旋转运动的控制律：
[0083] ,
[0084] 其中， 表示期望角度 的二阶导数， 表示 的第i个元素；
[0085] 选取Lyapunov函数 ，证明只需满足 即满足姿态环的稳定；
[0086] 滑模控制律消除颤振现象采用 替换为 的方法，其中：
[0087] ,
[0088] 其中，k为所需设计的参数。
[0089] 本发明的有益效果在于：
[0090] 本发明能够实现无人机高精度飞行的同时，保证其具有安全稳定的控制能力。相比于传统的飞行控制方法，本发明首先针对降雨对无人机的干扰，基于降雨终速度、雨滴谱、雨滴形状等关键因素提出了自然降雨冲击量化模型，并将其与传统的无人机动力学模型结合得到基于四旋翼无人机的抗雨动力学模型。随后在此基础上分别设计了无人机位置环与姿态环的干扰观测器，并将其应用于滑模控制律的设计。本发明可以应用于现有大多数具有一定防水能力的无人机，以真正赋予其在降雨环境中抗雨的能力，保证无人机在雨中依然具有较高的飞行品质。
附图说明
[0091] 图1为本发明的一种针对降雨干扰的四旋翼无人机分层抗干扰控制方法的流程图；
[0092] 图2为本发明针对四旋翼无人机平动动力学的无人机速度分解定义示意图。
具体实施方式
[0093] 为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
[0094] 对于无人机的控制器设计，分为无人机的位置环控制和姿态环控制。在实际的飞行过程中，无人机姿态环的工作频率要远大于位置环，因此无人机位置环和姿态环的控制器可以分开设计。
[0095] 如图1所示，本发明的一种针对降雨干扰的四旋翼无人机分层抗干扰控制方法具体包括如下步骤：
[0096] 第一步，建立针对不同降雨强度的雨水冲击载荷模型：
[0097] 针对降雨终速、雨滴谱等要素与降雨强度的关系，结合雨滴运动特点，可以构建以下雨水冲击载荷模型：
[0098] 1. 假定降雨强度R(mm/h)，方便确定后续相关参数；
[0099] 2. 计算雨滴谱（单位体积空气中雨滴的数量随雨滴大小的分布），即M‑P分布（Marshall和Palmer提出）N(D)：
[0100] ,
[0101] 其中， 个/m3/mm， 为斜率因子， ，D表示雨滴直径，单位为mm。
[0102] 3. 计算不同直径大小雨滴的终速 ：
[0103] ,
[0104] 4. 计算每一直径雨滴的单滴冲击力 ：
[0105]
[0106] 其中， 为雨滴密度，可取水密度 ， 表示降雨速度；
[0107] 5. 计算单位体积空气内所有雨滴冲击至平面产生的冲击力 ：
[0108] ,
[0109] 6. 计算单位体积空气内雨滴的平均速度 ：
[0110] ,
[0111] 这里仅统计直径大小在0‑2.5mm范围内的雨滴，原因在于天然降雨的雨滴直径一般在之间0.1‑6.5mm之间。其中直径小于2.0mm的雨滴占绝大多数，并且雨滴在降落过程中变形很小，基本保持球形。
[0112] 7. 计算单位时间内雨滴的冲击力，进而计算雨滴的冲击载荷，需要计算单位时间内的冲击体积流量 ：
[0113] ,
[0114] 其中，S表示等效受冲击平面的面积，可以直接取为无人机的水平投影面积。
[0115] 8. 计算单位时间内既定受冲击面所受冲击力 ：
[0116]
[0117] 9. 计算在某一时刻既定受冲击面所受冲击载荷 ：
[0118] ,
[0119] 10. 由不同降雨强度与其冲击载荷的对应关系，通过函数拟合得到取决于降雨强度的冲击载荷函数。
[0120] 第二步，根据降雨冲击载荷模型建立基于无人机的降雨动力学模型：
[0121] 已知四旋翼无人机动力学方程为：
[0122] ,
[0123] 其中， 为机体坐标系， 为地理坐标系；m为无人机质量， 表示地理坐标系下无人机速度的一阶导数， 表示重力加速度， 表示由无人机机体坐标系坐标转化为地理坐标系坐标所需的旋转矩阵， 表示无人机在机体坐标系下产生的推力， 表示机体坐标系下的无人机角速度， 为 的一阶导数；为控制力矩， 为无人机转动惯量。
[0124] 如图2所示，针对考虑降雨干扰的四旋翼无人机动力学模型，由降雨干扰模型可知，降雨对无人机冲击力影响和雨滴、无人机之间相对速度有关，则有必要考虑无人机运动时速度的相关参数，在地理坐标系下，令 表示四旋翼无人机飞行速度， 表示 在平面投影，其与 方向夹角为 ， 表示 在 平面的投影，其与方向夹角为 。 平面垂直于 。
[0125] 在无人机飞行速度影响下，降雨自然条件下平均速度 对于无人机的相对速度可以分解为 方向的速度 和垂直于 方向的速度 ，
[0126] ,
[0127] ,
[0128] 由基于无人机的降雨干扰模型可知，雨水在 平面与 平面的冲击载荷分别与 、 有关，则定义冲击载荷因子 、 ，可得在考虑无人机飞行速度时降雨冲击载荷 与无人机悬停时冲击载荷的关系：
[0129] ，
[0130] ，
[0131] 其中， 表示降雨在 平面的冲击载荷， 表示降雨在 平面的冲击载荷， 为无人机悬停时的冲击载荷。
[0132] 无人机在以 速度飞行时，假设滚转角为 ，俯仰角为 ，可得无人机在飞行时所受到的降雨冲击力：
[0133] ，
[0134] ，
[0135] 其中， 表示无人机在垂直于 平面方向受到的冲击力， 表示无人机在垂直于 平面方向受到的冲击力， 表示无人机的水平投影面积。
[0136] 由此，可得考虑无人机受到降雨冲击影响的无人机位置运动学模型：
[0137] ,
[0138] 其中， ， ，分别表示无人机在地理坐标系下三轴方向位置信息的二阶导数， ，， 分别表示无人机的三个欧拉角。
[0139] 第三步，根据微分平坦方法生成无人机能够实现的平滑轨迹：
[0140] 期望机体坐标系的z轴表示为： ,其中 表示欧几里得范数；
[0141] 借助与 相关的中间单位向量 ，（其中 表示期望的偏航角， 与 分别表示 与 ），得到其它两个轴的表示：
[0142] ,
[0143] 其中， ， ， 分别表示无人机期望机体坐标系的 ， ，轴在地理坐标系下的表示， 表示地理坐标系的 轴旋转 角度后的表示；
[0144] 由于 ,因此得到期望的欧拉角：
[0145] ,
[0146] 其中， 表示期望的机体坐标系三轴坐标， 为期望的旋转矩阵，代表 的 个元素， 与 分别表示期望的滚转角与俯仰角。
[0147] 第四步，根据建立的动力学模型设计位置环与姿态环干扰观测器：
[0148] 首先进行位置环干扰观测器的设计，借助辅助变量 ，设计观测器增益矩阵 ，即生成以 为对角线元素的3×3方阵：
[0149] ,
[0150] 其中， 表示与 有关的地理坐标系下的加速度， 表示地理坐标系下无人机位置的一阶导数， ， 表示观测器估计的干扰值， 为在地理坐标系下的期望升力， 为 的一阶导数，并且注意到 。
[0151] 其次进行姿态环干扰观测器的设计，借助辅助变量 ，设计观测器增益矩阵 ，即生成以 为对角线元素的3×3方阵：
[0152] ,
[0153] 其中， 表示机体角速度， 表示 的一阶导数， 表示无人机转动惯量， 表示期望力矩， 表示干扰力矩， 表示观测器估计的干扰力矩值，表示 的一阶导数。
[0154] 第五步，进行滑模控制律设计，同样分为位置环和姿态环两部分：
[0155] 由于降雨影响，无人机易出现升力不足现象，这里仅对 轴控制律进行设计并对干扰进行补偿，首先给出 轴方向动力学方程：
[0156] ,
[0157] 其中， 表示无人机位置 的二阶导数， 为控制输入期望加速度， 为干扰力；
[0158] 定义位置误差量 （ 与 分别表示地理坐标系下的期望 轴位置与实际位置），设计滑模面 ，并且采用指数趋近律 （ ， ， 均为设计参数，sgn表示符号函数），得到 轴方向的运动学控制律：
[0159] ,
[0160] 其中，上标 表示一阶导数，上标 表示二阶导数。
[0161] 选取Lyapunov函数 ，其中s为函数符号，证明只需满足 即满足位置环的稳定：
[0162] ,
[0163] 其中，上标 表示一阶导数。由上式得到，当 成立时， 恒成立，无人机位置环控制系统稳定；
[0164] 对旋转运动进行滑模控制律设计，定义 （ 表示机体坐标系相对与地理坐标系旋转角度， 为其三个元素）， （ 为 一阶导数， 为 的三个元素），则将原动力学方程 转化为如下
形式：
[0165] ,
[0166] 其 中 ， ，表示 第 个 元素 ， ， ， ， ， 表示无
人机的三个转动惯量， 表示力矩 的三个分量， 表示 的三
个分量， 表示地理坐标系下的无人机角速度，diag表示生成对角线矩阵，上标 表示一阶导数，上标 表示二阶导数。
[0167] 定义 ，其中 表示欧拉角的三个元素， 为期望欧拉角的三个元素，设计滑模面 （ ， 表示
的一阶导数），并且采用指数趋近律 ，得到可得旋转运动的控制
律：
[0168] ,
[0169] 其中， 表示期望角度 的二阶导数， 表示 的第i个元素；
[0170] 选取Lyapunov函数 ，证明只需满足 即满足姿态环的稳定；
[0171] 滑模控制律消除颤振现象采用 替换为 的方法，其中：
[0172] ,
[0173] 其中，k为所需设计的参数。
[0174] 本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。
