买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：哈尔滨工业大学

摘要：一种基于卡尔曼滤波器的IMU自适应姿态估计方法，涉及机器人运动跟踪、导航及定位技术领域。对加速度计和陀螺仪测量数据建模，使用陀螺仪的角速度测量对IMU姿态进行预测，根据姿态和姿态角的关系提取待估计的状态，并得到状态先验估计的协方差矩阵，使用加速度计的加速度测量构建姿态观测模型，定义加速度微调因子对各个轴进行加速度补偿，根据加速度微调因子计算卡尔曼滤波器增益来更新状态估计的均值和协方差矩阵，得到最终估计的状态以及状态后验估计的协方差矩阵，之后解算IMU的姿态角。实现了滤波器增益的自适应调整以及对外部加速度的动态补偿，能够有效提升IMU姿态估计的鲁棒性和精度。

主权项：1.一种基于卡尔曼滤波器的IMU自适应姿态估计方法，其特征在于：包括以下步骤：步骤一：输入IMU的加速度计测量数据am以及陀螺仪测量数据ωm；步骤二：对k时刻的加速度计测量数据am,k和陀螺仪测量数据ωm,k建模如下： ωm,k＝ωk+bg2式中，ak-1表示在k-1时刻的IMU外部加速度，ωk表示在k时刻的IMU角速度，ξ为常数，ξ∈[0,1]，表示在k时刻IMU在导航坐标系中的姿态，g为当地重力加速度矢量，ba和bg分别表示加速度计和陀螺仪的零偏，εk表示在k时刻的IMU外部加速度噪声；步骤三：使用陀螺仪的角速度测量对IMU姿态进行预测，表示如下： 式中，ωk-1和分别表示在k-1时刻的IMU角速度和IMU在导航坐标系中的姿态，I3表示3×3大小的单位矩阵，Δt为IMU的数据采样时间间隔，[·]×表示叉积运算符号；步骤四：姿态由三个姿态角构成：横滚角ψk，俯仰角θk和偏航角φk，转动次序为z-y-x，姿态和姿态角的关系如下： 取姿态的第3行元素为待估计的状态xk：xk＝[-sinθk,sinψkcosθk,cosψkcosθk]T5对公式3变形为： 式中，表示在k时刻状态xk的先验估计，在k-1时刻状态xk-1的先验估计，根据公式6得到状态先验估计的协方差矩阵如下： 式中，Pk-1为在k-1时刻的状态后验估计的协方差矩阵，Σg为陀螺仪噪声的协方差矩阵；步骤五：使用加速度计的加速度测量构建姿态观测模型，根据公式1得到：am,k-ξak-1＝gxk+εk+ba8式中，g表示当地重力加速度矢量的模长，定义： 式中，ak表示在k时刻的IMU外部加速度；步骤六：当系统无故障时，卡尔曼滤波器的实际误差ee应小于其理论误差et，即满足下式： 式中，Σa为加速度计噪声的协方差矩阵，Σε为外部加速度噪声；步骤七：根据对系统故障原因的分析，由公式10定义加速度微调因子λk如下： 通过加速度微调因子对各个轴进行加速度补偿；步骤八：确保加速度计微调因子的非负性，即：λki,j＝max{0,λki,j},i＝1,2,3；j＝1,2,312式中，λki,j表示加速度微调因子的第i行第j列元素值；步骤九：根据加速度微调因子计算卡尔曼滤波器增益Kk： 加速度微调因子使得卡尔曼滤波器能够动态调节滤波器增益；步骤十：根据卡尔曼滤波器增益来更新状态估计的均值和协方差矩阵，得到最终估计的状态xk以及状态后验估计的协方差矩阵Pk如下： 之后根据最终估计的状态xk解算IMU的姿态角如下： 式中，xkl,l＝1,2,3表示状态xk的第l个元素值。

全文数据：

权利要求：

百度查询： 哈尔滨工业大学 一种基于卡尔曼滤波器的IMU自适应姿态估计方法