CN113340297B - 基于坐标系传递的姿态估计方法、系统、终端、介质及应用 - Google Patents

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

Info

Publication number
CN113340297B
CN113340297B CN202110438121.4A CN202110438121A CN113340297B CN 113340297 B CN113340297 B CN 113340297B CN 202110438121 A CN202110438121 A CN 202110438121A CN 113340297 B CN113340297 B CN 113340297B
Authority
CN
China
Prior art keywords
attitude
coordinate system
ukf
error
sigma
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110438121.4A
Other languages
English (en)
Other versions
CN113340297A (zh
Inventor
常路宾
高端阳
卞强
李开龙
覃方君
查峰
胡柏青
田佳玉
吕旭
赵仁杰
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Naval University of Engineering PLA
Original Assignee
Naval University of Engineering PLA
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Naval University of Engineering PLA filed Critical Naval University of Engineering PLA
Priority to CN202110438121.4A priority Critical patent/CN113340297B/zh
Publication of CN113340297A publication Critical patent/CN113340297A/zh
Application granted granted Critical
Publication of CN113340297B publication Critical patent/CN113340297B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Navigation (AREA)

Abstract

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

Description

基于坐标系传递的姿态估计方法、系统、终端、介质及应用
技术领域
本发明属于惯性导航技术领域，尤其涉及一种基于坐标系传递的姿态估计方法、系统、终端、介质及应用。
背景技术
目前，利用固连于载体的惯性器件及其它传感器信息估计载体的姿态信息在运载体导航、机器人控制、航天器交会对接等领域具有广泛的应用。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、载体坐标系以及用于滤波的参考坐标系
Figure BDA0003033864520000021
步骤二，进行姿态误差定义，进行sigma点采样并构造全局姿态所对应的sigma点；
步骤三，将姿态sigma点在姿态运动学方程中传递并构造传递后的姿态误差sigma点；
步骤四，进行常规的UKF预测及预测方差计算，并分别进行UKF量测更新以及全局姿态更新；进行姿态误差估计置零；
步骤五，进入下一时刻的滤波流程，重复步骤一至步骤四。
进一步，步骤一中，所述建立用于姿态计算的参考坐标系r、载体坐标系以及用于滤波的参考坐标系
Figure BDA0003033864520000022
包括：
建立姿态计算所在的坐标系即参考坐标系r，所述参考坐标系r可为惯性坐标系、地球坐标系或当地地理坐标系；
建立载体坐标系b，所述载体坐标系b为惯性传感器即陀螺仪和加速度计所在的坐标系；
建立估计参考坐标系
Figure BDA0003033864520000031
所述估计参考坐标系
Figure BDA0003033864520000032
为SO(3)-UKF滤波算法中所估计的参考坐标系，所述估计参考坐标系
Figure BDA0003033864520000033
与所述参考坐标系r之间的误差即为姿态误差。
进一步，步骤二中，所述姿态误差定义如下：
Figure BDA0003033864520000034
其中，
Figure BDA0003033864520000035
表示真实的误差矩阵，
Figure BDA0003033864520000036
表示
Figure BDA0003033864520000037
对应的估计值。
进一步，步骤二中，所述进行sigma点采样并构造全局姿态所对应的sigma点包括：
利用上一时刻姿态方差Pk-1进行sigma点采样如下：
Figure BDA0003033864520000038
所述构造全局姿态所对应的sigma点包括：
将姿态误差sigma点转换为矩阵形式：
Figure BDA0003033864520000039
其中，so3_exp[·]所定义的转换公式为：
Figure BDA00030338645200000310
利用上一时刻的姿态估计值
Figure BDA00030338645200000311
构造全局姿态所对应的sigma点
Figure BDA00030338645200000312
进一步，步骤三中，所述将姿态sigma点在姿态运动学方程中传递包括：
Figure BDA00030338645200000313
其中，Ω(·)表示姿态运动学方程。
进一步，步骤三中，所述构造传递后的姿态误差sigma点；
根据定义的姿态误差，构造传递后的姿态误差sigma点：
Figure BDA0003033864520000041
将矩阵形式的姿态误差转换为矢量形式的误差：
Figure BDA0003033864520000042
其中，so3_log[·]所定义的转换公式为：
Figure BDA0003033864520000043
进一步，步骤四中，所述进行常规的UKF预测及预测方差计算如下：
Figure BDA0003033864520000044
Figure BDA0003033864520000045
其中，w(i)表示sigma点所对应的权值，Qk-1表示状态方差阵。
进一步，步骤四中，所述分别进行UKF量测更新以及全局姿态更新包括：所述进行UKF量测更新包括：
通过UKF量测更新得到矢量姿态误差估计及其对应的方差，即
Figure BDA0003033864520000046
其中，h(·)表示量测方程，yk表示k时刻的量测信息，Rk表示量测噪声方差；所述进行全局姿态更新包括：
将矢量姿态误差估计
Figure BDA0003033864520000047
转换为矩阵形式
Figure BDA0003033864520000048
根据姿态误差定义，得到当前时刻的全局姿态更新:
Figure BDA0003033864520000049
进一步，所述姿态误差估计置零如下：
Figure BDA00030338645200000410
进一步，所述基于坐标系传递的SO(3)-UKF姿态估计方法还包括：
当姿态误差定义为
Figure BDA0003033864520000051
时，姿态sigma点构造为
Figure BDA0003033864520000052
传递后的姿态误差sigma点为
Figure BDA0003033864520000053
姿态更新为
Figure BDA0003033864520000054
当姿态误差定义为
Figure BDA0003033864520000055
时，姿态sigma点构造为
Figure BDA0003033864520000056
传递后的姿态误差sigma点为
Figure BDA0003033864520000057
姿态更新为
Figure BDA0003033864520000058
当姿态误差定义为
Figure BDA0003033864520000059
时，姿态sigma点构造为
Figure BDA00030338645200000510
传递后的姿态误差sigma点为
Figure BDA00030338645200000511
姿态更新为
Figure BDA00030338645200000512
进一步，所述基于坐标系传递的SO(3)-UKF姿态估计方法还包括：
所述全局姿态可用四元数进行表示；所述姿态误差可用其它三维表示方法进行表示。
本发明另一目的在于提供一种基于坐标系传递的SO(3)-UKF姿态估计系统，包括：
坐标系构建模块，用于建立用于姿态计算的参考坐标系r、载体坐标系以及用于滤波的参考坐标系
Figure BDA00030338645200000513
全局姿态对应的sigma点构建模块，用于进行姿态误差定义，进行sigma点采样并构造全局姿态所对应的sigma点；
传递后的姿态误差sigma点构建模块，用于将姿态sigma点在姿态运动学方程中传递并构造传递后的姿态误差sigma点；
更新模块，用于进行常规的UKF预测及预测方差计算，并分别进行UKF量测更新以及全局姿态更新；进行姿态误差估计置零；
滤波模块，用于进入下一时刻的滤波流程。
本发明另一目的在于提供一种接收用户输入程序存储介质，所存储的计算机程序使电子设备执行所述基于坐标系传递的SO(3)-UKF姿态估计方法。
本发明另一目的在于提供一种所述基于坐标系传递的SO(3)-UKF姿态估计方法在运载体导航、机器人控制、航天器交会对接领域上的应用。
结合上述的所有技术方案，本发明所具备的优点及积极效果为：
本发明从坐标系角度定义特定的姿态误差，并从坐标系传递的角度对SO(3)-UKF算法流程中的sigma点采样、传递、重构以及全局姿态更新进行统一的坐标系约束和规范。根据特定的姿态误差定义，本发明给出了详细的SO(3)-UKF算法设计流程。利用本发明，从坐标系传递的角度统一sigma点采样、传递、重构以及姿态更新所涉及的姿态信息计算，从而结合实际应用问题中具体的姿态含义，高效的设计出相应的SO(3)-UKF算法，进而充分发挥UKF在非线性状态估计中的优势，实现对载体姿态信息的有效估计。
本发明从坐标系的角度明确姿态误差的含义，避免了传统以位置作为姿态误差区分而引起的含义不明确的问题；
本发明以坐标系传递约束SO(3)-UKF中sigma点采样、传递、重构以及全局姿态更新，从而避免了传统方法中因坐标系不明确而导致的上述执行过程不统一的问题；
本发明提供了四种不同的姿态定义方式，并给出了SO(3)-UKF相应的设计流程，从而可以针对实际应用问题、结合具体应用需求高效的设计出相应的滤波算法。
附图说明
图1是本发明实施例提供的基于坐标系传递的SO(3)-UKF姿态估计方法原理图。
图2是本发明实施例提供的基于坐标系传递的SO(3)-UKF姿态估计方法流程图。
图3是本发明实施例提供的仿真实验轨迹图。
图4是本发明实施例提供的位置误差曲线图。
图5是本发明实施例提供的方位误差曲线图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
针对现有技术存在的问题，本发明提供了一种基于坐标系传递的SO(3)-UKF姿态估计方法，下面结合附图对本发明作详细的描述。
如图1所示，本发明实施例提供的基于坐标系传递的SO(3)-UKF姿态估计方法包括：
通过建立坐标系、定义姿态误差、进行sigma点采样、构造全局姿态所对应的sigma点、将姿态sigma点在姿态运动学方程中传递，构造传递后的姿态误差sigma点、进行UKF预测、更新得到姿态估计结果。
如图2所示，本发明实施例提供的基于坐标系传递的SO(3)-UKF姿态估计方法包括以下步骤：
S101，建立用于姿态计算的参考坐标系r、载体坐标系以及用于滤波的参考坐标系
Figure BDA0003033864520000071
S102，进行姿态误差定义，进行sigma点采样并构造全局姿态所对应的sigma点；
S103，将姿态sigma点在姿态运动学方程中传递并构造传递后的姿态误差sigma点；
S104，进行常规的UKF预测及预测方差计算，并分别进行UKF量测更新以及全局姿态更新；进行姿态误差估计置零；
S105，进入下一时刻的滤波流程，重复步骤S101至步骤S104。
本发明实施例提供的建立用于姿态计算的参考坐标系r、载体坐标系以及用于滤波的参考坐标系
Figure BDA0003033864520000081
包括：
建立姿态计算所在的坐标系即参考坐标系r，所述参考坐标系r可为惯性坐标系、地球坐标系或当地地理坐标系；
建立载体坐标系b，所述载体坐标系b为惯性传感器即陀螺仪和加速度计所在的坐标系；
建立估计参考坐标系
Figure BDA0003033864520000082
所述估计参考坐标系
Figure BDA0003033864520000083
为SO(3)-UKF滤波算法中所估计的参考坐标系，所述估计参考坐标系
Figure BDA0003033864520000084
与所述参考坐标系r之间的误差即为姿态误差。
本发明实施例提供的姿态误差定义如下：
Figure BDA0003033864520000085
其中，
Figure BDA0003033864520000086
表示真实的误差矩阵，
Figure BDA0003033864520000087
表示
Figure BDA0003033864520000088
对应的估计值。
本发明实施例提供的进行sigma点采样并构造全局姿态所对应的sigma点包括：
利用上一时刻姿态方差Pk-1进行sigma点采样如下：
Figure BDA0003033864520000089
所述构造全局姿态所对应的sigma点包括：
将姿态误差sigma点转换为矩阵形式：
Figure BDA00030338645200000810
其中，so3_exp[·]所定义的转换公式为：
Figure BDA00030338645200000811
利用上一时刻的姿态估计值
Figure BDA00030338645200000812
构造全局姿态所对应的sigma点
Figure BDA00030338645200000813
本发明实施例提供的将姿态sigma点在姿态运动学方程中传递包括：
Figure BDA0003033864520000091
其中，Ω(·)表示姿态运动学方程。
本发明实施例提供的构造传递后的姿态误差sigma点；
根据定义的姿态误差，构造传递后的姿态误差sigma点：
Figure BDA0003033864520000092
将矩阵形式的姿态误差转换为矢量形式的误差：
Figure BDA0003033864520000093
其中，so3_log[·]所定义的转换公式为：
Figure BDA0003033864520000094
本发明实施例提供的进行常规的UKF预测及预测方差计算如下：
Figure BDA0003033864520000095
Figure BDA0003033864520000096
其中，w(i)表示sigma点所对应的权值，Qk-1表示状态方差阵。
本发明实施例提供的分别进行UKF量测更新以及全局姿态更新包括：
所述进行UKF量测更新包括：
通过UKF量测更新得到矢量姿态误差估计及其对应的方差，即
Figure BDA0003033864520000097
其中，h(·)表示量测方程，yk表示k时刻的量测信息，Rk表示量测噪声方差；
所述进行全局姿态更新包括：
将矢量姿态误差估计
Figure BDA0003033864520000098
转换为矩阵形式
Figure BDA0003033864520000099
根据姿态误差定义，得到当前时刻的全局姿态更新:
Figure BDA0003033864520000101
本发明实施例提供的姿态误差估计置零如下：
Figure BDA0003033864520000102
本发明实施例提供的基于坐标系传递的SO(3)-UKF姿态估计方法还包括：
当姿态误差定义为
Figure BDA0003033864520000103
时，姿态sigma点构造为
Figure BDA0003033864520000104
传递后的姿态误差sigma点为
Figure BDA0003033864520000105
姿态更新为
Figure BDA0003033864520000106
当姿态误差定义为
Figure BDA0003033864520000107
时，姿态sigma点构造为
Figure BDA0003033864520000108
传递后的姿态误差sigma点为
Figure BDA0003033864520000109
姿态更新为
Figure BDA00030338645200001010
当姿态误差定义为
Figure BDA00030338645200001011
时，姿态sigma点构造为
Figure BDA00030338645200001012
传递后的姿态误差sigma点为
Figure BDA00030338645200001013
姿态更新为
Figure BDA00030338645200001014
本发明实施例提供的基于坐标系传递的SO(3)-UKF姿态估计方法还包括：
所述全局姿态可用四元数进行表示；所述姿态误差可用其它三维表示方法进行表示。
本发明还提供一种基于坐标系传递的SO(3)-UKF姿态估计系统，包括：
坐标系构建模块，用于建立用于姿态计算的参考坐标系r、载体坐标系以及用于滤波的参考坐标系
Figure BDA00030338645200001015
全局姿态对应的sigma点构建模块，用于进行姿态误差定义，进行sigma点采样并构造全局姿态所对应的sigma点；
传递后的姿态误差sigma点构建模块，用于将姿态sigma点在姿态运动学方程中传递并构造传递后的姿态误差sigma点；
更新模块，用于进行常规的UKF预测及预测方差计算，并分别进行UKF量测更新以及全局姿态更新；进行姿态误差估计置零；
滤波模块，用于进入下一时刻的滤波流程。
下面结合具体实施例对本发明的技术方案作进一步说明。
实施例：
本发明提供了四种不同的姿态误差定义，并给出了基于相应姿态误差定义所需要的sigma点采用、传递、更新以及全局姿态更新所涉及的矩阵传递方程，具体如表1所示。
表1.不同姿态误差定义及其SO(3)-UKF滤波算法设计中的应用
Figure BDA0003033864520000111
以参考系姿态误差定义(即表1中方案1)为例，本发明提供的一种基于坐标系传递的SO(3)-UKF姿态估计设计方法具体步骤如下：
步骤一、建立坐标系；
定义参考坐标系为r，该坐标系为姿态解算所在的坐标系，根据实际应用可以为惯性坐标系、地球坐标系或当地地理坐标系等；
定义载体坐标系为b，该坐标系为惯性传感器即陀螺仪和加速度计所在的坐标系(忽略安装误差)；
定义估计参考坐标系为
Figure BDA0003033864520000112
该坐标系为SO(3)-UKF滤波算法中所估计的参考坐标系，该坐标系与参考坐标系r之间的误差即为姿态误差。
步骤二、定义姿态误差；
记真实的误差矩阵为
Figure BDA0003033864520000121
其对应的估计值为
Figure BDA0003033864520000122
根据姿态矩阵链式相乘法则，姿态误差定义为
Figure BDA0003033864520000123
步骤三、进行sigma点采样；
利用上一时刻姿态方差Pk-1进行sigma点采样
Figure BDA0003033864520000124
步骤四、构造全局姿态所对应的sigma点；
首先将(2)中姿态误差sigma点转换为矩阵形式
Figure BDA0003033864520000125
其中so3_exp[·]所定义的转换公式为
Figure BDA0003033864520000126
利用上一时刻的姿态估计值
Figure BDA0003033864520000127
构造全局姿态所对应的sigma点
Figure BDA0003033864520000128
步骤五、将姿态sigma点在姿态运动学方程中传递；
Figure BDA0003033864520000129
其中Ω(·)表示姿态运动学方程，为了简化起见，忽略了其中涉及的其它参数输入。
步骤六、构造传递后的姿态误差sigma点；
根据步骤一中所定义的姿态误差，构造传递后的姿态误差sigma点
Figure BDA00030338645200001210
将矩阵形式的姿态误差转换为矢量形式的误差
Figure BDA00030338645200001211
其中so3_log[·]所定义的转换公式为
Figure BDA0003033864520000131
步骤七、进行常规的UKF预测及预测方差计算；
Figure BDA0003033864520000132
Figure BDA0003033864520000133
其中w(i)表示sigma点所对应的权值，Qk-1为状态方差阵。
步骤八、进行UKF量测更新；
通过UKF量测更新得到矢量姿态误差估计及其对应的方差，即
Figure BDA0003033864520000134
其中h(·)表示量测方程，yk表示k时刻的量测信息，Rk表示量测噪声方差。同样，为了简化起见，此处忽略了量测方程h(·)中的参数输入。
步骤九、全局姿态更新；
将矢量姿态误差估计
Figure BDA0003033864520000135
转换为矩阵形式
Figure BDA0003033864520000136
根据步骤一中的姿态误差定义，得到当前时刻的全局姿态更新
Figure BDA0003033864520000137
步骤十、姿态误差估计置零。
将姿态误差估计
Figure BDA0003033864520000138
置零，即
Figure BDA0003033864520000139
进而进入下一时刻的滤波流程，重复上述步骤。
下面将本发明提供的方法应用至具体实施例中：
以地球平面导航为例，载体航行轨迹如图2所示，依据具体的运动过程仿真产生相应的加速度和角速度信息并叠加相应的漂移和噪声误差作为惯性传感器输出，用于惯导解算。以仿真轨迹附近的三个已知路标点作为观测信息。该仿真实例对应的状态方程和观测方程分别为
Figure BDA0003033864520000141
Figure BDA0003033864520000142
其中Ct，vt和pt分别表示载体的姿态矩阵、速度和位置。ωt表示由陀螺仪敏感的角速度信息，
Figure BDA0003033864520000143
是其对应的测量噪声。ut表示加速度计敏感的比力信息，
Figure BDA0003033864520000144
是其对应的测量噪声。g是重力矢量。(l1,l2,l3)是三个已知路标点。
分别利用表1中方案1和方案3中的姿态误差定义参考本发明设计相应的SO(3)-UKF算法，具体的轨迹跟踪结果如图3所示，位置和方位估计误差如图4和图5所示。从图中可以看出，本发明提供的方法可对运动载体的位置和姿态信息进行有效的跟踪，且精度由于传统的EKF算法。
在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上；术语“上”、“下”、“左”、“右”、“内”、“外”、“前端”、“后端”、“头部”、“尾部”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”、“第三”等仅用于描述目的，而不能理解为指示或暗示相对重要性。
应当注意，本发明的实施方式可以通过硬件、软件或者软件和硬件的结合来实现。硬件部分可以利用专用逻辑来实现；软件部分可以存储在存储器中，由适当的指令执行系统，例如微处理器或者专用设计硬件来执行。本领域的普通技术人员可以理解上述的设备和方法可以使用计算机可执行指令和/或包含在处理器控制代码中来实现，例如在诸如磁盘、CD或DVD-ROM的载体介质、诸如只读存储器(固件)的可编程的存储器或者诸如光学或电子信号载体的数据载体上提供了这样的代码。本发明的设备及其模块可以由诸如超大规模集成电路或门阵列、诸如逻辑芯片、晶体管等的半导体、或者诸如现场可编程门阵列、可编程逻辑设备等的可编程硬件设备的硬件电路实现，也可以用由各种类型的处理器执行的软件实现，也可以由上述硬件电路和软件的结合例如固件来实现。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，都应涵盖在本发明的保护范围之内。

Claims (9)

1.一种基于坐标系传递的SO(3)-UKF姿态估计方法，其特征在于，所述基于坐标系传递的SO(3)-UKF姿态估计方法包括：
通过建立坐标系、定义姿态误差、进行sigma点采样、构造全局姿态所对应的sigma点、将姿态sigma点在姿态运动学方程中传递；
构造传递后的姿态误差sigma点、进行UKF预测、更新得到姿态估计结果；
所述基于坐标系传递的SO(3)-UKF姿态估计方法具体包括以下步骤：
步骤一，建立用于姿态计算的参考坐标系r、载体坐标系以及用于滤波的参考坐标系
Figure FDA0003606464760000011
步骤二，进行姿态误差定义，进行sigma点采样并构造全局姿态所对应的sigma点；
步骤三，将姿态sigma点在姿态运动学方程中传递并构造传递后的姿态误差sigma点；
步骤四，进行常规的UKF预测及预测方差计算，并分别进行UKF量测更新以及全局姿态更新；进行姿态误差估计置零；
步骤五，进入下一时刻的滤波流程，重复步骤一至步骤四。
2.如权利要求1所述基于坐标系传递的SO(3)-UKF姿态估计方法，其特征在于，步骤一中，所述建立用于姿态计算的参考坐标系r、载体坐标系以及用于滤波的参考坐标系
Figure FDA0003606464760000012
包括：
建立姿态计算所在的坐标系即参考坐标系r，所述参考坐标系r可为惯性坐标系、地球坐标系或当地地理坐标系；
建立载体坐标系b，所述载体坐标系b为惯性传感器即陀螺仪和加速度计所在的坐标系；
建立估计参考坐标系
Figure FDA0003606464760000013
所述估计参考坐标系
Figure FDA0003606464760000014
为SO(3)-UKF滤波算法中所估计的参考坐标系，所述估计参考坐标系
Figure FDA0003606464760000015
与所述参考坐标系r之间的误差即为姿态误差。
3.如权利要求2所述基于坐标系传递的SO(3)-UKF姿态估计方法，其特征在于，步骤二中，所述姿态误差定义如下：
Figure FDA0003606464760000021
其中，
Figure FDA0003606464760000022
表示真实的误差矩阵，
Figure FDA0003606464760000023
表示
Figure FDA0003606464760000024
对应的估计值；
所述进行sigma点采样并构造全局姿态所对应的sigma点包括：
利用上一时刻姿态方差Pk-1进行sigma点采样如下：
Figure FDA0003606464760000025
所述构造全局姿态所对应的sigma点包括：
将姿态误差sigma点转换为矩阵形式：
Figure FDA0003606464760000026
其中，so3_exp[·]所定义的转换公式为：
Figure FDA0003606464760000027
利用上一时刻的姿态估计值
Figure FDA0003606464760000028
构造全局姿态所对应的sigma点
Figure FDA0003606464760000029
4.如权利要求3所述基于坐标系传递的SO(3)-UKF姿态估计方法，其特征在于，步骤三中，所述将姿态sigma点在姿态运动学方程中传递包括：
Figure FDA00036064647600000210
其中，Ω(·)表示姿态运动学方程；
步骤三中，所述构造传递后的姿态误差sigma点；
根据定义的姿态误差，构造传递后的姿态误差sigma点：
Figure FDA00036064647600000211
将矩阵形式的姿态误差转换为矢量形式的误差：
Figure FDA00036064647600000212
其中，so3_log[·]所定义的转换公式为：
Figure FDA0003606464760000031
5.如权利要求4所述基于坐标系传递的SO(3)-UKF姿态估计方法，其特征在于，步骤四中，所述进行常规的UKF预测及预测方差计算如下：
Figure FDA0003606464760000032
Figure FDA0003606464760000033
其中，w(i)表示sigma点所对应的权值，Qk-1表示状态方差阵；
步骤四中，所述分别进行UKF量测更新以及全局姿态更新包括：
所述进行UKF量测更新包括：
通过UKF量测更新得到矢量姿态误差估计及其对应的方差，即
Figure FDA0003606464760000034
其中，h(·)表示量测方程，yk表示k时刻的量测信息，Rk表示量测噪声方差；
所述进行全局姿态更新包括：
将矢量姿态误差估计
Figure FDA0003606464760000035
转换为矩阵形式
Figure FDA0003606464760000036
根据姿态误差定义，得到当前时刻的全局姿态更新:
Figure FDA0003606464760000037
所述姿态误差估计置零如下：
Figure FDA0003606464760000038
6.如权利要求5所述基于坐标系传递的SO(3)-UKF姿态估计方法，其特征在于，所述基于坐标系传递的SO(3)-UKF姿态估计方法还包括：
当姿态误差定义为
Figure FDA0003606464760000039
时，姿态sigma点构造为
Figure FDA00036064647600000310
传递后的姿态误差sigma点为
Figure FDA00036064647600000311
姿态更新为
Figure FDA0003606464760000041
当姿态误差定义为
Figure FDA0003606464760000042
时，姿态sigma点构造为
Figure FDA0003606464760000043
传递后的姿态误差sigma点为
Figure FDA0003606464760000044
姿态更新为
Figure FDA0003606464760000045
当姿态误差定义为
Figure FDA0003606464760000046
时，姿态sigma点构造为
Figure FDA0003606464760000047
传递后的姿态误差sigma点为
Figure FDA0003606464760000048
姿态更新为
Figure FDA0003606464760000049
所述基于坐标系传递的SO(3)-UKF姿态估计方法还包括：
所述全局姿态采用四元数进行表示；所述姿态误差采用三维表示方法进行表示。
7.一种基于坐标系传递的SO(3)-UKF姿态估计系统，其特征在于，所述基于坐标系传递的SO(3)-UKF姿态估计系统包括：
坐标系构建模块，用于建立用于姿态计算的参考坐标系r、载体坐标系以及用于滤波的参考坐标系
Figure FDA00036064647600000410
全局姿态对应的sigma点构建模块，用于进行姿态误差定义，进行sigma点采样并构造全局姿态所对应的sigma点；
传递后的姿态误差sigma点构建模块，用于将姿态sigma点在姿态运动学方程中传递并构造传递后的姿态误差sigma点；
更新模块，用于进行常规的UKF预测及预测方差计算，并分别进行UKF量测更新以及全局姿态更新；进行姿态误差估计置零；
滤波模块，用于进入下一时刻的滤波流程。
8.一种接收用户输入程序存储介质，所存储的计算机程序使电子设备执行权利要求1～6任意一项所述基于坐标系传递的SO(3)-UKF姿态估计方法。
9.一种如权利要求1～6任意一项所述基于坐标系传递的SO(3)-UKF姿态估计方法在运载体导航、机器人控制、航天器交会对接领域上的应用。
CN202110438121.4A 2021-04-22 2021-04-22 基于坐标系传递的姿态估计方法、系统、终端、介质及应用 Active CN113340297B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110438121.4A CN113340297B (zh) 2021-04-22 2021-04-22 基于坐标系传递的姿态估计方法、系统、终端、介质及应用

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110438121.4A CN113340297B (zh) 2021-04-22 2021-04-22 基于坐标系传递的姿态估计方法、系统、终端、介质及应用

Publications (2)

Publication Number Publication Date
CN113340297A CN113340297A (zh) 2021-09-03
CN113340297B true CN113340297B (zh) 2022-08-09

Family

ID=77468403

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110438121.4A Active CN113340297B (zh) 2021-04-22 2021-04-22 基于坐标系传递的姿态估计方法、系统、终端、介质及应用

Country Status (1)

Country Link
CN (1) CN113340297B (zh)

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101726295B (zh) * 2008-10-24 2011-09-07 中国科学院自动化研究所 考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法
FR2961897B1 (fr) * 2010-06-25 2012-07-13 Thales Sa Filtre de navigation pour un systeme de navigation par correlation de terrain
CN103940433B (zh) * 2014-05-12 2016-09-07 哈尔滨工业大学 一种基于改进的自适应平方根ukf算法的卫星姿态确定方法
CN104567871B (zh) * 2015-01-12 2018-07-24 哈尔滨工程大学 一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法
CN108731702B (zh) * 2018-07-03 2021-12-24 哈尔滨工业大学 一种基于Huber方法的大失准角传递对准方法

Also Published As

Publication number Publication date
CN113340297A (zh) 2021-09-03

Similar Documents

Publication Publication Date Title
JP7299261B2 (ja) 車両のデッドレコニング方法、装置、デバイス、記憶媒体、及びプログラム
Kong INS algorithm using quaternion model for low cost IMU
JP6094026B2 (ja) 姿勢判定方法、位置算出方法及び姿勢判定装置
CN112798021B (zh) 基于激光多普勒测速仪的惯导系统行进间初始对准方法
US20230366680A1 (en) Initialization method, device, medium and electronic equipment of integrated navigation system
CN114002725A (zh) 一种车道线辅助定位方法、装置、电子设备及存储介质
CN111380516A (zh) 基于里程计测量信息的惯导/里程计车辆组合导航方法及系统
CN114179825A (zh) 多传感器融合获取量测值置信度方法及自动驾驶车辆
Tang et al. Exploring the accuracy potential of IMU preintegration in factor graph optimization
CN113483765B (zh) 一种卫星自主姿态确定方法
CN115033844A (zh) 一种无人机状态估计方法、系统、设备及可读存储介质
CN112781617B (zh) 误差估计方法、组合导航处理系统及存储介质
CN113008229B (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN110940336B (zh) 捷联惯导仿真定位解算方法、装置及终端设备
KR20200074660A (ko) 위성 탑재용 소프트웨어에 탑재 가능한 위성 이벤트 예측 기법
CN111982126B (zh) 一种全源BeiDou/SINS弹性状态观测器模型设计方法
CN113340297B (zh) 基于坐标系传递的姿态估计方法、系统、终端、介质及应用
CN117213480A (zh) 一种传递对准方法、系统、设备及存储介质
CN109459769B (zh) 一种自主定位方法与系统
CN111141283A (zh) 一种通过地磁数据判断行进方向的方法
CN114147717B (zh) 机器人运动轨迹估计方法、装置、控制器及存储介质
CN114001730B (zh) 融合定位方法、装置、计算机设备和存储介质
CN116222551A (zh) 一种融合多种数据的水下导航方法及装置
Zhang et al. An improved inertial preintegration model in factor graph optimization for high accuracy positioning of intelligent vehicles
CN115096321A (zh) 一种车载捷联惯导系统鲁棒无迹信息滤波对准方法及系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant