一种新的旋翼无人机控制方法与流程

本发明属于旋翼无人机控制方法。

背景技术

无人机(uav)已经成为一个热词，不仅是展现一个国家军事实力的方式，也逐渐成为人们记录生活、享受生活的载体。旋翼无人机由于其操作简单、可垂直起降、悬停性能好，在民用领域比固定翼无人机占据更加广阔的市场。pid控制器因其算法简单，不依赖于系统模型，在旋翼无人机控制中应用最为成功且最为广泛。

旋翼无人机(ruav)质量轻，搭载微控计算机计算能力有限，具有强耦合的非线性动力学学特性，精细建模困难。ruav控制对于算法的要求主要包括三个方面：鲁棒性好、控制精度高、算法简单。pid算法简单，能够满足一般的ruav控制要求,可是在复杂环境或者质量变化的情况下其鲁棒性有待提高。对于单个ruav控制的研究，目前主要集中在控制精度和鲁棒性的提升上，而在致力于使算法简单，便于实现上研究甚少。

滑模控制在ruav控制中的应用是一个研究热点，它用于控制非线性系统具有极好的鲁棒性，能够适应ruav本身和环境的不确定性。“defoortm,floquett,kokosya,etal.anovelhigherorderslidingmodecontrolscheme[j].systems&controlletters,2009,58(2):102-8”提出了一种高阶滑模控制方法，使用该方法系统抵抗不确定性的鲁棒性能够被保证，参数设计也可以通过理论计算。然而如果将该方法用于ruav控制面临两个困难：需要在确切数学模型已知的情况下进行参数公式化设计，而ruav许多固定的参数通常是难以获得的；控制律计算涉及到众多复杂矩阵、分式幂、导数运算，计算量过大，难以在嵌入式系统中实时实现。“perozzig,efimovd,biannicjm,etal.trajectorytrackingforaquadrotorunderwindperturbations:slidingmodecontrolwithstate-dependentgains[j].journalofthefranklininstitute-engineeringandappliedmathematics,2018,355(12):4809-38”在考虑风场扰动和转子动态特性的基础上设计了四旋翼无人机一阶滑模控制和准连续高阶滑模控制器，并且通过仿真证明了其有效性。然而该研究中没有考虑ruav质量的不对称性，而且只是在悬停模式将模型大大简化来设计控制率，即便这样简化之后所设计的控制律依然很复杂。

“wangr,liuj.adaptiveformationcontrolofquadrotorunmannedaerialvehicleswithboundedcontrolthrust[j].chinesejournalofaeronautics,2017,30(2):807-17”、“aliza,wangdb,aamirm.fuzzy-basedhybridcontrolalgorithmforthestabilizationofatri-rotoruav[j].sensors,2016,16(5)”模型参考自适应控制利用参考模型和真实系统状态误差或输出误差，通过自适应机构生成参数调节信号或者辅助控制信号，能够使系统能够适应自身或者环境的变化。在ruav的控制中，模型参考自适应极大的提高了ruav的鲁棒性，特别是当系统发生严重错误，如桨叶损坏、电机故障，还能够保证一定的飞行能力。“i.sadeghzadeh,a.mehta,andy.zhang,“fault/damagetolerantcontrolofaquadrotorhelicopteruavusingmodelreferenceadaptivecontrolandgain-scheduledpid,”inproc.aiaaguid.,navigat.,controlconf.,portland,or,2011,pp.1–20”使用mit规则设计了高度通道模型参考自适应系统，该设计只考虑高度通道的自适应，因而算法较为简单。mit规则并不能保证所设计的系统是稳定的，为从理论上说明系统是稳定还需使用控制理论的稳定性判据分析。如果要在ruav的姿态和高度都设计成自适应控制，那么mit规则将不再适用。“dydekzt,annaswamyam,lavretskye.adaptivecontrolofquadrotoruavs:adesigntradestudywithflightevaluations[j].ieeetransactionsoncontrolsystemstechnology,2013,21(4):1400-6”使用李雅普诺夫稳定性理论设计了四旋翼无人机高度和姿态模型参考自适应控制系统，实现了ruav控制系统强大的容错能力。该系统的设计存在两个主要的局限性：假设了系统处于悬停的小角度模式，适应的实际场合狭隘；使用了地面辅助计算机进行自适应控制的计算，不能实际野外飞行。“zeghlaches,mekkih,bouguerraa,etal.actuatorfaulttolerantcontrolusingadaptiverbfnnfuzzyslidingmodecontrollerforcoaxialoctorotoruav[j].isatransactions,2018”使用人工神经网络，设计了模糊自适应滑模控制器用于控制八旋翼无人机，从仿真结果来看控制效果非常好。然而存在的局限性就是算法复杂，难以在嵌入式系统上实现。

