CN108225308A - 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法 - Google Patents
一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法 Download PDFInfo
- Publication number
- CN108225308A CN108225308A CN201711179949.2A CN201711179949A CN108225308A CN 108225308 A CN108225308 A CN 108225308A CN 201711179949 A CN201711179949 A CN 201711179949A CN 108225308 A CN108225308 A CN 108225308A
- Authority
- CN
- China
- Prior art keywords
- axis
- sampling instant
- matrix
- formula
- represent
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法,包括如下步骤:获取载体固定坐标系下多传感器数据;对加速度计和磁力计采集的数据进行滤波,并对这两个传感器采集的数据做归一化处理;根据四元数微分方程和姿态矩阵构建载体***的状态方程,并确定***的过程噪声方差矩阵;利用快速高斯‑牛顿法构建***观测模型,并确定***量测噪声方差矩阵;根据建立的***状态方程和观测模型建立卡尔曼滤波递推方程;利用递推得到的最佳四元数解算出载体的三个姿态角。本发明能够极大简化计算量,解决了现有参数计算不详的问题。
Description
技术领域
本发明涉及运动载体导航的多传感器数据融合技术领域,尤其是一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法。
背景技术
在人体运动跟踪、飞行器导航等运动载体导航的多传感器数据融合领域,对载体的姿态进行精准的实时估计有着广泛的应用。低成本的微机电***的迅速发展使得更小更便宜的惯性传感器得到了广泛的应用。但是低成本的惯性测量单元测得的数据容易受到高频噪声和时变的偏置的影响,所以传感器数据融合算法中需要进行平滑处理以及无偏置估计。为了获得更精准的姿态,常常将加速度计和磁力计的数据与陀螺仪输出的角速度数据融合。而最常用的非线性姿态估计方法所使用的技术就是互补滤波和扩展卡尔曼滤波。
互补滤波方法中的互补滤波系数在动态***中需要实时修正,但是如何修正并没有具体的模型方程,常使用的方法是采用自适应调参。黄鹤等人提出的“一种基于自适应互补融合的无人机姿态控制***及方法”提出了使用梯度下降法来获得第二姿态角的方法,但是不同的传感器的使用会导致自适应调参的方法不同,参数的估计会在实际过程中会造成很大的麻烦,甚至导致姿态估计不精确。李国朋等人提出的“一种基于互补卡尔曼滤波算法计算融合姿态角度的方法”提出了互补滤波与卡尔曼滤波结合的方法,但是建立的***模型只适用于单轴***,不适合实际的三轴陀螺仪和三轴加速度计***。陈洋等人提出的“共轭梯度与扩展卡尔曼滤波结合的四旋翼解算方法”提出了使用共轭梯度方法建立扩展卡尔曼滤波观测模型,但是***状态方程建立不够精确,没有给出过程噪声方差矩阵和测量噪声方差矩阵在实际过程中的计算方法。
发明内容
本发明所要解决的技术问题在于,提供一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法,能够极大简化计算量,解决了现有参数计算不详的问题。
为解决上述技术问题,本发明提供一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法,包括如下步骤:
(1)搭建姿态估计***,获取载体固定参考系坐标系(即b系,坐标原点位于惯性测量组件中心的“右-前-上”直角坐标系)下多轴传感器数据,陀螺仪采集的三轴角速度数据,加速度计采集的三轴加速度数据,磁力计采集的三轴磁感应强度数据;
(2)对采集的加速度数据和磁感应强度数据做滤波处理,并对这两个传感器采集的数据做归一化处理;三轴陀螺仪采集的数据为w=[wx wy wz]T,其中wx、wy、wz分别表示在载体固定坐标系中x轴、y轴和z轴采集到的三轴角速度数据;T表示转置;归一化后的三轴加速度计采集的数据为a=[ax ay az]T,其中ax、ay、az分别表示在载体固定坐标系中x轴、y轴和z轴采集到的三轴加速度数据;归一化后的三轴磁力计采集的数据为m=[mx my mz]T,其中mx、my、mz分别表示在载体固定坐标系中x轴、y轴和z轴采集到的三轴磁感应强度数据;
(3)根据四元数微分方程和姿态矩阵构建载体***的状态方程,并得到***的过程噪声方差矩阵;
(4)利用快速高斯-牛顿法构建***的观测模型,并得到***的测量噪声方差矩阵;
(5)根据建立的***状态方程和观测模型建立扩展卡尔曼滤波递推方程;
(6)利用每次递推得到的最佳四元数解算载体的三个姿态角:航向角为Ψ,俯仰角为θ,横滚角为γ。
优选的,步骤(3)中,根据四元数微分方程和姿态矩阵构建***的状态方程,并得到***过程噪声方差矩阵的具体过程如下:
四元数微分方程如下:
式中,qk表示k次采样时刻四元数,其中qk=[qk1 qk2 qk3 qk0]T,[qk1 qk2 qk3]T是四元数qk的矢量部分,用表示,qk0是标量部分;表示qk的微分;wk表示k次采样时刻的陀螺仪采集的数据;其中Ω(wk)的定义如下:
式中,wk=[wkx wky wkz]T,其中wkx、wky、wkz分别为k次采样时刻陀螺仪在载体固定坐标系采集的x轴、y轴和z轴方向上的角速度数据;T表示转置;
根据四元数微分方程得到的离散时间模型为:
式中,qk+1表示k+1次采样时刻四元数;Ωk是k次采样时刻的Ω(wk);qk为k次采样时刻的四元数,q(0)为初始条件得到的四元数;Ts为采样周期;
根据四元数qk=[qk1 qk2 qk3 qk0]T与姿态矩阵(n系:“东-北-天”地理坐标系)的关系得到姿态矩阵如下:
以姿态四元数作为***的状态,即:
Xk=qk
式中,Xk表示k次采样时刻状态矩阵;qk表示k次采样时刻四元数;
建立的***的状态方程如下式:
Xk+1=ΦkXk+VK
式中,Xk+1表示k+1次采样时刻状态矩阵;Φk表示k次采样时刻状态转移矩阵;Xk表示k次采样时刻四元数;VK为k次采样时刻过程噪声阵;
Φk=exp(ΩkTs)
式中,Ωk为k次采样时刻的Ω(wk);Ts为采样周期;
式中,为高斯白噪声,其协方差矩阵为Σk=||ek||I,I为3阶单位矩阵;Ξk表示k次采样时刻的误差系数矩阵;
式中,为k次采样时刻实测的加速度ak=[akx aky akz]T与预测的加速度vk=[vkxvky vkz]T之间的旋转误差,akx、aky、akz为k次采样时刻载体固定坐标系下实测的x轴、y轴和z轴采集到的三轴加速度数据,vkx、vky、vkz为k次采样时刻载体固定坐标系下预测的x轴、y轴和z轴采集到的三轴加速度数据;为k次采样时刻实测的磁感应强度mk=[mkx mky mkz]T与预测的磁感应强度hk=[hkx hky hkz]T之间的旋转误差,mkx、mky、mkz为k次采样时刻载体固定坐标系下实测的x轴、y轴和z轴采集到的三轴磁感应强度数据,hkx、hky、hkz为k次采样时刻载体固定坐标系下预测的x轴、y轴和z轴采集到的三轴磁感应强度数据;加速度预测向量vk=[vkx vky vkz]T和磁感应强度预测向量hk=[hkx hky hkz]T分别为理想状态下导航坐标系中的地球重力矢量和地磁场强度矢量在载体坐标系中的投影,同时投影可以通过姿态转换得到;
***过程噪声方差矩阵计算公式如下:
Qk=(Ts/2)2ΞkΣkΞk T。
优选的,步骤(4)中,利用快速高斯-牛顿法构建***的观测模型,并得到***测量噪声方差矩阵的具体过程如下:
利用实测的加速度ak=[akx aky akz]T减去预测的加速度vk=[vkx vky vkz]T和实测的磁感应强度mk=[mkx mky mkz]T减去预测的磁感应强度hk=[hkx hky hkz]T得到误差函数∈(qk),误差函数计算公式如下:
利用快速高斯-牛顿法求出满足的四元数的最优解,快速高斯-牛顿法公式如下:
式中,表示k+1次采样时刻的最优四元数解;为k次采样时刻的一步预测四元数值;λk在每次迭代过程中都会改变为最优值;J(0)为对应的雅可比矩阵,雅可比矩阵J(k)计算公式如下:
选取快速高斯牛顿法求出的最优解作为观测值,即:
令得到***量测噪声方差矩阵如下:
式中,I3×3为3阶单位矩阵。
优选的,步骤(5)中,根据建立的***状态方程和***观测模型建立如下的扩展卡尔曼滤波递推方程:
状态一步预测方程计算公式如下:
Xk,k-1=ΦkXk-1
式中,Xk-1表示k-1次采样时刻状态矩阵;Xk,k-1表示k次采样时刻预测状态矩阵;
一步预测方差矩阵计算公式如下:
式中,为矩阵Φk的转置;Qk-1为k-1次采样时刻的***过程噪声方差矩阵;Pk-1表示k-1次采样时刻的估计误差方差矩阵;Pk,k-1表示k次采样时刻的一步预测误差方差矩阵;
滤波增益矩阵计算公式如下:
式中,Rk表示k次采样时刻***量测噪声方差矩阵;T表示转置;Hk为4阶单位矩阵,且为k次采样时刻观测矩阵;Kk表示k次采样时刻的增益矩阵;
状态估计计算公式如下:
式中,Zk表示k次采样时刻的由快速高斯牛顿法计算出的观测值;
估计误差方差矩阵计算公式如下:
Pk=[I4×4-KkHk]Pk,k-1
式中,I4×4表示四阶单位矩阵;Pk表示k次采样时刻的估计误差方差矩阵。
优选的,步骤(6)中,利用每次递推得到的最佳四元数解算载体的三个姿态角:航向角为Ψ,俯仰角为θ,横滚角为γ的具体过程如下:
根据方向余弦矩阵和四元数之间的关系可得:
γk=arctan[-(qk1qk3-qk2qk0)/0.5-qk1 2-qk2 2)]
Ψk=[(qk1qk2-qk3qk0)/0.5-qk1 2-qk3 2)]
式中,θk、γk、Ψk分别表示k次采样时刻的航向角、俯仰角和横滚角。
本发明的有益效果为:(1)本发明的一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法,是一种四元数估计法,***过程方程是线性模型,具有估计精度高的优点,同时计算不复杂;(2)本发明提出了使用快速高斯-牛顿法来建立***观测模型,相对于其他的扩展卡尔曼滤波方法,它极大地简化了计算量,同时也有不错的效果;(3)本发明给出了过程噪声方差矩阵和测量噪声方差矩阵在实际过程中的计算方法,解决了现有参数计算不详等问题。
附图说明
图1为本发明的方法流程示意图。
图2为本发明采用的从AHRS设备记录的校准陀螺仪、加速度计和磁力计数据示意图。
图3为本发明通过姿态解算得到的欧拉角数据示意图。
具体实施方式
如图1所示,本发明的一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法,包括以下步骤:
步骤1:搭建姿态估计***,获取载体固定参考系坐标系下多轴传感器数据;
步骤2:对采集的加速度数据和磁感应强度数据做滤波处理,并对这两个传感器采集的数据做归一化处理;
三轴陀螺仪采集的数据为w=[wx wy wz]T,归一化后的三轴加速度计采集的数据为a=[ax ay az]T,归一化后的三轴磁力计采集的数据为m=[mx my mz]T;
步骤3:根据四元数微分方程和姿态矩阵构建载体***的状态方程,并得到***的过程噪声方差矩阵;
步骤4:利用快速高斯-牛顿法构建***的观测模型,并得到***的测量噪声方差矩阵;
步骤5:根据建立的***状态方程和观测模型建立扩展卡尔曼滤波递推方程;
步骤6:利用每次递推得到的最佳四元数解算载体的三个姿态角:航向角为Ψ,俯仰角为θ,横滚角为γ;
上述中,构建***的状态方程,并求出***的过程噪声方差矩阵的具体过程如下:
四元数微分方程如下:
式中,qk表示k次采样时刻四元数,其中qk=[qk1 qk2 qk3 qk0]T,[qk1 qk2 qk3]T是四元数qk的矢量部分,用表示,qk0是标量部分;表示qk的微分;wk为k次采样时刻的陀螺仪采集的数据;其中Ω(wk)的定义如下:
式中,wk=[wkx wky wkz]T,其中wkx、wky、wkz分别为k次采样时刻陀螺仪采集的x轴、y轴和z轴方向上的角速度数据;T表示转置;
根据四元数微分方程得到的离散化时间模型如下:
式中,qk+1表示k+1次采样时刻四元数;Ωk是k次采样时刻的Ω(wk);qk为k次采样时刻的四元数,q(0)为初始条件得到的四元数;Ts为采样周期;
通过四元数qk=[qk1 qk2 qk3 qk0]T可以得到姿态矩阵n系(导航坐标系):“东-北-天”地理坐标系;
以姿态四元数作为***的状态,即:
Xk=qk
式中,Xk表示k次采样时刻状态矩阵;qk表示k次采样时刻四元数;
建立***的状态方程如下式
Xk+1=ΦkXk+VK
式中,Xk+1表示k+1次采样时刻状态矩阵;Φk表示k次采样时刻状态转移矩阵;Xk表示k次采样时刻四元数;VK为k次采样时刻过程噪声阵;
Φk=exp(ΩkTs)
式中,Ωk为k次采样时刻的Ω(wk);Ts为采样周期;
式中,为高斯白噪声,其协方差矩阵为Σk=||ek||I,I为3阶单位矩阵;Ξk表示k次采样时刻的误差系数矩阵;
式中,为k次采样时刻实测的加速度向量ak=[akx aky akz]T与加速度预测向量vk=[vkx vky vkz]T之间的旋转误差,akx、aky、akz为k次采样时刻载体固定坐标系下实测的x轴、y轴和z轴采集到的三轴加速度数据,vkx、vky、vkz为k次采样时刻载体固定坐标系下预测的x轴、y轴和z轴采集到的三轴加速度数据;为k次采样时刻实测的磁感应强度向量mk=[mkxmky mkz]T与磁感应强度预测向量hk=[hkx hky hkz]T之间的旋转误差,mkx、mky、mkz为k次采样时刻载体固定坐标系下实测的x轴、y轴和z轴采集到的三轴磁感应强度数据,hkx、hky、hkz为k次采样时刻载体固定坐标系下预测的x轴、y轴和z轴采集到的三轴磁感应强度数据;
加速度预测向量vk=[vkx vky vkz]T和磁感应强度预测向量hk=[hkx hky hkz]T分别为理想状态下导航坐标系中的地球重力矢量和地磁场强度矢量在载体坐标系中的投影,投影过程通过姿态转换得到,计算公式如下:
bkz=dkz
式中,式中[dkx dky dkz]T是磁力计在n系的输出;[bkx 0 bkz]T是假设的实际地球磁场的大小和方向;是的转置;
***过程噪声方差矩阵计算公式如下:
Qk=(Ts/2)2ΞkΣkΞk T。
上述技术方案中,步骤4得到观测值并求出***的测量噪声方差矩阵的具体过程如下:利用实测的加速度ak=[akx aky akz]T减去预测的加速度vk=[vkx vky vkz]T和实测的磁感应强度mk=[mkx mky mkz]T减去预测的磁感应强度hk=[hkx hky hkz]T得到误差函数∈(qk),误差函数计算公式如下:
利用快速高斯-牛顿法求出满足的四元数 的最优解,快速高斯-牛顿法公式如下:
式中,表示k+1次采样时刻的最优四元数解;为k次采样时刻的一步预测四元数值;λk在每次迭代过程中都会改变为最优值;J(0)为对应的雅可比矩阵,雅可比矩阵J(k)计算公式如下:
选用快速高斯牛顿法求出的最优解作为观测值,即
令得到***量测噪声方差矩阵如下:
式中,I3×3为3阶单位矩阵;
上述技术方案中,步骤5得到的扩展卡尔曼滤波递推方程的具体过程如下:状态一步预测方程计算公式如下:
Xk,k-1=ΦkXk-1
式中,Xk-1表示k-1次采样时刻状态矩阵;Xk,k-1表示k次采样时刻预测状态矩阵;
一步预测方差矩阵计算公式如下:
式中,为矩阵Φk的转置,Qk-1为k-1次采样时刻的***过程噪声方差矩阵;Pk-1表示k-1次采样时刻的估计误差方差矩阵;Pk,k-1表示k次采样时刻的一步预测误差方差矩阵;
滤波增益矩阵计算公式如下:
式中,Rk表示k次采样时刻***量测噪声方差矩阵;T表示转置;Hk为4阶单位矩阵,且为k次采样时刻观测矩阵;Kk表示k次采样时刻的增益矩阵;
状态估计计算公式如下:
式中,Zk表示k次采样时刻的由快速高斯牛顿法计算出的观测值;
估计误差方差矩阵计算公式如下:
Pk=[I4×4-KkHk]Pk,k-1
式中,I4×4表示四阶单位矩阵;Pk表示k次采样时刻的估计误差方差矩阵;
上述技术方案中,步骤6利用每次递推得到的最佳四元数解算载体的三个姿态角:航向角为Ψ,俯仰角为θ,横滚角为γ的具体过程如下:
根据方向余弦矩阵和四元数之间的关系可得:
γk=arctan[-(qk1qk3-qk2qk0)/(0.5-qk1 2-qk2 2)]
Ψk=[(qk1qk2-qk3qk0)/(0.5-qk1 2-qk3 2)]
式中,θk、γk、Ψk分别表示k次采样时刻的航向角、俯仰角和横滚角。
尽管本发明就优选实施方式进行了示意和描述,但本领域的技术人员应当理解,只要不超出本发明的权利要求所限定的范围,可以对本发明进行各种变化和修改。
Claims (5)
1.一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法,其特征在于,包括如下步骤:
(1)搭建姿态估计***,获取载体固定参考系坐标系下多轴传感器数据,即b系,坐标原点位于惯性测量组件中心的“右-前-上”直角坐标系,陀螺仪采集的三轴角速度数据,加速度计采集的三轴加速度数据,磁力计采集的三轴磁感应强度数据;
(2)对采集的加速度数据和磁感应强度数据做滤波处理,并对这两个传感器采集的数据做归一化处理;三轴陀螺仪采集的数据为w=[wx wy wz]T,其中wx、wy、wz分别表示在载体固定坐标系中x轴、y轴和z轴采集到的三轴角速度数据;T表示转置;归一化后的三轴加速度计采集的数据为a=[ax ay az]T,其中ax、ay、az分别表示在载体固定坐标系中x轴、y轴和z轴采集到的三轴加速度数据;归一化后的三轴磁力计采集的数据为m=[mx my mz]T,其中mx、my、mz分别表示在载体固定坐标系中x轴、y轴和z轴采集到的三轴磁感应强度数据;
(3)根据四元数微分方程和姿态矩阵构建载体***的状态方程,并得到***的过程噪声方差矩阵;
(4)利用快速高斯-牛顿法构建***的观测模型,并得到***的测量噪声方差矩阵;
(5)根据建立的***状态方程和观测模型建立扩展卡尔曼滤波递推方程;
(6)利用每次递推得到的最佳四元数解算载体的三个姿态角:航向角为Ψ,俯仰角为θ,横滚角为γ。
2.如权利要求1所述的基于四元数的扩展卡尔曼滤波算法的姿态解算方法,其特征在于,步骤(3)中,根据四元数微分方程和姿态矩阵构建***的状态方程,并得到***过程噪声方差矩阵的具体过程如下:
四元数微分方程如下:
式中,qk表示k次采样时刻四元数,其中qk=[qk1 qk2 qk3 qk0]T,[qk1 qk2 qk3]T是四元数qk的矢量部分,用表示,qk0是标量部分;表示qk的微分;wk表示k次采样时刻的陀螺仪采集的数据;其中Ω(wk)的定义如下:
式中,wk=[wkx wky wkx]T,其中wkx、wky、wkz分别为k次采样时刻陀螺仪在载体固定坐标系采集的x轴、y轴和z轴方向上的角速度数据;T表示转置;
根据四元数微分方程得到的离散时间模型为:
式中,qk+1表示k+1次采样时刻四元数;Ωk是k次采样时刻的Ω(wk);qk为k次采样时刻的四元数,q(0)为初始条件得到的四元数;Ts为采样周期;
根据四元数qk=[qk1 qk2 qk3 qk0]T与姿态矩阵(n系:“东-北-天”地理坐标系)的关系得到姿态矩阵如下:
以姿态四元数作为***的状态,即:
Xk=qk
式中,Xk表示k次采样时刻状态矩阵;qk表示k次采样时刻四元数;
建立的***的状态方程如下式:
Xk+1=ΦkXk+VK
式中,Xk+1表示k+1次采样时刻状态矩阵;Φk表示k次采样时刻状态转移矩阵;Xk表示k次采样时刻四元数;VK为k次采样时刻过程噪声阵;
Φk=exp(ΩkTs)
式中,Ωk为k次采样时刻的Ω(wk);Ts为采样周期;
式中,为高斯白噪声,其协方差矩阵为Σk=||ek||I,I为3阶单位矩阵;Ξk表示k次采样时刻的误差系数矩阵;
式中,为k次采样时刻实测的加速度ak=[akx aky akz]T与预测的加速度vk=[vkx vkyvkz]T之间的旋转误差,akx、aky、akz为k次采样时刻载体固定坐标系下实测的x轴、y轴和z轴采集到的三轴加速度数据,vkx、vky、vkz为k次采样时刻载体固定坐标系下预测的x轴、y轴和z轴采集到的三轴加速度数据;为k次采样时刻实测的磁感应强度mk=[mkx mky mkz]T与预测的磁感应强度hk=[hkx hky hkz]T之间的旋转误差,mkx、mky、mkz为k次采样时刻载体固定坐标系下实测的x轴、y轴和z轴采集到的三轴磁感应强度数据,hkx、hky、hkz为k次采样时刻载体固定坐标系下预测的x轴、y轴和z轴采集到的三轴磁感应强度数据;加速度预测向量vk=[vkxvky vkz]T和磁感应强度预测向量hk=[hkx hky hkz]T分别为理想状态下导航坐标系中的地球重力矢量和地磁场强度矢量在载体坐标系中的投影,同时投影可以通过姿态转换得到;
***过程噪声方差矩阵计算公式如下:
Qk=(Ts/2)2ΞkΣkΞk T。
3.如权利要求1所述的基于四元数的扩展卡尔曼滤波算法的姿态解算方法,其特征在于,步骤(4)中,利用快速高斯-牛顿法构建***的观测模型,并得到***测量噪声方差矩阵的具体过程如下:
利用实测的加速度ak=[akx aky akz]T减去预测的加速度vk=[vkx vky vkz]T和实测的磁感应强度mk=[mkx mky mkz]T减去预测的磁感应强度hk=[hkx hky hkz]T得到误差函数∈(qk),误差函数计算公式如下:
利用快速高斯-牛顿法求出满足的四元数的最优解,快速高斯-牛顿法公式如下:
式中,表示k+1次采样时刻的最优四元数解;为k次采样时刻的一步预测四元数值;λk在每次迭代过程中都会改变为最优值;J(0)为对应的雅可比矩阵,雅可比矩阵J(k)计算公式如下:
选取快速高斯牛顿法求出的最优解作为观测值,即:
令得到***量测噪声方差矩阵如下:
式中,I3×3为3阶单位矩阵。
4.如权利要求1所述的基于四元数的扩展卡尔曼滤波算法的姿态解算方法,其特征在于,步骤(5)中,根据建立的***状态方程和***观测模型建立如下的扩展卡尔曼滤波递推方程:
状态一步预测方程计算公式如下:
Xk,k-1=ΦkXk-1
式中,Xk-1表示k-1次采样时刻状态矩阵;Xk,k-1表示k次采样时刻预测状态矩阵;
一步预测方差矩阵计算公式如下:
式中,为矩阵Φk的转置;Qk-1为k-1次采样时刻的***过程噪声方差矩阵;Pk-1表示k-1次采样时刻的估计误差方差矩阵;Pk,k-1表示k次采样时刻的一步预测误差方差矩阵;
滤波增益矩阵计算公式如下:
式中,Rk表示k次采样时刻***量测噪声方差矩阵;T表示转置;Hk为4阶单位矩阵,且为k次采样时刻观测矩阵;Kk表示k次采样时刻的增益矩阵;
状态估计计算公式如下:
式中,Zk表示k次采样时刻的由快速高斯牛顿法计算出的观测值;
估计误差方差矩阵计算公式如下:
Pk=[I4×4-KkHk]Pk,k-1
式中,I4×4表示四阶单位矩阵;Pk表示k次采样时刻的估计误差方差矩阵。
5.如权利要求1所述的基于四元数的扩展卡尔曼滤波算法的姿态解算方法,其特征在于,步骤(6)中,利用每次递推得到的最佳四元数解算载体的三个姿态角:航向角为Ψ,俯仰角为θ,横滚角为γ的具体过程如下:
根据方向余弦矩阵和四元数之间的关系可得:
γk=arctan[-(qk1qk3-qk2qk0)/(0.5-qk1 2-qk2 2)]
Ψk=[(qk1qk2-qk3qk0)/(0.5-qk1 2-qk3 2)]
式中,θk、γk、Ψk分别表示k次采样时刻的航向角、俯仰角和横滚角。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201711179949.2A CN108225308B (zh) | 2017-11-23 | 2017-11-23 | 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201711179949.2A CN108225308B (zh) | 2017-11-23 | 2017-11-23 | 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108225308A true CN108225308A (zh) | 2018-06-29 |
CN108225308B CN108225308B (zh) | 2021-06-25 |
Family
ID=62653564
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201711179949.2A Active CN108225308B (zh) | 2017-11-23 | 2017-11-23 | 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108225308B (zh) |
Cited By (19)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108827313A (zh) * | 2018-08-10 | 2018-11-16 | 哈尔滨工业大学 | 基于扩展卡尔曼滤波器的多模式旋翼飞行器姿态估计方法 |
CN109883451A (zh) * | 2019-04-15 | 2019-06-14 | 山东建筑大学 | 一种用于行人方位估计的自适应增益互补滤波方法及*** |
CN109990776A (zh) * | 2019-04-12 | 2019-07-09 | 武汉深海蓝科技有限公司 | 一种姿态测量方法及装置 |
CN110146077A (zh) * | 2019-06-21 | 2019-08-20 | 台州知通科技有限公司 | 移动机器人姿态角解算方法 |
CN110174121A (zh) * | 2019-04-30 | 2019-08-27 | 西北工业大学 | 一种基于地磁场自适应修正的航姿***姿态解算方法 |
CN110207647A (zh) * | 2019-05-08 | 2019-09-06 | 诺百爱(杭州)科技有限责任公司 | 一种基于互补卡尔曼滤波器的臂环姿态角计算方法 |
CN110307842A (zh) * | 2019-07-08 | 2019-10-08 | 常州大学 | 用于惯性-地磁组合的快速扩展卡尔曼算法 |
CN110319840A (zh) * | 2019-07-05 | 2019-10-11 | 东北大学秦皇岛分校 | 面向异常步态识别的共轭梯度姿态解算方法 |
CN111426318A (zh) * | 2020-04-22 | 2020-07-17 | 中北大学 | 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法 |
CN111551174A (zh) * | 2019-12-18 | 2020-08-18 | 无锡北微传感科技有限公司 | 基于多传感器惯性导航***的高动态车辆姿态计算方法及*** |
CN111625768A (zh) * | 2020-05-19 | 2020-09-04 | 西安因诺航空科技有限公司 | 一种基于扩展卡尔曼滤波的手持云台姿态估计方法 |
CN111896007A (zh) * | 2020-08-12 | 2020-11-06 | 智能移动机器人(中山)研究院 | 一种补偿足地冲击的四足机器人姿态解算方法 |
CN112181046A (zh) * | 2019-07-03 | 2021-01-05 | 深圳拓邦股份有限公司 | 一种旋钮档位输出方法、装置、旋钮装置以及存储装置 |
CN112414364A (zh) * | 2020-11-04 | 2021-02-26 | 国网福建省电力有限公司建设分公司 | 悬浮抱杆的姿态监测装置及方法 |
CN113114105A (zh) * | 2021-03-10 | 2021-07-13 | 上海工程技术大学 | 一种光伏电池组件输出特性动态测量方法 |
CN113830220A (zh) * | 2021-11-04 | 2021-12-24 | 江苏科技大学 | 基于信息融合的电动车助力控制方法 |
CN114053679A (zh) * | 2021-11-19 | 2022-02-18 | 上海复动医疗管理有限公司 | 运动训练方法及其*** |
CN115855104A (zh) * | 2022-11-25 | 2023-03-28 | 哈尔滨工程大学 | 一种组合导航滤波结果最优在线评价方法 |
CN116070066A (zh) * | 2023-02-20 | 2023-05-05 | 北京自动化控制设备研究所 | 一种制导炮弹滚动角计算方法 |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6453239B1 (en) * | 1999-06-08 | 2002-09-17 | Schlumberger Technology Corporation | Method and apparatus for borehole surveying |
CN101915580A (zh) * | 2010-07-14 | 2010-12-15 | 中国科学院自动化研究所 | 一种基于微惯性和地磁技术的自适应三维姿态定位方法 |
US20170003751A1 (en) * | 2015-06-30 | 2017-01-05 | Stmicroelectronics S.R.L. | Device and method for determination of angular position in three-dimensional space, and corresponding electronic apparatus |
CN106500693A (zh) * | 2016-12-07 | 2017-03-15 | 中国电子科技集团公司第五十四研究所 | 一种基于自适应扩展卡尔曼滤波的ahrs算法 |
CN106595711A (zh) * | 2016-12-21 | 2017-04-26 | 东南大学 | 一种基于递推四元数的捷联惯性导航***粗对准方法 |
CN106767837A (zh) * | 2017-02-23 | 2017-05-31 | 哈尔滨工业大学 | 基于容积四元数估计的航天器姿态估计方法 |
WO2017089236A1 (en) * | 2015-11-24 | 2017-06-01 | Vinati S.R.L. | Method of estimating an attitude of a control device for controlling operating machines |
CN106979780A (zh) * | 2017-05-22 | 2017-07-25 | 深圳市靖洲科技有限公司 | 一种无人车实时姿态测量方法 |
-
2017
- 2017-11-23 CN CN201711179949.2A patent/CN108225308B/zh active Active
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6453239B1 (en) * | 1999-06-08 | 2002-09-17 | Schlumberger Technology Corporation | Method and apparatus for borehole surveying |
CN101915580A (zh) * | 2010-07-14 | 2010-12-15 | 中国科学院自动化研究所 | 一种基于微惯性和地磁技术的自适应三维姿态定位方法 |
US20170003751A1 (en) * | 2015-06-30 | 2017-01-05 | Stmicroelectronics S.R.L. | Device and method for determination of angular position in three-dimensional space, and corresponding electronic apparatus |
WO2017089236A1 (en) * | 2015-11-24 | 2017-06-01 | Vinati S.R.L. | Method of estimating an attitude of a control device for controlling operating machines |
CN106500693A (zh) * | 2016-12-07 | 2017-03-15 | 中国电子科技集团公司第五十四研究所 | 一种基于自适应扩展卡尔曼滤波的ahrs算法 |
CN106595711A (zh) * | 2016-12-21 | 2017-04-26 | 东南大学 | 一种基于递推四元数的捷联惯性导航***粗对准方法 |
CN106767837A (zh) * | 2017-02-23 | 2017-05-31 | 哈尔滨工业大学 | 基于容积四元数估计的航天器姿态估计方法 |
CN106979780A (zh) * | 2017-05-22 | 2017-07-25 | 深圳市靖洲科技有限公司 | 一种无人车实时姿态测量方法 |
Non-Patent Citations (3)
Title |
---|
XU XIANG,ETC: "A Kalman Filter for SINS Self-Alignment Based on Vector Observation", 《SENSORS》 * |
徐晓苏等: "基于四元数自适应卡尔曼滤波的快速对准算法", 《中国惯性技术学报》 * |
赵文晔等: "Quaternion-EKF的多源传感器联合定向算法", 《测绘科学技术学报》 * |
Cited By (26)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108827313A (zh) * | 2018-08-10 | 2018-11-16 | 哈尔滨工业大学 | 基于扩展卡尔曼滤波器的多模式旋翼飞行器姿态估计方法 |
CN109990776A (zh) * | 2019-04-12 | 2019-07-09 | 武汉深海蓝科技有限公司 | 一种姿态测量方法及装置 |
CN109883451A (zh) * | 2019-04-15 | 2019-06-14 | 山东建筑大学 | 一种用于行人方位估计的自适应增益互补滤波方法及*** |
CN110174121A (zh) * | 2019-04-30 | 2019-08-27 | 西北工业大学 | 一种基于地磁场自适应修正的航姿***姿态解算方法 |
CN110207647A (zh) * | 2019-05-08 | 2019-09-06 | 诺百爱(杭州)科技有限责任公司 | 一种基于互补卡尔曼滤波器的臂环姿态角计算方法 |
CN110207647B (zh) * | 2019-05-08 | 2021-11-09 | 诺百爱(杭州)科技有限责任公司 | 一种基于互补卡尔曼滤波器的臂环姿态角计算方法 |
WO2020253854A1 (zh) * | 2019-06-21 | 2020-12-24 | 台州知通科技有限公司 | 移动机器人姿态角解算方法 |
CN110146077A (zh) * | 2019-06-21 | 2019-08-20 | 台州知通科技有限公司 | 移动机器人姿态角解算方法 |
CN112181046A (zh) * | 2019-07-03 | 2021-01-05 | 深圳拓邦股份有限公司 | 一种旋钮档位输出方法、装置、旋钮装置以及存储装置 |
CN110319840A (zh) * | 2019-07-05 | 2019-10-11 | 东北大学秦皇岛分校 | 面向异常步态识别的共轭梯度姿态解算方法 |
CN110307842A (zh) * | 2019-07-08 | 2019-10-08 | 常州大学 | 用于惯性-地磁组合的快速扩展卡尔曼算法 |
CN110307842B (zh) * | 2019-07-08 | 2022-03-25 | 常州大学 | 用于惯性-地磁组合的快速扩展卡尔曼方法 |
CN111551174A (zh) * | 2019-12-18 | 2020-08-18 | 无锡北微传感科技有限公司 | 基于多传感器惯性导航***的高动态车辆姿态计算方法及*** |
CN111426318A (zh) * | 2020-04-22 | 2020-07-17 | 中北大学 | 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法 |
CN111426318B (zh) * | 2020-04-22 | 2024-01-26 | 中北大学 | 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法 |
CN111625768B (zh) * | 2020-05-19 | 2023-05-30 | 西安因诺航空科技有限公司 | 一种基于扩展卡尔曼滤波的手持云台姿态估计方法 |
CN111625768A (zh) * | 2020-05-19 | 2020-09-04 | 西安因诺航空科技有限公司 | 一种基于扩展卡尔曼滤波的手持云台姿态估计方法 |
CN111896007A (zh) * | 2020-08-12 | 2020-11-06 | 智能移动机器人(中山)研究院 | 一种补偿足地冲击的四足机器人姿态解算方法 |
CN112414364A (zh) * | 2020-11-04 | 2021-02-26 | 国网福建省电力有限公司建设分公司 | 悬浮抱杆的姿态监测装置及方法 |
CN113114105A (zh) * | 2021-03-10 | 2021-07-13 | 上海工程技术大学 | 一种光伏电池组件输出特性动态测量方法 |
CN113830220A (zh) * | 2021-11-04 | 2021-12-24 | 江苏科技大学 | 基于信息融合的电动车助力控制方法 |
CN113830220B (zh) * | 2021-11-04 | 2022-09-13 | 浙江欧飞电动车有限公司 | 基于信息融合的电动车助力控制方法 |
CN114053679A (zh) * | 2021-11-19 | 2022-02-18 | 上海复动医疗管理有限公司 | 运动训练方法及其*** |
CN115855104A (zh) * | 2022-11-25 | 2023-03-28 | 哈尔滨工程大学 | 一种组合导航滤波结果最优在线评价方法 |
CN116070066A (zh) * | 2023-02-20 | 2023-05-05 | 北京自动化控制设备研究所 | 一种制导炮弹滚动角计算方法 |
CN116070066B (zh) * | 2023-02-20 | 2024-03-15 | 北京自动化控制设备研究所 | 一种制导炮弹滚动角计算方法 |
Also Published As
Publication number | Publication date |
---|---|
CN108225308B (zh) | 2021-06-25 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108225308A (zh) | 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法 | |
Ludwig et al. | Comparison of Euler estimate using extended Kalman filter, Madgwick and Mahony on quadcopter flight data | |
CN107655476B (zh) | 基于多信息融合补偿的行人高精度足部导航方法 | |
Wu et al. | Fast complementary filter for attitude estimation using low-cost MARG sensors | |
CN106225784B (zh) | 基于低成本多传感器融合行人航位推算方法 | |
CN109556632A (zh) | 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法 | |
CN102692225B (zh) | 一种用于低成本小型无人机的姿态航向参考*** | |
CN104969030B (zh) | 惯性装置、方法和程序 | |
CN104698486B (zh) | 一种分布式pos用数据处理计算机***实时导航方法 | |
CN107478223A (zh) | 一种基于四元数和卡尔曼滤波的人体姿态解算方法 | |
CN106780699A (zh) | 一种基于sins/gps和里程计辅助的视觉slam方法 | |
CN104698485B (zh) | 基于bd、gps及mems的组合导航***及导航方法 | |
CN108458714B (zh) | 一种姿态检测***中不含重力加速度的欧拉角求解方法 | |
CN110274588A (zh) | 基于无人机集群信息的双层嵌套因子图多源融合导航方法 | |
CN107390247A (zh) | 一种导航方法、***及导航终端 | |
CN102445200A (zh) | 微小型个人组合导航***及其导航定位方法 | |
CN108318038A (zh) | 一种四元数高斯粒子滤波移动机器人姿态解算方法 | |
CN105953795B (zh) | 一种用于航天器表面巡视的导航装置及方法 | |
CN110017837A (zh) | 一种姿态抗磁干扰的组合导航方法 | |
CN109916395A (zh) | 一种姿态自主冗余组合导航算法 | |
CN107728182A (zh) | 基于相机辅助的柔性多基线测量方法和装置 | |
Wahdan et al. | Three-dimensional magnetometer calibration with small space coverage for pedestrians | |
CN106403952A (zh) | 一种动中通低成本组合姿态测量方法 | |
CN108871319B (zh) | 一种基于地球重力场与地磁场序贯修正的姿态解算方法 | |
Carratù et al. | Energy characterization of attitude algorithms |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |