CN102902277B - 一种六旋翼飞行器有一个电机停机时的安全着陆方法 - Google Patents

Abstract

一种六旋翼飞行器有一个电机停机时的安全着陆方法，该方法有三大步骤：步骤一：六旋翼飞行器姿态和高度控制器设计；步骤二：六旋翼飞行器所有电机正常工作时控制分配器设计；步骤三：当六旋翼飞行器有一个电机停机时的控制重分配。六旋翼飞行器有一个电机停机后其全状态是不可控的，但是当放弃偏航通道后，其俯仰和滚转通道依然可控。本发明即在此基础上采用控制重分配的方式设计六旋翼飞行器有一个电机停机的安全着陆控制器。通过Matlab仿真和真实的实验可以验证本发明是可行的。它在飞行控制技术领域里有较好的实用价值和应用前景。

Description

一种六旋翼飞行器有一个电机停机时的安全着陆方法

技术领域

本发明涉及一种六旋翼飞行器有一个电机停机时的安全着陆方法，该发明属于飞行控制技术领域。

背景技术

在飞行控制领域，确保飞行器的飞行安全是十分重要和必要的，尤其在飞行器发生故障时保证飞行器的安全降落，这会最大限度的降低损失。六旋翼飞行器发生故障概率最大的部件就是其旋翼，所以设计一种在六旋翼飞行器有一个电机停机时的安全着陆控制器是非常有意义的。

六旋翼飞行器在一个旋翼停机后是全状态不可控的。但是当放弃偏航通道的控制时，其俯仰和滚转通道依然可控，从而可以控制其安全的着陆。

发明内容

本发明提供了一种六旋翼飞行器有一个电机停机时的安全着陆方法。它解决了当六旋翼飞行器有一个电机停机时的安全着陆问题。

本发明中采用的六旋翼飞行器模型如下：

如图1所示，该六旋翼飞行器的六个旋翼绕质心对称分布，其旋翼的旋转方向如图。S_I={e_x,e_y，e_z}表示惯性坐标系，S_b={e₁,e₂,e₃}表示机体坐标系。可以导出六旋翼的姿态线性动态模型可表示为：

$\stackrel{\·}{x}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]\mathrm{\Ω}---\left(1\right)$

$\stackrel{\·}{\mathrm{\Ω}}=J_f^{-1}{\mathrm{\τ}}_a$

其中x=(φ，θ，ψ)^T为滚转-俯仰-偏航角。Ω=(p,q,r)^T为飞行器绕机体轴的角速度。J_f∈R^3×3为惯性矩阵，可表示为 $J_f=\left[\begin{array}{ccc}{J}_x& 0& 0\\ 0& J_y& 0\\ 0& 0& J_z\end{array}\right],$ J_x,J_y,J_z分别表示六旋翼飞行器绕对应机体轴的转动惯量。τ_a=(L,M,N)^T为电机产生的绕机体轴的转动力矩，分别可表示为 其中d>0为电机到质心的距离，而f=(f₁,f₂…f₆)表示电机产生的旋翼升力。k_u为一个与空气密度、旋翼的尺寸、形状、扭转角以及一些其他因素有关的参数，在此假设为常值。b_l∈R^6×1,b_m∈R^6×1,b_n∈R^6×1为系数矩阵，按该模型其值为

$b_l={(0,-\frac{\sqrt{3}}{2},-\frac{\sqrt{3}}{2},0,\frac{\sqrt{3}}{2},\frac{\sqrt{3}}{2})}^T$

$b_m={(1,\frac{1}{2},-\frac{1}{2},-1,-\frac{1}{2},\frac{1}{2})}^T---\left(2\right)$

b_n=(-1,1,-1,1,-1,1)^T

六旋翼飞行器的高度动态模型可由下面的微分方程给出：

$\stackrel{\·}{h}=V_h$

${\stackrel{\·}{V}}_h=g-\frac{1}{m}F\cos \mathrm{\φ}\cos \mathrm{\θ}---\left(3\right)$

其中m为六旋翼质量，V_h为六旋翼的垂直速度。g为重力加速度。F为电机产生的总推力，可表示为其中系数矩阵b_f=(1,1,1,1,1,1)^T∈R^6×1。

