申请/专利权人：南京航空航天大学

申请日：2021-03-15

公开（公告）日：2024-07-05

公开（公告）号：CN113607167B

主分类号：G01C21/20

分类号：G01C21/20;G01S19/47;G01S19/48

优先权：

专利状态码：有效-授权

法律状态：2024.07.05#授权;2021.11.23#实质审查的生效;2021.11.05#公开

摘要：本发明公布了一种用于航姿系统的自适应姿态估计方法及其平滑切换算法，由基于四元数的低阶EKF姿态估计算法和基于GPS速度、位置的高阶组合导航融合算法构成自适应姿态平滑切换。该算法根据GPS状态实现低阶与高阶的切换，包括以下步骤：在GPS状态良好的情况下，采用高阶组合导航融合算法估计姿态信息；在GPS信号受到干扰时，停止高阶组合导航算法并切换为低阶姿态估计算法；当GPS信号恢复后，将低阶姿态估计状态作为高阶滤波算法的初值，实现姿态融合算法的平滑切换。针对该算法设计基于ARM架构的航姿系统，并通过仿真与试验验证设计算法的可靠性，在切换过程中姿态角误差小于0.5°，收敛时间小于0.2s，有效地提高了有、无GPS定位状态下无人机的姿态估计精度。

主权项：1.一种用于航姿系统的自适应姿态估计方法，其特征在于，所述方法的实现步骤包括：步骤1，在GPS状态良好的情况下，基于四元数的低阶EKF姿态估计算法和基于GPS速度、位置的高阶组合导航融合算法同时运行，实际采用高阶组合导航算法估计姿态信息；所述的基于四元数的低阶EKF姿态估计算法包括以下步骤：在基于四元数的低阶卡尔曼滤波器中，设系统状态量X为姿态四元数：X＝[q0q1q2q3]T1其中，q0、q1、q2、q3为姿态四元数；根据卡尔曼滤波器原理，定义离散系统的状态方程如下：Xk＝AkXk-1+ωk-12其中，Xk、Xk-1分别表示第k、k-时刻的系统状态量，Ak为从k-1时刻到k时刻的状态转移矩阵，ωk-1表示k-1时刻的系统过程噪声序列，则对应于卡尔曼滤波器的状态方程为： 表示系统k时刻先验估计状态，表示系统k-1时刻后验估计状态，其中： 其中，表示角速率矢量矩阵： 式中：ωx表示绕横滚轴角速率，ωy表示绕俯仰轴角速率，ωz表示绕航向轴角速率；基于GPS量测的高阶组合导航算法的过程具体包括以下步骤：在基于GPS量测的高阶卡尔曼滤波器中，设系统状态量为： 式中，q0、q1、q2、q3为姿态四元数，vN、vE、vd分别为NED坐标系下北向速度，东向速度，地向速度，PN、PE、PD分别为NED坐标系下北向位置、东向位置和地向位置，biasx、biasy、biasz分别为绕横滚轴、俯仰轴和航向轴的陀螺仪随机漂移估计值；利用公式4更新状态量中的前四个状态的先验估计xk1:4，xk1:4为由q0、q1、q2、q3构成的状态；利用运动学方程，机体加速度和方向余弦矩阵得到先验估计xk5:10，更新过程如下所示：xk1:4为由q0、q1、q2、q3构成的状态 式中，ax、ay、az分别为机体坐标系下的加速度量测值，即进行量纲变换以后的加速度计量测值，dt表示系统先验估计的递推时间；此时，系统状态转移矩阵为： 量测信号为NED坐标系下的三轴速度VGNSS，NED坐标系下的三轴位置PGNSS，体轴系下的三轴地磁场量测值mb，并由此计算出的航向角ψm，其中ψm由公式16得出： 则对应的量测矩阵为： 当进行速度、位置融合时，量测值为GPS接收机量测的速度VGNSS和位置PGNSS，量测矩阵为HVELk和HPOSk；当进行航向角融合时，量测值为磁力计测量的航向角ψm，量测矩阵为HMAGk；步骤2，在GPS信号受到干扰时，停止高阶组合导航算法并切换为采用低阶姿态估计算法；步骤3，当GPS信号恢复后，将低阶姿态估计算法得到的四元数值作为高阶滤波算法初值，实现姿态融合算法的平滑切换；切换算法包括以下步骤：首先，在系统初始运行时，同时构造基于四元数的低阶卡尔曼滤波器和基于GPS量测的高阶卡尔曼滤波器，并让其同步运行，等待其收敛；然后，在有速度、位置量测信号的情况下，系统输出的状态为基于GPS量测的高阶卡尔曼滤波器的估计值，如公式18所示：[φθψ]＝[φ2θ2ψ2]18式中，ψ、θ、ψ分别表示系统输出的横滚角、俯仰角和航向角，φ2、θ2、ψ2表示基于GPS量测的高阶卡尔曼滤波器估计值；在失去速度、位置量测信号的情况下，停止基于GPS量测的高阶卡尔曼滤波器的后验估计过程，同时停止其先验估计过程；输出基于四元数的低阶卡尔曼滤波器的估计值，如公式19：[φθψ]＝[φ1θ1ψ1]19式中，φ1、θ1、ψ1表示基于四元数的低阶卡尔曼滤波器；在重新获得速度、位置量测信号时，恢复运行基于GPS量测的高阶卡尔曼滤波器的先验估计过程和后验估计过程，此时，系统估计的协方差矩阵处于系统稳定时的协方差矩阵附近，并将基于四元数的低阶卡尔曼滤波器的姿态四元数估计值作为基于GPS量测的高阶卡尔曼滤波器恢复运行时姿态四元数初始估计值，GPS当前量测速度、位置信号作为基于GPS量测的高阶卡尔曼滤波器恢复运行时速度、位置初始估计值，此时，系统输出的状态为基于GPS量测的高阶卡尔曼滤波器的估计值，如公式20： 式中，x1表示基于四元数的低阶卡尔曼滤波器状态量，x2表示基于GPS量测的高阶卡尔曼滤波器状态量；所述切换算法应用于如下航姿系统：所述航姿系统包含导航计算机，所述导航计算机以ARMCortexM7为主处理器，用于获取传感器数据并进行两套姿态估计滤波算法运算，用于当检测GPS发生故障恢复时完成算法的自适应平滑切换；还包括惯性传感器，所述惯性传感器为ADIS16470惯性测量单元，分别用于测量三轴角速率和三轴加速度信息；还包括磁力计，所述磁力计为RM3100，用于测量三轴地磁场信息；以及GPS卫星导航系统，所述GPS卫星导航系统为NEO-M8N，用于测量三轴位置与三轴速度信息。

全文数据：

权利要求：

百度查询： 南京航空航天大学 一种用于航姿系统的自适应姿态估计方法及其平滑切换算法

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD