[发明专利]结合无人机动力学模型和MEMS传感器的姿态估计方法

权利要求书

1.一种结合无人机动力学模型和MEMS传感器的姿态估计方法，其特征在于，包括如下步骤：

步骤一：在GPS信号有效时，利用GPS/MEMS组合导航结果对无人机动力学模型的电机拉力系数和三轴常值干扰力矩进行估计，根据估计得到的电机拉力系数和三轴常值干扰力矩修正无人机动力学模型；

步骤二：在GPS信号失效时，利用磁力计数据计算并补偿磁偏角后得到磁航向角，利用雷达高度、气压高度和真实高度的冗余关系构造无人机的俯仰角和横滚角的余弦乘积量；

步骤三：在GPS信号失效时，利用步骤一中修正后的无人机动力学模型的平动动力学方程估计无人机运动加速度，进而构造重力加速度补偿量，得到重力加速度观测的俯仰角和横滚角；

步骤四：在GPS信号失效时，利用步骤一中修正后的无人机动力学模型的转动动力学方程对MEMS传感器的测量数据滤波，以抑制无人机机体震动带来数据噪声和异常值；

步骤五：在GPS信号失效时，根据步骤二到步骤四得到的无人机的俯仰角和横滚角的余弦乘积量、重力加速度观测的俯仰角和横滚角、滤波后的MEMS传感器的测量数据构建卡尔曼滤波器进行无人机系统的姿态估计；

步骤一中，在GPS信号有效时，利用GPS/MEMS组合导航结果对无人机动力学模型的电机拉力系数和三轴常值干扰力矩进行估计，具体过程如下：

下式(1)给出了估计电机拉力系数和三轴常值干扰力矩的状态量和非线性状态方程：

式中，p，q，r分别为无人机横滚、俯仰、偏航三个方向上的角速度；M_x,M_y,M_z为三轴力矩；为无人机三轴干扰力矩；β为电机拉力系数；c_i,i∈{1,2,…9}为与无人机转动惯量有关的系数；ξ_p,ξ_q,ξ_r分别为三轴角速度的过程噪声；为三轴干扰力矩和电机拉力系数的过程噪声；F_d(X,t)为非线性状态方程，W_d(t)为无人机系统噪声矩阵；上标·表示一阶求导；上标T表示转置矩阵；

对式(1)求雅克比矩阵得到线性化状态方程：

式中，F_d(t)是线性化的状态转移矩阵；U_d(t)为无人机系统输入；W_d(t)为无人机系统噪声矩阵；

估计电机拉力系数和三轴常值干扰力矩的量测量为以及非线性量测方程为：

式中，ω_x，ω_y，ω_z为补偿过零偏的三轴陀螺仪输出；为Z轴运动加速度；φ，θ分别为GPS/MEMS组合导航得到的横滚角和俯仰角；D_j,j∈{1,2,…n}为电机收到的PWM占空比信号，n为无人机电机数量；m为无人机总质量；为三轴陀螺仪输出量测噪声；为Z轴运动加速度计量测噪声；V_d(t)为量测噪声矩阵；g为当地重力加速度；H(X_d,t)为非线性量测方程；

对式(3)求雅克比矩阵进行线性化：

式中，H_d(t)为线性化后的量测矩阵；

对式(2)和(4)进行离散化得到离散模型(5)：

式中，X_k为当前时刻状态量；X_k-1为上一时刻状态量；Φ_k,k-1为一步状态转移矩阵；Γ_k-1为系统噪声转移矩阵；W_k-1为系统噪声；Z_k为当前时刻量测量；H_k为当前时刻量测矩阵；V_k为量测噪声阵；

利用离散模型(5)估计出电机拉力系数和三轴常值干扰力矩。