基于非线性扰动观测器的四旋翼无人机降落控制方法与流程

transactions on circuits and systems i：fundamentaltheory and applications；著者：xinghuo yu，man zhihong；出版年月：2002年；文章题目：fast terminal slidingmodecontrol design for nonlinear dynamical systems；页码：261
‑
264)。yong feng等人针对终端滑模控制算法中的奇异性问题，通过调整幂次项的系数使系统消除奇异性(期刊：automatica；著者：yong feng，xinghuo yu，zhihong man；出版年月：2002年；文章题目：nonsingularterminal sliding mode control of rigid manipulators；页码：2159
‑
2167)。
5.虽然通过鲁棒算法可以抑制扰动，但是这样一般会给控制输入带来较大的颤震问题，不利于精准的控制。xiang he等人提出将扰动估计值引入控制器进行前馈控制(期刊：journal of dynamic systems，measurement，and control；著者：xiang he，gordon kou，marc calaf等人；出版年月：2019年；文章题目：in
‑
ground
‑
effect modeling and nonlinear
‑
disturbance observer for multirotor unmanned aerialvehicle control；页码：071013)。对于传感器无法测量的扰动，较为常用的有两种方法可以进行估计，神经网络法和扰动观测器法。使用神经网估计扰动存在计算较大，对无人机机载计算单元硬件要求较高的问题，并且需要庞大的数据集。使用扰动观测器估计扰动计算量小且不需要增加额外的传感器。wen
‑
hua chen等人基于二阶非线性系统设计了非线性扰动观测器，并且通过仿真验证了观测器的有效性(期刊：ieee transactions on industrialelectronics；著者：wen
‑
hua chen，d.j.balance，p.j.gawthrop等人；出版年月：2000年；文章题目：anonlinear disturbance observer forrobotic manipulators；页码：932
‑
938)。mou chen等人针对单输入单输出系统设计了有限时间收敛的扰动观测器(期刊：isa transactions；著者：mou chen，qing
‑
xian wu，and rong
‑
xin cui；出版年月：2013年；文章题目：terminalsliding mode tracking control for a class ofsiso uncertain nonlinear systems；页码：198
‑
206)。以上文献都是通过数值仿真来验证算法的有效性。目前的研究中，很少有研究对扰动观测器进行实验验证。


技术实现要素：

