基于坐标系传递的姿态估计方法、系统、终端、介质及应用技术方案

本发明专利技术属于惯性导航技术领域，公开了一种基于坐标系传递的姿态估计方法、系统、终端、介质及应用，建立用于姿态计算的参考坐标系r、载体坐标系以及用于滤波的参考坐标系进行姿态误差定义，进行sigma点采样并构造全局姿态所对应的sigma点；将姿态sigma点在姿态运动学方程中传递并构造传递后的姿态误差sigma点；进行UKF预测及预测方差计算，并分别进行UKF量测更新以及全局姿态更新；进行姿态误差估计置零；进入滤波流程。本发明专利技术能充分发挥UKF在非线性状态估计中的优势，实现对载体姿态信息的有效估计。从坐标系的角度明确姿态误差的含义，避免了传统以位置作为姿态误差区分而引起的含义不明确的问题。含义不明确的问题。含义不明确的问题。

【技术实现步骤摘要】
基于坐标系传递的姿态估计方法、系统、终端、介质及应用


[0001]本专利技术属于惯性导航
，尤其涉及一种基于坐标系传递的姿态估计方法、系统、终端、介质及应用。

技术介绍

[0002]目前，利用固连于载体的惯性器件及其它传感器信息估计载体的姿态信息在运载体导航、机器人控制、航天器交会对接等领域具有广泛的应用。UKF(Unscented Kalman Filter)因其适中的计算量、较高的估计精度，近年来正逐步取代传统的EKF(Extended Kalman Filter)而广泛应用于非线性估计领域。然而将UKF应用姿态估计问题中则需要专门处理姿态约束在UKF滤波加权平均运算中的保持问题。针对这一问题，常规的做法是利用冗余姿态表示如四元数、方向余弦矩阵等进行全局姿态更新，利用三维姿态表示如旋转矢量、罗德里格斯参数等进行滤波估计，这种方法一般称之为SO(3)
‑
UKF。在SO(3)
‑
UKF中，UKF预测和估计的状态量本质上是三维姿态误差矢量，因此该算法设计的核心在于姿态误差的定义及传递。在传统的SO(3)
‑
UKF算法设计中，多以“左”和“右”来区分不同的姿态误差定义。然而，从坐标系定义角度来讲，由于不同的姿态定义，“左”误差既可以表示载体系误差也可以表示参考系误差。因此，单纯的以位置来表示姿态误差并不严谨。同时，同从坐标系定义角度来讲，相同位置的误差也可能表示不同的含义。这是因为，根据坐标系定义，载体的姿态表示从一个坐标系到另一个坐标系的传递，因此其对应的误差共有四种形式。不同误差的定义影响着SO(3)
‑
UKF中sigma点的采样、传递、重构以及全局姿态的更新。
[0003]通过上述分析，现有技术存在的问题及缺陷为：现有姿态估计方法本以位置作为姿态误差区分导致含义不明确且坐标系不明确导致的执行过程不统一。

技术实现思路

