CN110673623A - 一种基于双环pd控制算法控制的四旋翼无人机着陆方法 - Google Patents

Abstract

本发明公开了一种基于双环PD控制算法控制的四旋翼无人机着陆方法，通过设计四旋翼无人机着陆时的位置控制律u₁和姿态控制律，控制四旋翼无人机着陆时的位置坐标和三个姿态的欧拉角；设计基于双环的PD控制的着陆算法，外环控制位置，内环控制姿态；将外环产生的中间指令信号θ_d和ψ_d传递给内环，内环通过其内环控制律实现对两个指令信号的跟踪，闭环系统的稳定性通过调整内外环增益系数得到保证；PD控制器在工程上容易实现，可用于实时性较高的场合，具有算法简单、参数易调整等特点，可用于实时性较高的场合。

Description

一种基于双环PD控制算法控制的四旋翼无人机着陆方法

技术领域

本发明属于四旋翼无人机控制领域，具体涉及一种基于双环PD控制算法控制的四旋翼无人机着陆方法。

背景技术

近年来，无人机(UAV)越来越多地应用于民用领域，在环保、电力、交通、测绘、安防、农业等领域均有无人机的身影；随着现代技术的进步，特别是人工智能技术的快速发展，未来无人机+行业应用的模式具有广阔的应用前景；

目前无人机的智能化水平还有待提高，许多应用背后都有无人机操作员在控制无人机，要提高无人机的自动化和智能化水平，还需科研工作者进行大量研究，无人机的自主着陆问题是无人机智能化应用必须解决的关键技术问题，相比无人机航路规划、导航等已有较多的研究，自主着陆方面的研究还相对较少；

在现有的关于无人机的自主着陆的主要理论有：张建宏等针对固定翼无人机，采用在线稳定逆研究无人机自主精确着陆问题；高杨军等提出一种内外环混合迭代的滑模控制策略，双环都采用混合迭代滑模控制算法以克服滑模控制的缺点；豆清波等针对小型伞降无人机回收的特点，设计该类无人机全机着陆试验系统并进行着陆试验；针对固定翼无人机在着陆过程中速度和下沉率不断增大的问题，采用鲁棒伺服和PID相结合的控制方法设计着陆控制律并进行仿真试验；针对轮式无人机，根据自适应内模控制原理，以地速与下沉率为控制目标，设计着陆纵向飞行控制律；吴政隆等在分析固定翼无人机着陆阶段运行特性基础上，设计基于光流的自主着陆控制律，并用Simulink进行仿真；以上控制算法通常要依赖GPS导航或惯导系统，由于GPS的缺点，基于视觉的自主着陆吸引了大批研究人员，张小正等基于视觉导航方法，设计圆环型地标并基于此地标研究无人机着陆位姿估计方法；

综上，目前我国在固定翼无人机的着陆算法设计方面已有较多研究；但对于多旋翼无人机的自主着陆的研究和相关理论数据还相对较少，四旋翼无人机在起飞和着陆时不需要跑道，另外其可悬停的特性对于监测领域具有非常强的吸引力，其应用领域非常广，因此，对于四旋翼无人机的自主着陆控制研究，用以解决目前四旋翼无人机的着陆问题具有现实意义。

发明内容

为了克服上述现有技术的缺陷，本发明的目的在于提供一种基于双环PD控制算法控制的四旋翼无人机着陆方法，通过设计基于双环的PD控制的着陆算法，外环控制位置，内环控制姿态；将外环产生的中间指令信号θ_d和ψ_d传递给内环，内环通过其内环控制律实现对两个指令信号的跟踪，闭环系统的稳定性通过调整内外环增益系数得到保证；在工程上容易实现，可用于实时性较高的场合，实用性高。

为了实现上述目的，本发明所采用的技术方案如下：

一种基于双环PD控制算法控制的四旋翼无人机着陆方法，包括以下步骤：

步骤一：建立四旋翼无人机MIMO系统的动力学模型；

步骤二：通过步骤一的动力学模型设计外环位置控制律u₁，以实现x→x_d，y→y_d，z_d→0；

步骤三：根据步骤一的动力学模型设计内环姿态控制律，使θ→θ_d，ψ→ψ_d，φ→φ_d，φ_d→0；