6.为克服现有技术的不足，本发明旨在提出一种基于非线性扰动观测器的鲁棒控制器，实现四旋翼无人机降落过程中较好地抑制地面效应对无人机的干扰。为此，本发明采用的技术方案是，基于非线性扰动观测器的四旋翼无人机降落控制方法，包括如下步骤：建立四旋翼无人机降落过程的非线性动力学模型，设计非线性终端滑模有限时间收敛观测器，进而设计非线性鲁棒控制器实现四旋翼无人机的降落控制。
7.具体步骤如下：
8.步骤1)确定四旋翼无人机的坐标系定义；
9.四旋翼无人机坐标系定义主要涉及两个坐标系，惯性坐标系{i}＝{o
i
，x
i
，y
i
，z
i
}和机体坐标系{b}＝{o
b
，x
b
，y
b
，z
b
}，其中o
i
(i＝i，b)表示坐标系原点，x
i
，y
i
，z
i
(i＝i，b)分别对应坐标系三个主轴方向的单位矢量，各坐标系的定义均遵循右手定则，同时定义四旋翼姿态角在坐标系{i}下表示为η(t)＝[φ(t)，θ(t)，ψ(t)]
t
，φ(t)，θ(t)，ψ(t)分别对应滚转角、俯仰角和偏航角，目标位置在坐标系{i}下表示为p(t)＝[x
d
(t)，y
d
(t)，z
d
(t)]
t
，x
d
(t)，y
d
(t)，z
d
(t)分别对应目标在惯性坐标系下x轴，y轴，z轴的位置；
[0010]
步骤2)确定四旋翼无人机的动力学模型；
[0011]
通过分析四旋翼无人机作用原理，用拉格朗日方程来描述其动力学模型为：
[0012][0013]
式(1)中e3＝[0 0 1]
t
为z轴向量，v(t)∈r3×1为惯性坐标下的线速度，g∈r为重力加速度，m∈r为无人机的机体质量，f(t)∈r1×1为机体坐标系下四个电机在高度方向产生的总升力，d(t)∈r3×1为降落过程中地面效应造成的扰动力，ω(t)∈r3×1为机体坐标系下的角速度，[ω(t)]
×
∈r3×3为ω(t)张成的反对称矩阵，τ(t)∈r3×1为机体坐标系下的转矩。r为机体坐标系到惯性坐标系的旋转矩阵，如式(2)所示，其中s(
·
)、c(
·
)分别代表三角函数sin(
·
)与cos(
·
)，无人机的转动惯量矩阵为j＝diag([j
x j
y j
z
])：
[0014][0015]
为了便于后续的控制设计，定义辅助控制输出信号u(t)∈r3×1和辅助变量d(t)∈r3×1如下所示：
[0016][0017]
假设1地面效应造成的扰动存在上确界sup(d)。
[0018]
步骤3)设计非线性终端滑模扰动观测器对地面效应进行估计；
[0019]
定义x1＝p，式(1)中的前两个等式改写为
[0020][0021]
设计滑模面s1(t)∈r3×1为如下形式：
[0022]
s1＝z
‑
x2ꢀꢀꢀꢀ
(5)
[0023]
其中z(t)∈r3×1为非线性扰动观测器辅助变量，定义扰动观测器估计值为设计扰动观测器如下所示：
[0024][0025]
式(6)中p0，q0∈o为正奇数，并且p0＜q0，参数k，ε，β∈r为正实数，β＞sup(d)，定义扰动估计误差为
[0026][0027]
引理1存在连续正方程v(t)满足下列条件：
[0028][0029]
则v(t)能在有限时间t
s
内到达特征点
[0030]
[0031]
其中α＞0，λ＞0，0＜γ＜1。
[0032]
根据引理1得到：形如(4)
‑
(7)式的非线性终端滑模扰动观测器，扰动估计误差可在有限时间内收敛到0，收敛时间t
so
如下所示：
[0033][0034]
步骤4)设计快速终端滑模控制器；
[0035]
四旋翼无人机位置环动力学方程如(4)所示，设计快速终端滑模制器的滑模面s2(t)为：
[0036][0037]
在式(11)中，e(t)为无人机位置控制误差，其定义为：
[0038]
e＝p
d
‑
p
ꢀꢀꢀꢀ
(12)
[0039]
p
d
(t)∈r3×1为无人机期望位置，α1，ξ1∈r为正实数，μ1，v1∈o为正奇数，且μ1＜v1。控制器u(t)设计如下所示：
[0040][0041]
式(13)中α2，ξ2∈r为正实数，μ2，v2∈o为正奇数，并且有μ2＜v2；
[0042]
对于式(11)
‑
(13)中提出的控制器设计，在满足相应的参数要求条件，包括μ1＜v1和μ2＜v2时，误差e(t)会在有限时间内收敛到0，收敛时间t
sc
如下所示：
[0043][0044]
本发明的特点及有益效果是：
[0045]
1.本发明基于四旋翼无人机的非线性动力学模型，设计了非线性扰动观测器，对地面效应造成的扰动进行估计，并通过基于李雅普诺夫分析的方法证明估计误差的有限时间收敛性；
[0046]
2.本发明将快速终端滑模控制算法与非线性扰动观测器相结合，设计了一种新型非线性鲁棒控制算法，用于四旋翼无人机的降落控制系统设计，并使用基于李雅普诺夫分析的方法证明了位置控制误差的有限时间收敛性；
[0047]
3.本发明通过数值仿真验证了所设计的非线性扰动观测器对外界未知扰动的有效估计，然后利用四旋翼无人机平台通过飞行实验验证了本文提出的观测器
‑
控制器的降落控制效果。并与常规滑模控制器以及无地面效应补偿的控制器进行了性能对比实验。实验结果表明本文设计的控制策略对地面效应有较好的补偿效果，提高了无人机降落的控制精度。
附图说明：
[0048]
图1是本发明控制系统结构图；
[0049]
图2是matlab仿真中高度方向的扰动给定值与扰动估计值曲线图；
[0050]
图3是本发明所采用的实验平台；
[0051]
图4是无地面效应补偿高度方向轨迹跟踪实验时给定高度与无人机高度曲线图；
[0052]
图5是有地面效应补偿高度方向轨迹跟踪实验时给定高度与无人机高度曲线图；
[0053]
图6是有地面效应补偿高度方向轨迹跟踪实验时xyz三个方向的扰动估计值曲线图；
[0054]
图7是无地面效应补偿降落控制实验中给定高度与无人机高度曲线图；
[0055]
图8是有地面效应补偿降落控制实验中给定高度与无人机高度曲线图；
[0056]
图9是有地面效应补偿降落控制实验中xyz三个方向的扰动估计值曲线图。
具体实施方式
[0057]
为克服现有技术的不足，本发明旨在提出一种基于非线性扰动观测器的鲁棒控制器，实现四旋翼无人机降落过程中较好地抑制地面效应对无人机的干扰。为此，本发明采用的技术方案是，基于非线性扰动观测器的四旋翼无人机地面效应补偿降落控制方法，包括如下步骤：建立四旋翼无人机降落过程的非线性动力学模型，设计非线性终端滑模有限时间收敛观测器，进而设计非线性鲁棒控制器实现四旋翼无人机的降落控制。具体步骤如下：
[0058]
步骤1)确定四旋翼无人机的坐标系定义；
[0059]
四旋翼无人机坐标系定义主要涉及两个坐标系，惯性坐标系{i}＝{o
i
，x
i
，y
i
，z
i
}和机体坐标系{b}＝{o
b
，x
b
，y
b
，z
b
}，其中o
i
(i＝i，b)表示坐标系原点，x
i
，y
i
，z
i
(i＝i，b)分别对应坐标系三个主轴方向的单位矢量，各坐标系的定义均遵循右手定则，同时定义四旋翼姿态角在坐标系{i}下表示为η(t)＝[φ(t)，θ(t)，ψ(t)]
t
，φ(t)，θ(t)，ψ(t)分别对应滚转角、俯仰角和偏航角，目标位置在坐标系{i}下表示为p(t)＝[x
d
(t)，y
d
(t)，z
d
(t)]
t
，x
d
(t)，y
d
(t)，z
d
(t)分别对应目标在惯性坐标系下x轴，y轴，z轴的位置；
[0060]
步骤2)确定四旋翼无人机的动力学模型；
[0061]
通过分析四旋翼无人机作用原理，用拉格朗日方程来描述其动力学模型为：
[0062][0063]
式(1)中e3＝[0 0 1]
t
为z轴向量，v(t)∈r3×1为惯性坐标下的线速度，g∈r为重力加速度，m∈r为无人机的机体质量，f(t)∈r1×1为机体坐标系下四个电机在高度方向产生的总升力，d(t)∈r3×1为降落过程中地面效应造成的扰动力，ω(t)∈r3×1为机体坐标系下的角速度，[ω(t)]
×
∈r3×3为ω(t)张成的反对称矩阵，τ(t)∈r3×1为机体坐标系下的转矩。r为机体坐标系到惯性坐标系的旋转矩阵，如式(2)所示。其中s(
·
)、c(
·
)分别代表三角函数sin(
·
)与cos(
·
)，无人机的转动惯量矩阵为j＝diag([j
x j
y j
z
])。
[0064][0065]
为了便于后续的控制设计，定义辅助控制输出信号u(t)∈r3×1和辅助变量d(t)∈r3×1如下所示：
[0066]
[0067]
假设1地面效应造成的扰动存在上确界sup(d)。
[0068]
步骤3)设计非线性终端滑模扰动观测器对地面效应进行估计；
[0069]
定义x1＝p，式(1)中的前两个等式可改写为
[0070][0071]
设计滑模面s1(t)∈r3×1为如下形式：
[0072]
s1＝z
‑
x2ꢀꢀꢀꢀ
(5)
[0073]
其中z(t)∈r3×1为非线性扰动观测器辅助变量。定义扰动观测器估计值为设计扰动观测器如下所示：
[0074][0075]
式(6)中p0，q0∈o为正奇数，并且p0＜q0，参数k，ε，β∈r为正实数，β＞sup(d)。定义扰动估计误差为
[0076][0077]
引理1假设存在连续正方程v(t)满足下列条件：
[0078][0079]
则v(t)能在有限时间t
s
内到达特征点
[0080][0081]
其中α＞0，λ＞0，0＜γ＜1。
[0082]
根据引理1可得，形如(4)
‑
(7)式的非线性终端滑模扰动观测器，扰动估计误差可在有限时间内收敛到0，收敛时间t
so
如下所示：
[0083][0084]
步骤4)设计快速终端滑模控制器；
[0085]
四旋翼无人机位置环动力学方程如(4)所示，设计快速终端滑模制器的滑模面s2(t)为：
[0086][0087]
在式(11)中，e(t)为无人机位置控制误差，其定义为：
[0088]
e＝p
d
‑
p
ꢀꢀꢀꢀ
(12)
[0089]
p
d
(t)∈r3×1为无人机期望位置，α1，ξ1∈r为正实数，μ1，v1∈o为正奇数，且μ1＜v1。控制器u(t)设计如下所示：
[0090][0091]
式(13)中α2，ξ2∈r为正实数，μ2，v2∈o为正奇数，并且有μ2＜v2。
[0092]
对于式(11)
‑
(13)中提出的控制器设计，在满足相应的参数要求条件，包括μ1＜v1和μ2＜v2时，误差e(t)会在有限时间内收敛到0，收敛时间t
sc
如下所示：
[0093][0094]
本发明所要解决的技术问题是：四旋翼无人机在降落过程中会受到地面效应的干扰，增加了降落控制难度，降低了降落的精度。
[0095]
本发明采取的技术方案是：建立四旋翼无人机降落过程的非线性动力学模型，设计非线性终端滑模有限时间收敛观测器，进而设计非线性鲁棒控制器实现四旋翼无人机的降落控制。具体步骤如下：
[0096]
步骤1)确定四旋翼无人机的坐标系定义；
[0097]
小型无人直升机坐标系定义主要涉及两个坐标系，惯性坐标系{i}＝{o
i
，x
i
，y
i
，z
i
}和机体坐标系{b}＝{o
b
，x
b
，y
b
，z
b
}，其中o
i
(i＝i，b)表示坐标系原点，x
i
，y
i
，z
i
(i＝i，b)分别对应坐标系三个主轴方向的单位矢量，各坐标系的定义均遵循右手定则，同时定义直升机姿态角在坐标系{i}下表示为η(t)＝[φ(t)，θ(t)，ψ(t)]
t
，φ(t)，θ(t)，ψ(t)分别对应滚转角、俯仰角和偏航角，目标位置在坐标系{i}下表示为p(t)＝[x
d
(t)，y
d
(t)，z
d
(t)]
t
，x
d
(t)，y
d
(t)，z
d
(t)分别对应目标在惯性坐标系下x轴，y轴，z轴的位置；
[0098]
步骤2)确定四旋翼无人机的动力学模型；
[0099]
通过分析四旋翼无人机作用原理，用拉格朗日方程来描述其动力学模型为：
[0100][0101]
式(1)中e3＝[0 0 1]
t
为z轴向量，v(t)∈r3×1为惯性坐标下的线速度，g∈r为重力加速度，m∈r为无人机的机体质量，f(t)∈r1×1为机体坐标系下四个电机在高度方向产生的总升力，d(t)∈r3×1为降落过程中地面效应造成的扰动力，ω(t)∈r3×1为机体坐标系下的角速度，[ω(t)]
×
∈r3×3为ω(t)张成的反对称矩阵，τ(t)∈r3×1为机体坐标系下的转矩。r为机体坐标系到惯性坐标系的旋转矩阵，如式(2)所示。其中s(
·
)、c(
·
)分别代表三角函数sin(
·
)与cos(
·
)，无人机的转动惯量矩阵为j＝diag([j
x j
y j
z
])。
[0102][0103]
为了便于后续的控制设计，定义辅助控制输出信号u(t)∈r3×1和辅助变量d(t)∈r3×1如下所示：
[0104][0105]
假设1地面效应造成的扰动存在上确界sup(d)。
[0106]
步骤3)设计非线性终端滑模扰动观测器对地面效应进行估计；
[0107]
定义x1＝p，式(1)中的前两个等式可改写为
[0108][0109]
设计滑模面s1(t)∈r3×1为如下形式：
[0110]
s1＝z
‑
x2ꢀꢀꢀꢀ
(5)
[0111]
其中z(t)∈r3×1为非线性扰动观测器辅助变量。定义扰动观测器估计值为设计扰动观测器如下所示：
[0112][0113]
式(6)中p0，q0∈o为正奇数，并且p0＜q0，参数k，ε，β∈r为正实数，β＞sup(d)。定义扰动估计误差为
[0114][0115]
引理1假设存在连续正方程v(t)满足下列条件：
[0116][0117]
则v(t)能在有限时间t
s
内到达特征点
[0118][0119]
其中α＞0，λ＞0，0＜γ＜1。
[0120]
根据引理1可得，形如(4)
‑
(7)式的非线性终端滑模扰动观测器，扰动估计误差可在有限时间内收敛到0，收敛时间t
so
如下所示：
[0121][0122]
步骤4)设计快速终端滑模控制器；
[0123]
四旋翼无人机位置环动力学方程如(4)所示，设计快速终端滑模制器的滑模面s2(t)为：
[0124][0125]
在式(11)中，e(t)为无人机位置控制误差，其定义为：
[0126]
e＝p
d
‑
p
ꢀꢀꢀꢀ
(12)
[0127]
p
d
(t)∈r3×1为无人机期望位置，α1，ξ1∈r为正实数，μ1，v1∈o为正奇数，且μ1＜v1。控制器u(t)设计如下所示：
[0128][0129]
式(13)中α2，ξ2∈r为正实数，μ2，v2∈o为正奇数，并且有μ2＜v2。
[0130]
对于式(11)
‑
(13)中提出的控制器设计，在满足相应的参数要求条件，包括μ1＜v1和μ2＜v2时，误差e(t)会在有限时间内收敛到0，收敛时间t
sc
如下所示：
[0131][0132]
为验证本文提出本发明的基于非线性扰动观测器的四旋翼无人机地面效应补偿降落控制器的有效性，首先通过matlab仿真验证本文设计非线性扰动观测器的有效性，其次搭建实验平台进行实验验证。
[0133]
一、仿真实验验证
[0134]
由于实验中地面效应带来的扰动无法直接通过传感器获得，所以无法验证估计出的扰动是否准确。为此，首先使用matlab中的simulink平台进行仿真验证.给定扰动d(t)的形式为d(t)＝[5sin(3πt)m，5sin(3πt)m，5sin(3πt)m]
t
。四旋翼无人机仿真降落控制系统相关参数如表1所示。
[0135]
表1数值仿真相关参数
[0136]
变量名称参数值变量名称参数值m1p05g9.8α15j
x
0.082ξ110j
y
0.0845μ15j
z
0.1377v17k10α20.2β2ξ20.1ε50μ21q09v23
[0137]
系统内初始状态的初始值都为0，扰动从t＝0秒开始加入。由于三个方向的估计结果都相同，下面仅介绍高度方向的估计结果。扰动观测器估计的扰动值和给定扰动值d
z
(t)的曲线如图2所示。从仿真结果可以看出，对于仿真实验中给定的快速时变扰动，扰动观测器估计到的扰动值可以快速准确地跟踪给定扰动d(t)，即可以快速的收敛到0。证明本文设计非线性扰动观测器的有效性。
[0138]
二、实验平台介绍
[0139]
本文中使用的实验平台由一台pc计算机、一架四旋翼无人机以及运动捕捉系统构成。四旋翼无人机由轴距为0.38m的机架、pixhawk飞行控制器、树莓派4b计算板和电池电机电调动力系统组成。运动捕捉系统实时获取四旋翼无人机的位置信息，通过以太网与pc机连接，以250hz的频率将消息发送到pc机上。树莓派计算板上运行ros系统，通过串口与飞控相连。当pixhawk飞行控制器切换到offboard模式时，四旋翼无人机平台运行树莓派中本文设计的控制算法。pc机接收到无人机位置信息后通过wifi发送给树莓派计算板。无人机质量1.473kg，螺旋桨直径0.228m。如图3所示。实验中的相关参数如表2所示：
[0140]
表2飞行实验相关参数
[0141]
变量名称参数值变量名称参数值m1.473p03g9.8α13δ0.5ξ13ρ0.2μ13k
s
8v15k10α24β20ξ26
ε40μ23q07v25
[0142]
三、实验验证
[0143]
下面在搭建的四旋翼无人机实验平台上设计实验，验证本发明设计的非线性扰动观测器与快速终端滑模控制器在实际实验中的有效性及实用性。本节分为两个部分，第一部分是高度通道的轨迹跟踪实验。分别采用不加扰动观测器的快速终端滑模控制算法(实验中标记为ftsmc，即u
c
(t))、加扰动观测器的快速终端滑模控制算法(实验中标记为d0
‑
ftsmc，即u(t))、不加扰动观测器的常规滑模控制算法(实验中标记为smc，即u
s
(t))和加扰动观测器的常规滑模控制算法(实验中标记为do
‑
smc，即u
so
(t))进行实验验证，证明本文提出的扰动观测器
‑
快速终端滑模控制算法可以快速精确地跟踪给定轨迹并补偿地面效应扰动；第二部分是四旋翼无人机的降落实验，与第一部分所用控制算法相同，证明本文提出的控制算法可以缩短降落时间并提高降落控制精度。
[0144]
第一部分的实验结果如图4，图5，图6所示。图4与图5分别是有地面效应补偿的四旋翼无人机轨迹跟踪高度曲线图和无地面效应补偿的四旋翼无人机轨迹跟踪高度曲线图。对比可知，本发明设计的非线性扰动观测器可以有效的估计地面效应，应用本发明提出的控制策略使无人机可以在近地面精确地跟踪给定轨迹。相比于普通滑模控制算法，本发明提出的算法颤震现象更小，轨迹的跟踪精度更高，证明本发明提出的控制算法可以对地面效应进行良好的补偿，对时变的轨迹也可以精确快速地跟踪。第二部分的实验结果如图7，图8，图9所示。图7与图8分别是有地面效应补偿的四旋翼无人机降落控制高度曲线图和无地面效应补偿的四旋翼无人机降落控制高度曲线图。对比可知，本发明提出的算法可以有效快速的补偿地面效应带来的扰动，使四旋翼无人机成功降落。对比本发明提出的控制算法与常规滑模控制算法可以看出，常规滑模控制算法在降落成功后，由于颤震现象使无人机多次脱离降落点，降落效果较差。证明本发明提出的控制算法可以很好的补偿地面效应，完成降落任务。
[0145]
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。