﻿ 不同姿态表示方法下的姿态估计分析
 舰船科学技术  2018, Vol. 40 Issue (3): 137-141 PDF

Attitude estimation analysis under different attitude representation methods
LU Xin, GAO Jing-dong, LI Kai-long
Navigation Engineering Department, Naval University of Engineering, Wuhan 43000, China
Abstract: In the navigation, according to the different attitude representation method to construct the attitude estimation algorithm performance as the differences in the amount of calculation and accuracy, respectively to four yuan number tasteless estimator, modified Rodrigues parameters and high order parameters of Rodrigues attitude estimation algorithm is deduced and the theory of comparative analysis based on performance in numerical. In the experiment, the advantages and disadvantages of algorithm in the calculation of quantity and accuracy of the comparison of three kinds of attitude estimation, the results show that the four element number estimator accuracy is optimal but tasteless, large amount of calculation, and the high order Rodrigues parameter attitude estimation algorithm based on the minimum amount of computation, relatively high precision.
Key words: integrated navigation     attitude estimation     quaternion     rodrigues parameters
0 引　言

1 基于USQUE的姿态估计算法研究

 $\left\{ \begin{gathered} {x_k} = f({x_{k - 1}}) + {q_{k - 1}} \hfill \text{，}\\ {y_k} = h({x_k}) + {v_k} \text{。} \hfill \\ \end{gathered} \right.$ (1)

 $\delta \sigma = f\frac{{\delta \rho }}{{a + \delta {q_0}}}\text{。}$ (2)

 ${\chi _{k - 1}}(i) = \left[ \begin{gathered} \chi _{k - 1}^{\delta \sigma }(i) \hfill \\ \chi _{k - 1}^e(i) \hfill \\ \end{gathered} \right] = {{sig}}ma({\hat x_{k - 1|k - 1}},{P_{k - 1|k - 1}}),i = 0,1, \cdots 2n\text{，}$ (3)

 $\chi _{k - 1}^{\delta q}(i) = {[\delta {q_{k - 1,0}}(i)\;\;\delta {\rho _{k - 1}}(i)]^{{T}}}\text{，}$ (4)

$\chi _{k - 1}^{\delta q}(i)$ 可由式（2）的逆形式求得，即

 $\delta {q_{k - 1,0}}(i) = \frac{{ - a||\chi _{k - 1}^{\delta q}(i)|{|^2} + f\sqrt {{f^2} + (1 - {a^2})||\chi _{k - 1}^{\delta q}(i)|{|^2}} }}{{{f^2} + ||\chi _{k - 1}^{\delta q}(i)|{|^2}}}\text{，}$
 $\delta {\rho _{k - 1}}(i) = {f^{ - 1}}[a + \delta {q_{k - 1,0}}(i)]\chi _{k - 1}^{\delta q}(i)\text{。}$ (5b)

 $\chi _{k - 1}^q(i) = \chi _{k - 1}^{\delta q}(i) \otimes {\hat q_{k - 1|k - 1}}\text{，}$ (6)

$\otimes$ 表示四元数相乘，将 $\chi _{k - 1}^q(i)$ 和构造一组新的Sigma点，即

 ${\chi '_{k - 1}}(i) = \left[ \begin{gathered} \chi _{k - 1}^q(i) \hfill \\ \chi _{k - 1}^e(i) \hfill \\ \end{gathered} \right]\text{，}$ (7)

${\chi '_{k - 1}}(i)$ 在滤波中进行传递得到 ${\chi '_{k|k - 1}}(i)$ ，将 ${\chi '_{k|k - 1}}(i)$ 写成分解形式

 ${\chi '_{k|k - 1}}(i) = \left[ \begin{gathered} \chi _{k|k - 1}^q(i) \hfill \\ \chi _{k|k - 1}^e(i) \hfill \\ \end{gathered} \right]\text{，}$ (8)

 $\chi _{k|k - 1}^{\delta q}(i) = \chi _{k|k - 1}^q(i) \otimes {[\chi _{k|k - 1}^q(0)]^{ - 1}}\text{，}$
 $\chi _{k|k - 1}^{\delta q}(i) = {[\delta {q_{k|k - 1,0}}(i)\;\;\delta {\rho _{k|k - 1}}(i)]^{{T}}}\text{，}$ (9b)
 $\chi _{k|k - 1}^{\delta \sigma }(i) = f\frac{{\delta {\rho _{k|k - 1}}(i)}}{{a + \delta {q_{k|k - 1,0}}(i)}}\text{，}$

$\chi _{k|k - 1}^{\delta \sigma }(i)$ $\chi _{k|k - 1}^e(i)$ 组合构造一步预测后的状态传递Sigma点

 ${\chi _{k|k - 1}}(i) = \left[ \begin{gathered} \chi _{k|k - 1}^{\delta \sigma }(i) \hfill \\ \chi _{k|k - 1}^e(i) \hfill \\ \end{gathered} \right]\text{，}$ (10)

 ${\hat x_{k|k - 1}} = \sum\limits_{i = 0}^{2n} {w(i){\chi _{k|k - 1}}(i)} \text{，}$ (11)
 $\begin{split}{P_{k|k - 1}} = & \sum\limits_{i = 0}^{2n} {w(i)({\chi _{k|k - 1}}(i) - {{\hat x}_{k|k - 1}})}\\& {{({\chi _{k|k - 1}}(i) - {{\hat x}_{k|k - 1}})}^{{T}}} + {Q_{k - 1}}\end{split}\text{。}$ (12)

 ${y_k} = {{{H}}_k}{x_k} + {\eta _k},$ (13)

 ${{{H}}_{p,k}} = [{0_{3 \times 6}}\;\;{I_{3 \times 3}}\;\;{0_{3 \times 6}}]\text{，}$ (14)

 ${\hat x_{k|k}} = {\hat x_{k|k - 1}} + {K_k}({y_k} - {H_k}{\hat x_{k|k - 1}})\text{，}$
 ${P_{k|k}} = [{I_n} - {K_k}{H_k}]{P_{k|k - 1}}\text{，}$
 ${K_k} = {P_{k|k - 1}}H_k^{ T}{[{H_k}{P_{k|k - 1}}H_k^{ T} + {R_k}]^{ - 1}}\text{。}$ (15c)

${\hat x_{k|k}} = {[\delta {\sigma _{k|k}},\hat x_{k|k}^e]^{{T}}}$ ，则 $\delta {\sigma _{k|k}}$ 对应的四元数形式为

 $\hat x_{k|k}^{\delta q} = {[\delta {q_{k,0}}\;\;\delta {\rho _k}(i)]^{{T}}}\text{，}$ (16)

 $\delta {q_{k,0}} = \frac{{ - a{{\left\| {\delta {\sigma _{k|k}}} \right\|}^2} + f\sqrt {{f^2} + (1 - {a^2}){{\left\| {\delta {\sigma _{k|k}}} \right\|}^2}} }}{{{f^2} + {{\left\| {\delta {\sigma _{k|k}}} \right\|}^2}}}\text{，}$
 $\delta {\rho _k} = {f^{ - 1}}[a + \delta {q_{k,0}}]\delta {\sigma _{k|k}}\text{，}$ (17b)

 $\hat q_{b,k|k}^n = \hat x_{k|k}^{\delta q} \otimes \chi _{k|k - 1}^q(0)\text{。}$ (18)

2 基于MRP和HOMRP的姿态估计算法研究

Rodrigues参数族本质上是四元数在三维超平面上的投影。Rodrigues参数族并不存在四元数在非线性滤波中的约束问题，同时，Rodrigues参数少了一个冗余的标量，所以在理论上Rodrigues参数族要比四元数在姿态表示上的计算量更小，但存在着奇异性问题。Rodrigues参数族统一可表示为

 $R{P_{family}} = e\tan (\frac{\vartheta }{{2N}})\text{。}$ (19)

MRP姿态更新方程

 $\sigma {{ = }}\frac{{1{{ + }}{{\left\| \sigma \right\|}^2}}}{4}\left( {{I_3} + 2\frac{{{{[\sigma \times ]}^2} + [\sigma \times ]}}{{1 + {{\left\| \sigma \right\|}^2}}}} \right)\omega \text{，}$ (20)

 ${{C}}(\sigma ) = {I_3}{{ + }}\frac{{8{{[\sigma \times ]}^2} - 4(1 - {{\left\| \sigma \right\|}^2})[\sigma \times ]}}{{1 + {{\left\| \sigma \right\|}^2}}}\text{，}$ (21)

 $\tau = e\tan (\frac{\vartheta }{8})\text{，}$ (22)

 $\sigma = \pm \frac{{2\tau }}{{1 - {{\left\| \tau \right\|}^2}}}\text{，}$ (23)

 $\tau = \frac{\rho }{{1 + {q_0} \pm \sqrt {2(1 + {q_0})} }}\text{，}$ (24)

 $\rho = 4\tau \frac{{1 - {{\left\| \tau \right\|}^2}}}{{{{(1 + {{\left\| \tau \right\|}^2})}^2}}}\text{，}$
 ${q_0} = \frac{{1 - 6{{\left\| \tau \right\|}^2} + {{\left\| \tau \right\|}^4}}}{{{{(1 + {{\left\| \tau \right\|}^2})}^2}}}\text{。}$ (25b)

HOMRP姿态更新方程

 $\frac{{{{d}}\tau }}{{{{d}}t}} = G(\tau )\omega \text{，}$
 $\begin{split} G(\tau ) = & \frac{1}{{8(1 - {{\left\| \tau \right\|}^2})}}[2(3 - {\left\| \tau \right\|^2})\tau {\tau ^{ T}} - 4(1 - {\left\| \tau \right\|^2})[\tau \times ]+ \\& (1 - 6{\left\| \tau \right\|^2} + {\left\| \tau \right\|^4}){I_3}] \text{。}\end{split}$ (26b)

3 基于MRP、HOMRP和USQUE的姿态估计结果分析

 ${x_0} = {[trj({q_0})\;\;trj({v_0})\;\;trj({p_0})]^{{T}}}\text{，}$
 ${P_0} = { diag}({[{I_{3 \times 1}} \cdot \deg \;\;0.1 \cdot {I_{3 \times 1}} \cdot \deg \;\;10/{\mathop{ Re}\nolimits} \;\;10/{\mathop{ Re}\nolimits} \;\;5]^{{T}}})\text{，}$
 ${R_0} = { diag}{({[10/glv \cdot {\mathop{ Re}\nolimits} \;\;10/glv \cdot {\mathop{ Re}\nolimits} \;\;10]^{{T}}})^2}\text{，}$
 ${Q_0} = {{diag}}{({[{w_{eb}}\;\;{w_{db}}\;\;{0_{3 \times 3}}]^{{T}}})^2}\text{。}$ (28d)

 ${w_{eb}} = {[0.29 \times {10^{ - 4}}\;\;0.29 \times {10^{ - 4}}\;\;0.29 \times {10^{ - 4}}]^{{T}}}\text{，}$
 ${w_{db}} = {[0.97 \times {10^{ - 4}}\;\;0.97 \times {10^{ - 4}}\;\;0.97 \times {10^{ - 4}}]^{{T}}}\text{。}$ (29b)

 图 1 仿真轨迹 Fig. 1 Simulation of trajectory

 图 2 姿态估计比较结果 Fig. 2 Comparison results of pose estimation

 图 3 速度估计比较结果 Fig. 3 Comparison results of velocity estimation

 图 4 位置估计比较结果 Fig. 4 Comparison results of position estimation

 $y = \sqrt {\frac{1}{n}\sum\limits_{i = 1}^n {x_i^2} } \text{。}$ (30)

4 结　语

 [1] 李文魁, 高敬东, 陈永冰, 等. 舰船综合导航系统[M]. 武汉: 海军工程大学, 2015. 11. [2] 李增科, 王坚, 高井祥. 精密单点定位在GPS/INS组合导航中的应用[J]. 武汉大学学报: 信息科学版, 2013(1): 48–51. [3] MARKLEY F L, CRASSIDIS J L. Fundamentals of spacecraft attitude determination and control[M]. Springer New York, 2014. [4] 张科, 刘海鹏, 李恒年, 等.SINS/GPS/CNS组合导航联邦滤波算法[J]. 中国惯性技术学报, 2013, 21(2): 226–230. [5] 周琪. 大飞机全球惯性导航算法研究[D]. 西安: 西北工业大学, 2013. [6] CHANG L, HU B, LI Y. Backtracking integration for fast attitude determination-based initial alignment[J]. IEEE Transactions on Instrumentation and Measurement, 2015, 64(3): 795–803. [7] CHANG L, LI J, CHEN S. Initial alignment by attitude estimation for strapdown inertial navigation systems[J]. IEEE Transactions on Instrumentation and Measurement, 2015, 64(3): 784–794. [8] TANG X, LIU Z, ZHANG J. Square-root quaternion cubature Kalman filtering for spacecraft attitude estimation[J]. Acta Astronautica, 2012, 76: 84–94. [9] O′KEEFE S A, SCHAUB H. Shadow set considerations for modified rodrigues parameter attitude filtering[J]. Journal of Guidance, Control, and Dynamics, 2014, 37(6): 2030–2035. [10] BITTANTI S, SAVARESI S M. On the parametrization and design of an extended Kalman filter frequency tracker[J]. IEEE Transactions on Automatic Control, 2000, 45(9): 1718–1724. [11] KIM K H, LEE J G, PARK C G, Adaptive two-stage extended Kalman filter for a fault-tolerant INS-GPS loosely coupled system [J]. IEEE Transactions on Aerospace and Electronic Systems, 2009, 45(1): 125–137. [12] 常路宾. Unscented Kalman滤波及其在捷联惯导中的应用研究[D]. 武汉: 海军工程大学, 2014.