步骤四：分别利用位置控制律u₁和姿态控制律控制的位置子系统和姿态子系统控制四旋翼无人机着陆时的位置和姿态，实现四旋翼无人机的自主着陆；

其中：(x,y,z)为UAV质心在惯性坐标系中的位置坐标，(θ,ψ,φ)为无人机三个姿态的欧拉角，分别为俯仰角、偏航角和滚转角，(x_d，y_d)为四旋翼无人机着陆时的位置坐标，(θ_d,ψ_d,φ_d)为四旋翼无人机着陆时的三个姿态的欧拉角。

进一步的，步骤一所述的四旋翼无人机MIMO系统的动力学模型具体为：

其中：(x,y,z)为UAV质心在惯性坐标系中的位置坐标，(θ,ψ,

)为无人机三个姿态的欧拉角，分别为俯仰角、偏航角和滚转角，g为重力加速度，l为无人机半径长度，m为UAV之负载总质量，I_i为围绕每个轴之转动惯量，K_i为阻力系数，

为UAV质心位置一阶导数，

分别代表四旋翼无人机在x、y、z方向的线速度，

为四旋翼无人机在飞行时三个姿态欧拉角的一阶导数，

分别代表四旋翼无人的欧拉角角速度、偏航角角速度、滚转角角速度，

为UAV质心位置二阶导数，

为四旋翼无人机在飞行时三个姿态欧拉角的二阶导数。

进一步的，步骤二所述位置控制律u₁设计的具体步骤为：

S1.由公式(1)定义：

S2.对于第一个位置子系统采用基于前馈补偿的PD控制算法设计控制律:

其中：

为四旋翼无人机飞行在某一位置时该点在x轴上的加速度，

为前馈补充项，

为PD控制，即比例、微分控制；令x_e＝x-x_d，则

则第一个位置子系统

可化简为

k_p1为x分量比例系数、k_d1为x分量微分系数、x_e为无人机x方向的误差，根据二阶系统Hurwitz判据，需要满足k_p1＞0，可取k_p1＝4.0，k_d1＝4.0；

S3.对于第二个位置子系统

采用基于前馈补偿的PD控制算法设计控制律：

其中：

为四旋翼无人机飞行在某一位置时该点在y轴上的加速度，

为前馈补充项，

为PD控制，即比例、微分控制；令y_e＝y-y_d，则则第二个位置子系统

可化简为

k_p2为y分量比例系数、k_d2为y分量微分系数、y_e为无人机y方向的误差，根据二阶系统Hurwitz判据，需要满足k_p2＞0，

要使系统稳定，取k_p2＝4.0，k_d2＝4.0；

S4.对于第三个位置子系统采用基于前馈补偿的PD控制算法设计控制律：

其中：

为四旋翼无人机飞行在某一位置时该点在z轴上的加速度，

为前馈补充项，为PD控制，即比例、微分控制；令z_e＝z-z_d，则

则第二个位置子系统

可化简为

k_p3为z分量比例系数、k_d3为z分量微分系数、z_e为无人机z方向的误差，根据二阶系统Hurwitz判据，需要满足k_p3＞0，

要使系统稳定，可取k_p3＝4.0，k_d3＝4.0；

S5.假定满足控制律公式(3)-(5)所需要的姿态角度为θ_d和ψ_d，由公式(2)可得:

由u_1z＝u₁ cosφcosψ_d，可得将其代入公式(6)可得:

其中：若X>1时，取θ_d＝π/2；若X<-1时，取θ_d＝-π/2。

进一步的，步骤三所述的姿态控制律设计的具体步骤为：

S1.取θ_e＝θ-θ_d，采用前馈补偿的PD控制算法设计第一个姿态角子系统

设计控制律：

其中：

为四旋翼无人机飞行在某一姿态时其俯仰角θ的加速度，

因θ_e＝θ-θ_d，等式两遍求导，则

则第一个姿态角子系统可化简为

k_p4为比例系数、k_d4为微分系数、θ_e为俯仰角误差，根据二阶系统Hurwitz判据，需要满足k_p4＞0，

要使系统稳定，取k_p4＝16，k_d4＝16；

