CN107608368B - 一种无人机在极端初始状态的快速平衡控制方法 - Google Patents

Abstract

本发明涉及一种无人机在极端初始状态的快速平衡控制方法，属于多旋翼无人机飞行控制技术领域。本方法包括如下步骤：（1）无人机上电，对陀螺仪、磁力计、加速度计进行校准，并检查电机解锁后是否运转正常；（2）各个传感器校准完毕，电机解锁后一切正常，手持无人机准备进行抛飞；（3）向上以任意方向抛出无人机，当无人机在空中自由落体的时候，拨动遥控器档位；（4）无人机自由落体时，接收到遥控器的档位变化，根据各个传感器反馈的信息，飞行控制器调用算法，使得自身快速进入平稳飞行状态。该方法增强了飞行控制系统的鲁棒性，同时提高了整个飞行系统状态的收敛速度。

Description

一种无人机在极端初始状态的快速平衡控制方法

技术领域

本发明涉及一种无人机在极端初始状态的快速平衡控制方法，属于多旋翼无人机飞行控制技术领域。

背景技术

无人机为了提升娱乐性提出了手抛即飞的任务需求，此时飞行控制系统的任务是使无人机能在任意的初始状态收到地面站发出的平衡指令后快速进入平稳飞行状态。

在当前的工程实践中，传统的比例积分微分(PID)控制策略依然被广泛采用。在飞行控制系统的设计中，常用的方法是利用小扰动分析建立俯仰、滚转、航向和高度四个通道的近似线性模型。虽然比例积分微分控制策略在分通道控制中具有不错的控制效果，但其更适用于单变量系统。而无人机属于复杂的多变量系统，各个状态之间的耦合较为严重，使用比例积分微分控制策略在设计上通常忽略各个状态之间的耦合影响，控制参数较多，控制参数的设计过程也较为复杂。在抛飞过程中的任意初始状态，无人机需要进行快速翻滚等大机动动作，既要保证比较好的动态品质特性(如快速性与弱超调性)，又要兼顾性能鲁棒性与稳定性。通道间的耦合作用使得比例积分微分控制难以让无人机快速进入平稳飞行状态。

发明内容

本发明针对无人机在抛飞场景对姿态和高度控制的快速响应要求，提出了一种无人机在极端初始状态的快速平衡控制方法。首先，结合精准的无人机非线性数学模型而设计了分层级联结构控制器，基于辅助面的快速终端滑模控制算法，由此推导出姿态控制律、角速率控制律和高度控制律，达到加快收敛速度的效果。该方法增强了飞行控制系统的鲁棒性，同时提高了整个飞行系统状态的收敛速度。

本发明为解决其技术问题采用如下技术方案：

一种无人机在极端初始状态的快速平衡控制方法，包括如下步骤：

(1)无人机上电，对陀螺仪、磁力计、加速度计进行校准，并检查电机解锁后是否运转正常，如果不正常，对无人机下电进行相关检查；

(2)各个传感器校准完毕，电机解锁后一切正常，手持无人机准备进行抛飞；

(3)向上以任意方向抛出无人机，当无人机在空中自由落体的时候，拨动遥控器档位，使得遥控器档位设置的无人机飞行模式处于抛飞状态；

(4)无人机自由落体时，接收到遥控器的档位变化，根据各个传感器反馈的信息，飞行控制器调用算法，使得自身快速进入平稳飞行状态。

步骤(4)中所述飞行控制器调用算法分别在飞行控制器的各个子控制器内部进行运算，按照角速率子控制器、姿态子控制器、速度子控制器、位置子控制器的顺序依次进行解算，具体设计步骤如下：

步骤1：对系统状态X_e，设计非线性切换面s_1i,s_2i为

其中s₁＝[s₁₁,s₁₂,s₁₃]^T,s₁₁为系统切换面s₁的第一个切换面，s₁₂为系统切换面s₁的第二个切换面，s₁₃为系统切换面s₁的第三个切换面，s₂＝[s₂₁,s₂₂,s₂₃]^T,s₂₁为系统切换面s₂的第一个切换面，s₂₂为系统切换面s₂的第二个切换面，s₂₃为系统切换面s₂的第三个切换面，X_e＝[x₁,x₂,x₃]^T,x₁为系统状态X_e的第一个分量，x₂为系统状态X_e的第二个分量，x₃为系统状态X_e的第三个分量，∫X_e＝[∫x₁,∫x₂,∫x₃]^T,符号∫x_e表示∫x_edt，∫x₁为系统状态积分∫X_e的第一个分量，∫x₂为系统状态积分∫X_e的第二个分量，∫x₃为系统状态积分∫X_e的第三个分量，切换面的系数为ck₁＝diag{ck₁₁,ck₁₂,ck₁₃},ck₁₁为系数ck₁的第一个分量，ck₁₂为系数ck₁的第二个分量，ck₁₃为系数ck₁的第三个分量，ck₂＝diag{ck₂₁,ck₂₂,ck₂₃}，ck₂₁为系数ck₂的第一个分量，ck₂₂为系数ck₂的第二个分量，ck₂₃为系数ck₂的第三个分量，ck₃＝diag{ck₃₁,ck₃₂,ck₃₃},ck₃₁为系数ck₃的第一个分量，ck₃₂为系数ck₃的第二个分量，ck₃₃为系数ck₃的第三个分量，ck_1i＞ck_2i＞0,ck_3i＞0,i＝1,2,3，ξ＝q/p,ξ是吸引子，p和q是正奇数且p＞q；