上述ruav控制方法都存在计算复杂，难以在嵌入式系统实现的问题。而且在理论分析过程中，使用的ruav数学模型作了过强的假设，与实际情况有出入。

技术实现要素：

本发明的目的在于针对上述问题，提出一种简单，鲁棒性优良的控制方法，并且充分从实际情况出发。

本发明的技术方案为：

一种新的旋翼无人机控制方法，其特征在于，包括以下步骤：

s1、建立旋翼无人机状态空间模型，具体包括：

s11、建立坐标系：

1)地心惯性系i系：原点与地球质心重合，x轴的方向从地球质心指向春分点，z轴指向协议地理北极，x、y、z轴构成右手螺旋关系；

2)导航坐标系n系：原点位于载体质心，x轴指向东向，y指向北向，z轴指向天向；

3)载体坐标系b系：原点位于载体质心，y指向前进方向，x轴指向前进方向右侧，x、y、z轴构成右手螺旋关系；

s12、采用欧拉角法对无人机姿态进行描述，即用三个欧拉角来描述：偏航角y、俯仰角p、滚转角r；定义姿态微分方程为：

其中，[ωxωyωz]^t为无人机在惯性空间转动的角速度在b系中的投影，角速度通过陀螺仪测量；

s13、根据动量定理和姿态微分方程，建立旋翼无人机状态空间模型为：

其中，

fx,fy,fz,mx,my,mz为载体在惯性坐标系中所受到的合力在b系中各个轴向的投影，转动惯量矩阵j是一个常量，b系中的转动惯量矩阵为：

[vxvyvzωxωyωz]^t代表选取的状态变量，表示载体速度和角速度在b系中各个轴向的投影；

s2、设定只考虑姿态控制，即选取旋翼无人机状态空间模型中角速度方程为被控制对象的数学模型，记为s系统，s的输出值为偏航角、俯仰角、滚转角：

s状态方程：

s输出方程：

将问题定义为如何设计控制律，即mx,my,mz，使得s系统的输出角度值达到期望的值；

s3、设定由于空气阻力而产生的阻力矩为：

其中，fa为空气阻力，c1、c2是正常数，r为目标长度，z1、z2是待调参数；

s4、根据空气阻力矩模型，建立控制律为：

通过设定参数z1、z2的值，实现对旋翼无人机的控制。

本发明的有益效果为，为旋翼无人机的控制提出了一种新的控制方法，即在控制中引入了状态阻尼控制(空气阻力)，从而实现控制精度高、鲁棒性好。

附图说明

图1是坐标系统示意图；

图2是空气阻力估计模型示意图；

图3是参数c对近似符号函数的影响示意图；

图4是不同k值系统稳定性仿真示意图；

图5是不同z1,z2值系统稳定性仿真示意图；

图6是跟踪方波仿真示意图；

图7是扰动抑制鲁棒性仿真示意图；

图8是z1过大时系统鲁棒性降低仿真示意图；

图9是转动惯量矩阵变化仿真示意图。

具体实施方式

下面结合附图和实施例，对本发明进行详细的说明。

本发明的方案中，将ruav视为一个质量近似对称但又不完全对称刚体，基于向量恒等式、动量定理、角动量定理推导ruav动力学方程。在推导过程中系统受到的力记为合外力的形式，空气阻力、推力都只是所受合外力的一部分。为了便于运用现代控制进行理论分析和仿真试验，将所推导的方程化为状态空间描述的形式，并给出基于泰勒级数的线性化方法。

