中国科技核心期刊      中国指挥与控制学会会刊     军事装备类重点期刊
指挥控制与仿真

指挥控制与仿真 >

2024 , Vol. 46 >Issue 2: 104 - 114

DOI: https://doi.org/10.3969/j.issn.1673-3819.2024.02.015

指挥与控制

基于MPC和ESO-DO的四旋翼轨迹跟踪控制*

  • 孙嫚憶 ,
  • 毕文豪 ,
  • 张安 ,
  • 刁玉豪
展开
  • 西北工业大学, 陕西 西安 710072

孙嫚憶(1994—),女,硕士研究生,研究方向为无人机控制。

毕文豪(1986—),男,博士,副研究员。

Office editor: 张培培

收稿日期: 2023-02-14

  修回日期: 2023-03-29

  网络出版日期: 2024-04-01

基金资助

国家自然科学基金项目(62073267)

国家自然科学基金项目(61903305)

收起

Trajectory tracking control for quadrotor based on MPC and ESO-DO

  • SUN Manyi ,
  • BI Wenhao ,
  • ZHANG An ,
  • DIAO Yuhao
Expand
  • Northwestern Polytechnical University, Xi’an 710072, China

Received date: 2023-02-14

  Revised date: 2023-03-29

  Online published: 2024-04-01

Fold

摘要

针对扰动作用和模型不确定性下四旋翼无人机精确轨迹跟踪控制问题,提出了一种主动干扰抑制和模型预测控制(Model Predictive Control,MPC)策略。模型预测控制器通过扩张状态观测器(Extended State Observer, ESO)和扰动观测器(Disturbance Observer,DO)来估计和补偿干扰,从而实现位置环精确控制。在存在外部干扰和参数不确定性的情况下,通过仿真实验,证明了所提出的方法提高了对建模误差和干扰的鲁棒性,同时实现了对参考轨迹的平滑跟踪。

关键词： 四旋翼无人机; 模型预测控制; 扩张状态观测器; 扰动观测器; 轨迹跟踪

本文引用格式

孙嫚憶 , 毕文豪 , 张安 , 刁玉豪 . 基于MPC和ESO-DO的四旋翼轨迹跟踪控制*[J]. 指挥控制与仿真, 2024 , 46(2) : 104 -114 . DOI: 10.3969/j.issn.1673-3819.2024.02.015

Abstract

An active disturbance suppression and model predictive control (MPC) strategy is proposed for the quadrotor precision trajectory tracking control problem under disturbance action and model uncertainty. The model predictive controller estimates and compensates the disturbance by the extended state observer (ESO) and disturbance observer (DO) to achieve the position loop accurate control. In the presence of external disturbances and parameter uncertainties, it is demonstrated through simulation experiments that the proposed method improves the robustness to modeling errors and disturbances, while achieving smooth tracking of the reference trajectory.

Key words： quadrotor; model predictive control; expansive state observer; disturbance observer; trajectory tracking

近年来,在机器人领域,多旋翼特别是四旋翼飞行器正在迅速普及,四旋翼飞行器已经成为机器人研究的标准平台,由于其控制简单,质量轻巧,反应敏捷,被全球大学、企业和科研单位持续关注,广泛应用在国防和国民经济建设的各个领域[1]。在军用方面,可用于执行空中预警、目标侦察与打击和电子干扰[2]等战术任务;在民用领域,可用于农林植保[3]、森林防火[4]、三维测绘[5]、油气管道勘探[6]等各个方面。
四旋翼无人机系统是一个强耦合、欠驱动的非线性系统,其控制问题是研究的热点。近年来相关的控制方法有很多,如PID控制法[7]、反步法[8]、滑模控制[9]、LQR控制[10]、自适应控制[11]等。Moreno等[12]提出了用非线性PID控制器来实现四旋翼的轨迹跟踪,利用Lyapunov函数证明了闭环控制系统的有界性,在确保跟踪期望位置、姿态准确的情况下,确定了增益调整准则,仿真证明了该控制方法的有效性。Muñoz等[13]采用了三个二阶滑模控制器(超扭滑模控制器,改进的超扭滑模式控制器和非奇异终端超扭滑模态控制器)来实现四旋翼飞机的高度跟踪,减少了抖振现象,同时保证了系统的鲁棒性。Chen等[14]针对轨迹跟踪过程中存在的外部扰动,利用鲁棒自适应跟踪控制器来补偿扰动,仿真结果表明,该控制器对轨迹跟踪过程中存在的扰动不确定性的情况具有较强的鲁棒性。
模型预测控制[15](Model Predictive Control, MPC)的机理可以描述为:在每一采样时刻,根据获得的当前测量信息,在线求解一个有限时域开环优化问题,并将得到的控制序列的第一个元素作用于被控对象;在下一个采样时刻,重复上述过程,用新的测量值刷新优化问题并重新求解,在线求解开环优化问题获得开环优化序列是模型预测控制和传统控制方法的主要区别,后者通常是离线求解一个反馈控制律,并将得到的反馈控制律作用于系统。
本文针对扰动作用和模型不确定性下四旋翼无人机精确轨迹跟踪控制问题,提出了一种主动干扰抑制和模型预测控制策略,将基于误差的状态空间预测控制策略应用于外环的位置控制。为了获得更好的控制效果,本文将四旋翼在飞行过程中受到的扰动进行分类,在前馈的位置引入扩张状态观测器(Extended State Observer, ESO)和扰动观测器(Disturbance Observer, DO)对扰动进行针对性补偿,从而提高了控制器抑制干扰的能力;在存在外部干扰和参数不确定性的情况下,通过仿真实验,证明本方法提高了对建模误差和干扰的鲁棒性,同时实现了对参考轨迹的平滑跟踪。

