基于滑模控制律和ESO的四旋翼飞行器姿态控制方法及系统与流程

技术特征：

1.一种基于滑模控制律和ESO的四旋翼飞行器姿态控制方法，其特征在于，包括：

根据四旋翼飞行器动力学模型参数推导出四旋翼飞行器动力学模型，根据所述四旋翼飞行器动力学模型构造滑模面，根据所述滑模面获取滑模控制律，以实现对四旋翼飞行器三个姿态角回路的控制；

根据四旋翼的滚转角回路、俯仰角回路和偏航角回路的控制输入输出构建ESO，利用所述ESO对系统总扰动进行实时估计；

采用所述滑模控制律与所述ESO相结合，以实现对所述四旋翼飞行器姿态的控制。

2.根据权利要求1所述的基于滑模控制律和ESO的四旋翼飞行器姿态控制方法，其特征在于，所述四旋翼飞行器动力学模型参数包括转动惯量J_x、J_y、J_z；升力系数c_T；扭矩系数c_Q；电机的时间常数T。

3.根据权利要求1所述的基于滑模控制律和ESO的四旋翼飞行器姿态控制方法，其特征在于，获取所述滑模控制模型包括如下步骤：

(1)将四旋翼视为刚体，则所述四旋翼飞行器姿态非线性动态方程为：

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}=p+qsin\mathrm{\φ}tan\mathrm{\θ}+rcos\mathrm{\φ}tan\mathrm{\θ}\\ \stackrel{\·}{\mathrm{\θ}}=qcos\mathrm{\φ}-rsin\mathrm{\φ}\\ \stackrel{\·}{\mathrm{\ψ}}=\sec \mathrm{\θ}(q\sin \mathrm{\φ}+rcos\mathrm{\φ})\end{array}\right.$

$\left\{\begin{array}{c}\stackrel{\·}{p}=\[qr(J_y-J_z)+{\mathrm{\τ}}_{\mathrm{\φ}}\]\·1/J_x\\ \stackrel{\·}{q}=\[pr(J_z-J_x)+{\mathrm{\τ}}_{\mathrm{\θ}}\]\·1/J_y\\ \stackrel{\·}{r}=\[pq(J_x-J_y)+{\mathrm{\τ}}_{\mathrm{\ψ}}\]\·1/J_z\end{array}\right.$

其中:φ,θ,ψ分别为滚转角、俯仰角、偏航角；

p,q,r分别为本体角速度ω在本体坐标系x,y,z轴上的分量；

τ_φ,τ_θ,τ_ψ分别为三个本体轴方向的控制力矩；

J_x,J_y,J_z分别为四旋翼沿着x,y,z轴方向的转动惯量；

(2)将所述非线性动态方程改写为状态空间的形式：

$\stackrel{\·}{X}=f(X,U)$

其中，U为输入矢量，X为状态矢量，具体表达式如下：

状态变量：

x₁＝φ x₃＝θ x₅＝ψ

$\begin{array}{ccc}{x}_2={\stackrel{\·}{x}}_1=\stackrel{\·}{\mathrm{\φ}}& x_4={\stackrel{\·}{x}}_3=\stackrel{\·}{\mathrm{\θ}}& x_6={\stackrel{\·}{x}}_5=\stackrel{\·}{\mathrm{\ψ}}\end{array}$

输入矢量：U＝[U₁ U₂ U₃]^T＝[τ_φ τ_θ τ_ψ]^T

(3)姿态角变化率与本体角速率之间的转换矩阵，在悬停或者小角度飞行情况下，作为单位矩阵，获得所述滑模控制模型：

$f(X,U)=\left(\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}\\ \frac{(J_y-J_z)}{J_x}\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\ψ}}+\frac{1}{J_x}{U}_1\\ \stackrel{\·}{\mathrm{\θ}}\\ \frac{(J_z-J_x)}{J_y}\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\ψ}}+\frac{1}{J_y}{U}_2\\ \stackrel{\·}{\mathrm{\ψ}}\\ \frac{(J_x-J_y)}{J_z}\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\θ}}+\frac{1}{J_z}{U}_3\end{array}\right)$

其中，φ,θ,ψ分别为滚转角、俯仰角、偏航角；

p,q,r分别为本体角速度ω在本体坐标系x,y,z轴上的分量；

τ_φ,τ_θ,τ_ψ分别为三个本体轴方向的控制力矩；

J_x,J_y,J_z分别为四旋翼沿着x,y,z轴方向的转动惯量。

