[发明专利]四旋翼飞行器对角动力损失状态下的失控保护控制算法

权利要求书

1.一种四旋翼飞行器对角动力损失状态下的失控保护控制算法，其特征在于：该四旋翼飞行器失控保护控制算法的步骤为：

S1、如果四旋翼飞行器对称的两个螺旋桨损坏，则对角动力损失；假设四旋翼飞行器1、3号电机失去动力，则升力F₁＝F₃＝0；

其中，F表示单个电机配合螺旋桨产生的升力，F₁和F₃分别表示1、3号电机产生的升力；

则模型控制量与电机升力之间的关系式为：

F_input＝GF_r, (0.1)

其中：F₂和F₄分别表示2、4号电机产生的升力；

其中，螺旋桨升力系数为c_T，螺旋桨转矩系数为c_Q；l为电机到飞行器质心的距离；

因为rank(G:F_input)＝3＞rank(G)＝2，等式无解；

为了能够使方程有解，放弃偏航和俯仰角度的控制；

S2、基于τ_θ＝0及俯仰角和横滚角对四旋翼飞行器平稳飞行有着至关重要的作用；选取下式求解F_r

F′_input＝G′F_r, (0.2)

其中：

可得：

由上式可知τ_ψ＝-σF_l,τ_θ＝0；τ_φ是横滚转矩，F_l是飞行器总升力，τ_Ψ是偏航转矩，τ_θ是俯仰转矩；

S3、确定在2个对角电机损坏后，四旋翼飞行器的数学模型表示为：

s*＝sin*，c*＝cos*，t^*＝tan^*；

η＝[φ θ ψ]^T为欧拉角，用于描述四旋翼飞行器的姿态信息；该欧拉角称为卡尔丹角，采用z-y-x轴的顺序旋转定义，分别是绕z轴旋转为偏航角，用ψ表示；绕y轴旋转为俯仰角，用θ表示；绕x轴旋转为横滚角，用φ表示；

ξ＝[x y z]^T惯性坐标系下四旋翼飞行器的位置；υ＝[υ_x υ_y υ_z]^T惯性坐标系下四旋翼飞行器的速度；转动惯量用I＝diag(I_xx,I_yy,I_zz),飞行器质量用m表示，

ω＝[p q r]^T体坐标系下四旋翼飞行器绕轴角速度；

g为重力系数；

和分别表示坐标轴x，y和z方向的阻力系数；

D_φ，D_θ和D_ψ分别表示欧拉角φ，θ和ψ方向的阻力系数；

S4、令期望姿态为φ_d，期望的位置为x_d、y_d和z_d；

建立如下的误差系统：

e₁＝φ-φ_d,

e₇＝x-x_d,

e₉＝y-y_d,

e₁₁＝z-z_d,

由上式可得：

其中k_i(i＝1…12)为控制参数；

将式(0.7)和式(0.8)中的位置误差重新组合为下式所示：

由此可得其中k_h1为控制参数。