S2.取ψ_e＝ψ-ψ_d，采用前馈补偿的PD控制算法设计第二个姿态角子系统

姿态控制律为：

其中：

为四旋翼无人机飞行在某一姿态时其偏航角Ψ的加速度，因ψ_e＝ψ-ψ_d，等式两遍求导，则

则第二个姿态角子系统

可化简得到

k_p5为比例系数、k_d5为微分系数，ψ_e为俯仰角误差，根据二阶系统Hurwitz判据，需要满足k_p4＞0，

要使系统稳定，取k_p5＝16，k_d5＝16；

S3.取

采用前馈补偿的PD控制算法设计第三个姿态角子系统

姿态控制律为：

其中：

为四旋翼无人机飞行在某一姿态时其滚转角φ的加速度，因

等式两遍求导，则

则第三个姿态角子系统可化简得到

k_p6为比例系数、k_d6为微分系数，φ_e为滚转角误差，根据二阶系统Hurwitz判据，需要满足k_p6＞0，

要使系统稳定，取k_p6＝16，k_d6＝16。

进一步的，步骤四所述的基于内外环控制率控制的位置子系统和姿态子系统控制四旋翼无人机着陆时的位置和姿态，实现四旋翼无人机的自主着陆的具体过程为：

S1.指令信号发生器将四旋翼无人机自主着陆时的质心位置(x_d，y_d，z_d)和姿态的欧拉角(θ_d,ψ_d,φ_d)分别发送到外环位置子系统控制器和内环姿态子系统控制器；

S2.外环位置子系统控制器利用位置控制率计算出位置控制律u₁和中间指令信号θ_d和ψ_d，并将位置控制律u₁发送给外环位置子系统，中间指令信号θ_d和ψ_d发送给内环姿态子系统，内环通过其内环控制律实现对两个指令信号的跟踪；同时内环姿态子系统控制器计算出姿态控制律u₂、u₃、u₄，并将姿态控制律u₂、u₃、u₄发送给内环姿态子系统；

S3.姿态子系统收到姿态控制律u₂、u₃、u₄后，经计算得到给定的姿态的欧拉角(θ_d,ψ_d,φ_d)对应在惯性坐标系中姿态的欧拉角(θ,ψ,

)，控制四旋翼无人机着陆时的姿态；同时，位置子系统收到位置控制律u₁后，经计算得到给定的质心位置(x_d，y_d，z_d)对应在惯性坐标系中的位置坐标(x,y,z)，控制四旋翼无人机着陆时的位置。

本发明的有益效果是：本发明公开了一种基于双环PD控制算法控制的四旋翼无人机着陆方法，通过设计基于双环的PD控制的着陆算法，外环控制位置，内环控制姿态；将外环产生的中间指令信号θ_d和ψ_d传递给内环，内环通过其内环控制律实现对两个指令信号的跟踪，闭环系统的稳定性通过调整内外环增益系数得到保证；PD控制器在工程上容易实现，具有可用于实时性较高的场合，实用性高的特点。

附图说明

图1为本发明控制系统框图。

图2为本发明三个位置状态收敛曲线图。

图3-1为本发明俯仰角收敛曲线图。

图3-2为本发明偏航角收敛曲线图。

图3-3为本发明滚转角收敛曲线图。

图4为本发明系统存在白噪声时x,y,z三个位置状态收敛曲线图。

具体实施方式

为了使本领域的普通技术人员能更好的理解本发明的技术方案，下面结合附图和实施例对本发明的技术方案做进一步的描述。

参照附图1-2所示的一种基于双环PD控制算法控制的四旋翼无人机着陆方法，包括以下步骤：

步骤一：建立四旋翼无人机MIMO系统的动力学模型；

步骤二：通过步骤一的动力学模型设计外环位置控制律u₁，以实现x→x_d，y→y_d，z_d→0；

步骤三：根据步骤一的动力学模型设计内环姿态控制律，使θ→θ_d，ψ→ψ_d，φ→φ_d，φ_d→0；

步骤四：分别利用位置控制律u₁和姿态控制律控制的位置子系统和姿态子系统控制四旋翼无人机着陆时的位置和姿态，实现四旋翼无人机的自主着陆；