如图1所示，建立状态空间描述数学模型，涉及到三个个坐标系，地心惯性系、导航坐标系和载体坐标系。

地心惯性系(i系)：原点与地球质心重合x轴的方向从地球质心指向春分点，z轴指向协议地理北极，x、y、z轴构成右手螺旋关系。春分点不会因为地球自转而移动。

导航坐标系(n系)：又称东北天坐标系，原点位于载体质心，x轴指向东向，y指向北向，z轴指向天向。

载体坐标系(b系)：原点位于载体质心，y指向前进方向，x轴指向前进方向右侧，x、y、z轴构成右手螺旋关系。将y指向前进方向，是为了与航向的通常定义相吻合。载体轴指向真北时航向角为0°，若此时载体没有发生倾斜，则载体坐标系和当地导航坐标系重合。

在ruav姿态解算中，需要在n系和b系中变换坐标，b系和n系中的坐标变换关系如式(1)，是一个正交矩阵。

本发明采用欧拉角法对无人机姿态进行描述，并设定ruav的俯仰角和滚转角小于90°，根据该设定欧拉角法不会出现奇异的情况。

欧拉角描述是基于任何一个空间笛卡尔坐标系，可以经过绕其轴的三次旋转而与空间任意坐标系重合。ruav的姿态可用三个欧拉角来描述，偏航角(y)、俯仰角(p)、滚转角(r)。欧拉角是有顺序的，只有先进行偏航旋转，再进行俯仰旋转，最后进行滚转旋转，才能保证ruav机体的轴线与水平面的夹角就是俯仰角。

ruav是利用陀螺仪测得的角速度，通过求解姿态微分方程解算欧拉角的。

设定陀螺仪的测量值即是ruav相对于惯性坐标系转动的角速度。

于是姿态微分方程可写为：

(2)式中[ωxωyωz]^t为无人机在惯性空间转动的角速度在b系中的投影，可以通过陀螺仪测量。

建立数学模型：

引理1：向量恒等式，b系在i系中以角速度ω旋转，则对于任意向量a，满足式(3)，式(3)中的等号是等价的意思而不是数值上相等。

引理2：动量定理，物体所受冲量等于其动量的变化量

fdt＝d(mv)(4)

引理3：角动量定理物体所受冲量矩等于其角动量的变化量，j表示转动惯量

mdt＝d(jω)(5)

由式(1)、(3)可得：

(4)式表示将转动方程投影在b系中，这样使得转动惯量矩阵j是一个常量。b系中的转动惯量矩阵写成如下形式：

设定转动惯量矩阵j对角线以外的元素是小量，则当两个或两个以上j对角线以外的元素相乘时可以略去。

综合式(2)、(4)式可得：

其中，

fx,fy,fz,mx,my,mz为载体在惯性坐标系中所受到的合力在b系中各个轴向的投影。

[vxvyvzωxωyωz]^t代表选取的状态变量，表示载体速度和角速度在b系中各个轴向的投影。

(8)式即是建立的ruav数学模型，相对于当前研究中常使用的数学模型有如下几个特征：

1)合理考虑了ruav质量的不对称性，更加符合实际情况；

2)写成状态空间描述的形式，便于使用现代控制理论进行分析，便于在仿真时实现矩阵化编程；

3)充分考虑了ruav各个转动轴之间的耦合，为实现更高精度的控制提供模型基础。

在(8)式中建立方程为非线性微分方程组，在仿真和设置参考模型时难于使用。为此需要将(8)式线性化，并写成矩阵形式。

在点[vx0vy0vz0ωx0ωy0ωz0]^t处展开成一阶泰勒级数。得到系统矩阵a和b。四旋翼无人机的数学模型可写为：

其中，

选取u＝[fx/mfy/mfz/mmxmymz]^t，可得：