当六旋翼飞行器有一个电机停机时，设该电机为第i个电机i∈{1,2,3,4,5,6}。定义b_f[i],b_l[i],b_m[i],b_n[i]和f_[i]分别为从b_f,b_l,b_m,b_n和f中去除第i个元素后的向量。则总推力F_[i]和转动力矩τ_a[i]=(L_[i]，M_[i]，N_[i])^T可以表示为

$F_{\left[i\right]}=b_{f\left[i\right]}^T{f}_{\left[i\right]},L_{\left[i\right]}=d{b}_{l\left[i\right]}^T{f}_{\left[i\right]},M_{\left[i\right]}=d{b}_{m\left[i\right]}^T{f}_{\left[i\right]},N_{\left[i\right]}=k_{\mathrm{\μ}}{b}_{n\left[i\right]}^T{f}_{\left[i\right]}---\left(4\right)$

由于六旋翼飞行器是通过六个控制输入来控制四个状态，所以它是过驱动的。通常并不直接设计关于每个电机控需要产生的旋翼升力f的控制律。而是先根据控制目标r设计关于虚拟控制量的控制律，其中F_v,τ_av=(L_v,M_v，N_v)^T分别为六旋翼期望的总推力和转动力矩。再通过控制分配器得到每个电机的需要产生的旋翼升力f，控制器框架如附图2所示。本发明提出的当六旋翼有一个电机停机时的安全着陆控制器，并不改变虚拟控制量的控制律，而是通过控制重分配的方式来解决这一问题。在本发明中控制分配器采用经典的伪逆法来实现。

本发明一种六旋翼飞行器有一个电机停机时的安全着陆方法，该方法具体步骤如下：

步骤一：六旋翼飞行器姿态和高度控制器设计

六旋翼飞行器姿态控制器可采用如下控制器：

τ_a=-K_ΩΩ-K_qq     (5)

其中K_Ω∈R^3×3为对称正定阵，Ω为六旋翼飞行器绕机体轴的角速度。K_q∈R是正系数。四元数(q₀,q^T)^T，其中q₀∈R,q∈R³表示六旋翼的方向。用欧拉角表示的姿态角x=(ψ,θ，φ)^T与四元数的对应关系如下：

q₀=cos(φ/2)cos(θ/2)cos(ψ/2)+sin(φ/2)sin(θ/2)sin(ψ/2)

q₁=sin(φ/2)cos(θ/2)cos(ψ/2)-cos(φ/2)sin(θ/2)sin(ψ/2)           (6)

q₂=cos(φ/2)sin(θ/2)cos(ψ/2)+sin(φ/2)cos(θ/2)sin(ψ/2)

q₃=cos(φ/2)cos(θ/2)sin(ψ/2)+sin(φ/2)sin(θ/2)cos(ψ/2)

六旋翼飞行器的高度控制采用的控制器为：

$F=\frac{m}{\cos \mathrm{\φ}\cos \mathrm{\θ}}(-K_P(h-h_0)-K_D{V}_h-g)---\left(7\right)$

其中m为飞行器质量，K_P,K_D为正系数，h为飞行器高度，h₀为期望的高度，V_h为垂直速度，g为重力加速度。

步骤二：六旋翼飞行器所有电机正常工作时控制分配器设计

虚拟控制量u和旋翼升力f之间的映射关系可由下列等式表示：

$u=\mathrm{Wf},W=\left[\begin{array}{c}{b}_f^T\\ d{b}_l^T\\ d{b}_m^T\\ k_u{b}_n^T\end{array}\right]---\left(8\right)$

其中W称为控制效率矩阵。通过伪逆矩阵法可以得到期望的旋翼升力，其计算公式如下：

f=W^T(WW^T)^-1u(9)

步骤三：当六旋翼飞行器有一个电机停机时的控制重分配

当六旋翼飞行器有一个电机i,i∈{1,2,3,4,5,6}停机后，由公式（8）可导出虚拟控制量u和电机升力f_[i]的映射关系如下：

$u=W_{\left[i\right]}{f}_{\left[i\right]},W_{\left[i\right]}=\left[\begin{array}{c}{b}_{f\left[i\right]}^T\\ d{b}_{l\left[i\right]}^T\\ d{b}_{m\left[i\right]}^T\\ k_u{b}_{n\left[i\right]}^T\end{array}\right]---\left(10\right)$

其中W_[i]为当电机i停机时的控制效率矩阵。由伪逆矩阵法可得从虚拟控制量u到旋翼升力f_[i]的分配关系如下：