其中：(x,y,z)为UAV质心在惯性坐标系中的位置坐标，(θ,ψ,φ)为无人机三个姿态的欧拉角，分别为俯仰角、偏航角和滚转角，(x_d，y_d)为四旋翼无人机着陆时的位置坐标，(θ_d,ψ_d,φ_d)为四旋翼无人机着陆时的三个姿态的欧拉角。

步骤一所述的四旋翼无人机MIMO系统的动力学模型具体为：

其中：(x,y,z)为UAV质心在惯性坐标系中的位置坐标，(θ,ψ,

)为无人机三个姿态的欧拉角，分别为俯仰角、偏航角和滚转角，g为重力加速度，l为无人机半径长度，m为UAV之负载总质量，I_i为围绕每个轴之转动惯量，K_i为阻力系数，

为UAV质心位置一阶导数，分别代表四旋翼无人机在x、y、z方向的线速度，

为四旋翼无人机在飞行时三个姿态欧拉角的一阶导数，

分别代表四旋翼无人的欧拉角角速度、偏航角角速度、滚转角角速度，

为UAV质心位置二阶导数，

为四旋翼无人机在飞行时三个姿态欧拉角的二阶导数。

进一步的，步骤二所述位置控制律u₁设计的具体步骤为：

S1.由公式(1)定义：

S2.对于第一个位置子系统

采用基于前馈补偿的PD控制算法设计控制律:

其中：：

为四旋翼无人机飞行在某一位置时该点在x轴上的加速度，为前馈补充项，为PD控制，即比例、微分控制；令x_e＝x-x_d，则

则第一个位置子系统

可化简为k_p1为x分量比例系数、k_d1为x分量微分系数、x_e为无人机x方向的误差，根据二阶系统Hurwitz判据，需要满足k_p1＞0，

可取k_p1＝4.0，k_d1＝4.0；

S3.对于第二个位置子系统

采用基于前馈补偿的PD控制算法设计控制律：

其中：

为四旋翼无人机飞行在某一位置时该点在y轴上的加速度，

为前馈补充项，

为PD控制，即比例、微分控制；令y_e＝y-y_d，则

则第二个位置子系统

可化简为k_p2为y分量比例系数、k_d2为y分量微分系数、y_e为无人机y方向的误差，根据二阶系统Hurwitz判据，需要满足k_p2＞0，

要使系统稳定，取k_p2＝4.0，k_d2＝4.0；

S4.对于第三个位置子系统

采用基于前馈补偿的PD控制算法设计控制律：

其中：

为四旋翼无人机飞行在某一位置时该点在z轴上的加速度，为前馈补充项，

为PD控制，即比例、微分控制；令z_e＝z-z_d，则

则第二个位置子系统

可化简为

k_p3为z分量比例系数、k_d3为z分量微分系数、z_e为无人机z方向的误差，根据二阶系统Hurwitz判据，需要满足k_p3＞0，

要使系统稳定，可取k_p3＝4.0，k_d3＝4.0；

S5.假定满足控制律公式(3)-(5)所需要的姿态角度为θ_d和ψ_d，由可公式(2)可得:

由u_1z＝u₁ cosφcosψ_d，可得将其代入公式(6)可得:

其中：若X>1时，取θ_d＝π/2；若X<-1时，取θ_d＝-π/2。

进一步的，步骤三所述的姿态控制律设计的具体步骤为：

S1.取θ_e＝θ-θ_d，采用前馈补偿的PD控制算法设计第一个姿态角子系统设计控制律：

其中：

为四旋翼无人机飞行在某一姿态时其俯仰角θ的加速度，因θ_e＝θ-θ_d，等式两遍求导，则

则第一个姿态角子系统

可化简为

k_p4为比例系数、k_d4为微分系数、θ_e为俯仰角误差，根据二阶系统Hurwitz判据，需要满足k_p4＞0，

要使系统稳定，取k_p4＝16，k_d4＝16；

S2.取ψ_e＝ψ-ψ_d，采用前馈补偿的PD控制算法设计第二个姿态角子系统

姿态控制律为：

其中：

为四旋翼无人机飞行在某一姿态时其偏航角Ψ的加速度，因ψ_e＝ψ-ψ_d，等式两遍求导，则

则第二个姿态角子系统

可化简得到k_p5为比例系数、k_d5为微分系数，ψ_e为俯仰角误差，根据二阶系统Hurwitz判据，需要满足k_p4＞0，

要使系统稳定，取k_p5＝16，k_d5＝16；

S3.取

采用前馈补偿的PD控制算法设计第三个姿态角子系统姿态控制律为：

其中：

为四旋翼无人机飞行在某一姿态时其滚转角φ的加速度，因等式两遍求导，则

则第三个姿态角子系统可化简得到k_p6为比例系数、k_d6为微分系数，φ_e为滚转角误差，根据二阶系统Hurwitz判据，需要满足k_p6＞0，

要使系统稳定，取k_p6＝16，k_d6＝16；

在公式(8)和公式(9)中，需要对中间指令信号θ_d和ψ_d求导，可采用有限时间收敛三阶微分器实现

进一步的，步骤四所述的基于内外环控制率控制的位置子系统和姿态子系统控制四旋翼无人机着陆时的位置和姿态，实现四旋翼无人机的自主着陆的具体过程为：

S1.指令信号发生器将四旋翼无人机自主着陆时的质心位置(x_d，y_d，z_d)和姿态的欧拉角(θ_d,ψ_d,φ_d)分别发送到外环位置子系统控制器和内环姿态子系统控制器；

S2.外环位置子系统控制器利用位置控制率计算出位置控制律u₁和中间指令信号θ_d和ψ_d，并将位置控制律u₁发送给外环位置子系统，中间指令信号θ_d和ψ_d发送给内环姿态子系统，内环通过其内环控制律实现对两个指令信号的跟踪；同时内环姿态子系统控制器计算出姿态控制律u₂、u₃、u₄，并将姿态控制律u₂、u₃、u₄发送给内环姿态子系统；

S3.姿态子系统收到姿态控制律u₂、u₃、u₄后，经计算得到给定的姿态的欧拉角(θ_d,ψ_d,φ_d)对应在惯性坐标系中姿态的欧拉角(θ,ψ,

)，控制四旋翼无人机着陆时的姿态；同时，位置子系统收到位置控制律u₁后，经计算得到给定的质心位置(x_d，y_d，z_d)对应在惯性坐标系中的位置坐标(x,y,z)，控制四旋翼无人机着陆时的位置。

实施例1：如附图3-1、3-2和3-3所示，对步骤二和步骤三计算得到的(x_d，y_d)和(θ_d,ψ_d,φ_d)进行仿真模拟，具体是在Matlab R2017a下进行仿真，仿真时各参数取值如表1所示：

表1被控对象仿真参数

参数	值	说明
			m	2	无人机总质量
l	0.2	无人机半径长度
			g	9.8	重力加速度
K<sub>1</sub>	0.01	阻力系数1
			K<sub>2</sub>	0.01	阻力系数2
K<sub>3</sub>	0.01	阻力系数3
			K<sub>4</sub>	0.012	阻力系数4
K<sub>5</sub>	0.012	阻力系数5
			I<sub>1</sub>	1.25	转动惯量1
I<sub>2</sub>	1.25	转动惯量2
			I<sub>3</sub>	2.5	转动惯量3

仿真的具体步骤为：

S1.设被控对象之初始状态取[3 0 2 0 10 0]，角度初始状态取[0 0 0 0π/6 0]；

S2.采用位置控制律式(3)-式(5)，姿态控制律式(8)-式(10)，取x_d＝10，y_d＝8，z_d＝0,

系统在没有干扰的理想状态下，进行仿真计算，得到四旋翼无人机着陆时三个位置的收敛过程过程如附图2所示，四旋翼无人机着陆时三个姿态的收敛过程如附图3所示；

从附图2和附图3的收敛曲线可看出，四旋翼无人机着陆于(10,8,0)，着陆过程平稳，收敛速度较快，三个位置收敛于期望值，没有出现超调现象，采用内环收敛速度大于外环收敛速度的方法，可保证闭环系统稳定性。