步骤2：令z₁＝X_e＝[z₁₁,z₁₂,z₁₃]^T，z₁₁为z₁的第一个分量，z₁₂为z₁的第二个分量，z₁₃为z₁的第三个分量，z₂＝∫X_e+ck₃·∫X_e ^ξ，方程(25)又表示为

步骤3：基于切换面s_1i和s_2i，定义编号为0_i，1_i，2_i，3_i的子空间其数学表达式为：

其中：z_1i代表z₁₁、z₁₂、z₁₃中的任意一个，z_2i代表z₂₁、z₂₂、z₂₃中的任意一个，s_1i代表s₁₁、s₁₂、s₁₃中的任意一个，s_2i代表s₂₁、s₂₂、s₂₃中的任意一个，i∈[1,2,3]；

接着，在切换面s_1i和s_2i上确定4个点P_s1i+,P_s1i-,P_s2i+和P_s2i-满足如下条件：

s_1i(P_s1i+)＝0；s_1i(P_s1i-)＝0；s_2i(P_s2i+)＝0；s_2i(P_s2i-)＝0； (28)

由P_s1i+,P_s1i-,P_s2i+和P_s2i-两两相连的连线P_s1i-P_s2i-,P_s1i+P_s2i-,P_s1i-P_s2i+,P_s1i+P_s2i+构成四条辅助面h_0i,h_1i,h_2i,h_3i，其数学表达式为：

h_ki＝ω_ki1z_1i+ω_ki2z_2i+m_i (29)

其中ω_ki1≠0,ω_ki2≠0,ω_ki1和ω_ki2是切换系数，z_1i代表z₁₁、z₁₂、z₁₃中第i个状态量，z_2i代表z₂₁、z₂₂、z₂₃中第i个状态量，m_i＞0，m_i是常量，h_ki为第k子空间的辅助面，下标k表示第几个子空间且k＝0,1,2,3，下标i表示系统状态X_e的第i个状态变量，切换系数ω_ki1,ω_ki2需要满足如下条件：

ω_0i1＝-ω_3i1,ω_0i2＝-ω_3i2,ω_1i1＝-ω_2i1,ω_1i2＝-ω_2i2 (30)

其中ω_0i1为第i个状态量对应的辅助面在第0子空间的第一个系数，ω_0i2为第i个状态量对应的辅助面在第0子空间的第二个系数，ω_1i1为第i个状态量对应的辅助面在第1子空间的第一个系数，ω_1i2为第i个状态量对应的辅助面在第1子空间的第二个系数，ω_3i1为第i个状态量对应的辅助面在第3子空间的第一个系数，ω_3i2为第i个状态量对应的辅助面在第3子空间的第二个系数，ω_2i1为第i个状态量对应的辅助面在第2子空间的第一个系数，ω_2i2为第i个状态量对应的辅助面在第2子空间的第二个系数；

步骤4：当系统状态进入子空间0_i，1_i，2_i，3_i后，辅助面就被用来设计控制器输出u，状态Z_i的当前辅助面被定义为：

h_i＝ω_i1z_1i+ω_i2z_2i+m_i,i＝1,2,3 (31)

其中m_i＞0并且

其中K_INF和K_UAS为需要计算得到的切换系数，ω_0i1为系统状态z_1i在第0子空间的切换系数，ω_1i1为系统状态z_1i在第1子空间的切换系数，ω_2i1为系统状态z_1i在第2子空间的切换系数，ω_3i1为系统状态z_1i在第3子空间的切换系数，ω_0i2为系统状态z_2i在第0子空间的切换系数，ω_1i2为系统状态z_2i在第1子空间的切换系数，ω_2i2为系统状态z_2i在第2子空间的切换系数，ω_3i2为系统状态z_2i在第3子空间的切换系数，式(32)中ω_i1和ω_i2的计算步骤如下：

(a)在S_2i上确定P_s2i+(-ck₂,1)和P_s2i-(ck₂,-1)；

(b)计算第3_i子空间的辅助面h_3i的斜率K_UAS＝0.5·(0.6·ck₂+0.4·ck₁)∈[ck₂,ck₁]；

(c)基于(29)，根据K_UAS＝ω_3i2/ω_3i1和P_s2i+(-ck₂,1)，我们得到：

ω_3i1＝1,ω_3i2＝K_UAS,m_i＝-(1·(-CK₂)+K_UAS·1) (33)

(d)计算h_3i和S_1i的交点P_s1i+(X_p1,Y_p1)；

(e)根据点P_s1i+(X_p1,Y_p1)和P_s2i-(ck₂,-1)，我们得到：

h_1i＝ω_1i1z_1i+ω_1i2z_2i+m₁

其中ω_1i1＝K_INF＝(Y_p1-Y_p2)/(X_p1-X_p2),ω_1i2＝-1,X_p2＝ck₂,Y_p2＝-1，X_p1为辅助面h_3i和滑模面S_1i交点的横坐标，Y_p1为辅助面h_3i和滑模面S_1i交点的纵坐标，m₁为辅助面h_1i的常量系数；

通过上述五个步骤和条件(30)，我们得到：

其中K_INF和K_UAS为上述计算得到的切换系数；相应的，当前辅助面被写成紧凑的形式：

h＝Ω₁z₁+Ω₂z₂+m (34)