$f_{\left[i\right]}=W_{\left[i\right]}^T{\left(W_{\left[i\right]}{W}_{\left[i\right]}^T\right)}^{-1}u---\left(11\right)$

然而，当六旋翼飞行器有一个电机停机时全状态是不可控的，但是当放弃偏航通道，并且满足总推力其中K为每个电机提供的最大升力，其俯仰和滚转通道依然可控，从而可以控制其安全着陆。所以由公式（11）可得控制重分配为

$f_{\left[i\right]}=W_{\left[i\right]}^T{\left(W_{\left[i\right]}{W}_{\left[i\right]}^T\right)}^{-1}{u}^{*}---\left(12\right)$

其中u^*=(F_v,L_v,M_v,0)^T，L_v,M_v和F_v分别由控制律（5）和（7）产生。

优点及功效：本发明一种六旋翼飞行器有一个电机停机时的安全着陆方法，该方法的优点是：成功解决了六旋翼飞行器有一个电机停机时的安全着陆问题。当六旋翼飞行器有一个电机停机时并不需要采用新的控制器，只是放弃偏航通道，即改变控制分配就可以实现六旋翼飞行器的安全着陆。

附图说明

图1：六旋翼飞行器模型示意图

图2：六旋翼飞行器控制器框架示意图

图3：六旋翼飞行器健康时控制仿真结果图

图4：六旋翼飞行器2号电机停机并采用原有控制器仿真结果图

图5：六旋翼飞行器2号电机停机放弃偏航通道控制仿真结果图

图6：本发明流程框图

图7：真实实验采用的六旋翼飞行器

图8：六旋翼飞行器真实实验数据图

图中符号说明如下：

图1中的符号说明：O_I表示惯性坐标系原点，e_x,e_y,e_z为惯性坐标系的坐标轴。O_b表示六旋翼飞行器机体坐标系的原点，e₁,e₂,e₃为机体坐标系的坐标轴。Front、Right、Rear表示六旋翼飞行器的方向，分别为前、左、后。Roll、Pitch、Yaw表示六旋翼飞行器的三种姿态运动，分别为滚转、俯仰和偏航。φ，θ,ψ为六旋翼飞行器的姿态角，分别为滚转角、俯仰角和偏航角。f_i,i＝{1,2,3,4,5,6}表示每个电机产生的升力。ω_i,i＝{1,2,3,4,5,6}为每个电机的角速度，箭头表示旋转方向。Q_i,i＝{1,2,3,4,5,6}表示每个电机上产生的阻尼力矩，箭头表示方向。

图2中的符号说明：r表示控制目标，u表示虚拟控制量，f表示电机需要产生的升力，ω表示电机需要的转动角速度。

具体实施方式

本发明提供了当六旋翼飞行器有一个电机停机时的安全着陆控制器设计方法。

仿真过程是在主频2.66Hz、内存2.00GB的计算机上，win7环境下的Matlab2010b上进行的。实验对象采用的是F550六旋翼机架，好赢28301000KV电机，好赢30A电调，APM自驾仪，3S 3300mAh锂电池.

首先介绍一下仿真中六旋翼飞行器的各个参数，这些参数根据真实六旋翼飞行器测量而得。M=1.535kg,g=9.8m/s²,d=0.275m,k_u=1,K=6.125N，六旋翼飞行器的惯性矩阵为

$J_f=\left[\begin{array}{ccc}{J}_x& 0& 0\\ 0& J_y& 0\\ 0& 0& J_z\end{array}\right]=\left[\begin{array}{ccc}0.0411& 0& 0\\ 0& 0.0478& 0\\ 0& 0& 0.0599\end{array}\right]---\left(13\right)$

（1）见图6，本发明一种六旋翼飞行器有一个电机停机时的安全着陆方法，该方法具体步骤如下：

步骤一：六旋翼飞行器姿态和高度控制器设计

六旋翼飞行器姿态控制器采用如下控制器：

τ_a=-K_ΩΩ-K_qq     (14)

其中K_Ω＝diag{5.5,5.5,5.5}，K_q=100。

六旋翼飞行器的高度控制采用控制器为：

$F=\frac{m}{\cos \mathrm{\φ}\cos \mathrm{\θ}}(-K_P(h-h_0)-K_D{V}_h-g)---\left(15\right)$