实施例2：与实施例1不同的是，在仿真过程中，系统中引入有限带宽白噪声，所得到的四旋翼无人机着陆时三个位置的收敛过过程如附图4所示；

从附图4的收敛曲线可看出，引入白噪声后，系统位置状态曲线总体收敛，存在超调但幅度较小，最终四旋翼无人机着陆于(10,8,0)附近。从仿真结果可看出，系统存在扰动情况下，双环PD控制算法也具有实用性。

从附图2-附图4可以得到，本申请通过设计基于双环的PD控制的着陆算法，外环控制位置，内环控制姿态；将外环产生的中间指令信号θ_d和ψ_d传递给内环，内环通过其内环控制律实现对两个指令信号的跟踪，闭环系统的稳定性通过调整内外环增益系数得到保证；PD控制器在工程上容易实现，可用于实时性较高的场合；有效保证了四旋翼无人机降落时的准确位置和姿态。

以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (5)

1.一种基于双环PD控制算法控制的四旋翼无人机着陆方法，其特征在于，包括以下步骤：

步骤一：建立四旋翼无人机MIMO系统的动力学模型；

步骤二：通过步骤一的动力学模型设计外环位置控制律u₁，以实现x→x_d，y→y_d，z_d→0；

步骤三：根据步骤一的动力学模型设计内环姿态控制律，使θ→θ_d，ψ→ψ_d，φ→φ_d，φ_d→0；

步骤四：分别利用位置控制律u₁和姿态控制律控制的位置子系统和姿态子系统控制四旋翼无人机着陆时的位置和姿态，实现四旋翼无人机的自主着陆；

其中：(x,y,z)为UAV质心在惯性坐标系中的位置坐标，(θ,ψ,φ)为无人机三个姿态的欧拉角，分别为俯仰角、偏航角和滚转角，(x_d，y_d)为四旋翼无人机着陆时的位置坐标，(θ_d,ψ_d,φ_d)为四旋翼无人机着陆时的三个姿态的欧拉角。

2.根据权利要求1所述的一种基于双环PD控制算法控制的四旋翼无人机着陆方法，其特征在于，步骤一所述的四旋翼无人机MIMO系统的动力学模型具体为：

其中：(x,y,z)为UAV质心在惯性坐标系中的位置坐标，

为无人机三个姿态的欧拉角，分别为俯仰角、偏航角和滚转角，g为重力加速度，l为无人机半径长度，m为UAV之负载总质量，I_i为围绕每个轴之转动惯量，K_i为阻力系数，为UAV质心位置一阶导数，

分别代表四旋翼无人机在x、y、z方向的线速度，

为四旋翼无人机在飞行时三个姿态欧拉角的一阶导数，

分别代表四旋翼无人的欧拉角角速度、偏航角角速度、滚转角角速度，

为UAV质心位置二阶导数，

为四旋翼无人机在飞行时三个姿态欧拉角的二阶导数。

3.根据权利要求2所述的一种基于双环PD控制算法控制的四旋翼无人机着陆方法，其特征在于，步骤二所述位置控制律u₁设计的具体步骤为：

S1.由公式(1)定义：

S2.对于第一个位置子系统

采用基于前馈补偿的PD控制算法设计控制律:

其中：

为四旋翼无人机飞行在某一位置时该点在x轴上的加速度，

为前馈补充项，为PD控制，即比例、微分控制；令x_e＝x-x_d，则

则第一个位置子系统

可化简为

k_p1为x分量比例系数、k_d1为x分量微分系数、x_e为无人机x方向的误差，根据二阶系统Hurwitz判据，需要满足k_p1>0，

可取k_p1＝4.0，k_d1＝4.0；

S3.对于第二个位置子系统

采用基于前馈补偿的PD控制算法设计控制律：

其中：

为四旋翼无人机飞行在某一位置时该点在y轴上的加速度，

为前馈补充项，为PD控制，即比例、微分控制；令y_e＝y-y_d，则

则第二个位置子系统

可化简为

k_p2为y分量比例系数、k_d2为y分量微分系数、y_e为无人机y方向的误差，根据二阶系统Hurwitz判据，需要满足k_p2>0，

要使系统稳定，取k_p2＝4.0，k_d2＝4.0；

