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

摘要

本发明公开了一种四旋翼飞行器对角动力损失状态下的失控保护控制算法，其步骤为：S1、假设四旋翼飞行器1、3号电机失去动力，则放弃偏航和俯仰角度的控制；S2、基于τθ＝0及俯仰角和横滚角对四旋翼飞行器平稳飞行有着至关重要的作用；选取Fr，F′input＝G′Fr,求解；S3、确定在2个对角电机损坏后，四旋翼飞行器的数学模型；S4、令期望姿态为φd和θd，期望的位置为xd、yd和zd；S5、建立误差系统。

主权项

一种四旋翼飞行器对角动力损失状态下的失控保护控制算法，其特征在于：该四旋翼飞行器失控保护控制算法的步骤为：S1、如果四旋翼飞行器对称的两个螺旋桨损坏，则对角动力损失；假设四旋翼飞行器1、3号电机失去动力，则升力F1＝F3＝0；其中，F表示单个电机配合螺旋桨产生的升力，F1和F3分别表示1、3号电机产生的升力；则模型控制量与电机升力之间的关系式为：Finput＝GFr,\*MERGEFORMAT (0.1)其中：其中，螺旋桨升力系数为cT，螺旋桨转矩系数为cQ；l为电机到飞行器质心的距离；因为rank(G:Finput)＝3＞rank(G)＝2，等式无解；为了能够使方程有解，放弃偏航和俯仰角度的控制；S2、基于τθ＝0及俯仰角和横滚角对四旋翼飞行器平稳飞行有着至关重要的作用；选取下式求解Fr：F′input＝G′Fr,\*MERGEFORMAT (0.2)其中：可得：由上式可知τψ＝‑σFl,τθ＝0；τφ是横滚转矩，Fl是飞行器总升力，τΨ是偏航转矩，τθ是俯仰转矩；S3、确定在2个对角电机损坏后，四旋翼飞行器的数学模型表示为：x·=υxy·=υyυ·x=1m((cφsθcψ+sφsψ)Fl-Dxiυx)υ·y=1m((cφsθsψ-sφcψ)Fl-Dyiυy)\*MERGEFORMAT---(0.4)]]>φ·=p+sφtθq+cφtθrθ·=cφq-sφrψ·=sφcθq+cφcθrz·=υzp·=1Ixx(τφ-(Izz-Iyy)qr-Dφp)q·=1Iyy(-(Ixx-Izz)pr-Dθq)r·=1Izz(-σFl-Dψr)υ·z=g+1m(-(cφcθ)Fl-Dziυz)\*MERGEFORMAT---(0.5)]]>s*＝sin*，c*＝cos*，t*＝tan*；η＝[φ θ ψ]T为欧拉角，用于描述四旋翼飞行器的姿态信息；该欧拉角称为卡尔丹角，采用z‑y‑x轴的顺序旋转定义，分别是绕z轴旋转为偏航角，用ψ表示；绕y轴旋转为俯仰角，用θ表示；绕x轴旋转为横滚角，用φ表示；ξ＝[x y z]T惯性坐标系下四旋翼飞行器的位置；υ＝[υx υy υz]T惯性坐标系下四旋翼飞行器的速度；转动惯量用I＝diag(Ixx,Iyy,Izz),飞行器质量用m表示，ω＝[p q r]T体坐标系下四旋翼飞行器绕轴角速度；S4、令期望姿态为φd，期望的位置为xd、yd和zd；建立如下的误差系统：e1＝φ‑φd,e7＝x‑xd,e9＝y‑yd,e11＝z‑zd,由上式可得：将式(1.7)和式(1.8)中的位置误差重新组合为下式所示：eh1=12(e72+e92),eh2=e·h1+kh1eh1′\*MERGEFORMAT---(0.10)]]>由此可得