其中m为飞行器质量，取K_p=15,K_D=10，h为飞行器高度，h₀为期望的高度，V_h为垂直速度，g为重力加速度。

步骤二：六旋翼飞行器控制分配器设计

虚拟控制量u和旋翼升力f之间的映射关系可由下列等式表示：

$u=\mathrm{Wf},W=\left[\begin{array}{c}{b}_f^T\\ d{b}_l^T\\ d{b}_m^T\\ k_u{b}_n^T\end{array}\right]---\left(16\right)$

其中W称为控制效率矩阵。其中

$b_l={(0,-\frac{\sqrt{3}}{2},-\frac{\sqrt{3}}{2},0,\frac{\sqrt{3}}{2},\frac{\sqrt{3}}{2})}^T$

$b_m={(1,\frac{1}{2},-\frac{1}{2},-1,-\frac{1}{2},\frac{1}{2})}^T---\left(17\right)$

b_n=(-1,1,-1,1,-1,1)^T

b_f=(1,1,1,1,1,1)^T

通过伪逆矩阵法可以得到期望的旋翼升力，其计算公式如下：

f＝W^T(WW^T)^-1u       (18)

步骤三：当六旋翼飞行器有一个电机停机时的控制重分配设计

当六旋翼飞行器有一个电机i,i∈{1,2,3,4,5,6}停机后，虚拟控制量u和电机升力f_[i]的映射

关系如下：

$u=W_{\left[i\right]}{f}_{\left[i\right]},W_{\left[i\right]}=\left[\begin{array}{c}{b}_{f\left[i\right]}^T\\ d{b}_{l\left[i\right]}^T\\ d{b}_{m\left[i\right]}^T\\ k_u{b}_{n\left[i\right]}^T\end{array}\right]---\left(19\right)$

其中b_f[i],b_l[i],b_m[i],b_n[i]和f_[i]分别为从b_f,b_l,b_m,b_n和f中去除第i个元素后的向量。由此可得六旋翼飞行器有一个电机停机后的控制重分配为

$f_{\left[i\right]}=W_{\left[i\right]}^T{\left(W_{\left[i\right]}{W}_{\left[i\right]}^T\right)}^{-1}{u}^{*}---\left(20\right)$

其中u^*=(F_v,L_v,M_v,0)^T，L_v,M_v和F_v分别由控制律（14）和（15）产生。

（2）仿真结果分析

通过该设计方法设计控制器在Matlab上的仿真结果如下。

首先将六旋翼飞行器保持在一定高度（h₀=5），偏航角保持常值。然后再1秒钟时刻使2号电机停转并给出着陆指令。仿真结果如图3，图4和图5所示。图3为没有电机停转情况下的仿真结果，六旋翼飞行器的高度和姿态都可以很好的控制的期望值。图4为2号电机停机但不放弃偏航通道即不进行新的控制重分配的仿真结果，可以看到六旋翼飞行器的姿态偏离期望值，这样六旋翼飞行器在着陆时不能保持水平，这很可能导致事故发生。图5为2号电机停机并放弃偏航通道即采用本发明的控制方法得到的仿真结果，可以看到六旋翼飞行器的俯仰、滚转和高度都能很好的控制到期望值，这将保证六旋翼飞行器的安全着陆。由此可以得出本发明是可行的。

（3）实物实验验证

为了进一步验证本发明的可行性。我们在真实的六旋翼飞行器上进行了测试，采用的六旋翼飞行器如图7所示。六旋翼飞行器真实实验数据如图8。在前1秒钟六旋翼处于稳定模态，在1秒钟以后让2号电机停机，控制器控制六旋翼飞行器的俯仰和滚转通道，放弃偏航通道即本发明的控制方法。从图8中可以看到六旋翼飞行器的俯仰和滚转都可以控制到零附近。从而可以控制六旋翼飞行器安全的着陆。图1为六旋翼飞行器模型示意图，图2为六旋翼飞行器控制器框架示意图。

Claims (1)

1.一种六旋翼飞行器有一个电机停机时的安全着陆方法，其特征在于：该方法具体步骤如下：

步骤一：六旋翼飞行器姿态和高度控制器设计

六旋翼飞行器姿态控制器采用如下控制器：

τ_a＝-K_ΩΩ-K_qq   (5)