值得说明的是，在仿真或者设计参考模型时，点[vx0vy0vz0ωx0ωy0ωz0]^t是一个可以随时间变化的点。这样(8)式表示的非线性系统就转化为了(9)式表示的线性时变系统。在计算机中表示一个非线性系统是困难的，而表示一个线性时变系统是容易的，只需要按时更新参数即可。

通过(8)式可以看出，ruav系统耦合最强的是角速度。角速度通过(2)式决定ruav的三个姿态角，所以姿态角的控制具有非线性和强耦合性，是ruav控制的难点。姿态角又直接影响着ruav前进的方向和前进的速度，是ruav最核心的部分，因此本发明只关注姿态的控制。

由于只考虑姿态控制，所以(8)式第4、5、6个方程即为被控制对象的数学模型，记为s系统。s的输出值为偏航角、俯仰角、滚转角，输出方程为式(2)。

s状态方程：

s输出方程：

需要解决的问题是如何设计控制律，即mx,my,mz，使得s系统的输出角度值达到期望的值。

本发明中提出“空气阻力的存在有利于ruav姿态系统s的稳定”，因此额外的空气阻力，就等同于用于控制姿态的控制律的一部分。

为了获取空气阻力模型，使用一个非常简单的模型来估计空气阻力。如图2所示，一块长为r，高为l的平板以角速度ω作定轴转动。平板的前侧需排开空气，会受到一个反作用力记为ff，平板的后侧由于空气的吸附作用会受到吸附力fb。

吸附力fb可以近似看成一个静摩檫力：

fb＝c2sign(ω)

c2是一个正常数，sign为符号函数。

不妨设空气的密度为ρ，则ff可由动量定理估计

c1是一个正常数。

得空气阻力fa

fa＝fb+ff＝c1ω²sign(ω)+c2sign(ω)

在实际过程中，更加关心的是由于空气阻力而产生的阻力矩，下面将写成阻力矩的形式

为了将真实的空气阻力矩和为得到控制律而引入的虚加的空气阻力矩进行对比，现对估算z1的值。

假设r＝l＝0.45m,ρ＝1.2kg/m³

那么

z1不带单位，使用时其它量应统一为国际单位制形式。

(11)式即为所得到的空气阻力矩模型，类比该模型引入如下控制律，来使系统s状态方程稳定，z1、z2是待调参数。

(13)式中右边后一项形式可能不会出现在通常阻力模型中[9]，然而对于控制律来说这一项是非常重要的。在s系统的控制中当输出的角度值接近期望值时，角速度ω很小，那么us中前一项将几乎不起作用，这样系统就要在平衡位置附近花费很长的时间振动才能达到期望的平衡点。而引入后一项之后，即便ω很小，仍有较大的控制来使s系统快速达到平衡状态。

所引入的控制律中包含两个符号函数sign，当系统的状态接近平衡态时，符号函数的正负频繁切换，那么可以预见所设计的控制器会像滑模控制那样出现颤振。所以借鉴滑模控制消除颤振的方法，来解决状态阻尼控制中出现的类似的问题。引用文献“hallimalwi,christopheredwards,cheepintan,faultdetectionandfaulttolerantcontrolusingslidingmodes[m]nationaldefenseindustrypress,2014.6”中的方法，即使用一个平滑函数来代替原本的符号函数，仍记为sign，如式(14)所示：

其中c为正的常数。由图3可以看出，当c＝0.001时对符号函数的近似较好，且在x值在零附近较平滑，因此本发明所选的c＝0.001。

状态阻尼控制即是形如(13)那样的方式引入控制律。(13)式中只引入了两个可调参数，并且角速度ω可以取陀螺仪的测量值，因此算法是非常简洁的。需要说明的是，当前引入的控制律只是为了使s系统状态方程稳定，要使s系统能够输出期望的角度值，还需做修正。

记俯仰角(p)、滚转角(r)、偏航角(y)实际值与期望值的误差为[exeyez]^t。

则用于控制整个s系统的控制律为

考虑s状态方程的稳定性时，使用控制律us。进行整个s系统仿真，看s系统能否输出期望角度时，使用控制律u。

