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

申请日：2020-09-10

公开（公告）日：2024-03-29

公开（公告）号：CN112070170B

主分类号：G06F18/25

分类号：G06F18/25;G06F18/10;G01C21/00;G01C21/10;G01C21/20;G06F17/16

优先权：

专利状态码：有效-授权

法律状态：2024.03.29#授权;2020.12.29#实质审查的生效;2020.12.11#公开

摘要：本发明提出的一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法，属于数字滤波和多传感器数据融合技术领域，主要用于提高载体姿态估计的精度。该方法以标准粒子滤波算法作为框架，融合陀螺仪、加速度计及磁力计数据，通过对传感器误差分析建立较为准确的传感器测量模型，并选用四元数作为姿态参数，对残差的合理范围进行预测判断，继而自适应调整系统的量测噪声矩阵，实现传感器的数据融合。本发明适用于非线性姿态测量系统，具有良好的抗干扰性和姿态解算精度，可以有效解决复杂环境下载体姿态解算问题。

主权项：1.一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法，其特征在于，具体包括以下步骤：步骤1：选择四元数q＝[q0qv]T作为四元数粒子滤波状态量，即x＝q，其中q0和qv分别表示四元数标量和矢量部分，根据陀螺仪输出和陀螺仪误差模型建立四元数粒子滤波状态方程，根据加速度计输出fb和磁力计输出mb作为观测量，建立四元数粒子滤波量测方程；步骤2：滤波器初始化，根据初始姿态角求得初始四元数q0，采样得到一系列四元数粒子集初始化每个粒子权值为1N，其中上标i表示粒子索引，下标0表示初始时刻，N为粒子总数；步骤3：时间更新，根据建立的动态残差阈值自适应四元数粒子滤波状态方程对k时刻的四元数粒子集进行更新得到k+1时刻的预测四元数粒子集步骤4：根据k时刻的残差均值μk和方差σk2信息，预测k+1时刻动态残差阈值区间，判断k+1时刻残差值rk+1是否超出动态残差阈值区间S～[μk+1-εσk+1，μk+1+εσk+1]，若残差值超出动态残差阈值区间，则自适应调整系统k+1时刻的量测噪声矩阵Rk+1；其中ε表示相关系数，μk+1和σk+1表示k+1时刻残差的均值和标准差；步骤5：利用自适应估计得到的量测噪声矩阵及步骤1的量测方程进行系统量测更新，计算k+1时刻粒子的重要性权值并归一化；步骤6：重采样，若有效粒子数则对粒子集进行随机重采样操作，其中表示k+1时刻第i个粒子的权值，Nthrehold表示重采样粒子阈值；步骤7：对量测更新后的粒子集进行状态估计得到最优四元数估计值并进行归一化处理，利用四元数滤波值姿态解算方法求解得到俯仰角θ，横滚角γ和航向角ψ；步骤8：循环步骤3～步骤7，直到滤波结束。

全文数据：

权利要求：

百度查询： 南京航空航天大学 一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法

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

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