[发明专利]一种四旋翼飞行器姿态的线性自抗扰控制方法

摘要

本发明公开了一种四旋翼飞行器姿态的线性自抗扰控制方法，属于无人飞行器自动控制领域。针对机体刚性且严格对称的四旋翼飞行器建立数学模型，利用线性自抗扰控制器结构简单、参数调节简便结合模型已知动态设计了线性扩张状态观测器(LESO)，把各通道间的动态耦合部分视为系统内部不确定干扰，将其与外界干扰作为作用于系统的未知综合扰动。该观测器只对当前系统的未知扰动进行快速估计，降低了观测器的负担，从而提高了对扰动的估计能力。利用LESO对扰动的估计值在俯仰通道的控制输入中引入相应补偿，既可实现对扰动的抑制，又可实现姿态控制。在此基础上，设计了线性状态反馈控制器对扰动进行在线补偿，实现了稳定的姿态控制。

主权项

一种四旋翼飞行器姿态的线性自抗扰控制方法；其特征在于：包括如下步骤：步骤101：建立四旋翼系统的动力学模型：具体为：针对机体刚性且对称的四旋翼飞行器建立数学模型，根据动量矩定理，飞行器绕质心运动的动力学方程表示为：τ_f‑τ_d‑τ_g＝Jw+w×Jw其中：J＝dig(J_x，J_y，J_z)为机体坐标系下的转动惯量矩阵，J_x、J_y、J_z依次为机体绕滚转轴、俯仰轴、偏航轴的转动惯量；w＝[w_x w_y w_z]^T∈R³为机体坐标系下绕上述机体绕滚转轴、俯仰轴、偏航轴三个轴的角速度；用Φ＝[γ θ ψ]^T表示欧拉角，γ θ ψ依次为滚转角、俯仰角、偏航角；则在小角度姿态下，机体转动力矩来源于：机体受到的升力力矩，空气阻力扭矩，陀螺效应下的扭矩；τ_f为机体受到的升力力矩，表示为：$\begin{array}{c}{\mathrm{\τ}}_f=\left[\begin{array}{c}l\left(F_r-F_l\right)\\ l\left(F_f-F_b\right)\\ M_f+M_b+M_l+M_r\end{array}\right]\\ =\left[\begin{array}{c}{\mathrm{lK}}_f\left(V_r-V_l\right)\\ {\mathrm{lK}}_f\left(V_f-V_b\right)\\ K_{t,n}\left(V_r+V_l\right)+K_{t,c}\left(V_f+V_b\right)\end{array}\right]\end{array}$其中：K_f为执行器驱动旋翼转动产生转矩时，电机电压与旋翼产生升力之间的系数，K_t，n为顺时针力矩系数，K_t，c为逆时针力矩系数，并且K_t，n＝‑K_t，c，l为俯仰轴与旋翼中心的距离，F_i、M_i(i＝f，b，l，r)分别为前旋翼，后旋翼，左旋翼，右旋翼产生的升力和力矩，V_i(i＝f，b，l，r)为四个电机电压；τ_d为空气阻力扭矩，表示为：τ_d＝K_afw其中：K_af为空气阻力系数，且K_af＝dig(K_afx，K_afy，K_afz)τ_g为陀螺效应下的力矩，表达式为：${\mathrm{\τ}}_g={\mathrm{\Σ}}_{i=1}^{4}w\×J_{r}W$其中：J_r为旋翼转子的转动惯量，qi(i＝f，b，l，r)是前旋翼，后旋翼，左旋翼，右旋翼四个旋翼的角速度，且W＝[0 0 q_l+q_r ‑q_b ‑q_f]^T；四旋翼飞行器姿态系统的动态模型为：$\left\{\begin{array}{c}\stackrel{\·\·}{\mathrm{\γ}}=\[{\mathrm{lK}}_f\left(V_r-V_l\right)+\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\ψ}}\left(J_y-J_z\right)-K_{afx}\stackrel{\·}{\mathrm{\γ}}\]/J_x-\stackrel{\·}{\mathrm{\θ}}{J}_{rz}\left(q_l+q_r-q_b-q_f\right)/J_x\\ \stackrel{\·\·}{\mathrm{\θ}}=\[{\mathrm{lK}}_f\left(V_f-V_b\right)+\stackrel{\·}{\mathrm{\γ}}\stackrel{\·}{\mathrm{\ψ}}\left(J_z-J_x\right)-K_{afy}\stackrel{\·}{\mathrm{\θ}}\]/J_y+\stackrel{\·}{\mathrm{\γ}}{J}_{rz}\left(q_l+q_r-q_b-q_f\right)/J_y\\ \stackrel{\·\·}{\mathrm{\ψ}}=\[K_{t,n}\left(V_r+V_l\right)+K_{t,c}\left(V_f+V_b\right)-K_{afz}\stackrel{\·}{\mathrm{\ψ}}\]/J_z+\stackrel{\·}{\mathrm{\γ}}\stackrel{\·}{\mathrm{\θ}}\left(J_x-J_y\right)/J_z\end{array}\right.$步骤102：建立四旋翼姿态的数学模型如下：$\left\{\begin{array}{c}\stackrel{\·\·}{\mathrm{\γ}}=\[{\mathrm{lK}}_f\left(V_r-V_l\right)+\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\ψ}}\left(J_y-J_z\right)-K_{afx}\stackrel{\·}{\mathrm{\γ}}\]/J_x-\stackrel{\·}{\mathrm{\θ}}{J}_{rz}\left(q_l+q_r-q_b-q_f\right)/J_x\\ \stackrel{\·\·}{\mathrm{\θ}}=\[{\mathrm{lK}}_f\left(V_f-V_b\right)+\stackrel{\·}{\mathrm{\γ}}\stackrel{\·}{\mathrm{\ψ}}\left(J_z-J_x\right)-K_{afy}\stackrel{\·}{\mathrm{\θ}}\]/J_y+\stackrel{\·}{\mathrm{\γ}}{J}_{rz}\left(q_l+q_r-q_b-q_f\right)/J_y\\ \stackrel{\·\·}{\mathrm{\ψ}}=\[K_{t,n}\left(V_r+V_l\right)+K_{t,c}\left(V_f+V_b\right)-K_{afz}\stackrel{\·}{\mathrm{\ψ}}\]/J_z+\stackrel{\·}{\mathrm{\γ}}\stackrel{\·}{\mathrm{\θ}}\left(J_x-J_y\right)/J_z\end{array}\right.$当四旋翼飞行器大角度动作时，系统表现为强耦合非线性特性；引入控制量U＝[U₁ U₂ U₃]^T；此时，将整个系统分为滚转通道、俯仰通道、偏航通道，B为多变量变换矩阵：$U=\left[\begin{array}{c}{U}_1\\ U_2\\ U_3\end{array}\right]=\left[\begin{array}{cccc}0& 0& 1& -1\\ 1& -1& 0& 0\\ -1& -1& 1& 1\end{array}\right]\left[\begin{array}{c}{V}_f\\ V_b\\ V_r\\ V_l\end{array}\right]=Bu$同时，将三个通道之间的动态耦合部分视为系统内部不确定扰动，则$\left\{\begin{array}{c}\stackrel{\·\·}{\mathrm{\γ}}={\mathrm{lK}}_f{U}_1/J_x-K_{afx}\stackrel{\·}{\mathrm{\γ}}/J_x+{\mathrm{\ϵ}}_1\\ \stackrel{\·\·}{\mathrm{\θ}}={\mathrm{lK}}_f{U}_2/J_y-K_{afy}\stackrel{\·}{\mathrm{\θ}}/J_y+{\mathrm{\ϵ}}_2\\ \stackrel{\·\·}{\mathrm{\ψ}}=K_{t,n}{U}_3/J_z-K_{afz}\stackrel{\·}{\mathrm{\ψ}}/J_z+{\mathrm{\ϵ}}_3\end{array}\right.$其中ε_i(i＝1，2，3)为系统内部不确定扰动，${\mathrm{\ϵ}}_1=\[\stackrel{\·}{\mathrm{\θ}}{J}_{rz}(q_b+q_f-q_l-q_r)+\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\ψ}}(J_y-J_z)\]/J_x$${\mathrm{\ϵ}}_2=\[\stackrel{\·}{\mathrm{\γ}}{J}_{rz}(q_l+q_r-q_b-q_f)+\stackrel{\·}{\mathrm{\γ}}\stackrel{\·}{\mathrm{\ψ}}(J_z-J_x)\]/J_y$${\mathrm{\ϵ}}_3=\stackrel{\·}{\mathrm{\γ}}\stackrel{\·}{\mathrm{\θ}}(J_x-J_y)/J_z$四旋翼飞行器在实际飞行时，将系统内部不确定扰动和外界扰动作为作用于系统的未知综合扰动，则姿态系统的模型为：$\left\{\begin{array}{c}\stackrel{\·\·}{\mathrm{\γ}}={\mathrm{lK}}_f{U}_1/J_x-K_{afx}\stackrel{\·}{\mathrm{\γ}}/J_x+f_1\\ \stackrel{\·\·}{\mathrm{\θ}}={\mathrm{lK}}_f{U}_2/J_y-K_{afy}\stackrel{\·}{\mathrm{\θ}}/J_y+f_2\\ \stackrel{\·\·}{\mathrm{\ψ}}=K_{t,n}{U}_3/J_z-K_{afz}\stackrel{\·}{\mathrm{\ψ}}/J_z+f_3\end{array}\right.$其中f_i＝ε_i+d_i(i＝1，2，3)为作用于系统的未知综合扰动，d_i为作用于三个通道的外界扰动；步骤103：设计自抗扰控制器；对系统的三个通道分别进行姿态控制；LESO直接利用输入输出信息对系统的状态以及“综合扰动”进行实时估计；设计线性状态反馈控制器在线补偿综合扰动，实现姿态控制；步骤1031：俯仰通道自抗扰姿态控制器的设计；当x₃＝f₂为俯仰通道的扩张状态变量时，f₂可导，则俯仰通道的模型表达为：$\left\{\begin{array}{c}{\stackrel{\·}{x}}_1=x_2\\ {\stackrel{\·}{x}}_2=x_3+{\mathrm{lK}}_f{U}_2/J_y-K_{afy}{x}_2/J_y\\ {\stackrel{\·}{x}}_3=h\\ \mathrm{\θ}=x_1\end{array}\right.$其中：U₂为俯仰通道控制输入，‑K_afyx₂/J_y为系统已知动态，x₁，x₂，x₃依次为俯仰角，俯仰角速度，俯仰通道综合扰动；基于已知俯仰角速度信息设计如下LESO对系统的状态和扰动进行观测：$\left\{\begin{array}{c}{\stackrel{\·}{z}}_1=z_2-{\mathrm{\β}}_1(z_1-x_1)\\ {\stackrel{\·}{z}}_2=z_3-{\mathrm{\β}}_2(z_1-x_1)-a{z}_2+b_0{U}_2\\ {\stackrel{\·}{z}}_3=-{\mathrm{\β}}_3(z_1-x_1)\end{array}\right.$其中：a＝K_afy/J_y，b₀＝lK_f/J_y，z_i(i＝1，2，3)分别为x_i(i＝1，2，3)的观测值，β_i(i＝1，2，3)为观测器增益，将观测器极点都配置到‑w_o，且β₁＝3w_o，β₂＝3w_o²，β₃＝w_o³，w_o为观测器带宽；利用LESO对扰动的估计值在俯仰通道的控制输入中引入相应补偿；设计的线性状态反馈控制器如下：$\left\{\begin{array}{c}{U}_2=(u_2-z_3)/b_0\\ u_2=k_p({\mathrm{\θ}}_d-z_1)-k_d{z}_2\end{array}\right.$其中：θ_d为偏航通道给定值，k_p，k_d为控制器增益，且k_p＝w_c²，k_d＝2w_c，w_c为控制器带宽，u₂为反馈控制量；重复步骤1031，实现滚转通道和偏航通道的自抗扰姿态控制；步骤104：闭环稳定性分析；具体为步骤1041：俯仰通道的闭环稳定性分析基于频域理论，建立俯仰通道的闭环控制回路，对式和式进行拉氏变换，则$\left\{\begin{array}{c}{z}_1\left(s\right)=\frac{3w_o{s}^2+3w_{o}s(a+w_o)+{w_o}^3}{{(s+w_o)}^3+as(s+3w_o)}\mathrm{\θ}\left(s\right)+\frac{b_{0}s}{{(s+w_o)}^3+as(s+3w_o)}{U}_2\left(s\right)\\ z_2\left(s\right)=\frac{3{w_o}^2{s}^2+{w_o}^{3}s}{{(s+w_o)}^3+as(s+3w_o)}\mathrm{\θ}\left(s\right)+\frac{(s+3w_o)b_{0}s}{{(s+w_o)}^3+as(s+3w_o)}{U}_2\left(s\right)\\ z_3\left(s\right)=\frac{({w_o}^3{s}^2+{{\mathrm{aw}}_o}^{3}s)}{{(s+w_o)}^3+as(s+3w_o)}\mathrm{\θ}\left(s\right)-\frac{b_0{w_o}^3}{{(s+w_o)}^3+as(s+3w_o)}{U}_2\left(s\right)\end{array}\right.$$U_2\left(s\right)=\frac{1}{b_0}\left({w_c}^2\right({\mathrm{\θ}}_d\left(s\right)-z_1\left(s\right))-2w_c{z}_2(s)-z_3(s\left)\right)$通过LESO对俯仰通道扰动项估计，则其中：为f₂(s)与z3(s)之间的估计误差；将俯仰通道控制对象记为：$\mathrm{\θ}\left(s\right)=\frac{b_0}{s(s+a)}{U}_2\left(s\right)+z_3\left(s\right)+\widetilde{\mathrm{\θ}}\left(s\right)$得到俯仰角输出θ(s)与俯仰角给定值θ_d(s)和扰动的估计误差之间的关系式：$\mathrm{\θ}\left(s\right)=G_{\mathrm{\θ}}\left(s\right){\mathrm{\θ}}_d\left(s\right)+G_{\widetilde{\mathrm{\θ}}}\left(s\right)\widetilde{\mathrm{\θ}}\left(s\right)$其中$G_{\mathrm{\θ}}\left(s\right)=\frac{{w_c}^2}{s^2+(2w_c+a)s+{w_c}^2}$$G_{\widetilde{\mathrm{\θ}}}\left(s\right)=\frac{s^2+(2w_c+3w_o+a)s+6w_c{w}_o+3{w_o}^2+3{\mathrm{aw}}_o+{w_c}^2}{s^4+c_1{s}^3+c_2{s}^2+c_{3}s+c_4}$c₁＝3w_o+2a+2w_cc₂＝(2w_c+a)(3w_o+a)+3w_o²+3aw_o+w_c²c₃＝(3w_o²+3aw_o)(2w_c+a)+w_c²(3w_o+a)c₄＝w_c²(3w_o²+3aw_o)以θ_d(s)为输入，θ(s)为输出，忽略对扰动的估计误差，则俯仰通道闭环传递函数为：$G_{cl}\left(s\right)=\frac{{w_c}^2}{s^2+(2w_c+a)s+{w_c}^2}$根据Routh判据，闭环系统稳定的充要条件是：$\left\{\begin{array}{c}2w_c+a\>0\\ {w_c}^2\>0\end{array}\right.$由于a>0，w_c>0，则整个俯仰通道闭环系统稳定；重复步骤1041，对滚转通道和偏航通道的闭环稳定性进行分析。