基于四元数平方根容积卡尔曼滤波的姿态估计_钱华明
合集下载
相关主题
- 1、下载文档前请自行甄别文档内容的完整性,平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益,请联系客服反馈,我们会尽快为您处理(人工客服工作时间:9:00-18:30)。
欧拉角、 修正罗德里格斯参数、 四元数等是飞 [1 ] 行器 姿 态 主 要 描 述 参 数 . 四 元 数 由 于 计 算 简 单, 无三角函数运算, 同时又能避免欧拉角的奇异 性问题, 因此, 基于四元数的滤波问题成为研究的 热点
[2 - 3 ]
4]提 出 中 乘 性 扩 展 卡 尔 曼 滤 波 性问题, 文 献[ ( MEKF,Multiplicative Extended Kalman Filter ) 来 5]提出无迹四元数估 解决了这两个问题. 文献[ 计法 ( USQUE ,Unscented Quaternion Estimator ) , 采用罗德里格斯参数与四元数相切换, 避免了四
647
Ω ( ω) =
[ ω ×] ω [ -- ] 0 ω
T P xz, k| k - 1 = γk λk
x k 为 n × 1 维状态向量; z k 为 s × 1 维观测向 其中, 量; f( ·) 和 h ( · ) 为非线性状态函数和量测函 v k 分别为系统噪声、 数; w k - 1 , 量测噪声, 其均值 Rk 为 协 方 差 互 不 相 关 的 高 斯 白 为零, Q k - 1 , 噪声. SCKF( Squareroot Cubature Kalman Filter ) 算 法的基本容积点和对应权值为 εj = ωj =
[9 ] 8] 的复杂性, 文献[ 的方法得到广泛应用 . 本文
状态预测及方差预测 Cholesky 分解因子为
m
x k- =
* ω j χ j, ∑ k j =1
( 6) ( 7)
* S k- = tria( [ λk
S Q k -1 ] )
tria ( · ) 表 示 对 矩 阵 进 行 三 角 化, 其中, 若 A∈ n×l T l×l R 且 n ≤ l, Q ∈ R 为对角阵, 对 A 进行分解, ^∈ R∈R l × n 为上三角矩阵, 取 R 的上三角部分 R ^ T; S Rn × n , 则 S = tria( A ) = R Q k - 1 表示 Q k - 1 的 Cholesky 分解因子;
Hale Waihona Puke Baidu46
北 京 航 空 航 天 大 学 学 报
2013 年
元数规范化问题, 但是这种切换增加了计算负担, 6]提出一种基于平方根四元 影响实时性. 文献[ 数容积卡尔曼滤波的姿态估计, 采用两步投影理 论来保证估计四元数的归一化, 但未考虑四元数 是旋转矢量而非一般向量, 存在加权均值计算问 7] 文献[ 提出一种基于姿态矩阵 题. 针对此问题, 8]提出一种基于特征 奇异值分解的方法. 文献[ . 向量求解的代价函数法 考虑到矩阵奇异值分解
2013 年 5 月 第 39 卷 第 5 期
北京航空航天大学学报 Journal of Beijing University of Aeronautics and Astronautics
May 2013 Vol. 39 No. 5
基于四元数平方根容积卡尔曼滤波的姿态估计
钱华明 黄 蔚 葛 磊 张广拓
. 针对四元数规范化问题和协方差奇异
0523 ; 网络出版时间 : 20130507 11 : 38 收稿日期: 2012网络出版地址: www. cnki. net / kcms / detail /11. 2625. V. 20130507. 1138. 009. html 基金项目: 国家自然科学基金资助项目( 61104036 ) qianhuam@ sina. com. 作者简介: 钱华明( 1965 - ) , 男, 安徽池州人, 教授,
( 天津航海仪器研究所,天津 300131 ) ( 哈尔滨工程大学 自动化学院,哈尔滨 150001 )
摘 要: 针对飞行器姿态确定中乘性扩展卡尔曼滤波 ( MEKF,Multiplicative Extended Kalman Filter) 在较大初始姿态误差角情况下存在估计精度低及收敛速度慢的问题 , 提出了 ( QSCKF , Quaternion Squareroot Cubature Kalman Filter )算 一种四元数平方根容积卡尔曼滤波 法. 在推导姿态确定系统四元数非线性误差模型的基础上 , 采用容积数值积分理论来计算非线 性函数的均值与方差, 同时使用平方根的形式来提高数值稳定性; 针对四元数规范化问题, 采 用拉格朗日代价函数法求解四元数加权均值 . 仿真结果表明: 在初始姿态误差较大的情况下, 该算法相比较于 MEKF 以及无迹四元数估计法 ( USQUE ,Unscented Quaternion Estimator ) , 估 计精度高且收敛速度快, 滤波稳定性好, 同时估计时间比 USQUE 缩短了 1 /3. 关 键 词: 平方根容积卡尔曼滤波; 四元数; 拉格朗日代价函数法; 姿态估计 中图分类号: U 666. 12 5965 ( 2013 ) 05064505 文献标识码: A 文 章 编 号: 1001-
T
}
( 16 )
~ ( t) 为陀螺的量测输出值; β ( t ) 为陀螺漂 其中, ω 移; η v ( t) 和 η u ( t ) 为互不相关的零均值白噪声,
2 2 均方差分别为 σ v 与 σ u . 2 ) 系统状态方程. 根据文献[ 1] , 飞行器姿
( 3)
chol{ ·} 表示对矩阵的 Cholesky 分解. 其中, 2 ) 时间更新. 容积点及容积点经状态方程的 传递值为 ^ χ j, k -1 = x k -1 + S k -1 ε j j = 1, 2, …, 2n ( 4) ( 5)
Z j, ( 9) k = h( χ ) 预测、 方差 Cholesky 分解因子和协方差为
m
^k = z
}
ω j Z j, ∑ k j =1 SRk ] )
( 10 ) ( 11 ) ( 12 ) ^ Z m, k - z k]
- - χ m, k - xk ]
( 1) 其中 λk = γk =
S zz, λk k = tria( [
* χ j, k = f ( χ j, k -1 )
态运动学方程为
· q =
1 ω 1 q = Ω ( ω) · q 2 0 2
[ ]
( 17 )
T q4 ] , 其中, 四元数 q = [ ρ , ρ 为四元数向量部分; [ ω ×] 表示由向量 ω 的分量构成的反对称矩阵:
第5 期
钱华明等: 基于四元数平方根容积卡尔曼滤波的姿态估计
[10 ]
1 ^ [ Z1, k - zk m 槡 1 - - [ χ1 , k - xk m 槡
^ Z2, k - zk
- - χ2 , k - xk
… …
槡
1 m
m [ 1] j 2 j = 1, 2, …, m
}
}
增益、 估计状态及方差 Cholesky 分解因子为 T W k = ( P xz, ( 13 ) k| k - 1 / S zz, k ) / S zz, k ^ k = x k- + W k ( z k - z ^k ) x ( 14 ) ( 15 ) S k = tria( [ γk - W k λk Wk SRk ] )
Attitude estimation based on quaternion squareroot cubature Kalman filter
Qian Huaming Huang Wei Ge Lei
( College of Automation,Harbin Engineering University,Harbin 150001 ,China)
( 2)
2
2. 1
QSCKF 算法
误差四元数非线性姿态确定模型 1 ) 陀螺测量模型. 假设陀螺的数学模型为 ~ ( t ) = ω( t ) + β ( t ) + η ( t ) ω v β ( t) = η u ( t)
·
m 为容积点总数, 其中, 且 m = 2 n; ε j 为第 j 个容 1, 0, 积点, 其产生方式为: 设 n 维单位向量 e = [
T …, 0] , 1]表示对 e 的元素进行全排 使用符号[ 称为完整全对 列和改变元素符号所产生的点集,
[ 1] 1] 称点集, 的第 j 个点; ω j 为对 j 表示点集中[ 应点的权值. SCKF 算法步骤如下. 1 ) 状态参数初始化: ^ 0 = E[ x x0] ^ 0 ) ( x0 - x ^0 ) ] S0 = chol{ E [ ( x0 - x }
1 * ^- [ χ1 , k - xk m 槡
* ^- χ2 , k - xk
…
* ^- χ m, k - xk ]
3 ) 量测更新. 容积点及容积点经量测方程的 传递值为
- - - χ j, k = xk + Sk εj
j = 1, 2, …, 2n
- j, k
( 8)
1
平方根容积卡尔曼滤波
考虑离散非线性系统为 x k = f( x k -1 ) + w k -1 zk = h( x k ) + v k
* λk =
采用一种相对简单的拉格朗日代价函数法来求解 四元数加权均值. 基于此, 本文推导了基于误差四 并提出一种四元数 元数的非线性姿态确定模型, 平方 根 容 积 卡 尔 曼 滤 波 ( QSCKF, Quaternion Squareroot Cubature Kalman Filter ) 算法. 通 过 仿 真验证了该算法的有效性.
Zhang Guangtuo
( Tianjin Navigation Instrument Research Institute,Tianjin 300131 ,China)
Abstract: Considering that a multiplicative extended Kalman filter ( MEKF) existed lower estimation accuracy and slower convergence rate for large initialization errors,a quaternion squareroot cubature Kalman filter ( QSCKF) was proposed. A quaternion nonlinear error model was derived for attitude determination system,and then the cubature numerical integration theory was used to calculate the mean and variance of the nonlinear function. The squareroot forms were used to improve numerical stability. To avoid the quaternion normalization problem,a Lagrange loss function was derived to compute the average quaternion. The simulation results indicate that the proposed algorithm provides higher estimation precision with faster convergence rate and better filter’ s stability than MEKF and unscented quaternion estimator ( USQUE ) for large initialization errors. And,the estimated time of this algorithm is shortened by 1 /3 compared to USQUE. Key words: squareroot cubature Kalman filter; quaternion; Lagrange loss function; attitude estimation