其中h＝[h₁,h₂,h₃]^T,h₁为第一个状态量的辅助面，h₂为第二个状态量的辅助面，h₃为第三个状态量的辅助面，Ω₁＝diag{ω₁₁,ω₂₁,ω₃₁}，ω₁₁为第一个状态量的辅助面中对应的第一个系数,ω₂₁为第二个状态量的辅助面中对应的第一个系数，ω₃₁为第三个状态量的辅助面中对应的第一个系数，Ω₂＝diag{ω₁₂,ω₂₂,ω₃₂},ω₁₂为第一个状态量的辅助面中对应的第二个系数,ω₂₂为第二个状态量的辅助面中对应的第二个系数，ω₃₂为第三个状态量的辅助面中对应的第二个系数，m＝[m₁,m₂,m₃]^T，m₁为第一个状态量的辅助面中对应的常量系数，m₂为第二个状态量的辅助面中对应的常量系数，m₃为第三个状态量的辅助面中对应的常量系数；

步骤5：无抖震趋近律N_i被设计为：

其中：

k_i＞0,a_i＝-ω_0i2/ω_0i1＝-ω_3i2/ω_3i1,b_i＝-ω_1i2/ω_1i1＝-ω_2i2/ω_2i1，ck_3i为公式(25)系数ck₃的第i个分量，

步骤6：通过解算微分方程(36)

其中：为状态误差z₁的导数，

为状态误差z₂的导数，

为常量m的导数，d为干扰项，

我们得到针对通用子系统(24)的辅助面滑模控制器的输出为

其中Ω₁，Ω₂在(34)得到，N＝[N₁,N₂,N₃]^T是趋近律且N_i＞sup{-ω_i1·d_i}，i＝1,2,3，N₁为趋近律的第一个分量，N₂为趋近律的第二个分量，N₃为趋近律的第三个分量，ω_i1为辅助面的第一个系数，d_i为第i个干扰项；

通过上述6个步骤，我们得到每个电机转速平方的表达式为：

其中：δ_col为四个旋翼产生的总升力，

将(38)转化成对每个电机所施加的控制输出，表达式为：

其中：u₁为对电机1产生的控制输出，u₂为对电机2产生的控制输出，u₃为对电机3产生的控制输出，u₄为对电机4产生的控制输出。

本发明的有益效果如下：

(1)在滑模切换面的设计中将终端吸引子引入到设计过程中来。因为终端吸引子的存在，非线性滑模面具有有限时间内收敛的优良品质。这样，凭借终端滑模的快速收敛能力，使得本发明提出的辅助面终端滑模控制算法具有响应速度快的优势。

与比例积分微分控制器相比，新型飞行控制系统能够使无人机更快地实现姿态跟踪，消除滚转角和俯仰角误差，并保持高度基本恒定，总体跟踪速度较快。

(2)因为非线性滑模面的固有特性，使得辅助面构成的正不变集在系统状态空间扩大了控制区域。这样，凭借扩大的正不变集控制范围广以及辅助面终端滑模控制鲁棒性强的优势，基于辅助面的正不变集终端滑模进一步拓展了鲁棒性强的优点，使得其特别适合于干扰存在时快速翻滚等大机动飞行控制的强鲁棒要求。

与传统的控制器相比，新型飞行控制系统具有更强的鲁棒性，即使无人机在受到风的干扰的情况下，依然能够保持翻滚等大机动飞行动作的平稳执行。

附图说明

图1表示无人机飞行控制系统分层级联控制器设计原理框图。

图2表示辅助面的终端滑模控制算法结构图。

图3表示切换面S_1i、S_2i切割的四个子空间。

图4表示辅助面的示意图。

图5表示姿态跟踪控制响应曲线图，其中图5(a)表示滚转角响应曲线，图5(b)表示俯仰角响应曲线。

图6表示高度跟踪控制响应曲线图。

具体实施方式

下面结合附图对本发明创造做进一步详细说明。

图1是无人机飞行控制系统的分层级联控制器的设计原理框图。分层级联控制器包含4个子控制器，他们分别是①位置控制器，②速度控制器，③姿态控制器，④角速率控制器。这4个控制器通过串联，构成了整个无人机的飞行控制系统。

图2是辅助面的终端滑模控制算法结构图。辅助面的终端滑模控制算法包括两条切换面S_1i、S_2i和四条辅助面h_0i、h_1i、h_2i、h_3i。

图3是切换面S_1i和S_2i切割的四个子空间。切换面S_1i和S_2i把空间切割成编号为0_i，1_i，2_i，3_i的四个子空间。

图4是辅助面的示意图。在切换面S_1i和S_2i上确定4个点P_s1i+，P_s1i-，P_s2i+，P_s2i-，由这四个点两两相连的连线P_s1i+P_s2i+，P_s1i-P_s2i-，P_s1i+P_s2i-，P_s1i-P_s2i+构成四条辅助面h_0i、h_1i、h_2i、h_3i。

图5是姿态跟踪控制响应图，图5(a)是滚转角的跟踪响应曲线，图5(b)是俯仰角的跟踪响应曲线。通过对比可以看出，滑模控制方法(SMC)的响应曲线比比例积分微分控制方法(PID)的响应曲线具有更快的响应速度。

图6是高度跟踪控制响应图，通过对比可以看出，滑模控制方法(SMC)的响应曲线比比例积分微分控制方法(PID)的响应曲线更快的趋于稳定，而且整体的波动幅度比PID方法更小，说明SMC方法具有更好的鲁棒性。

无人机飞行控制系统的设计包括飞行器非线性模型的建立、各个子系统的划分、基于辅助面的终端滑模控制算法在分层级联控制器中的实现。

首先，给出四旋翼无人机的精准的非线性数学模型：

其中m是无人机质量，X,Y,Z是无人机相对坐标原点的位置，

为前向距离，

为侧向距离，

