[发明专利]用于四旋翼飞行器的控制方法

摘要

本发明公开了一种对四旋翼飞行器的姿态角和位置均能有效、快速地控制的用于四旋翼飞行器的控制方法。该用于四旋翼飞行器的控制方法通过利用INS惯性导航和GPS导航组合导航的方法解决了单一的GPS导航技术易受干扰和遮挡，短时定位精度不高，输出频率有限并且输出不连续的缺点；可以获得四旋翼飞行器较为准确的导航信息参数，接着利用准确的导航信息参数计算得到四旋翼飞行器的控制输入量；进而将其输入四旋翼飞行器的控制系统进行飞行控制，该控制方法对导航信息参数进行优化，可以得到较为准确的导航信息参数，同时对控制算法进行优化，可以使四旋翼飞行器的姿态角和位置均能有效、快速地控制。适合在飞行控制领域推广应用。

主权项

1.用于四旋翼飞行器的控制方法，其特征在于包括以下步骤：S1、利用INS惯性导航与GPS导航以及磁力计的导航算法计算得到四旋翼飞行器的导航信息参数；所述INS惯性导航与GPS导航以及磁力计的导航算法的具体计算方法如下所述：A、将INS惯性导航系统的陀螺仪测得的四旋翼飞行器角速度参数代入四元数微分方程求解得到四元数q₀，q₁，q₂，q₃；其中为陀螺仪在四旋翼飞行器自身坐标系下的测得的三个轴的角速度信息；所述四元数微分方程为：B、将步骤A中求解的q₀，q₁，q₂，q₃代入下式求解得到姿态矩阵根据下述与方向余弦的关系式计算得出四旋翼飞行器的INS惯性导航模块姿态角θ、γ、C、利用磁力计测得的偏航角替换步骤B计算得到的偏航角D、将INS惯性导航系统的加速度计测得的加速度参数f^b和步骤B中求解得到姿态矩阵代入下述微分方程中求解得到四旋翼飞行器在INS惯性导航坐标系下的东、北、天三个方向上的速度信息v_N v_E v_U，所述微分方程为：其中，vⁿ＝[v_N v_E v_U]'为四旋翼飞行器在INS惯性导航坐标系下的东、北、天三个方向上的速度；为在INS惯性导航坐标系下地球自转角速度，为四旋翼飞行器在INS惯性导航坐标系下绕INS惯性导航坐标系各轴向的转动角速率，gⁿ为在INS惯性导航坐标系下重力加速度；E、将步骤D计算得出的v_N v_E v_U分别代入下式求解得出四旋翼飞行器在INS惯性导航系统中的位置信息，其中L为纬度，λ为经度，h为高度，h＝h(0)+∫v_Udt，其中L(0)表示四旋翼飞行器初始位置的纬度值，λ(0)表示四旋翼飞行器初始位置的经度值，h(0)表示四旋翼飞行器距离地球表面的初始高度，R_M表示地球子午圈上的曲率半径，R_N表示纬度圈上的曲率半径；F、建立状态方程和观测方程Z(t)＝H(t)X_I(t)+V(t)，X_I(t)表示INS惯性导航系统在t时刻的误差状态，它是一个15维的向量，如下所示：X_I(t)＝[δv_x δv_y δv_z φ_x φ_y φ_z δL δλ δh ε_x ε_y ε_z ▽_x ▽_y ▽_z]，δv_x,δv_y,δv_z为INS惯性导航系统沿东、北、天方向上的速度误差；φ_x,φ_y,φ_z为四旋翼飞行器的姿态角误差；δL,δλ,δh分别代表四旋翼飞行器所在纬度、经度和高度误差；ε_x,ε_y,ε_z分别代表陀螺仪的随机漂移；▽_x,▽_y,▽_z分别为加速度计的随机漂移，其中是一个15×15的矩阵；其中F_N(t)对应于δv_x,δv_y,δv_z，φ_x,φ_y,φ_z，δL,δλ,δh这9个参数的INS惯性导航系统矩阵，其非零元素如下：F(1,5)＝‑f_z F(1,6)＝f_yF(2,4)＝f_z F(2,6)＝‑f_xF(3,4)＝‑f_y F(3,5)＝f_x F(3,7)＝‑2ω_iev_xsinLF(5,7)＝‑ω_iesinLF(9,3)＝1F_S(t)为δv_x,δv_y,δv_z，φ_x,φ_y,φ_z，δL,δλ,δh这9个参数与陀螺仪及加速度计漂移之间的变换矩阵，其维数是9×6，F_M(t)为ε_x,ε_y,ε_z，▽_x,▽_y,▽_z与陀螺仪及加速度计漂移对应的INS惯性导航系统矩阵，是一个维数为6×6的对角线矩阵，表示如下：F_M(t)＝diag[‑1/T_gx ‑1/T_gy ‑1/T_gz ‑1/T_ax ‑1/T_ay ‑1/T_az]；其中，T_gx表示陀螺仪x轴的误差模型的时间常数，T_gy表示陀螺仪y轴的误差模型的时间常数，T_gz表示陀螺仪z轴的误差模型的时间常数，T_ax表示加速度计x轴误差模型的时间常数，T_ay表示加速度计y轴误差模型的时间常数，T_az表示加速度计z轴误差模型的时间常数；G_I(t)＝diag[1 1 1......1 1]_15×15；W_I(t)是一个15维的向量，如下所示：W_I(t)＝[a₁ a₂ a₃ a₄ a₅ a₆ a₇ a₈ a₉ a₁₀ a₁₁ a₁₂ a₁₃ a₁₄ a₁₅]，a₁ a₂ a₃ a₄ a₅ a₆ a₇ a₈ a₉ a₁₀ a₁₁ a₁₂ a₁₃ a₁₄ a₁₅表示系统过程噪声序列；Z(t)为四旋翼飞行器在INS惯性导航中的位置速度信息与四旋翼飞行器在GPS导航系统中的位置速度信息的差值，是一个6维向量，Z(t)＝[δv_x+N_vx δv_y+N_vy δv_z+N_vz (R_M+h)δL+N_y (R_M+h)cosLδλ+N_x δh+N_h]^T，其中，N_vx表示GPS导航系统在x方向上的速度误差，N_vy表示GPS导航系统在y方向上的速度误差，N_vz表示GPS导航系统在z方向上的速度误差，N_x表示GPS导航系统在x方向上的位置误差，N_y表示GPS导航系统在y方向上的位置误差，N_h表示GPS导航系统在z方向上的位置误差；其中V_v(t)＝[N_vx N_vy N_vz]^TV_p(t)＝[N_x N_y N_z]^TG、将上述得到的连续状态方程离散化后得到X_k＝Φ_k,k‑1X_k‑1+W_k‑1，其中将上述得到的连续观测方程Z(t)＝H(t)X_I(t)+V(t)离散化后得到Z_k＝H_kX_k+V_k；其中I是单位矩阵，F是INS惯性导航系统的状态转移矩阵，Δt是离散化后INS惯性导航系统的采样时间；H、将四旋翼飞行器在INS惯性导航系统中的位置速度信息与四旋翼飞行器在GPS导航系统中的位置速度信息作差得到Z(t)在k时刻的观测信息z；I、计算k时刻INS惯性导航系统状态的最优估计值其中，为在k‑1时刻INS惯性导航系统状态的最优估计值，Q_k‑1是INS惯性导航系统的噪声矩阵，其大小是由INS惯性导航元件的性能决定，R_k是系统测量噪声的方差阵，其大小是由GPS接收机的性能决定；J、将计算得到的值与四旋翼飞行器在INS惯性导航系统中的位置速度信息作差得到最优的导航参数；K、重复步骤H‑J，得到连续的四旋翼飞行器的导航信息参数；S2、将步骤S1得到的导航信息参数代入如下公式得到四旋翼飞行器的控制输入量U₁、U₂、U₃、U₄；其中，x₁＝γ，x₃＝θ，θ是俯仰角，γ是滚转角，是偏航角，Ω_r为转速扰动，m为四旋翼飞行器的质量，g为重力加速度，α₁、α₂、α₃、α₄、α₅、α₆、α₇、α₈为大于零的正数，l表示四旋翼飞行器的机臂长度，Ixx,Iyy,Izz分别是四旋翼飞行器绕x轴，y轴，z轴的转动惯量，J_r为四旋翼飞行器的电机转子的转动惯量，z₁是俯仰角的目标值和通过步骤S1计算得到实际值之差，z₂是俯仰角速度目标值和通过步骤S1计算得到实际值之差；z₃是滚转角的目标值和通过步骤S1计算得到实际值之差，z₄是滚转角速度目标值和通过步骤S1计算得到实际值之差；z₅是偏航角的目标值和通过步骤S1计算得到实际值之差，z₆是偏航角速度目标值和通过步骤S1计算得到实际值之差；z₇是Z轴高度的目标值和通过步骤S1计算得到实际值之差，z₈是Z轴高度方向上速度目标值和通过步骤S1计算得到实际值之差；S3、将步骤S2计算得到的四旋翼飞行器的控制输入量U₁、U₂、U₃、U₄输入四旋翼飞行器的控制系统进行飞行控制。