4.根据权利要求1所述的基于滑模控制律和ESO的四旋翼飞行器姿态控制方法，其特征在于，在根据模型参数推导出四旋翼飞行器动力学模型，根据所述四旋翼飞行器动力学模型构造滑模面的过程中，

构造的滑模面为获得的滑模控制律为：

$U_1=(J_z-J_y)x_4{x}_6-J_x({{\mathrm{\α}}_1}^2{z}_1+k_{1}sign(s_2)+k_2{s}_2-{\stackrel{\·\·}{x}}_{1d})$

$U_2=(J_x-J_z)x_2{x}_6-J_y({{\mathrm{\α}}_2}^2{z}_3+k_{3}sign(s_3)+k_4{s}_3-{\stackrel{\·\·}{x}}_{3d})$

$U_3=(J_y-J_x)x_2{x}_4-J_z({{\mathrm{\α}}_3}^2{z}_5+k_{5}sign(s_4)+k_6{s}_4-{\stackrel{\·\·}{x}}_{5d})$

其中，

$\left\{\begin{array}{c}{z}_3=x_{3d}-x_3\\ s_3=x_4-{\stackrel{\·}{x}}_{3d}-{\mathrm{\α}}_2{z}_3\\ z_5=x_{5d}-x_5\\ s_4=x_6-{\stackrel{\·}{x}}_{5d}-{\mathrm{\α}}_3{z}_5\end{array}\right.$

其中，sign代表符号函数，用近似的连续饱和函数sat(s)来替代所述符号函数；函数表达式为：

sat(s)＝s/(|s|+e)e∈[0,1]，取e＝0.5；

构造二阶滤波器：

其中，为输入值，X_c为输出值。

由此，得到在时域中的表达式：选取阻尼比ξ＝0.8，自然频率ω_n＝4.375。

5.根据权利要求1所述的基于滑模控制律和ESO的四旋翼飞行器姿态控制方法，其特征在于，在根据四旋翼的滚转角回路、俯仰角回路和偏航角回路的控制输入输出构建ESO的过程中，

$\left\{\begin{array}{c}e=z_1-y\\ {\stackrel{\·}{z}}_1=z_2-{\mathrm{\β}}_{01}e\\ {\stackrel{\·}{z}}_2=z_3-{\mathrm{\β}}_{02}fal(e,a_1,h)+b_0{U}_1\\ {\stackrel{\·}{z}}_3=-{\mathrm{\β}}_{03}fal(e,a_2,h)\end{array}\right.$

其中，b₀为控制量系数1/J_x，选择预设的参数β₀₁,β₀₂,β₀₃和a_i(i＝1,2)，令a₁＝0.5,a₂＝0.25。

6.根据权利要求1所述的基于滑模控制律和ESO的四旋翼飞行器姿态控制方法，其特征在于，在采用所述滑模控制律与所述ESO相结合的过程中，

$\left\{\begin{array}{c}e=z_{1\mathrm{\φ}}-x_1\\ {\stackrel{\·}{z}}_{1\mathrm{\φ}}=z_{2\mathrm{\φ}}-{\mathrm{\β}}_{01}e\\ {\stackrel{\·}{z}}_{2\mathrm{\φ}}=z_{3\mathrm{\φ}}-{\mathrm{\β}}_{02}fal(e,0.5,h)+(1/J_x)U_1\\ {\stackrel{\·}{z}}_{3\mathrm{\φ}}=-{\mathrm{\β}}_{03}fal(e,0.25,h)\\ U_1=(J_z-J_y)x_4{x}_6-J_x({{\mathrm{\α}}_1}^2{z}_1+k_{1}sat(s_2)+k_2{s}_2-{\stackrel{\·\·}{x}}_{1d})-J_x{z}_{3\mathrm{\φ}}\end{array}\right.$

同理，得到其他两个姿态回路的控制输出为：

$U_2=(J_x-J_z)x_2{x}_6-J_y({{\mathrm{\α}}_2}^2{z}_3+k_{3}sat(s_3)+k_4{s}_3-{\stackrel{\·\·}{x}}_{3d})-J_y{z}_{3\mathrm{\θ}}$

$U_3=(J_y-J_x)x_2{x}_4-J_z({{\mathrm{\α}}_3}^2{z}_5+k_{5}sat(s_4)+k_6{s}_4-{\stackrel{\·\·}{x}}_{5d})-J_z{z}_{3\mathrm{\ψ}}$

其中，z_3φ,z_3θ,z_3ψ分别为滚转角回路、俯仰角回路和偏航角回路的ESO得到的被扩张状态量。

7.一种基于滑模控制律和ESO的四旋翼飞行器姿态控制系统，其特征在于，包括：姿态角回路控制单元，用于根据模型参数推导出四旋翼飞行器动力学模型，根据所述四旋翼飞行器动力学模型构造滑模面，根据所述滑模面获取滑模控制律，以实现对四旋翼飞行器三个姿态角回路的控制；

系统总扰实时估计单元，用于根据四旋翼的滚转角回路、俯仰角回路和偏航角回路的控制输入输出构建ESO，利用所述ESO对系统总扰动进行实时估计；

四旋翼飞行器姿态控制单元，用于采用所述滑模控制律与所述ESO相结合，以实现对所述四旋翼飞行器姿态的控制。

8.根据权利要求7所述的基于滑模控制律和ESO的四旋翼飞行器姿态控制系统，其特征在于，所述四旋翼飞行器动力学模型参数包括转动惯量J_x、J_y、J_z；升力系数c_T；扭矩系数c_Q；电机的时间常数T。

9.根据权利要求7所述的基于滑模控制律和ESO的四旋翼飞行器姿态控制系统，其特征在于，所述姿态角回路控制单元获取所述滑模控制模型包括如下步骤：

(1)将四旋翼视为刚体，则所述四旋翼飞行器姿态非线性动态方程为：

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}=p+qsin\mathrm{\φ}tan\mathrm{\θ}+rcos\mathrm{\φ}tan\mathrm{\θ}\\ \stackrel{\·}{\mathrm{\θ}}=qcos\mathrm{\φ}-rsin\mathrm{\φ}\\ \stackrel{\·}{\mathrm{\ψ}}=\sec \mathrm{\θ}(q\sin \mathrm{\φ}+rcos\mathrm{\φ})\end{array}\right.$

$\left\{\begin{array}{c}\stackrel{\·}{p}=\[qr(J_y-J_z)+{\mathrm{\τ}}_{\mathrm{\φ}}\]\·1/J_x\\ \stackrel{\·}{q}=\[pr(J_z-J_x)+{\mathrm{\τ}}_{\mathrm{\θ}}\]\·1/J_y\\ \stackrel{\·}{r}=\[pq(J_x-J_y)+{\mathrm{\τ}}_{\mathrm{\ψ}}\]\·1/J_z\end{array}\right.$

其中：φ,θ,ψ分别为滚转角、俯仰角、偏航角；

p,q,r分别为本体角速度ω在本体坐标系x,y,z轴上的分量；

τ_φ,τ_θ,τ_ψ分别为三个本体轴方向的控制力矩；

J_x,J_y,J_z分别为四旋翼沿着x,y,z轴方向的转动惯量；

(2)将所述非线性动态方程改写为状态空间的形式：

$\stackrel{\·}{X}=f(X,U)$

其中，U为输入矢量，X为状态矢量，具体表达式如下：

状态变量：

x₁＝φ x₃＝θ x₅＝ψ

$\begin{array}{ccc}{x}_2={\stackrel{\·}{x}}_1=\stackrel{\·}{\mathrm{\φ}}& x_4={\stackrel{\·}{x}}_3=\stackrel{\·}{\mathrm{\θ}}& x_6={\stackrel{\·}{x}}_5=\stackrel{\·}{\mathrm{\ψ}}\end{array}$

输入矢量：U＝[U₁ U₂ U₃]^T＝[τ_φ τ_θ τ_ψ]^T

(3)姿态角变化率与本体角速率之间的转换矩阵，在悬停或者小角度飞行情况下，作为单位矩阵，获得所述滑模控制模型：

$f(X,U)=\left(\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}\\ \frac{(J_y-J_z)}{J_x}\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\ψ}}+\frac{1}{J_x}{U}_1\\ \stackrel{\·}{\mathrm{\θ}}\\ \frac{(J_z-J_x)}{J_y}\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\ψ}}+\frac{1}{J_y}{U}_2\\ \stackrel{\·}{\mathrm{\ψ}}\\ \frac{(J_x-J_y)}{J_z}\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\θ}}+\frac{1}{J_z}{U}_3\end{array}\right)$

