[发明专利]一种无人机在极端初始状态的快速平衡控制方法

权利要求书

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波的电机截距值。