根据本发明的方案，需要确定的是如何选定z1、z2能使系统渐进稳定。针对该问题本发明给出以下示意：

使用克拉索夫斯基定理来分析这个问题。

克拉索夫斯基定理表述：

定义：

对连续时间非线性时不变系统和围绕原点平衡状态的一个区域ω∈rⁿ，原点x＝0为域ω内唯一的平衡点，若f^t(x)+f(x)<0即为负定，则系统平衡态x＝0为域ω内渐近稳定的。

设定x＝0是上述自治系统唯一的零点，依据设定，只要z1、z2的选取能使f^t(x)+f(x)负定，那么系统状态方程是渐进稳定的。

由图1可知，sign函数的导数具有对称性，为了简化表达，在求取sign函数的导数时可以认为x>0。

记

令

由(10)、(13)、(14)、(16)式得

令w＝f^t(x)+f(x)

设定矩阵-w满足如下条件：

①主对角线的元素大于零；

②-w严格对角占优。

通过以上两条规则，限定角速度的取值范围，即可估计z1,z2的值。

本发明还给出如何仿真一个线性时变系统的如下说明：

对于下式所表述的系统，系统矩阵a是一个时变矩阵，然而在仿真过程中总是一步一步计算的，在每一步中可以认为系统参数是没有发生变化的。那么在一步计算中就可以当成线性系统来处理。

x＝[ωxωyωz]^t，u＝[mxmymz]^t

根据线性系统理论，系统的解可以表示成如下形式：

转化为离散系统形式，考虑时间t只能取为t的整数倍，取t为典型的ruav主循环时间间隔，t＝0.01s。

x(k+1)＝e^atx(k)+te^atbu(k)(20)

由于采样时间间隔t取得足够小，所以at是一个小量，在本发明中将e^at展开成二阶泰勒级数，就具有很高的仿真精度了。即：

至此一步计算就可以完成，完成一步计算之后，利用式(9)刷新矩阵a即可进入下一步的计算。这就是本发明提出的对线性时变系统的仿真方法。

实施例

本例中假设质量m＝1.373kg

jx＝0.10125kg·m²,jy＝0.10203kg·m²,jz＝0.14374kg·m²

jxy＝0.00217kg·m²,jyz＝0.00153kg·m²,jzx＝0.00030kg·m²

可得参数为：

表1参数计算表(kg·m²)

根据(18)式和定理1，确定使s系统状态方程稳定的参数z1,z2。

由于jz在转动惯量各个元素中最大，所以在确定z1,z2时，-w33严格对角占优条件将起决定性作用。

设三个方向角速度的最大值为10rad/s。

记

式(22)条件满足即可保证s系统状态方程稳定。

通过式(22)、(23)来估计z1,z2的范围。从(23)式可知当ωz值很小时，式(23)前两项很小可以忽略，当ωz很大时式(23)后两项可以忽略，这种忽略总是保守的。而且假设ωz>0不会影响结果的估计。

当ωz≥1rad/s

当z1值取得不是很大时(小于转动惯量矩阵主对角元素与最大非对角元素值除以最大角速度)，上式可以略去。

将|ωx|、|ωy|取为最大角速度值10rad/s

可以估计出z1>0.19。z1不宜取过大，否则(22)式将不满足，z1上界值难以估计，只能通过仿真去测试。

当ωz≤0.01rad/s

可以估计出z2>0.05，z2取大一些会利于式(22)满足。

因而通过z1,z2参数的估计，可以知道这两个参数对于系统稳定性的影响：z1的值可由式(22)估计下界，z1不宜取过大，否则系统会失去稳定性；z2的值可由式(22)估计下界，z2取大不会使系统失去稳定性。

使用(13)式所确定的控制律，对s系统能否输出期望角度进行仿真。首先随意选取几个比例参数k，看能否使得系统稳定。然后调整k,z1,z2的值，优化系统性能，根据对z1,z2参数的估计，取z1＝0.2，z2＝0.1。取参数k分别为0.1，1，5，20。s系统0初始条件，输入信号为单位阶跃信号，单位阶跃信号(相当于53°)的输入通道为俯仰角通道，其余两个通道输入为0。仿真时间100s，即10000步。

