[发明专利]基于坐标系传递的姿态估计方法、系统、终端、介质及应用

说明书

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

技术领域

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

背景技术

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

通过上述分析，现有技术存在的问题及缺陷为：现有姿态估计方法本以位置作为姿态误差区分导致含义不明确且坐标系不明确导致的执行过程不统一。

发明内容

针对现有技术存在的问题，本发明提供了一种基于坐标系传递的姿态估计方法、系统、终端、介质及应用，具体涉及一种基于坐标系传递的SO(3)-UKF姿态估计方法。

本发明是这样实现的，一种基于坐标系传递的SO(3)-UKF姿态估计方法，包括：

通过建立坐标系、定义姿态误差、进行sigma点采样、构造全局姿态所对应的sigma点、将姿态sigma点在姿态运动学方程中传递，构造传递后的姿态误差sigma点、进行UKF预测、更新得到姿态估计结果。

进一步，所述基于坐标系传递的SO(3)-UKF姿态估计方法包括以下步骤：

步骤一，建立用于姿态计算的参考坐标系r、载体坐标系以及用于滤波的参考坐标系

步骤二，进行姿态误差定义，进行sigma点采样并构造全局姿态所对应的sigma点；

步骤三，将姿态sigma点在姿态运动学方程中传递并构造传递后的姿态误差sigma点；

步骤四，进行常规的UKF预测及预测方差计算，并分别进行UKF量测更新以及全局姿态更新；进行姿态误差估计置零；

步骤五，进入下一时刻的滤波流程，重复步骤一至步骤四。

进一步，步骤一中，所述建立用于姿态计算的参考坐标系r、载体坐标系以及用于滤波的参考坐标系包括：

建立姿态计算所在的坐标系即参考坐标系r，所述参考坐标系r可为惯性坐标系、地球坐标系或当地地理坐标系；