为高度，u为无人机在机体坐标系下的前向速度，ν为无人机在机体坐标系下的侧向高度，w为无人机在机体坐标系下的垂向高度，

为滚转角，θ为俯仰角，ψ为偏航角，为前向速度的导数，为侧向速度的导数，

为垂向速度的导数，p为滚转角速率，q为俯仰角速率，r为偏航角速率，为滚转角的导数，

为俯仰角的导数，

为偏航角的导数，I_x,I_y,I_z分别是绕机体3轴的转动惯量，I_x为绕着X轴的转动惯量，I_y为绕着Y轴的转动惯量，I_z为绕着Z轴的转动惯量，d₁～d₁₂代表无人机因为内部的不确定性以及风的存在而造成的有界干扰，d₁为第一个干扰项，d₂为第二个干扰项，以此类推，d₁₂为第十二个干扰项，前向力F_u＝-m·g·sinθ，侧向力

垂向力

滚转力矩

俯仰力矩

偏航力矩

d,k_T,k_Q是通过测量得到的力和力矩的参系数，d为拉力和力矩的产生系数，k_T为拉力系数，k_Q为力矩系数，g为重力加速度，电机1的转速Ω₁＝k_M·u₁+w_M，电机2的转速Ω₂＝k_M·u₂+w_M，电机3的转速Ω₃＝k_M·u₃+w_M，电机4的转速Ω₄＝k_M·u₄+w_M，控制器的总输出u₁,u₂,u₃,u₄分别代表控制4个电机转速的控制量，k_M是归一化为PWM波的电机角速度，w_M是归一化为PWM波的电机截距值。

根据无人机的非线性数学模型，我们将整个无人机系统划分成4个子系统：

1).位置子系统：

其中x_pos＝[X_e,Y_e,Z_e]^T，u_pos＝[u_ref,v_ref,w_ref]^T，f_pos(x_pos)＝0，d_pos＝[0,0,0]^T，

，c是cos的缩写，s是sin的缩写，X_e为前向位置的误差值，Y_e为侧向位置的误差值，Z_e为垂向位置的误差值，u_ref为前向速度的参考给定值，v_ref为侧向速度的参考给定值，w_ref为垂向速度的参考给定值，T为转置符号。

2).速度子系统：

其中x_vel＝[u_e,v_e,w_e]^T，d_vel＝[d₄,d₅,d₆]^T，g_vel(x_vel)＝[g,-g·cosθ_ref,k_T/m]^T，

u_e为前向速度的误差值，v_e为侧向速度的误差值，w_e为垂向速度的误差值，

为滚转角的参考给定值，θ_ref为俯仰角的参考给定值，δ_col为总距值，d₄为第四个干扰项，d₅为第五个干扰项，d₆为第六个干扰项。

3).姿态子系统：

其中f_atu(X_atu)＝0,u_atu＝[p_ref,q_ref,r_ref]^T,and d_atu＝[0,0,0]^T，

t是tan的缩写，s是sin的缩写，c是cos的缩写。为滚转角误差值，θ_e为俯仰角误差值，ψ_e为偏航角误差值，p_ref为滚转角速率的参考给定值，q_ref为俯仰角速率的参考给定值，r_ref为偏航角速率的参考给定值。

4).角速率子系统：

其中X_ars＝[p_e,q_e,r_e]^T，u_Mars＝[M_x,M_y,M_z]^T，d_ars＝[d₁₀,d₁₁,d₁₂]^T，g_ars(X_ars)＝[1/I_x,1/I_y,1/I_z]^T,f_ars(X_ars)＝[(I_y-I_z)qr/I_x,(I_z-I_x)pr/I_y,(I_x-I_y)pq/I_z]^T，p_e为滚转角误差值，q_e为俯仰角误差值，r_e为偏航角误差值，M_x为滚转力矩，M_y为俯仰力矩，M_z为偏航力矩，d₁₀为第十个干扰项，d₁₁为第十一个干扰项，d₁₂为第十二个干扰项。

基于公式(20)、(21)、(22)、(23)，归纳得到各个子系统的统一表达式为：

其中x＝[x₁,x₂,x₃]^T是子系统的状态误差,其中：x₁为状态误差的第一项，x₂为状态误差的第二项，x₃为状态误差的第三项，u＝[u₁,u₂,u₃]^T是子系统的控制输入，其中：u₁为输入向量的第一项，u₂为输入向量的第二项，u₃为输入向量的第三项，d＝[d₁,d₂,d₃]^T是子系统受到的内部不确定和外界干扰的总和，其中：d₁为干扰向量的第一项，d₂为干扰向量的第二项，d₃为干扰向量的第三项，f(x)∈R^3×3和g(x)∈R^3×3是连续函数。

其次，根据非线性数学模型的表达式(16)、(17)、(18)、(19)，无人机飞行控制系统的分层级联控制器的设计原理框图如图1所示。分层级联控制器包含4个子控制器，他们分别是①位置控制器，②速度控制器，③姿态控制器，④角速率控制器。这4个控制器通过串联，构成了整个无人机的飞行控制系统。

再次，如图2所示，辅助面的终端滑模控制算法包括两条切换面S_1i、S_2i和四条辅助面h_0i、h_1i、h_2i、h_3i。基于辅助面的终端滑模控制算法设计步骤如下：

步骤1：对系统状态X_e，设计非线性切换面s_1i,s_2i为

其中s₁＝[s₁₁,s₁₂,s₁₃]^T,s₁₁为系统切换面s₁的第一个切换面，s₁₂为系统切换面s₁的第二个切换面，s₁₃为系统切换面s1的第三个切换面，s₂＝[s₂₁,s₂₂,s₂₃]^T,s₂₁为系统切换面s₂的第一个切换面，s₂₂为系统切换面s₂的第二个切换面，s₂₃为系统切换面s₂的第三个切换面，X_e＝[x₁,x₂,x₃]^T,x₁为系统状态X_e的第一个分量，x₂为系统状态X_e的第二个分量，x₃为系统状态X_e的第三个分量，∫X_e＝[∫x₁,∫x₂,∫x₃]^T,符号∫x_e表示∫x_edt，∫x₁为系统状态积分∫X_e的第一个分量，∫x₂为系统状态积分∫X_e的第二个分量，∫x₃为系统状态积分∫X_e的第三个分量，切换面的系数为ck₁＝diag{ck₁₁,ck₁₂,ck₁₃},ck₁₁为系数ck₁的第一个分量，ck₁₂为系数ck₁的第二个分量，ck₁₃为系数ck₁的第三个分量，ck₂＝diag{ck₂₁,ck₂₂,ck₂₃}，ck₂₁为系数ck₂的第一个分量，ck₂₂为系数ck₂的第二个分量，ck₂₃为系数ck₂的第三个分量，ck₃＝diag{ck₃₁,ck₃₂,ck₃₃},ck₃₁为系数ck₃的第一个分量，ck₃₂为系数ck₃的第二个分量，ck₃₃为系数ck₃的第三个分量，ck_1i＞ck_2i＞0,ck_3i＞0,i＝1,2,3。ξ＝q/p,ξ是吸引子，p和q是正奇数且p＞q。

步骤2：令z₁＝X_e＝[z₁₁,z₁₂,z₁₃]^T，z₁₁为z₁的第一个分量，z₁₂为z₁的第二个分量，z₁₃为z₁的第三个分量，z₂＝∫X_e+ck₃·∫X_e ^ξ，方程(25)又可以表示为

步骤3：基于切换面s_1i和s_2i，定义编号为0_i，1_i，2_i，3_i的子空间如图3所示，其数学表达式为：

No.0_i子空间＝{(z_1i,z_2i)|s_1i＜0,s_2i＜0}；No.1_i子空间＝{(z_1i,z_2i)|s_1i＜0,s_2i≥0}

No.2_i子空间＝{(z_1i,z_2i)|s_1i≥0,s_2i＜0}；No.3_i子空间＝{(z_1i,z_2i)|s_1i≥0,s_2i≥0} (27)

其中：z_1i代表z₁₁、z₁₂、z₁₃中的任意一个，z_2i代表z₂₁、z₂₂、z₂₃中的任意一个，s_1i代表s₁₁、s₁₂、s₁₃中的任意一个，s_2i代表s₂₁、s₂₂、s₂₃中的任意一个，i∈[1,2,3]。

接着，在切换面s_1i和s_2i上确定4个点P_s1i+,P_s1i-,P_s2i+和P_s2i-(如图4所示)满足如下条件：

s_1i(P_s1i+)＝0；s_1i(P_s1i-)＝0；s_2i(P_s2i+)＝0；s_2i(P_s2i-)＝0； (28)

由P_s1i+,P_s1i-,P_s2i+和P_s2i-两两相连的连线P_s1i-P_s2i-,P_s1i+P_s2i-,P_s1i-P_s2i+,P_s1i+P_s2i+构成四条辅助面h_0i,h_1i,h_2i,h_3i，其数学表达式为：

h_ki＝ω_ki1z_1i+ω_ki2z_2i+m_i (29)

其中ω_ki1≠0,ω_ki2≠0,ω_ki1和ω_ki2是切换系数，z_1i代表z₁₁、z₁₂、z₁₃中第i个状态量，z_2i代表z₂₁、z₂₂、z₂₃中第i个状态量，m_i＞0，m_i是常量，h_ki为第k子空间的辅助面，下标k表示第几个子空间且k＝0,1,2,3，下标i表示系统状态X_e的第i个状态变量，切换系数ω_ki1,ω_ki2需要满足如下条件：

ω_0i1＝-ω_3i1,ω_0i2＝-ω_3i2,ω_1i1＝-ω_2i1,ω_1i2＝-ω_2i2 (30)

其中ω_0i1为第i个状态量对应的辅助面在第0子空间的第一个系数，ω_0i2为第i个状态量对应的辅助面在第0子空间的第二个系数，ω_1i1为第i个状态量对应的辅助面在第1子空间的第一个系数，ω_1i2为第i个状态量对应的辅助面在第1子空间的第二个系数，ω_3i1为第i个状态量对应的辅助面在第3子空间的第一个系数，ω_3i2为第i个状态量对应的辅助面在第3子空间的第二个系数，ω_2i1为第i个状态量对应的辅助面在第2子空间的第一个系数，ω_2i2为第i个状态量对应的辅助面在第2子空间的第二个系数。

步骤4：当系统状态进入子空间0i，1i，2i，3i后，图4中的辅助面就可以被用来设计控制器输出u。状态Z_i的当前辅助面被定义为：

h_i＝ω_i1z_1i+ω_i2z_2i+m_i,i＝1,2,3 (31)

其中m_i＞0并且

其中K_INF和K_UAS为需要计算得到的切换系数，ω_0i1为系统状态z_1i在第0子空间的切换系数，ω_1i1为系统状态z_1i在第1子空间的切换系数，ω_2i1为系统状态z_1i在第2子空间的切换系数，ω_3i1为系统状态z_1i在第3子空间的切换系数，ω_0i2为系统状态z_2i在第0子空间的切换系数，ω_1i2为系统状态z_2i在第1子空间的切换系数，ω_2i2为系统状态z_2i在第2子空间的切换系数，ω_3i2为系统状态z_2i在第3子空间的切换系数，式(32)中ω_i1和ω_i2的计算步骤如下：