其中，φ,θ,ψ分别为滚转角、俯仰角、偏航角；

p,q,r分别为本体角速度ω在本体坐标系x,y,z轴上的分量；

τ_φ,τ_θ,τ_ψ分别为三个本体轴方向的控制力矩；

J_x,J_y,J_z分别为四旋翼沿着x,y,z轴方向的转动惯量。

10.根据权利要求7所述的基于滑模控制律和ESO的四旋翼飞行器姿态控制系统，其特征在于，所述姿态角回路控制单元在根据模型参数推导出四旋翼飞行器动力学模型，根据所述四旋翼飞行器动力学模型构造滑模面，根据所述滑模面获取滑模控制律的过程中，

构造的滑模面为获得的滑模控制律为：

$U_1=(J_z-J_y)x_4{x}_6-J_x({{\mathrm{\α}}_1}^2{z}_1+k_{1}sign(s_2)+k_2{s}_2-{\stackrel{\·\·}{x}}_{1d})$

$U_2=(J_x-J_z)x_2{x}_6-J_y({{\mathrm{\α}}_2}^2{z}_3+k_{3}sign(s_3)+k_4{s}_3-{\stackrel{\·\·}{x}}_{3d})$

$U_3=(J_y-J_x)x_2{x}_4-J_z({{\mathrm{\α}}_3}^2{z}_5+k_{5}sign(s_4)+k_6{s}_4-{\stackrel{\·\·}{x}}_{5d})$

