一种变负载下四旋翼无人机的高度控制方法与流程

：
1.本发明属于四旋翼无人机位置控制技术领域，具体涉及一种变负载下四旋翼无人机的高度控制方法。


背景技术：

2.近年来，随着科学技术的进步，机器人制造业得到了迅速的发展，四旋翼作为空中机器人的一种，以其价格低廉、结构精巧、可操作性强等特点吸引了大量学者的研究。由于其具有垂直起降功能，对飞行场地要求不高，因此它可以适应于各种复杂的场景：如空地协同、航空摄影、军事侦察、救险救灾、快递送餐等，而这些应用都离不开对四旋翼的高度控制。四旋翼是依靠四个螺旋桨的升力来克服自身的重力，在空中稳定飞行。但在实际应用中，四旋翼常常需要悬挂负载飞行，比如无人机运输货物，当给四旋翼悬挂一个未知负载，并且在加载与投放负载或者负载质量变化时(忽略负载摆动造成的干扰，考虑纵向高度)，则相当于四旋翼整体质量发生变化。而稳定飞行过程中螺旋桨的转速与四旋翼整体质量本身有对应关系，质量瞬时的改变势必会对高度控制产生大的扰动，此时，我们仍希望四旋翼能够稳定地保持在一定高度或快速切换高度，精确地跟踪期望轨迹。但是对于这种瞬时的扰动采用传统的pid控制已经不能满足人们的需求，因此设计一个鲁棒性强的高度控制器势在必行。但四旋翼本身是一个非线性、强耦合、多变量的系统，如果再加上未知负载的干扰，控制的难度将大为增加。而自抗扰控制不依赖于精确的数学模型，能够实时估计和补偿内外部扰动，对四旋翼这样复杂的系统有先天的控制优势。因此，基于自抗扰控制的方法来解决变负载下的四旋翼高度控制是一个绝佳的选择。
3.自抗扰控制是韩京清先生在20世纪90年代提出的一种基于pid控制、结合现代控制技术等优点的一种新型实用控制技术。其主要由跟踪微分器(td)、扩展状态观测器(eso)和非线性误差反馈控制器(nlsef)组成。跟踪微分器(td)可以安排输入信号的过渡过程，缓冲输入信号突变对设备造成的冲击，并能提取出微分信号供后续控制器使用；扩张状态观测器(eso)作为自抗扰控制技术的核心，可以在不需要精确模型的情况下实时估计内外部扰动；非线性误差反馈控制(nlsef)通过引入非线性函数可以增强控制器的鲁棒性和适应性，并对扩张状态观测器的扰动估计进行补偿，从而实现主动抑制干扰的效果。


技术实现要素：