(a)在S_2i上确定P_s2i+(-ck₂,1)和P_s2i-(ck₂,-1)；

(b)计算第3i子空间的辅助面h_3i的斜率K_UAS＝0.5·(0.6·ck₂+0.4·ck₁)∈[ck₂,ck₁]；

(c)基于(29)，根据K_UAS＝ω_3i2/ω_3i1和P_s2i+(-ck₂,1)，我们得到：

ω_3i1＝1,ω_3i2＝K_UAS,m_i＝-(1·(-CK₂)+K_UAS·1) (33)

(d)计算h_3i和S_1i的交点P_s1i+(X_p1,Y_p1)；

(e)根据点P_s1i+(X_p1,Y_p1)和P_s2i-(ck₂,-1)，我们得到：

h_1i＝ω_1i1z_1i+ω_1i2z_2i+m₁

其中ω_1i1＝K_INF＝(Y_p1-Y_p2)/(X_p1-X_p2),ω_1i2＝-1,X_p2＝ck₂,Y_p2＝-1，X_p1为辅助面h_3i和滑模面S_1i交点的横坐标，Y_p1为辅助面h_3i和滑模面S_1i交点的纵坐标，m₁为辅助面h_1i的常量系数，。

通过上述五个步骤和条件(30)，我们得到：

其中K_INF和K_UAS为上述计算得到的切换系数。相应的，当前辅助面可以被写成紧凑的形式：

h＝Ω₁z₁+Ω₂z₂+m (34)

其中h＝[h₁,h₂,h₃]^T,h₁为第一个状态量的辅助面，h₂为第二个状态量的辅助面，h₃为第三个状态量的辅助面，Ω₁＝diag{ω₁₁,ω₂₁,ω₃₁}，ω₁₁为第一个状态量的辅助面中对应的第一个系数,ω₂₁为第二个状态量的辅助面中对应的第一个系数，ω₃₁为第三个状态量的辅助面中对应的第一个系数，Ω₂＝diag{ω₁₂,ω₂₂,ω₃₂},ω₁₂为第一个状态量的辅助面中对应的第二个系数,ω₂₂为第二个状态量的辅助面中对应的第二个系数，ω₃₂为第三个状态量的辅助面中对应的第二个系数，m＝[m₁,m₂,m₃]^T，m₁为第一个状态量的辅助面中对应的常量系数，m₂为第二个状态量的辅助面中对应的常量系数，m₃为第三个状态量的辅助面中对应的常量系数。

步骤5：无抖震趋近律N_i被设计为：

其中：k_i＞0,a_i＝-ω_0i2/ω_0i1＝-ω_3i2/ω_3i1,b_i＝-ω_1i2/ω_1i1＝-ω_2i2/ω_2i1，ck_3i为公式(25)系数ck₃的第i个分量，

步骤6：通过解算微分方程(21)

其中：

为状态误差z₁的导数，

为状态误差z₂的导数，

为常量m的导数，d为干扰项，

我们得到针对通用子系统(24)的辅助面滑模控制器的输出为

其中Ω₁，Ω₂可以在(34)得到，N＝[N₁,N₂,N₃]^T是趋近律且N_i＞sup{-ω_i1·d_i}，i＝1,2,3，N₁为趋近律的第一个分量，N₂为趋近律的第二个分量，N₃为趋近律的第三个分量，ω_i1为辅助面的第一个系数，d_i为第i个干扰项，。

通过上述6个步骤，我们可以得到每个电机转速平方的表达式为：

x₁＝(M_y/(d·k_T)+M_z/k_Q+δ_col+M_x/(d·k_T))/4；

x₂＝(M_y/(d·k_T)-M_z/k_Q+δ_col-M_x/(d·k_T))/4；

x₃＝(δ_col+M_z/k_Q-M_x/(d·k_T)-M_y/(d·k_T))/4；

x₄＝(δ_col+M_x/(d·k_T)-M_y/(d·k_T)-M_z/k_Q)/4； (36)

其中：δ_col为四个旋翼产生的总升力，

将(38)转化成对每个电机所施加的控制输出，表达式为：

其中：u₁为对电机1产生的控制输出，u₂为对电机2产生的控制输出，u₃为对电机3产生的控制输出，u₄为对电机4产生的控制输出。

通过上述算法得到无人机的控制信号，即是对前后左右四个电机的控制量。该控制信号输出到无人机电机系统中，能够控制飞机快速进入平稳飞行状态。

本发明针对某四旋翼无人机进行了真实飞行验证。参考姿态指令中滚转角和俯仰角给定为0度，参考高度指令给定为3.2米，无人机初始姿态为任意值，高度初始值任意。需要利用非线性控制系统，使得无人机能够用最短的时间快速跟踪上给定姿态指令和高度指令，从而保证无人机在抛飞过程中不落地且能够保持平稳飞行状态。通过试飞得到了无人机姿态和高度的跟踪响应结果，并将其与PID控制方法进行了对比，试验结果如图5和6所示。

Claims (1)

1.一种无人机在极端初始状态的快速平衡控制方法，包括如下步骤：

(1)无人机上电，对陀螺仪、磁力计、加速度计进行校准，并检查电机解锁后是否运转正常，如果不正常，对无人机下电进行相关检查；

(2)各个传感器校准完毕，电机解锁后一切正常，手持无人机准备进行抛飞；