S4.对于第三个位置子系统

采用基于前馈补偿的PD控制算法设计控制律：

其中：

为四旋翼无人机飞行在某一位置时该点在z轴上的加速度，

为前馈补充项，

为PD控制，即比例、微分控制；令z_e＝z-z_d，则

则第二个位置子系统

可化简为k_p3为z分量比例系数、k_d3为z分量微分系数、z_e为无人机z方向的误差，根据二阶系统Hurwitz判据，需要满足k_p3>0，

要使系统稳定，可取k_p3＝4.0，k_d3＝4.0；

S5.假定满足控制律公式(3)-(5)所需要的姿态角度为θ_d和ψ_d，由公式(2)可得:

由u_1z＝u₁cosφcosψ_d，可得

将其代入公式(6)可得:

其中：若X>1时，取θ_d＝π/2；若X<-1时，取θ_d＝-π/2。

4.根据权利要求2所述的一种基于双环PD控制算法控制的四旋翼无人机着陆方法，其特征在于，步骤三所述的姿态控制律设计的具体步骤为：

S1.取θ_e＝θ-θ_d，采用前馈补偿的PD控制算法设计第一个姿态角子系统设计控制律：

其中：

为四旋翼无人机飞行在某一姿态时其俯仰角θ的加速度，

因θ_e＝θ-θ_d，等式两遍求导，则

则第一个姿态角子系统

可化简为

k_p4为比例系数、k_d4为微分系数、θ_e为俯仰角误差，根据二阶系统Hurwitz判据，需要满足k_p4>0，要使系统稳定，取k_p4＝16，k_d4＝16；

S2.取ψ_e＝ψ-ψ_d，采用前馈补偿的PD控制算法设计第二个姿态角子系统

姿态控制律为：

其中：

为四旋翼无人机飞行在某一姿态时其偏航角Ψ的加速度，因ψ_e＝ψ-ψ_d，等式两遍求导，则

则第二个姿态角子系统

可化简得到

k_p5为比例系数、k_d5为微分系数，ψ_e为俯仰角误差，根据二阶系统Hurwitz判据，需要满足k_p4>0，

要使系统稳定，取k_p5＝16，k_d5＝16；

S3.取

采用前馈补偿的PD控制算法设计第三个姿态角子系统

姿态控制律为：

其中：

为四旋翼无人机飞行在某一姿态时其滚转角φ的加速度，因

等式两遍求导，则

则第三个姿态角子系统可化简得到k_p6为比例系数、k_d6为微分系数，φ_e为滚转角误差，根据二阶系统Hurwitz判据，需要满足k_p6>0，要使系统稳定，取k_p6＝16，k_d6＝16。

5.根据权利要求1所述的一种基于双环PD控制算法控制的四旋翼无人机着陆方法，其特征在于，步骤四所述的基于内外环控制率控制的位置子系统和姿态子系统控制四旋翼无人机着陆时的位置和姿态，实现四旋翼无人机的自主着陆的具体过程为：

S1.指令信号发生器将四旋翼无人机自主着陆时的质心位置(x_d，y_d，z_d)和姿态的欧拉角(θ_d,ψ_d,φ_d)分别发送到外环位置子系统控制器和内环姿态子系统控制器；

S2.外环位置子系统控制器利用位置控制率计算出位置控制律u₁和中间指令信号θ_d和ψ_d，并将位置控制律u₁发送给外环位置子系统，中间指令信号θ_d和ψ_d发送给内环姿态子系统，内环通过其内环控制律实现对两个指令信号的跟踪；同时内环姿态子系统控制器计算出姿态控制律u₂、u₃、u₄，并将姿态控制律u₂、u₃、u₄发送给内环姿态子系统；

S3.姿态子系统收到姿态控制律u₂、u₃、u₄后，经计算得到给定的姿态的欧拉角(θ_d,ψ_d,φ_d)对应在惯性坐标系中姿态的欧拉角

控制四旋翼无人机着陆时的姿态；同时，位置子系统收到位置控制律u₁后，经计算得到给定的质心位置(x_d，y_d，z_d)对应在惯性坐标系中的位置坐标(x,y,z)，控制四旋翼无人机着陆时的位置。

Images