其中，

$\left\{\begin{array}{c}{z}_3=x_{3d}-x_3\\ s_3=x_4-{\stackrel{\·}{x}}_{3d}-{\mathrm{\α}}_2{z}_3\\ z_5=x_{5d}-x_5\\ s_4=x_6-{\stackrel{\·}{x}}_{5d}-{\mathrm{\α}}_3{z}_5\end{array}\right.$

其中，sign代表符号函数，用近似的连续饱和函数sat(s)来替代所述符号函数；函数表达式为：

sat(s)＝s/(|s|+e)e∈[0,1]，取e＝0.5；

构造二阶滤波器：

其中，为输入值，X_c为输出值。

由此，得到在时域中的表达式：选取阻尼比ξ＝0.8，自然频率ω_n＝4.375。

所述系统总扰实时估计单元在根据四旋翼的滚转角回路、俯仰角回路和偏航角回路的控制输入输出构建ESO的过程中，

$\left\{\begin{array}{c}e=z_1-y\\ {\stackrel{\·}{z}}_1=z_2-{\mathrm{\β}}_{01}e\\ {\stackrel{\·}{z}}_2=z_3-{\mathrm{\β}}_{02}fal(e,a_1,h)+b_0{U}_1\\ {\stackrel{\·}{z}}_3=-{\mathrm{\β}}_{03}fal(e,a_2,h)\end{array}\right.$

其中，b₀为控制量系数1/J_x，选择预设的参数β₀₁,β₀₂,β₀₃和a_i(i＝1,2)，令a₁＝0.5,a₂＝0.25；

四旋翼飞行器姿态控制单元在采用所述滑模控制律与所述ESO相结合的过程中，

$\left\{\begin{array}{c}e=z_{1\mathrm{\φ}}-x_1\\ {\stackrel{\·}{z}}_{1\mathrm{\φ}}=z_{2\mathrm{\φ}}-{\mathrm{\β}}_{01}e\\ {\stackrel{\·}{z}}_{2\mathrm{\φ}}=z_{3\mathrm{\φ}}-{\mathrm{\β}}_{02}fal(e,0.5,h)+(1/J_x)U_1\\ {\stackrel{\·}{z}}_{3\mathrm{\φ}}=-{\mathrm{\β}}_{03}fal(e,0.25,h)\\ U_1=(J_z-J_y)x_4{x}_6-J_x({{\mathrm{\α}}_1}^2{z}_1+k_{1}sat(s_2)+k_2{s}_2-{\stackrel{\·\·}{x}}_{1d})-J_x{z}_{3\mathrm{\φ}}\end{array}\right.$

同理，得到其他两个姿态回路的控制输出为：

$U_2=(J_x-J_z)x_2{x}_6-J_y({{\mathrm{\α}}_2}^2{z}_3+k_{3}sat(s_3)+k_4{s}_3-{\stackrel{\·\·}{x}}_{3d})-J_y{z}_{3\mathrm{\θ}}$

$U_3=(J_y-J_x)x_2{x}_4-J_z({{\mathrm{\α}}_3}^2{z}_5+k_{5}sat(s_4)+k_6{s}_4-{\stackrel{\·\·}{x}}_{5d})-J_z{z}_{3\mathrm{\ψ}},$

其中，z_3φ,z_3θ,z_3ψ分别为滚转角回路、俯仰角回路和偏航角回路的ESO得到的被扩张状态量。