(3)向上以任意方向抛出无人机，当无人机在空中自由落体的时候，拨动遥控器档位，使得遥控器档位设置的无人机飞行模式处于抛飞状态；

(4)无人机自由落体时，接收到遥控器的档位变化，根据各个传感器反馈的信息，飞行控制器调用算法，使得自身快速进入平稳飞行状态；

其特征在于，步骤(4)中所述飞行控制器调用算法分别在飞行控制器的各个子控制器内部进行运算，按照角速率子控制器、姿态子控制器、速度子控制器、位置子控制器的顺序依次进行解算，具体设计步骤如下：

步骤1：对系统状态X_e，设计非线性切换面s_1i,s_2i为

其中s₁＝[s₁₁,s₁₂,s₁₃]^T,s₁₁为系统切换面s₁的第一个切换面，s₁₂为系统切换面s₁的第二个切换面，s₁₃为系统切换面s₁的第三个切换面，s₂＝[s₂₁,s₂₂,s₂₃]^T,s₂₁为系统切换面s₂的第一个切换面，s₂₂为系统切换面s₂的第二个切换面，s₂₃为系统切换面s₂的第三个切换面，X_e＝[x₁,x₂,x₃]^T,x₁为系统状态X_e的第一个分量，x₂为系统状态X_e的第二个分量，x₃为系统状态X_e的第三个分量，∫X_e＝[∫x₁,∫x₂,∫x₃]^T,符号∫x_e表示∫x_edt，∫x₁为系统状态积分∫X_e的第一个分量，∫x₂为系统状态积分∫X_e的第二个分量，∫x₃为系统状态积分∫X_e的第三个分量，切换面的系数为ck₁＝diag{ck₁₁,ck₁₂,ck₁₃},ck₁₁为系数ck₁的第一个分量，ck₁₂为系数ck₁的第二个分量，ck₁₃为系数ck₁的第三个分量，ck₂＝diag{ck₂₁,ck₂₂,ck₂₃}，ck₂₁为系数ck₂的第一个分量，ck₂₂为系数ck₂的第二个分量，ck₂₃为系数ck₂的第三个分量，ck₃＝diag{ck₃₁,ck₃₂,ck₃₃},ck₃₁为系数ck₃的第一个分量，ck₃₂为系数ck₃的第二个分量，ck₃₃为系数ck₃的第三个分量，ck_1i＞ck_2i＞0,ck_3i＞0,i＝1,2,3，ξ＝q/p,ξ是吸引子，p和q是正奇数且p＞q；

步骤2：令z₁＝X_e＝[z₁₁,z₁₂,z₁₃]^T，z₁₁为z₁的第一个分量，z₁₂为z₁的第二个分量，z₁₃为z₁的第三个分量，

方程又表示为

步骤3：基于切换面s_1i和s_2i，定义编号为0_i，1_i，2_i，3_i的子空间其数学表达式为：

其中：z_1i代表z₁₁、z₁₂、z₁₃中的任意一个，z_2i代表z₂₁、z₂₂、z₂₃中的任意一个，s_1i代表s₁₁、s₁₂、s₁₃中的任意一个，s_2i代表s₂₁、s₂₂、s₂₃中的任意一个，i∈[1,2,3]；

接着，在切换面s_1i和s_2i上确定4个点P_s1i+,P_s1i-,P_s2i+和P_s2i-满足如下条件：

s_1i(P_s1i+)＝0；s_1i(P_s1i-)＝0；s_2i(P_s2i+)＝0；s_2i(P_s2i-)＝0； (28)

由P_s1i+,P_s1i-,P_s2i+和P_s2i-两两相连的连线P_s1i-P_s2i-,P_s1i+P_s2i-,P_s1i-P_s2i+,P_s1i+P_s2i+构成四条辅助面h_0i,h_1i,h_2i,h_3i，其数学表达式为：

h_ki＝ω_ki1z_1i+ω_ki2z_2i+m_i (29)

其中ω_ki1≠0,ω_ki2≠0,ω_ki1和ω_ki2是切换系数，z_1i代表z₁₁、z₁₂、z₁₃中第i个状态量，z_2i代表z₂₁、z₂₂、z₂₃中第i个状态量，m_i＞0，m_i是常量，h_ki为第k子空间的辅助面，下标k表示第几个子空间且k＝0,1,2,3，下标i表示系统状态X_e的第i个状态变量，切换系数ω_ki1,ω_ki2需要满足如下条件：

ω_0i1＝-ω_3i1,ω_0i2＝-ω_3i2,ω_1i1＝-ω_2i1,ω_1i2＝-ω_2i2 (30)

其中ω_0i1为第i个状态量对应的辅助面在第0子空间的第一个系数，ω_0i2为第i个状态量对应的辅助面在第0子空间的第二个系数，ω_1i1为第i个状态量对应的辅助面在第1子空间的第一个系数，ω_1i2为第i个状态量对应的辅助面在第1子空间的第二个系数，ω_3i1为第i个状态量对应的辅助面在第3子空间的第一个系数，ω_3i2为第i个状态量对应的辅助面在第3子空间的第二个系数，ω_2i1为第i个状态量对应的辅助面在第2子空间的第一个系数，ω_2i2为第i个状态量对应的辅助面在第2子空间的第二个系数；

步骤4：当系统状态进入子空间0_i，1_i，2_i，3_i后，辅助面就被用来设计控制器输出u，状态z₁＝X_e的当前辅助面被定义为：

h_i＝ω_i1z_1i+ω_i2z_2i+m_i,i＝1,2,3 (31)