其中，K_Ω∈R^3×3为对称正定阵，Ω为六旋翼飞行器绕机体轴的角速度，K_q∈R是正系数，四元数(q₀,q^T)^T，其中q₀∈R,q∈R³表示六旋翼的方向，用欧拉角表示的姿态角x＝(ψ,θ,φ)^T与四元数的对应关系如下：

q₀＝cos(φ/2)cos(θ/2)cos(ψ/2)+sin(φ/2)sin(θ/2)sin(ψ/2)

q₁＝sin(φ/2)cos(θ/2)cos(ψ/2)-cos(φ/2)sin(θ/2)sin(ψ/2)

                                                                  (6)

q₂＝cos(φ/2)sin(θ/2)cos(ψ/2)+sin(φ/2)cos(θ/2)sin(ψ/2)

q₃＝cos(φ/2)cos(θ/2)sin(ψ/2)+sin(φ/2)sin(θ/2)cos(ψ/2)

六旋翼飞行器的高度控制采用的控制器为：

$F=\frac{m}{\cos \mathrm{\φ}\cos \mathrm{\θ}}(-K_P(h-h_0)-K_D{V}_h-g)---\left(7\right)$

其中，m为飞行器质量，K_P,K_D为正系数，h为飞行器高度，h₀为期望的高度，V_h为垂直速度，g为重力加速度；

步骤二：六旋翼飞行器所有电机正常工作时控制分配器设计

虚拟控制量u和旋翼升力f之间的映射关系由下列等式表示：

$u=\mathrm{Wf},W=\left[\begin{array}{c}{b}_f^T\\ {\mathrm{db}}_l^T\\ {\mathrm{db}}_m^T\\ k_u{b}_n^T\end{array}\right]---\left(8\right)$

其中，d＞0为电机到质心的距离；k_u为一个与空气密度、旋翼的尺寸、形状、扭转角以及一些其他因素有关的参数，在此设为常值；其中W称为控制效率矩阵，表示如下：

$b_l={(0,-\frac{\sqrt{3}}{2},-\frac{\sqrt{3}}{2},0,\frac{\sqrt{3}}{2},\frac{\sqrt{3}}{2})}^T$

$b_m={(1,\frac{1}{2},-\frac{1}{2},-1,-\frac{1}{2},\frac{1}{2})}^T$

b_n＝(-1,1,-1,1,-1,1)^T

b_f＝(1,1,1,1,1,1)^T

其中，W称为控制效率矩阵，通过伪逆矩阵法得到期望的旋翼升力，其计算公式如下：

f＝W^T(WW^T)^-1u   (9)

步骤三：当六旋翼飞行器有一个电机停机时的控制重分配

当六旋翼飞行器有一个电机i,i∈{1,2,3,4,5,6}停机后，由公式(8)导出虚拟控制量u和电机升力f_[i]的映射关系如下：

$u=W_{\left[i\right]}{f}_{\left[i\right]},W_{\left[i\right]}=\left[\begin{array}{c}{b}_{f\left[i\right]}^T\\ {\mathrm{db}}_{l\left[i\right]}^T\\ {\mathrm{db}}_{m\left[i\right]}^T\\ k_u{b}_{n\left[i\right]}^T\end{array}\right]---\left(10\right)$

其中，b_f[i],b_l[i],b_m[i],b_n[i]和f_[i]分别为从b_f,b_l,b_m,b_n和f中去除第i个元素后的向量；

其中，W_[i]为当电机i停机时的控制效率矩阵，由伪逆矩阵法可得从虚拟控制量u到旋翼升力f_[i]的分配关系如下：

$f_{\left[i\right]}=W_{\left[i\right]}^T{\left(W_{\left[i\right]}{W}_{\left[i\right]}^T\right)}^{-1}u---\left(11\right)$

然而，当六旋翼飞行器有一个电机停机时全状态是不可控的，但是当放弃偏航通道，并且满足总推力其中K为每个电机提供的最大升力，其俯仰和滚转通道依然可控，从而控制其安全着陆；所以由公式(11)得控制重分配为

$f_{\left[i\right]}=W_{\left[i\right]}^T{\left(W_{\left[i\right]}{W}_{\left[i\right]}^T\right)}^{-1}{u}^{*}---\left(12\right)$

其中，u^*＝(F_v,L_v,M_v,0)^T；L_v,M_v和F_v分别由公式(5)和(7)产生；

其中F_v和τ_av＝(L_v,M_v,N_v)^T分别为六旋翼期望的总推力和转动力矩。