[0004]针对现有技术存在的问题，本专利技术提供了一种基于坐标系传递的姿态估计方法、系统、终端、介质及应用，具体涉及一种基于坐标系传递的SO(3)
‑
UKF姿态估计方法。
[0005]本专利技术是这样实现的，一种基于坐标系传递的SO(3)
‑
UKF姿态估计方法，包括：
[0006]通过建立坐标系、定义姿态误差、进行sigma点采样、构造全局姿态所对应的sigma点、将姿态sigma点在姿态运动学方程中传递，构造传递后的姿态误差sigma点、进行UKF预测、更新得到姿态估计结果。
[0007]进一步，所述基于坐标系传递的SO(3)
‑
UKF姿态估计方法包括以下步骤：
[0008]步骤一，建立用于姿态计算的参考坐标系r、载体坐标系以及用于滤波的参考坐标系
[0009]步骤二，进行姿态误差定义，进行sigma点采样并构造全局姿态所对应的sigma点；
[0010]步骤三，将姿态sigma点在姿态运动学方程中传递并构造传递后的姿态误差sigma点；
[0011]步骤四，进行常规的UKF预测及预测方差计算，并分别进行UKF量测更新以及全局
姿态更新；进行姿态误差估计置零；
[0012]步骤五，进入下一时刻的滤波流程，重复步骤一至步骤四。
[0013]进一步，步骤一中，所述建立用于姿态计算的参考坐标系r、载体坐标系以及用于滤波的参考坐标系包括：
[0014]建立姿态计算所在的坐标系即参考坐标系r，所述参考坐标系r可为惯性坐标系、地球坐标系或当地地理坐标系；
[0015]建立载体坐标系b，所述载体坐标系b为惯性传感器即陀螺仪和加速度计所在的坐标系；
[0016]建立估计参考坐标系所述估计参考坐标系为SO(3)
‑
UKF滤波算法中所估计的参考坐标系，所述估计参考坐标系与所述参考坐标系r之间的误差即为姿态误差。
[0017]进一步，步骤二中，所述姿态误差定义如下：
[0018][0019]其中，表示真实的误差矩阵，表示对应的估计值。
[0020]进一步，步骤二中，所述进行sigma点采样并构造全局姿态所对应的sigma点包括：
[0021]利用上一时刻姿态方差P
k
‑1进行sigma点采样如下：
[0022][0023]所述构造全局姿态所对应的sigma点包括：
[0024]将姿态误差sigma点转换为矩阵形式：
[0025][0026]其中，so3_exp[
·
]所定义的转换公式为：
[0027][0028]利用上一时刻的姿态估计值构造全局姿态所对应的sigma点
[0029][0030]进一步，步骤三中，所述将姿态sigma点在姿态运动学方程中传递包括：
[0031][0032]其中，Ω(
·
)表示姿态运动学方程。
[0033]进一步，步骤三中，所述构造传递后的姿态误差sigma点；
[0034]根据定义的姿态误差，构造传递后的姿态误差sigma点：
[0035][0036]将矩阵形式的姿态误差转换为矢量形式的误差：
[0037][0038]其中，so3_log[
·
]所定义的转换公式为：
[0039][0040]进一步，步骤四中，所述进行常规的UKF预测及预测方差计算如下：
[0041][0042][0043]其中，w(i)表示sigma点所对应的权值，Q
k
‑1表示状态方差阵。
[0044]进一步，步骤四中，所述分别进行UKF量测更新以及全局姿态更新包括：所述进行UKF量测更新包括：
[0045]通过UKF量测更新得到矢量姿态误差估计及其对应的方差，即
[0046][0047]其中，h(
·
)表示量测方程，y
k
表示k时刻的量测信息，R
k
表示量测噪声方差；所述进行全局姿态更新包括：
[0048]将矢量姿态误差估计转换为矩阵形式
[0049][0050]根据姿态误差定义，得到当前时刻的全局姿态更新:
[0051][0052]进一步，所述姿态误差估计置零如下：
[0053][0054]进一步，所述基于坐标系传递的SO(3)
‑
UKF姿态估计方法还包括：
[0055]当姿态误差定义为时，姿态sigma点构造为传递后的姿态误差sigma点为姿态更新为
[0056]当姿态误差定义为时，姿态sigma点构造为传递后的姿态误差sigma点为姿态更新为
[0057]当姿态误差定义为时，姿态sigma点构造为传递后的姿态误差sigma点为姿态更新为
[0058]进一步，所述基于坐标系传递的SO(3)
‑
UKF姿态估计方法还包括：
[0059]所述全局姿态可用四元数进行表示；所述姿态误差可用其它三维表本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于坐标系传递的SO(3)
‑
UKF姿态估计方法，其特征在于，所述基于坐标系传递的SO(3)
‑
UKF姿态估计方法包括：通过建立坐标系、定义姿态误差、进行sigma点采样、构造全局姿态所对应的sigma点、将姿态sigma点在姿态运动学方程中传递；构造传递后的姿态误差sigma点、进行UKF预测、更新得到姿态估计结果。2.如权利要求1所述基于坐标系传递的SO(3)
‑
UKF姿态估计方法，其特征在于，所述基于坐标系传递的SO(3)
‑
UKF姿态估计方法具体包括以下步骤：步骤一，建立用于姿态计算的参考坐标系r、载体坐标系以及用于滤波的参考坐标系步骤二，进行姿态误差定义，进行sigma点采样并构造全局姿态所对应的sigma点；步骤三，将姿态sigma点在姿态运动学方程中传递并构造传递后的姿态误差sigma点；步骤四，进行常规的UKF预测及预测方差计算，并分别进行UKF量测更新以及全局姿态更新；进行姿态误差估计置零；步骤五，进入下一时刻的滤波流程，重复步骤一至步骤四。3.如权利要求2所述基于坐标系传递的SO(3)
‑
UKF姿态估计方法，其特征在于，步骤一中，所述建立用于姿态计算的参考坐标系r、载体坐标系以及用于滤波的参考坐标系包括：建立姿态计算所在的坐标系即参考坐标系r，所述参考坐标系r可为惯性坐标系、地球坐标系或当地地理坐标系；建立载体坐标系b，所述载体坐标系b为惯性传感器即陀螺仪和加速度计所在的坐标系；建立估计参考坐标系所述估计参考坐标系为SO(3)
‑
UKF滤波算法中所估计的参考坐标系，所述估计参考坐标系与所述参考坐标系r之间的误差即为姿态误差。4.如权利要求2所述基于坐标系传递的SO(3)
‑
UKF姿态估计方法，其特征在于，步骤二中，所述姿态误差定义如下：其中，表示真实的误差矩阵，表示对应的估计值；所述进行sigma点采样并构造全局姿态所对应的sigma点包括：利用上一时刻姿态方差P
k
‑1进行sigma点采样如下：所述构造全局姿态所对应的sigma点包括：将姿态误差sigma点转换为矩阵形式：其中，so3_exp[
·
]所定义的转换公式为：利用上一时刻的姿态估计值构造全局姿态所对应的sigma点
5.如权利要求2所述基于坐标系传递的SO(3)
‑
UKF姿态估计方法，其特征在于，步骤三中，所述将姿态sigma点在姿态运动学方程中传递包括：其中，Ω(
·
)表示姿态运动学方程；步骤三中，所述构造传递后的姿态误差sigma点；根据定义的姿态误差，构造传递后的姿态误差sigma点：将矩阵形式的姿态误差转换...

【专利技术属性】
技术研发人员：常路宾，高端阳，卞强，李开龙，覃方君，查峰，胡柏青，田佳玉，吕旭，赵仁杰，
申请(专利权)人：中国人民解放军海军工程大学，
类型：发明
国别省市：