其中m_i＞0并且

其中K_INF和K_UAS为需要计算得到的切换系数，ω_0i1为系统状态z_1i在第0子空间的切换系数，ω_1i1为系统状态z_1i在第1子空间的切换系数，ω_2i1为系统状态z_1i在第2子空间的切换系数，ω_3i1为系统状态z_1i在第3子空间的切换系数，ω_0i2为系统状态z_2i在第0子空间的切换系数，ω_1i2为系统状态z_2i在第1子空间的切换系数，ω_2i2为系统状态z_2i在第2子空间的切换系数，ω_3i2为系统状态z_2i在第3子空间的切换系数，式中ω_i1和ω_i2的计算步骤如下：

(a)在公式(27)中的切换面s _2i 上确定P_s2i+(-ck₂,1)和P_s2i-(ck₂,-1)；

(b)计算第3_i子空间的辅助面h_3i的斜率K_UAS＝0.5·(0.6·ck₂+0.4·ck₁)∈[ck₂,ck₁]；

(c)基于(29) ，根据K_UAS＝ω_3i2/ω_3i1和P_s2i+(-ck₂,1)，我们得到：

ω_3i1＝1,ω_3i2＝K_UAS,m_i＝-(1·(-ck₂)+K_UAS·1) (33)

(d)计算h_3i和公式(27)中的切换面s _1i 的交点P_s1i+(X_p1,Y_p1)；

(e)根据点P_s1i+(X_p1,Y_p1)和P_s2i-(ck₂,-1)，我们得到：

h_1i＝ω_1i1z_1i+ω_1i2z_2i+m₁

其中ω_1i1＝K_INF＝(Y_p1-Y_p2)/(X_p1-X_p2),ω_1i2＝-1,X_p2＝ck₂,Y_p2＝-1，X_p1为辅助面h_3i和公式(27)中的切换面s _1i 交点的横坐标，Y_p1为辅助面h_3i和滑模面s _1i 交点的纵坐标，m₁为辅助面h_1i的常量系数；

通过上述五个步骤和条件，我们得到：

其中K_INF和K_UAS为上述计算得到的切换系数；相应的，当前辅助面被写成紧凑的形式：

h＝Ω₁z₁+Ω₂z₂+m (34)

其中h＝[h₁,h₂,h₃]^T,h₁为第一个状态量的辅助面，h₂为第二个状态量的辅助面，h₃为第三个状态量的辅助面，Ω₁＝diag{ω₁₁,ω₂₁,ω₃₁}，ω₁₁为第一个状态量的辅助面中对应的第一个系数,ω₂₁为第二个状态量的辅助面中对应的第一个系数，ω₃₁为第三个状态量的辅助面中对应的第一个系数，Ω₂＝diag{ω₁₂,ω₂₂,ω₃₂},ω₁₂为第一个状态量的辅助面中对应的第二个系数,ω₂₂为第二个状态量的辅助面中对应的第二个系数，ω₃₂为第三个状态量的辅助面中对应的第二个系数，m＝[m₁,m₂,m₃]^T，m₁为第一个状态量的辅助面中对应的常量系数，m₂为第二个状态量的辅助面中对应的常量系数，m₃为第三个状态量的辅助面中对应的常量系数；

步骤5：无抖震趋近律N_i被设计为：

其中：

k_i＞0,a_i＝-ω_0i2/ω_0i1＝-ω_3i2/ω_3i1,b_i＝-ω_1i2/ω_1i1＝-ω_2i2/ω_2i1，ck_3i为公式系数ck₃的第i个分量，

步骤6：通过解算微分方程

其中：

为状态误差z₁的导数，

为状态误差z₂的导数，

为常量m的导数，d为干扰项；

通用子系统的统一表达式为：

其中X_e＝[x₁,x₂,x₃]^T是子系统的状态误差,其中：x₁为状态误差的第一项，x₂为状态误差的第二项，x₃为状态误差的第三项，u＝[u₁,u₂,u₃]^T是子系统的控制输入，其中：u₁为输入向量的第一项，u₂为输入向量的第二项，u₃为输入向量的第三项，d＝[d₁,d₂,d₃]^T是子系统受到的内部不确定和外界干扰的总和，其中：d₁为干扰向量的第一项，d₂为干扰向量的第二项，d₃为干扰向量的第三项，f(x_e)∈R^3×3和g(x_e)∈R^3×3是系统误差的连续函数；

根据(36)，我们得到针对通用子系统的辅助面滑模控制器的输出为

其中Ω₁，Ω₂在得到，N＝[N₁,N₂,N₃]^T是趋近律且N_i＞sup{-ω_i1·d_i}，i＝1,2,3，N₁为趋近律的第一个分量，N₂为趋近律的第二个分量，N₃为趋近律的第三个分量，ω_i1为辅助面的第一个系数，d_i为第i个干扰项；

通过上述6个步骤，我们得到每个电机转速平方的表达式为：

其中：δ_col为四个旋翼产生的总升力，M_x为滚转力矩，M_y为俯仰力矩，M_z为偏航力矩，d,k_T,k_Q是通过测量得到的力和力矩的参系数，d为飞机重心到电机的距离，k_T为拉力系数，k_Q为力矩系数；

将转化成对每个电机所施加的控制输出，表达式为：

其中：u₁为对电机1产生的控制输出，u₂为对电机2产生的控制输出，u₃为对电机3产生的控制输出，u₄为对电机4产生的控制输出，k_M是归一化为PWM波的电机角速度，w_M是归一化为PWM波的电机截距值。

Images