1 四旋翼无人机动力学模型

四旋翼无人机是一个典型的多输入多输出、非线性、强耦合、欠驱动的复杂系统。因此,需要在表征出四旋翼飞行器运动特性的基础上,简化其数学模型。本文以“X”型四旋翼为例,假设在地面坐标系O-XeYeZe下,飞行器相对于原点的坐标为Γ=[x y z],欧拉角为Θ=[ϕ θ ψ],在机体坐标系O-xbybzb下,飞行器转动的角速度为ω=[ωx ωy ωz],四旋翼无人机结构如图1所示。
图1 四旋翼飞行器结构图

Fig.1 Quadrotor structure diagram

四旋翼飞行器的动力学方程可表述为如下形式:
  x ¨ = ( s i n ψ s i n ϕ + c o s ψ s i n θ c o s ϕ ) f m y ¨ = ( s i n ψ s i n θ c o s ϕ - c o s ψ s i n ϕ ) f m z ¨ = c o s θ · c o s ϕ · f m - g
ω ˙ x = J y y - J z z J x x ω y ω z - G a , ϕ J x x + τ x J x x ω ˙ y = J z z - J x x J y y ω x ω z - G a , θ J y y + τ y J y y ω ˙ z = J x x - J y y J z z ω x ω y + τ z J z z
式中,m为飞行器的质量,f为螺旋桨产生的总拉力,τx、τy、τz为螺旋桨上产生的力矩, 具体表示为
f = C T ( ω ˜ 1 2 + ω ˜ 2 2 + ω ˜ 3 2 + ω ˜ 4 2 ) τ x = 2 2 d C T ( ω ˜ 1 2 - ω ˜ 2 2 - ω ˜ 3 2 + ω ˜ 4 2 ) τ y = 2 2 d C T ( ω ˜ 1 2 + ω ˜ 2 2 - ω ˜ 3 2 - ω ˜ 4 2 ) τ z = C m ( ω ˜ 1 2 - ω ˜ 2 2 + ω ˜ 3 2 - ω ˜ 4 2 )
Jxx、Jyy、Jzz分别为四旋翼无人机沿xb轴、yb轴、zb轴的转动惯量,Ga=[ G a , ϕ G a , θ G a , ψ]T∈R3是电机在产生俯仰和滚转动作的陀螺仪扭矩,可以表示为
G a , ϕ = J m ω y ( ω ˜ 1 - ω ˜ 2 + ω ˜ 3 - ω ˜ 4 ) G a , θ = J m ω x ( - ω ˜ 1 + ω ˜ 2 - ω ˜ 3 + ω ˜ 4 ) G a , ψ = 0
其中, ω ˜ i(i=1、2、3、4)表示四个螺旋桨的转速, C T ∈ R +是推力系数,Cm∈R+是扭矩系数,d∈R+表示飞行器质心到旋翼旋转轴之间的距离,Jm∈R+是总惯性矩。

2 基于MPC的四旋翼轨迹跟踪

针对四旋翼飞行过程中所面临的外部干扰、参数不确定性和未建模动力学的情况,本文提出了基于误差模型的状态空间预测控制方法进行轨迹跟踪,控制结构如图2所示。
图2 四旋翼飞行器控制结构图

Fig.2 Control structure diagram of quadrotor