4.本发明的目的是针对四旋翼无人机在变负载下的高度控制技术存在的不足，提出了一种基于高度
‑
速度双闭环控制框架下的自抗扰高度控制方法，使四旋翼无人机能够在未知负载变动下仍然能保持期望高度或者快速切换高度，精确地跟踪期望轨迹。
5.本发明的技术解决方案为：一种变负载下四旋翼无人机的高度控制方法，所述方法基于高度
‑
速度双闭环控制框架，采用自抗扰算法进行高度控制；其中，所述高度
‑
速度双闭环控制框架由外环高度控制模块和内环速度控制模块构成；所述外环高度控制模块由跟踪微分器、高度非线性控制器和高度测量单元构成；所述内环速度控制模块由扩张状态观
测器、速度非线性误差反馈控制器和速度测量单元构成。所述高度
‑
速度双闭环控制框架通过如下步骤实现变负载下四旋翼无人机的高度控制：
6.步骤1：为简化分析，通过采用欧拉拉格朗日方法对无人机悬挂负载系统进行二维建模，生成高度模型，即：
[0007][0008]
其中：z(t)表示四旋翼高度，u(t)表示控制输入，b0表示可调控制增益，f(t)表示四旋翼无人机所受的总扰动；
[0009]
步骤2：所述二阶跟踪微分器根据高度模型对输入的高度信号安排过渡过程；
[0010]
步骤3：所述扩张状态观测器根据高度模型中的总扰动进行实时估计；
[0011]
步骤4：所述内环速度非线性误差反馈控制器对纵向速度进行控制并对估计出的总扰动进行实时补偿；
[0012]
步骤5：所述外环高度非线性控制器通过引入非线性函数进行设计从而保证高度收敛的速度与精度。
[0013]
进一步，所述高度模型建立过程：
[0014]
四旋翼无人机与悬挂负载之间的位置关系可表示为：
[0015][0016]
其中，(x
q
(t)，z
q
(t))，(x
l
(t)，z
l
(t))分别是四旋翼无人机和负载在地理坐标系中水平\方向和竖直z方向的位置；l是绳长；α(t)是负载摆动的角度。
[0017]
整个系统的总动能可表示为：
[0018]
e
k
(t)＝e
q
(t) e
l
(t)
[0019]
其中四旋翼无人机动能：
[0020][0021]
负载动能：
[0022][0023]
i
θ
(t)是四旋翼无人机的转动惯量；θ(t)是四旋翼无人机的俯仰角；m
q
和m
l
分别是四旋翼和负载的质量。
[0024]
整个系统的总势能可表示为
[0025]
e
p
(t)＝m
q
gz
q
(t) m
l
g(z
q
(t)
‑
lcosα(t))
[0026]
其中，g为重力加速度。综合上述方程可以得到拉格朗日方程为：
[0027]
[0028]
则四旋翼悬挂负载整个模型的运动方程可表示为：
[0029][0030][0031][0032]
其中f(t)为四旋翼螺旋桨产生的总升力，τ(t)为沿机体坐标的水平方向的旋转力矩。
[0033]
四旋翼不挂负载时有：
[0034][0035][0036][0037]
考虑高度方向，并将z
q
(t)表示为z(t)，则带负载的高度模型可表示为：
[0038][0039]
令：
[0040][0041][0042]
其中u(t)是控制输入，b0是可调控制增益，f(t)表示四旋翼无人机所受的总扰动，包括悬挂负载后对四旋翼高度模型的改变、重力加速度及负载摆动的影响。因此最终高度模型可以重写为：
[0043][0044]
进一步，所述二阶跟踪微分器为：
[0045][0046]
其中：v1(t)为输出的跟踪信号，v2(t)为跟踪信号的微分信号，r为速度因子，r越大，则跟踪的越快，v1(t)也越接近于原始输入信号；a，b均为可调参数，a为幅值增益，可以调节函数的幅值大小，a越大，幅值越大；b为指数因子，可以调节函数线性区段的范围，b越大，线性区段越窄。
[0047]
进一步，所述扩张状态观测器根据高度模型估计总扰动过程：
[0048]
令高度模型中将f(t)扩张成新的状态变量x2(t)，故f(t)＝x2(t)。考虑实际应用，f(t)是有界可微的，令
同样考虑ω(t)也是有界的。因此对二维高度模型调整为：
[0049][0050]
由此可设计二阶扩张状态观测器：
[0051][0052]
其中，函数z1(t)，z2(t)分别为x1(t)，x2(t)的观测值；β1，β2为扩张状态观测器的可调增益。
[0053]
进一步，所述内环速度非线性误差反馈控制器对纵向速度进行控制并对估计出的总扰动进行实时补偿，其形式为：
[0054][0055]
其中速度误差σ(t)＝χ(t)
‑
z1(t)，χ(t)为外环的输出，同时它为内环的期望速度；k
p1
为可调控制器增益；b0与扩张状态观测器中相对应，可以调节对扰动补偿的强弱。
[0056]
进一步，在外环高度引入非线性函数设计高度控制器保证高度收敛的速度与精度，所述高度非线性控制器形式为：
[0057][0058]
其中，高度误差ζ(t)＝v1(t)
‑
z(t)；高度误差的微分λ1、λ2∈[0，1]，λ1和λ2是决定控制器非线性程度的值，其值越小，非线性度越高；k
p2
和k
d2
是两个可调的控制器增益。
[0059]
有益效果
[0060]
本发明与现有技术相比有如下优点：
[0061]
(1)传统的pid控制算法鲁棒性差、抗干扰能力弱，在外界干扰下不能保证控制的速度与精度。而本发明方法基于双闭环控制框架，通过引入非线性函数改进了经典pid控制算法，提高了控制器的控制精度与响应速度，使控制效率更高，控制性能更好。
[0062]
(2)与大多数现有的控制方法相比，本发明方法不依赖于精确的数学模型，在不需要安装力传感器等设备就可以对系统内外部总扰动实时进行估计与补偿，实现主动抑制干扰的效果。
[0063]
(3)本发明方法在外环设计了跟踪微分器来安排过渡过程，可以避免输入信号突变对设备造成的冲击，解决快速与超调的矛盾，同时能扩大参数适应性。
[0064]
(4)本发明方法相对其他控制技术，控制结构简单、参数调整方便、实践性强、可操作性高，能够实现变负载扰动下四旋翼无人机的高性能高度控制。
附图说明：
[0065]
图1为四旋翼无人机高度控制结构图；
[0066]
图2为四旋翼无人机悬挂负载二维模型图；
[0067]
图3为四旋翼无人机实验平台图；
[0068]
图4为恒定高度下四旋翼无人机变负载实验结果图；
[0069]
图5为恒定高度下eso与控制器输出结果图；
[0070]
图6为四旋翼无人机变负载下轨迹跟踪实验结果图。
具体实施方式：
[0071]
以下结合附图和具体实施例对本发明进行详细说明。
[0072]
如图1给出了整个系统的控制结构图。本发明基于高度
‑
速度双闭环控制框架采用自抗扰控制算法来解决变负载下四旋翼飞行器的高度控制问题。所述高度
‑
速度双闭环控制框架由外环高度控制模块和内环速度控制模块构成：所述外环高度控制模块由跟踪微分器、高度非线性控制器和高度测量单元构成；所述内环速度控制模块由扩张状态观测器、速度非线性误差反馈控制器和速度测量单元构成。本发明通过高度
‑
速度双闭环控制框架实现变负载下四旋翼无人机的高度控制过程：
[0073]
首先，为了减小期望高度信号突变造成的不利影响，外环采用跟踪微分器td来对输入的高度位置信号安排过渡过程；其次，针对实际应用中未知负载带来的扰动，在内环设计扩张状态观测器(eso)对内外部总扰动进行实时估计，并在内环速度非线性误差反馈控制器中补偿；然后在外环设计高度非线性控制器，保证四旋翼高度收敛的速度与精度；最终实现四旋翼无人机在变负载下仍然能够稳定地保持在期望高度或者快速切换高度，精确地跟踪期望轨迹。具体过程：
[0074]
步骤1：为简化分析，用欧拉拉格朗日方法对四旋翼无人机悬挂负载进行二维建模，生成高度模型：
[0075]
如图2所示，四旋翼无人机与悬挂负载之间的位置关系可表示为：
[0076][0077]
其中，(x
q
(t)，z
q
(t))，(x
l
(t)，z
l
(t))分别是四旋翼无人机和负载在地理坐标系中水平x方向和竖直z方向的位置；l是绳长；α(t)是负载摆动的角度。
[0078]
整个系统的总动能可表示为：
[0079]
e
k
(t)＝e
q
(t) e
l
(t)
[0080]
其中四旋翼无人机动能：
[0081][0082]
负载动能：
[0083][0084]
i
θ
(t)是四旋翼无人机的转动惯量；θ(t)是四旋翼无人机的俯仰角；m
q
和m
l
分别是
四旋翼和负载的质量。
[0085]
整个系统的总势能可表示为
[0086]
e
p
(t)＝m
q
gz
q
(t) m
l
g(z
q
(t)
‑
lcosα(t))
[0087]
其中，g为重力加速度。综合上述方程可以得到拉格朗日方程为：
[0088][0089]
则四旋翼悬挂负载整个模型的运动方程可表示为：
[0090][0091][0092][0093]
其中f(t)＝f1(t) f2(t)，表示四旋翼螺旋桨产生的总升力；τ(t)＝(f1(t)
‑
f2td，表示沿机体坐标水平方向的旋转力矩。
[0094]
四旋翼不挂负载时有：
[0095][0096][0097][0098]
考虑高度方向，并将z
q
(t)表示为z(t)，则带负载的高度模型可表示为：
[0099][0100]
令：
[0101][0102][0103]
其中u(t)是控制输入，b0是可调控制增益，f(t)表示四旋翼无人机所受的总扰动，包括悬挂负载后对四旋翼高度模型的改变、重力加速度及负载摆动的影响。因此最终高度模型可以重写为：
[0104][0105]
步骤2：所述二阶跟踪微分器根据高度模型对输入的高度信号安排过渡过程；
[0106]
跟踪微分器形式为：
[0107][0108]
其中v1(t)为输出的跟踪信号，v2(t)为跟踪信号的微分信号，r为速度因子，r越大，则跟踪的越快，v1(t)也越接近于原始输入信号；a，b均为可调参数，a为幅值增益，可以调节函数的幅值大小，a越大，幅值越大；b为指数因子，可以调节函数线性区段的范围，b越大，线性区段越窄。
[0109]
步骤3所述扩张状态观测器根据高度模型中的总扰动进行实时估计；
[0110]
令高度模型中将f(t)扩张成新的状态变量x2(t)，故f(t)＝x2(t)。考虑实际应用，f(t)是有界可微的，令同样考虑ω(t)也是有界的。所以高度模型可以重新表示为：
[0111][0112]
因此，可设计二阶扩张状态观测器：
[0113][0114]
其中，函数z1(t)，z2(t)分别为x1(t)，x2(t)的观测值；β1，β2为扩张状态观测器的可调增益。
[0115]
步骤4：所述内环速度非线性误差反馈控制器对纵向速度进行控制并对估计出的总扰动进行实时补偿；
[0116]
定义速度误差σ(t)＝χ(t)
‑
z1(t)，其中χ(t)为外环的输出，同时它为内环的期望速度。所以速度非线性误差反馈控制器的形式为：
[0117][0118]
其中k
p1
为可调控制器增益；b0与扩张状态观测器中相对应，可以调节对扰动补偿的强弱。
[0119]
步骤5：通过引入非线性函数设计所述外环高度非线性控制器，从而保证高度收敛的速度与精度。外环高度非线性控制器形式为：
[0120][0121]
其中，高度误差ζ(t)＝v1(t)
‑
z(t)；高度误差的微分λ1、λ2∈[0，1]，λ1和λ2是决定控制器非线性程度的值，其值越小，非线性度越高；k
p2
和k
d2
是两个可
调的控制器增益。其实质上是一个非线性的pd控制器，但非线性函数的引入，使其控制效率更高，控制性能更好。
[0122]
实施例
[0123]
如图3所示，本发明基于自主搭建的四旋翼无人机作为实验平台，该平台主要由一个基于stm32的飞控和框架为x250的四旋翼无人机、wfly系列遥控器、pc地面站组成。
[0124]
实验中首先将四旋翼稳定在130cm高度，在稳定飞行时突加一个重量为0.125kg的负载，待其收敛时撤去负载。从图4可以看出，四旋翼在负载变化的干扰下能够在2秒左右快速收敛。图5.(a)展示了z1对反馈速度的跟踪效果，可以看出z1几乎和实际的反馈速度x1重合，说明实际的反馈速度x1能够被观测器eso精确估计；图5.(b)为eso对四旋翼高度总扰动的估计，从图中可以看出z2的值会随着负载变化引起的扰动而变化；总扰动的估计值在内环非线性反馈控制器中得以补偿，如图5.(c)所示，可以看出，控制器u的值会随着总扰动z2的变化而实时变化。
[0125]
此外，为验证本发明在变负载下四旋翼无人机的高度轨迹能力，用该无人机对幅度为40cm(最低值为90cm最高值为130cm)的矩形波进行了跟踪，结果如图6所示。从图中可以看出，四旋翼无人机能够迅速克服负载变化引起的扰动，并快速跟踪上期望轨迹，并且在td安排的过渡过程作用下，能够有效缓解信号突变造成的冲击，此外由于本发明中的扩张状态观测器估计扰动时将重力加速度也作为扰动补偿掉，因此其上升和下降的收敛速度都为1.5秒左右，避免了重力作用对高度控制的干扰。