从图4中俯仰角和滚转角的图可以看到不同比例k值对于s系统角度响应影响的细节。偏航角图可以证明系统在整个仿真过程中都是稳定的。因此通过仿真证明了式(15)所提控制律的有效性，以及本发明对参数估计是可靠的。

为了优化s系统的动态响应性能，需要进一步调整k,z1,z2值。从图4俯仰角图，可知道k值越大，s系统响应速度越快，超调量也会增加。而且k值太小存在较大的稳态误差。不妨选定k＝5，分别调整z1,z2的值观察s系统俯仰角响应。

情况一：取定k＝5，z2＝0.1，变化z1分别取0.2、0.5、0.8、1.0；

情况二：取定k＝5，z1＝0.5，变化z2分别取0.1、0.5、1.0、5.0；

由图5可知，当k＝5,z1＝0.5,z2＝0.5时，控制效果最佳。s系统响应快，没有超调，控制精度极高。不论是增加z1、还是增加z2，都会使系统的响应速度变慢。

为了体现k＝5,z1＝0.5,z2＝0.5良好的控制效果，做一次方波测试，方波的周期为400步，也就是每隔200步跳变一次。仿真结果如图6所示。

根据上述内容，可以得出参数调整规律为：

1)估计出来的z1,z2是有效的，但不是最优的。

2)增大k值使系统响应速度加快，但会增加超调量。

3)增大z1,z2会减小超调量，但是会减慢系统响应。

总的来说参数的调整具有单调性，即将参数往一个方向调整时，系统响应就往对应的方向变化。

鲁棒性检验

鲁棒性检验是指，所设计系统干扰抑制性能和适应系统参数变化的能力。这里假设两个仿真场景，一个是受到脉动风影响突然获得一个角速度，另一个是系统转动惯量矩阵发生变化。

情况1：对受到脉动风影响获得的角速度分别为-15、-5、5、15rad/s。

仿真的方法是首先在俯仰角通道加一个单位阶跃信号，等仿真到300步，然后在俯仰、滚转、偏航三个通道都加上受脉动风影响的等效角速度。参数设置为k＝5,z1＝0.5,z2＝0.5。

从图7可知，系统具备抵抗较大扰动的能力。不过受到扰动之后，s系统会产生一个较大的误动作。主要原因是，系统抵抗这类扰动需要等误差形成后才做出反应，这也是算法需要改进的地方。

当z1过大时系统鲁棒性降低。参数设置为k＝5,z1＝2,z2＝0.5。在仿真到300步，施加15rad/s，这时系统将失去稳定。

由图8可知，当z1过大时，s系统不再能抵抗15rad/s以上的扰动。

情况2：转动惯量矩阵发生变化，jx取0.050625、0.10125、0.151875、0.2025kg·m²。其它转动惯量矩阵元素不发生变化。参数设置为k＝5,z1＝0.5,z2＝0.5。俯仰角通道输入单位阶跃信号，其它通道输入为0。

图9证明了算法对于转动惯量矩阵变化的鲁棒性。同时，可以看出当往一个方向调整转动惯量矩阵元素，系统性能也往一个方向运动

同时上述实施例及仿真示意，验证了状态阻尼控制对于扰动和转动惯量变化的鲁棒性。

综上可得，本发明所提出的新的控制方法，叫做状态阻尼控制，用于控制ruav的姿态。在充分考虑ruav质量不对称性的基础上，建立了ruav非线性动力学模型，并将其线性化用于理论分析和仿真。使用这个模型，运用克拉索夫斯基定理，分析了怎样选择状态阻尼控制的参数能够保证系统的稳定，给出了参数对系统性能影响规律的表述。最后通过仿真，验证了使用克拉索夫斯基定理估计的状态阻尼控制参数是有效的，状态阻尼控制能够使姿态系统输出期望的角度值。进行参数调优后，可以实现无超调的快速响应，并且通过仿真验证了调优后的参数具有抵抗外部扰动，适应内部参数变化的鲁棒性。