[发明专利]基于李群描述的捷联惯性导航解算方法

摘要

本发明公开了基于李群描述的捷联惯性导航解算方法，采用李群描述代替传统四元数算法中的四元数描述进行捷联解算，将姿态矩阵和载体速度构建成SE(3)群，通过对惯性敏感器件采集到的数据积分用来对SE(3)群进行迭代更新。本发明直接对姿态矩阵进行计算，可以有效避免传统四元数捷联解算过程中由于四元数描述而产生的非唯一性问题和归一化计算过程，并且省略了传统四元数捷联解算过程中四元数和姿态矩阵之间相互转换的计算过程，避免了转换计算所带来的不可避免的误差，在确保解算精度的同时减少了计算量，在实际工程中具有良好的应用前景。

主权项

基于李群描述的捷联惯性导航解算方法，本方法的详细描述中坐标系定义如下：地球坐标系e系，选取地球中心为原点，X轴位于赤道平面内，从地心指向本初子午线，Z轴从地心指向地理北极，X轴、Y轴和Z轴构成右手坐标系，随地球自转而转动；地心惯性坐标系i系，原点选取地球中心，X轴位于赤道平面内，从地心指向春分点，Z轴从地心指向地理北极，X轴、Y轴和Z轴构成右手坐标系；导航坐标系n系，即导航基准的坐标系，导航相关运算都在下述的坐标系下进行，位于舰载机重心为原点，X轴指向东向E，Y轴指向北向N，Z轴指向天向U；载体坐标系b系，原点位于舰载机重心，X轴、Y轴、Z轴分别沿舰载机机体横轴指向右、沿纵轴指向前、沿立轴指向上；其特征在于：该方法通过下述流程实现，(1)捷联惯导系统进行预热准备，启动系统，获得初始对准得到的载体所在位置的经度λ、纬度L，姿态航向角H，俯仰角P，横滚角R，以及载体东向速度ve，北向速度vn，天向速度vu基本信息，采集惯性测量单元IMU中陀螺的输出角度信息和加速度计的输出信息fb；(2)对采集到的陀螺和加速度计的数据进行处理，应用李群方法解算姿态矩阵与机体在n系下的速度；将导航的姿态矩阵和机体在n系下的速度用一个4×4的正交变换矩阵来表示；该矩阵符合李群的特殊欧式群SE(3)的性质，构成了SE(3)群：其中，R∈SO(3)对应了特定的导航姿态矩阵，表示3×3的向量空间，上标T表示矩阵的转置，I表示三维单位矩阵，det(R)表示为矩阵R的行列式，T∈SE(3)对应了包括姿态速度的变换矩阵，表示4×4的向量空间，t表示平动矢量，表示3×1的向量空间；机体姿态与速度位置的求解问题转化为对变换矩阵的更新问题；根据李群的微分方程：T·=Tξ^---(3)]]>其中，ξ是一个六维向量，前三维为平移记做ρ，即速度信息，由三轴加速度计能够测量，后三维为旋转，记做φ，即角速度信息，由三轴陀螺仪可以测量，符号^是将六维向量转换成四维矩阵的运算，运算法则如下：其中φ×表示将三维向量转换成一个反对称矩阵的运算，运算规则如下：在实际解算中，需要将李群微分方程离散化之后，再进行迭代更新，离散化结果如下：Tk+1＝Tk exp(ξ^)  (6)其中Tk是k时刻的变换矩阵，exp(ξ^)是一个矩阵的指数，将矩阵ξ^分块，先计算exp(φ×)部分，对于任意矩阵A的指数写成一个泰勒展开：exp(A)=Σn=0∞1n!An---(7)]]>对于exp(φ×)部分，也按照这种方式进行展开：exp(φ×)=Σn=0∞1n!(φ×)n---(8)]]>由于φ是三维向量，定义该三维向量的模值和方向分别记做θ和a，即φ＝θa，a是一个长度为1的方向向量，对于a×，有以下两条性质：a×a×＝aaT‑I  (9)a×a×a×＝‑a×  (10)根据(9)和(10)式，将(8)式展开计算：exp(φ×)=exp(θa×)=Σn=0∞1n!(θa×)n=I+θa×+12!θ2a×a×+13!θ3a×a×a×+14!θ4(a×)4+...=aaT-a×a×+θa×+12!θ2a×a×-13!θ3a×-14!θ4a×a×+...=aaT+(θ-13!θ3-15!θ5-...)a×-(1-12!θ2+14!θ4-...)a×a×=a×a×+I+sinθa×-cosθa×a×=(1-cosθ)a×a×+I+sinθa×=cosθI+(1-cosθ)aaT+sinθa×]]>最后得到式(11)：exp(θa×)＝cosθI+(1‑cosθ)aaT+sinθa×  (11)式(11)与表示旋转的罗德里格斯公式相似，即exp(θa×)是一个旋转矩阵，该旋转矩阵代表k时刻的姿态矩阵Rk与k+1时刻的姿态矩阵Rk+1之间的转动关系，记做ΔR，即Rk+1＝Rkexp(φ×)＝RkΔR  (12)计算平动部分ρ的指数形式，平动部分对应的是k时刻至k+1时刻的速度变化量；由于速度信息由加速度计提供，得到的速度信息是在k+1时刻机体坐标系下的信息；在计算载体位置时，需要用到的是导航坐标系，即地理坐标系下的速度信息，故需在指数计算的过程中对速度信息左乘ΔR来调整；即平动部分ρ的指数形式为ΔRρ；综上得到exp(ξ^)的展开形式：exp(ξ^)=ΔRΔRρ0T1---(13)]]>则变换矩阵的迭代更新方程为：Rk+1ρk+10T1=Rkρk0T1ΔRΔRρ0T1=RkΔRRkΔRρ+ρk0T1.]]>。