首先,由轨迹生成器模块离线提供平移运动的参考轨迹。从轨迹跟踪的期望路线xr、yr、zr及其导数开始,计算参考控制输入f、τx、τy。偏航角期望值单独定义。该轨迹是在以下假设条件下生成的:
1) 四旋翼飞行器不存在外部干扰;
2) 四旋翼飞行器的姿态是稳定的。
在提供期望状态轨迹的基础上,提出了一种预测控制器来实现四旋翼外环位置的平移运动。基于误差模型的状态空间预测控制器在状态向量中包含位置误差的积分,以便在考虑实时干扰时实现零稳态误差。
位置控制器分为两个阶段。第一阶段中,依据四旋翼的期望高度,求解控制输入f的大小,f为螺旋桨产生的总推力。在第二阶段,依据x、y的大小解算出俯仰角和滚转角的期望值。在这一阶段,控制变量f被视为时变参数。由期望俯仰角、偏航角和滚转角的大小,计算出作用于三个轴上的扭矩输入,从而实现四旋翼的内环稳定。
基于误差模型的状态空间预测控制器,根据误差模型,设计两个预测控制器,第一个控制器依据四旋翼的期望高度,求解控制输入f的大小。第二个控制器依据x、y的大小,解算俯仰角和滚转角的期望值。
系统状态可以用向量表示为ξ(t),则有:ξ(t)=[x(t) vx(t) y(t) vy(t) z(t) vz(t)]T,其中vx、vy、vz是四旋翼质心的线速度,w1(t)、w2(t)、w3(t)是外部扰动。根据式(1)可以将四旋翼位置子系统写成如下形式:
ξ ˙(t)= v x ( t ) u x ( t ) f ( t ) m + w 1 ( t ) v y ( t ) u y ( t ) f ( t ) m + w 2 ( t ) v z ( t ) - g + c o s θ ( t ) c o s ϕ ( t ) f ( t ) m + w 3 ( t )
其中,
u x ( t ) = c o s ψ ( t ) s i n θ ( t ) c o s ϕ ( t ) + 　 　 s i n ψ ( t ) s i n ϕ ( t ) u y ( t ) = s i n ψ ( t ) s i n θ ( t ) c o s ϕ ( t ) - 　 　 c o s ψ ( t ) s i n ϕ ( t )
系统(4)可以解耦为两个子系统,这两个子系统分别控制x-y平面和z方向运动。对于z方向的高度子系统,模型如下所示:
ξ ˙ z(t)=   v z ( t ) - g + c o s θ ( t ) c o s ϕ ( t ) f ( t ) m + w 3 ( t )
其中,ξz(t)= z ( t ) v z ( t ) T。
由轨迹生成器(见图2)离线生成期望轨迹。轨迹是时变的,因此本文提出了一种具有与四旋翼相同动力学特性的虚拟参考飞行器。假设虚拟参考飞行器没有外部干扰,参考飞行器的动力学可以用以下形式表示:
ξ ˙ r z(t)= v z   r ( t ) - g + c o s θ ( t ) c o s ϕ ( t ) f r m
其中, ξ r z(t)= z r ( t ) v z   r ( t ) T。
则可以获得控制输入:
fr(t)= m ( z ¨ r ( t ) + g ) c o s θ ( t ) c o s ϕ ( t )
通过从位置系统(6)减去虚拟系统(7),可以得到位置误差模型:
ξ · ~ z(t)= v ˜ z ( t ) c o s θ ( t ) c o s ϕ ( t ) u ˜ z ( t ) m + w 3 ( t )
其中, ξ ˜ z(t)=ξz(t)- ξ r z(t)表示位置误差向量, v ˜ z= v ˜ z(t)- v ˜ z r(t)表示速度误差, u ˜ z(t)=f(t)-fr(t)表示控制输入误差。
下面设计预测高度控制器,考虑以下形式的标称系统:
ξ · ~ z(t)= v ˜ z ( t ) c o s θ ( t ) c o s ϕ ( t ) u ˜ z ( t ) m
首先,将状态误差向量定义为
xz(t)= ξ ˜ z(t)= z ˜ ( t ) v ˜ z ( t )= z ( t ) - z r ( t ) v z ( t ) - v z r ( t )
将系统离散化,可以得到
xz(k+1)=Az·xz(k)+Bz(k)· u ˜ z(k)
Δt是采样时间,通常选择足够小的采样时间,以便更好地获得轨迹跟踪的动态误差。横滚角、俯仰角、偏航角(ϕ,θ,ψ)被视为时变参数。
设计控制律使得所定义的代价最小:
Jz=[ x ^ z- x ^ r z]TQz[ x ^ z- x ^ r z]+
[ u ˜ ^ z- u ˜ ^ r z]TRz[ u ˜ ^ z- u ˜ ^ r z]+
Ω[ x ^ z(k+ N 2 z|k)- x ^ r z(k+ N 2 z|k)]
其中Qz∈ R N 2 z × N 2 z,Rz∈ R N u z × N u z是对角正权重矩阵, N 2 z是预测范围, N u z是控制范围, x ^ z和 u ˜ ^ z形式如下:
x ^ z≜ x ^ z ( k + 1 k ) ︙ x ^ z ( k + N 2 z k ), u ˜ ^ z≜ u ˜ ^ z ( k | k ) ︙ u ˜ ^ z ( k + N u z - 1 k )
参考矢量为
x ^ r z ≜ x r z ( k + 1 k ) - x r z ( k | k ) ︙ x r z ( k + N 2 z k ) - x r z ( k | k ) u ˜ ^ r z ≜ f r ( k | k ) - f r ( k - 1 k ) ︙ f r ( k + N u z - 1 k ) - f r ( k - 1 k )
Ω[ x ^ z(k+ N 2 z|k)- x ^ r z N 2 z|k]是定义的终端状态代价函数,其具体形式为
Ω[ x ^ z(k+ N 2 z|k)- x ^ r z(k+ N 2 z|k)]=
[ x ^ z(k+ N 2 z|k)- x ^ r z(k+ N 2 z|k)]T·
Gz[ x ^ z(k+ N 2 z|k)- x ^ r z(k+ N 2 z|k)]
其中,Gz∈ R N 2 z × N 2 z是终端状态的对角权重矩阵[16],Gz>0。
预测的系统输出量 x ^ z(k+j|k)由标称状态空间模型(12)得出,故可得
x ^ z=Pz(k|k)·xz(k)+Hz(k|k)· u ˜ ^ z
其中,
Pz(k|k)≜ A z A z 2 ︙ A z N 2 z  
Hz(k|k)≜ B z ( k | k ) A z B z ( k | k ) ︙ A z N 2 z - 2   B z ( k | k ) A z N 2 z - 1   B z ( k | k )
0 … 0 B ( k + 1 k ) … 0 ︙ ⋱ ︙ A z N 2 z - 3   B z ( k + 1 k ) … 0 A z N 2 z - 2   B z ( k + 1 k ) … B z ( k + N u z - 1 k )
为了计算简便,定义Fz作为Hz(k|k)的最后一行:
Fz≜ A z N 2 z - 1Bz(k|k) A z N 2 z - 2Bz(k+1|k)
… B z(k+ N u z-1|k)
所以
x ^ z(k+ N 2 z|k)= A z N 2 z xz+Fz u ˜ ^ z
将式(13)中的Jz最小化,可以构造如下的控制律,用终端代价函数来加快收敛速度:
u ˜ ^ z 0=[ H T zQzHz+Rz+ F T zGzFz]-1×
[ H T zQz( x ^ r z-Pzxz(k))+Rz u ˜ ^ r z+
F T zGz( x ^ r z(k+ N 2 z|k)- A z N 2 zxz]
控制输入 u ˜ ^ z= u ˜ ^ z 0,在每一时刻k,只将最优控制输入序列的第一分量 u ˜ ^ z(k|k)作用于系统。因此,k时刻高度系统的控制输入为
f(k)= u ˜ ^ z(k|k)+fr(k)
类似地,x-y平面也可以用相同的方法来设计,则平面x-y的控制律为
u ˜ ^ x y 0=[ H T x yQxyHxy+Rxy+ F T x yGxyFxy]-1×
[ H T x yQxy( x ^ r x y(k)-Pxyxxy(k))+Rxy u ˜ ^ r x y+
F T x yGxy( x ^ r x y(k+ N 2 x y|k)- A x y N 2 x y xxy]
Qxy∈ R N 2 x y × N 2 x y,Rxy∈ R N u x y × N u x y是对角正权重矩阵。令控制输入 u ˜ ^ x y= u ˜ ^ x y 0,其他矩阵的计算与前面的矩阵类似。因此控制输入可以写成如下形式:
u x ( k ) u y ( k )= u ˜ x ( k | k ) u ˜ y ( k | k )+ u x r ( k ) u y r ( k )
由式(5)可得
u x ( k ) = c o s ψ ( k ) s i n   θ r ( k ) c o s   ϕ r ( k ) + 　 　 s i n ψ ( k ) s i n   ϕ r ( k ) u y ( k ) = s i n ψ ( k ) s i n   θ r ( k ) c o s   ϕ r ( k ) - 　 　 c o s ψ ( k ) s i n   ϕ r ( k )

3 基于ESO的MPC四旋翼轨迹跟踪

为了提高基于误差模型的状态空间预测控制器的抗干扰能力,在控制器中引入了基于ESO的前馈补偿。ESO是一种特殊的状态观测器,它不仅估计系统的状态,同时估计由系统的未知动态和外部扰动组成的扩张状态。适当设计的观测器可以提供相对准确的估计,从而在控制输入中进行补偿。本文以x-y平面为例,提出控制策略框图如图3所示。
图3 x-y平面上基于ESO的MPC四旋翼轨迹跟踪结构图

Fig.3 Structural diagram of MPC quadrotor trajectory tracking based on ESO on x-y plane

在高度子系统中,将扩张状态向量定义为Xz=[ z z ˙ n z]T,yz=z代表系统输出量。那么式(6)的状态空间形式为
X ˙ z = A g z X z + B g z f + E g z n ˙ z - F g z g y z = C g z X z
其中, A g z= 0 0 0 1 0 0, B g   z= 0 b z 0, E g   z= 0 0 1, F g   z= 0 1 0, bz= 1 mcos θcos ϕ,nz(·)=w3(t)。
因此,三阶线性扩张状态观测器(Linear Extended State Observer, LESO)可以构造如下:
Z ˙ z = A g z Z z + B g z f - F g z g + L g z ( y z - y ^ z ) y ^ z = C g z Z z
其中,Zz=[ z 1 z z 2 z z 3 z]T是观测器的状态向量,提供Xz的估计值, L g z=[ l 1 z l 2 z l 3 z]T为观测器增益向量, y ^ z是yz的输出估计量。
nz(·)是总扰动,包括内部不确定性和外部扰动。以高度控制系统为例,考虑非结构不确定性和参数不确定性,可得到下式:
z ¨(t)=-g+cosθ(t)cosϕ(t) f ( t ) m + Δ m+w3(t)+h(·)=
-g+cos θ(t)cos ϕ(t) f ( t ) m+w3(t)-
cos θ(t)cos ϕ(t) f ( t ) Δ m m ( m + Δ m )+h(·)
其中,Δm是m的不确定参数,h(·)是非结构不确定性,可能与时刻t和状态量x、y、z、ϕ、θ、ψ及其导数有关。
nz(·)≜w3(t)-cosθ(t)cosϕ(t) f ( t ) Δ m m ( m + Δ m )+h(·)是总扰动,可以将上述方程转化为式(25)。在式(25)中,扰动的形式为nz(·)=w3(t)。此外,由于本章假设由阵风引起的位置和角加速度的扰动是可微且有界的,所以,此处nz(·)是可微且有界的。其他方向上与此类似。
对于观测器(26)的设计,对系统信息的唯一要求是确定bz。然而,在某些情况下,四旋翼的动力学并不完全符合模型(1),因此,可以使用估计值 b ^ z代替。实验证明,当(bz- b ^ z)/bz很小时,控制器仍能正常工作,因此可以将(bz- b ^ z)/f视为nz(·)的一部分,由ESO估计得到[17]。
将观测器的带宽定义为 w 0 z>0,观测器的增益可以参数化,得到 L g z=[3 ω 0 z,3 ω 0 z 2, ω 0 z 3]T。
定理1 对于系统(25),如果nz(·)是有界的,根据参数化调整增益 L g z,三阶LESO(26)是有界输入和有界输出(BIBO)稳定的[18]。
证明 假设估计误差Ez=Xz-Zz,将 L g z的取值代入式(26)中,与式(25)相减,误差方程为
E ˙ z= A e zEz+ E g z n ˙ z
其中,
A e z= A g z- L g z C g z= - 3 ω 0 z 1 0 - 3 ω 0 z 2 0 1 - ω 0 z 3 0 0。
A e z的特征多项式:
λ1(s)=s3+3 ω 0 zs2+3 ω 0 z 2 s+ ω 0 z 3=(s+ ω 0 z)3
λ1(s)是Hurwitz稳定的,所有的特征值为 u ˜ ^ z= u ˜ ^ z 0- z 3 z b z,选择适当的 ω 0 z,则所有的极点都位于左半平面内足够远的位置,而且 n ˙ z是有上界的,因此,LESO是BIBO稳定的。
L g z可以用其他形式设计,只要确保 A e z是Hurwitz稳定的。上述方法的目的在于简化参数调优过程,这样就只需要 ω 0 z一个参数。
通过调整好的观测器,使得 z 1 z→z, z 2 z→ z ˙, z 3 z→nz。更重要的是,总扰动nz(·)是由 z 3 z估计的,可以在控制输入中进行补偿,控制输入的形式 u ˜ ^ z表示为
u ˜ ^ z= u ˜ ^ z 0- z 3 z b z
其中 u ˜ ^ z 0可以根据式(20)计算得到,f可以根据式(21)计算得到。
将式(30)代入式(9),可得
ξ · ~ z(t)= v ˜ z ( t ) c o s θ ( t ) c o s ϕ ( t ) u ˜ z 0 ( t ) m + ( w 3 ( t ) - z 3 z ( t ) )
其中,扰动 n ^ z(t)=w3(t)- z 3 z(t),这比调整好的ESO的原始扰动小得多,假设x-y的估计误差可以忽略,则式(31)是与式(10)相同形式的系统。此时,可以通过基于ESO的误差模型的状态空间预测控制方法进行控制,因为扰动要小得多。
对于x-y通道,计算方法一样,则x-y通道的控制输入可以写作:
u ˜ ^ x y= u ˜ x u ˜ y= u ˜ ^ x y 0- 1 b x y z 3 x z 3 y
其中 u ˜ ^ x y 0由式(22)计算得到。

4 基于ESO-DO的MPC四旋翼轨迹跟踪

考虑四旋翼飞行器载荷变化的情况,为了更好地模拟四旋翼飞行状态,本节将四旋翼在飞行过程中的扰动分类处理,加入DO,如图4所示。DO的用途是抑制可以被建模为外生系统的扰动,而ESO可以处理没有任何先验知识的干扰(ni(i=x,y,z))。通过这种方式可以使系统的抗干扰能力得到加强。四旋翼无人机在完成实际任务中受到载荷、风、旋翼阻力、模型不确定性等多种干扰[19]。通常扰动可分为两类:
图4 基于ESO-DO的MPC四旋翼轨迹跟踪

Fig.4 Quadrotor trajectory tracking based on MPC ESO-DO

1)由外生系统建模引起的扰动(例如周期性扰动和斜率扰动),记为dm;
2)导数有界变量的扰动(例如风扰动和模型不确定性),记为ni(i=x,y,z)。
在自然界中,风的干扰是很难准确预测的,特别是它的方向和速度。对于四旋翼无人机来说,如果风的干扰横向侵入,它主要影响无人机的位置。当风向上或向下吹时,则主要影响无人机的姿态,使得无法与质心准确对齐[20]。在大多数情况下,风扰动对四旋翼无人机的影响是前面两种情况的结合,其变化可以认为是有界的。因此,可以认为干扰主要分为两种类型:力干扰部分和扭矩干扰部分。为了方便计算,这部分干扰视为整体,记作ni(i=x,y,z)。在不丧失一般性的前提下,假设四旋翼无人机所受到的风扰动具有一定的变化, n ˙ i的上界用正标量 n - i表示,即‖ n ˙ i‖≤ n - i。
定义位置误差和线速度误差为
e γ = γ d - γ e v = v d - v
其中,γd和vd是四旋翼无人机的期望轨迹和速度,γ和v是当前位置和速度。
DO的设计理念类似于状态观测器[21],可以估计动态系统的不可测状态。则根据DO估计dm可以得到如下式子:
ζ · ^ = A ζ ^ + l ( γ , v ) [ γ ˙ T , v ˙ T ] T - 　 　 l ( γ , v ) ( f ( v ) + G F + G n ^ i + G d ^ m ) d ^ m = B ζ ^
其中,l(γ,v)是DO的增益函数, ζ ^和 d ^ m分别是ζ和dm的估计值, n ^ i是通过ESO得到的ni的估计值,G=1/m[03×3,I3×3]T,f(v)=[vT,0,0,-g]T,对速度导数v的测量是有噪声的,在实践中使用会带来不必要的影响。为了规避这个问题,引入了辅助状态变量z和辅助函数p(γ,v):
z = ζ ^ - p ( γ , v ) p ( γ , v ) = ∫ l ( γ , v ) [ γ ˙ T , v ˙ T ] T d t
将式(36)代入式(35),可得到DO:
z ˙ = ( A - l ( γ , v ) G B z + A p ( γ , v ) - 　 　 l ( γ , v ) ( G B p ( γ , v ) + f ( v ) + G F + G n ^ i ) ζ ^ = z + p ( γ , v ) d ^ m = B ζ ^
观察器增益l(γ,v)与辅助函数p(γ,v)的关系为
l(γ,v)= ∂ p ( γ , v ) ∂ γ , ∂ p ( γ , v ) ∂ v

5 仿真分析

在本节中,对所提出的控制策略进行仿真,以证明该算法的有效性。模拟中使用的四旋翼飞行器参数如表1所示。
表1 四旋翼飞行器参数

Tab.1 Quadrotor parameters

参数符号 物理意义 数值
m 四旋翼飞行器的质量 1.4 kg
d 旋翼中心到质心的距离 0.2 m
g 重力加速度 9.8 m·s-2
CT 升力系数 8.898e-6 N·s-2
Cm 反扭矩系数 1.109e-7 N·m·s-2
Jxx/Jyy xb轴yb轴转动惯量 1.777e-2 kg·m2
Jzz zb轴转动惯量 3.451e-2 kg·m2
Jm 旋翼转动惯量 8.85e-5 kg·m2
下面通过悬停控制和轨迹控制两方面的仿真实验,对比基于MPC的轨迹跟踪控制和基于ESO-DO的MPC轨迹跟踪控制两种控制方法的效果(以下简称MPC和ED-MPC),为了排除姿态环的影响,两种控制方法均采用相同的ADRC控制器控制姿态环[22]。
仿真中用到的其他参数如下:
N 2 z = N u z = 10 I n z , Q '   z = d i a g ( 15,0.8 ) R '   z = 0.05 , Q z = d i a g ( Q '   z , … , Q '   z ⏟ N 2 z ) R z = d i a g ( R '   z , … , R '   z ⏟ N u z ) , G z = 3 Q '   z N 2 x y = N u x y = 10 I n x y , Q '   x y = d i a g ( 50,5 , 50,5 ) R '   x y = d i a g ( 15,15 ) , Q x y = d i a g ( Q '   x y , … , Q '   x y ⏟ N 2 x y ) R x y = d i a g ( R '   x y , … , R '   x y ⏟ N u x y ) , G x y = 3 Q '   x y
其中,上述控制器和观测器的增益可以通过带宽法调参得到:
ω 0 x= ω 0 y= ω 0 z=50, ω 0 ϕ= ω 0 θ= ω 0 ψ=30,
ω c ϕ= ω c θ= ω c ψ=10

5.1 悬停仿真实验

假设飞行器的初始位置为(1,2,0),为了验证ED-MPC的鲁棒性,实验过程在螺旋桨产生的推力中加入如图5所示的信号来模拟外部扰动。
图5 扰动信号图

Fig.5 Disturbance signal diagram

图6~图14从位置、速度、姿态角三个方面表现四旋翼飞行器从初始位置达到稳定悬停状态的过程。
图6 x方向跟踪

Fig.6 x direction tracking

图7 y方向跟踪

Fig.7 y direction tracking

图8 z方向跟踪

Fig.8 z direction tracking

图9 vx跟踪

Fig.9 vx tracking

图10 vy跟踪

Fig.10 vy tracking

图11 vz跟踪

Fig.11 vz tracking

图12 ϕ角跟踪

Fig.12 ϕ angle tracking

图13 θ角跟踪

Fig.13 θ angle tracking

图14 ψ角跟踪

Fig.14 ψ angle tracking

从图6~图8可以看出,位置x(t)使用MPC控制时调节时间约为2.4 s,ED-MPC的调节时间约为2 s;y(t)、z(t)的输出几乎没有超调量,调节时间约为2 s;两种方法对姿态角的控制效果都比较理想,四旋翼飞行器在MPC和ED-MPC的控制下都能较快回到零点,但ED-MPC明显抗扰动性能要好一些。
本节选用均方根误差即RMSE值作为两种控制方法控制精度的比较指标,位置和速度的悬停状态跟踪误差图如图15和16所示,位置和速度的RMSE值如表2所示。由表2可以看出,ED-MPC位置的RMSE值比MPC少0.043 8,ED-MPC速度的RMSE值比MPC少0.154 3。很明显,ED-MPC的跟踪性能更好一些。
图15 位置均方根误差

Fig.15 Root mean square error of position

图16 速度均方根误差

Fig.16 Root mean square error of Velocity

表2 悬停状态RMSE值

Tab.2 RMSE value of hover state

位置 速度
MPC 0.150 8 0.305 1
ED-MPC 0.107 0 0.150 8

5.2 轨迹跟踪仿真实验

本节仿真中给出的参考轨迹为
x = 4 c o s 5 36 π t y = 4 s i n 5 36 π t z = 3 25 t
为了验证ED-MPC的鲁棒性,实验过程在螺旋桨产生的推力中加入如图5所示的信号来模拟外部扰动。轨迹跟踪如图17所示,可以看出,两种方法都能较快跟踪上给定轨迹,总体上保持稳定。
图17 轨迹跟踪图

Fig.17 Trajectory tracking diagram

从图18~20可以看出,在x、y、z三个方向两种控制方法都能较好跟上给定的轨迹;图21~26可以看出,两种控制方法跟踪效果接近,但ED-MPC的抗扰性能明显优于MPC。
图18 x方向跟踪

Fig.18 x direction tracking

图19 y方向跟踪

Fig.19 y direction tracking

图20 z方向跟踪

Fig.20 z direction tracking

图21 vx跟踪

Fig.21 vx tracking

图22 vy跟踪

Fig.22 vy tracking

图23 vz跟踪

Fig.23 vz tracking

图24 ϕ角跟踪

Fig.24 ϕ angle tracking

图25 θ角跟踪

Fig.25 θ angle tracking

图26 ψ角跟踪

Fig.26 ψ angle tracking

位置和速度的轨迹跟踪误差图如图27和28所示,RMSE位置和速度指标如表3所示,由表3可以看出,ED-MPC位置的RMSE值比MPC少0.005 8,ED-MPC速度的RMSE值比MPC少0.017 3。对此RMSE值可以看出,ED-MPC的跟踪性能更好一些。
图27 位置均方根误差

Fig.27 Root mean square error of position

图28 速度均方根误差

Fig.28 Root mean square error of Velocity

表3 轨迹跟踪RMSE值

Tab.3 Trajectory tracking RMSE value

位置 速度
MPC 0.263 7 0.341 3
ED-MPC 0.257 9 0.324 0

6 结束语

本文主要研究了四旋翼的轨迹跟踪问题。总体控制策略由两个独立的控制器组成。将基于误差的状态空间预测控制策略应用于外环的位置控制,在前馈的位置引入ESO和DO进行补偿,可提高控制器抑制干扰的能力。仿真结果显示了对指定轨迹的良好跟踪和鲁棒性。

参考文献

原文顺序 | 文献年度倒序 | 文中引用次数倒序
[1]
许喆. 四旋翼无人机控制系统的设计与实现[D]. 南京: 南京理工大学, 2017.

XU Z. Design and implementation of control system for quadrotor UAV[D]. Nanjing: Nanjing University of Science and Technology, 2017.

[2]
亓岳鑫. 四旋翼无人机控制系统的设计与实现[D]. 哈尔滨: 哈尔滨工业大学, 2018.

QI Y X. Design and implementation of control system for four-rotor UAV[D]. Harbin: Harbin Institute of Technology, 2018.

[3]
姬江涛, 扈菲菲, 贺智涛, 等. 四旋翼无人机在农田信息获取中的应用[J]. 农机化研究, 2013, 35(2): 1-4.

JI J T, HU F F, HE Z T, et al. The application of quad-rotor UAV in farmland information acquisition[J]. Journal of Agricultural Mechanization Research, 2013, 35(2): 1-4.

[4]
BIN L I, WANG H, YANG X. Unmanned aerial vehicle(UAV) system in the application and development of forest fire prevention[J]. Electronic Technology, 2015, 44(5): 15-18.

[5]
LUO X. Application of low-altitude UAV aerial photogrammetric system in large scale topographic surveying and mapping[J]. Geospatial Information, 2013, 11(3): 23-25,5.

[6]
QI Y L, ZHANG Z N, LIU J B, et al. Application of UAV remote sensing in oil and gas pipeline inspection[J]. Infrared, 2014, 35(3): 37-42.

[7]
KANG B, MIAO Y, LIU F, et al. A second-order sliding mode controller of quad-rotor UAV based on PID sliding mode surface with unbalanced load[J]. Journal of Systems Science and Complexity, 2021, 34(2): 520-536.

DOI

[8]
薛晶勇, 王斌锐. 指数型时变增益反步滑模四旋翼姿态控制[J]. 控制工程, 2022, 29(5): 935-943.

XUE J Y, WANG B R. Backstepping sliding mode attitude controller of exponential varying-time gain for quadrotor[J]. Control Engineering of China, 2022, 29(5): 935-943.

[9]
XU L X, MA H J, GUO D, et al. Backstepping sliding-mode and cascade active disturbance rejection control for a quadrotor UAV[J]. IEEE/ASME Transactions on Mechatronics, 2020, 25(6): 2743-2753.

DOI

[10]
NASCIMENTO T P, SASKA M. Position and attitude control of multi-rotor aerial vehicles: a survey[J]. Annual Reviews in Control, 2019, 48: 129-146.

DOI

[11]
NAVABI M, DAVOODI A, MIRZAEI H. Trajectory tracking of under-actuated quadcopter using Lyapunov-based optimum adaptive controller[J]. Proceedings of the Institution of Mechanical Engineers, Part G: Journal of Aerospace Engineering, 2022, 236(1): 202-215.

DOI

[12]
MORENO-VALENZUELA J, PÉREZ-ALCOCER R, GUERRERO-MEDINA M, et al. Nonlinear PID-type controller for quadrotor trajectory tracking[J]. IEEE/ASME Transactions on Mechatronics, 2018, 23(5): 2 436-2 447.

[13]
MUÑOZ F, GONZÁLEZ-HERNÁNDEZ I, SALAZAR S, et al. Second order sliding mode controllers for altitude control of a quadrotor UAS: real-time implementation in outdoor environments[J]. Neurocomputing, 2017, 233: 61-71.

DOI

[14]
CHEN Z, NIAN X H, XU P C, et al. Robust adaptive tracking control of a quadrotor helicopter with uncertainties[C]// 2021 40th Chinese Control Conference (CCC). Shanghai, 2021: 7 748.

[15]
RODRIGUEZ J, CORTES P, KENNEL R, et al. Model predictive control—a simple and powerful method to control power converters[C]// 2009 IEEE 6th International Power Electronics and Motion Control Conference, Wuhan, 2009: 41-49.

[16]
KUHNE F, LAGES W F,DA SILVA J M G. Point stabilization of mobile robots with nonlinear model predictive control[C]// IEEE International Conference Mechatronics and Automation. Niagara Falls, 2006: 1 163-1 168.

[17]
XUE W C, HUANG Y. On frequency-domain analysis of ADRC for uncertain system[C]// 2013 American Control Conference. Washington, 2013: 6 637-6 642.

[18]
金辉宇, 刘丽丽, 兰维瑶. 二阶系统线性自抗扰控制的稳定性条件[J]. 自动化学报, 2018, 44(9): 1 725-1 728.

JIN H Y, LIU L L, LAN W Y. On stability condition of linear active disturbance rejection control for second-order systems[J]. Acta Automatica Sinica, 2018, 44(9): 1 725-1 728.

[19]
LU H, LIU C, GUO L, et al. Flight control design for small-scale helicopter using disturbance-observer-based backstepping[J]. Journal of Guidance, Control, and Dynamics, 2015, 38(11): 2 235-2 240.

[20]
GUO L, CHEN W H. Disturbance attenuation and rejection for systems with nonlinearity via DOBC approach[J]. International Journal of Robust and Nonlinear Control, 2005, 15(3): 109-125.

DOI

[21]
CHEN W H. Disturbance observer based control for nonlinear systems[J]. IEEE/ASME Transactions on Mechatronics, 2004, 9(4): 706-710.

DOI

[22]
刘一莎, 杨晟萱, 王伟. 四旋翼飞行器的自抗扰飞行控制方法[J]. 控制理论与应用, 2015, 32(10): 1 351-1 360.

LIU Y S, YANG S X, WANG W. An active disturbance-rejection flight control method for quad-rotor unmanned aerial vehicles[J]. Control Theory & Applications, 2015, 32(10): 1 351-1 360.

Options
文章导航

/

〈 〉