CN117146806A - 一种位姿、安装角的矩阵李群估计方法及系统 - Google Patents

一种位姿、安装角的矩阵李群估计方法及系统 Download PDF

Info

Publication number
CN117146806A
CN117146806A CN202310673728.XA CN202310673728A CN117146806A CN 117146806 A CN117146806 A CN 117146806A CN 202310673728 A CN202310673728 A CN 202310673728A CN 117146806 A CN117146806 A CN 117146806A
Authority
CN
China
Prior art keywords
matrix
representing
error
speed
pose
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.)
Pending
Application number
CN202310673728.XA
Other languages
English (en)
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.)
Wuhan University WHU
Original Assignee
Wuhan University WHU
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 Wuhan University WHU filed Critical Wuhan University WHU
Priority to CN202310673728.XA priority Critical patent/CN117146806A/zh
Publication of CN117146806A publication Critical patent/CN117146806A/zh
Pending legal-status Critical Current

Links

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
    • G01C21/165—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 combined with non-inertial navigation instruments
    • 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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G06F17/13—Differential equations
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Mathematical Physics (AREA)
  • Mathematical Analysis (AREA)
  • Pure & Applied Mathematics (AREA)
  • Computational Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Optimization (AREA)
  • General Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • Algebra (AREA)
  • Manufacturing & Machinery (AREA)
  • Operations Research (AREA)
  • Navigation (AREA)

Abstract

本发明提供一种位姿、安装角的矩阵李群估计方法及系统，根据IMU模块输出的比力和角速率，进行惯性导航机械编排，在李群上构建导航系统的姿态、速度、位置及其误差，并构建导航系统的误差状态向量；构建导航系统的姿态、速度和位置误差方程，并据此得到误差状态微分方程；如果存在GNSS或里程计观测值，构建相应的量测方程；进行卡尔曼滤波的状态预测和量测更新，并对当前时刻的导航状态进行修正；重复以上步骤，以得到每一时刻的姿态、位置和速度。本发明解决了组合导航系统中扩展卡尔曼滤波方差估计不一致等问题，提高了组合导航系统的鲁棒性和精度。

Description

一种位姿、安装角的矩阵李群估计方法及系统
技术领域
本发明属于导航方法及应用技术领域，尤其涉及一种位姿估计、安装角估计的矩阵李群方案。
背景技术
在定位导航领域中，如果要得到载体的高精度位置和姿态信息，最常使用的导航方法为GNSS(Global Navigation Satellite System，全球卫星导航系统)和INS(InertialNavigation System，惯性导航系统)组合导航系统。
同时，利用里程计辅助导航已经被证明可以显著提高车辆的GNSS/INS组合导航精度。而要充分发挥里程计的潜力，需要精确知道IMU(Inertial Measurement Unit，惯性测量单元)的安装角。由于安装位置等操作因素导致的IMU坐标系和载体的坐标系不完全吻合，两个坐标系之间的关系即为安装角。
为了得到更好的导航结果，组合导航系统需要对GNSS、INS和里程计等设备的观测结果进行数据融合。目前，使用最多的数据融合算法是扩展卡尔曼滤波算法，该方法在大多数情况下能较好地将数据进行融合，取得良好的定位结果。然而，扩展卡尔曼滤波也有一些局限性。在扩展卡尔曼滤波的预测和更新过程中，需要利用估计的导航状态构造过程和状态模型的雅克比矩阵，如果导航状态估计的结果不准确，会导致计算出的雅克比矩阵不正确。且扩展卡尔曼滤波存在方差估计不一致的问题，当导航系统的非线性较高时会导致滤波的效果下降甚至发散。
针对现有问题，本发明提出一种位姿估计、安装角估计的矩阵李群方法。利用矩阵李群整体上表征系统的旋转、平移和速度，可以从李群的角度来考虑系统的微分方程，这样非常的自然而且更加准确，相比于单个变量的表征形式，以一个整体考虑系统的微分方程，可以耦合状态之间存在的关系，使得模型变得更加准确。
发明内容
为了克服现有技术的不足，本发明提供了一种位姿估计、安装角估计的矩阵李群方案，以解决扩展卡尔曼滤波方差估计不一致、对导航初始状态要求高等问题。
本发明提供一种位姿、安装角的矩阵李群估计方法，包括获取载体的初始姿态、初始速度和初始位置，并将其作为惯性导航系统的初始起算数据；所述姿态包括载体的横滚角、俯仰角和航向角；根据IMU模块输出的比力和角速率，进行惯性导航机械编排，将上一时刻的姿态、速度和位置更新为当前时刻的姿态、速度和位置；进行以下步骤，
根据IMU模块输出的比力和角速率，进行惯性导航机械编排，将上一时刻的姿态、速度和位置更新为当前时刻的姿态、速度和位置；
将导航系统的姿态、速度和位置构建为一个矩阵李群；在矩阵李群上定义导航系统的误差；根据定义的导航系统的误差得到导航系统的姿态误差、速度误差和位置误差；在矩阵李群上定义IMU安装角误差；并将矩阵李群上的姿态误差、速度误差、位置误差以及IMU的器件误差、矩阵李群上定义的IMU安装角误差和里程计尺度因子误差构建为导航系统的误差状态向量；
根据当前时刻的姿态、速度和位置和姿态误差、速度误差和位置误差构建姿态误差方程、速度误差方程和位置误差方程，对IMU的器件误差、IMU安装角误差和里程计尺度因子误差进行建模，并据此构建卡尔曼滤波的误差状态微分方程；
如果当前时刻存在GNSS观测值或里程计观测值，根据GNSS提供的位置观测值、速度观测值和里程计提供的速度观测值，构建对应的量测方程；
根据卡尔曼滤波的公式，进行卡尔曼滤波的状态预测；如果当前时刻存在GNSS观测值或里程计观测值，则根据卡尔曼滤波的公式进行量测更新，并根据量测更新得到误差状态量对当前时刻的姿态、速度和位置以及IMU安装角进行修正；
进入下一个时刻后，重复以上处理以获得载体每一时刻的姿态、速度和位置。
而且，所述载体的姿态为速度为/>位置为/>其中，/>表示载体坐标系相对于世界坐标系的旋转矩阵，/>表示载体坐标系相对于世界坐标系的速度向量在世界坐标系中的投影，/>表示载体坐标系相对于世界坐标系的位置向量在世界坐标系中的投影；e表示世界坐标系，b表示载体坐标系。
而且，导航系统的姿态、速度和位置所构成的矩阵李群为：
其中，03×1表示大小为3×1的零向量；
在矩阵李群上定义的导航系统的误差为：
其中，η为在矩阵李群上定义的导航系统的误差；χ为导航系统状态的真实值；为导航系统状态的估计值；
定义在矩阵李群上的导航系统的姿态误差为：
其中，φ为矩阵李群上定义的导航系统的姿态误差； 表示真实的载体姿态；/>表示估计的载体姿态；I3×3表示大小为3×3的单位矩阵，φ×表示由三维向量φ生成的反对称矩阵；
定义在矩阵李群上的导航系统的速度误差为：
其中，Jρv为矩阵李群上定义的导航系统的速度误差；表示从e系到b系的姿态旋转矩阵，/> 表示真实的载体姿态；/>表示真实的载体速度；/>表示估计的载体速度；
定义在矩阵李群上的导航系统的位置误差为：
其中，Jρr为矩阵李群上定义的导航系统的位置误差； 表示真实的载体姿态；/>表示真实的载体位置；/>表示估计的载体位置；
定义在矩阵李群上的IMU安装角误差为：
其中，为矩阵李群上定义的IMU安装角误差；/> 表示真实的载体到里程计的旋转矩阵，即真实的里程计安装角；/>表示估计的里程计安装角；
导航系统的误差状态向量的元素包括矩阵李群上的姿态误差φ、矩阵李群上的速度误差Jρv、矩阵李群上的位置误差Jρr、IMU的陀螺仪零偏改正项δbg、IMU的加速度计零偏改正项δba、矩阵李群上定义的IMU安装角误差和里程计尺度因子误差δs。
而且，所述姿态误差状态方程为：
其中，表示载体坐标系相对于惯性坐标系的角速度在载体坐标系的投影；δbg表示IMU陀螺仪的零偏改正项；wg表示IMU陀螺仪的测量白噪声；/>表示等效旋转矢量φ的微分；
所述速度误差状态方程为：
其中，表示载体坐标系相对于惯性坐标系的角速度在载体坐标系的投影；/>表示比力；δba表示IMU加速度计的零偏改正项；wa表示IMU加速度计的测量白噪声；
所述位置误差状态方程为：
其中，表示载体坐标系相对于惯性坐标系的角速度在载体坐标系的投影。
而且，所述的量测方程包括GNSS位置量测方程、GNSS速度量测方程、里程计速度量测方程；
所述GNSS位置量测方程为：
其中， 表示通过惯性导航机械编排得到的GNSS相位中心位置，/>表示通过GNSS测量得到的GNSS相位中心位置；/>表示GNSS相位中心到载体坐标系的杆臂；/>表示GNSS的位置观测噪声向量；
所述GNSS速度量测方程为：
其中， 表示通过惯性导航机械编排得到的GNSS相位中心速度，/>表示通过GNSS测量得到的GNSS相位中心速度；/>表示GNSS相位中心到载体坐标系的杆臂；/>表示GNSS的速度观测噪声向量；
步骤5所述里程计速度量测方程为：
其中， 表示通过惯性导航机械编排得到的里程计速度，/>表示通过里程计测量得到的里程计速度；/>表示里程计到载体坐标系的杆臂；/>表示里程计的速度观测噪声向量。
另一方面，本发明提供一种位姿、安装角的矩阵李群估计系统，用于实现如上所述的一种位姿、安装角的矩阵李群估计方法。
而且，包括处理器和存储器，存储器用于存储程序指令，处理器用于调用存储器中的存储指令执行如上所述的一种位姿、安装角的矩阵李群估计方法。
或者，包括可读存储介质，所述可读存储介质上存储有计算机程序，所述计算机程序执行时，实现如上所述的一种位姿、安装角的矩阵李群估计方法。
本发明提出了一种位姿估计、安装角估计的矩阵李群方案。获取载体的初始姿态、位置和速度，并将其作为惯性导航系统的初始起算数据；根据IMU模块的输出进行惯性导航机械编排；在李群上构建导航系统的姿态、速度、位置及其误差，并构建导航系统的误差状态向量；构建导航系统的姿态、速度和位置误差方程，并据此得到误差状态微分方程；如果存在GNSS或里程计观测值，构建相应的量测方程；进行卡尔曼滤波的状态预测和量测更新，并对当前时刻的导航状态进行修正；重复以上步骤，以得到每一时刻的姿态、位置和速度。本发明解决了组合导航系统中扩展卡尔曼滤波方差估计不一致等问题，提高了组合导航系统的鲁棒性和精度。
附图说明
图1为本发明实施例提供的一种位姿估计、安装角估计的矩阵李群方法流程图。
具体实施方式
为了使得本发明实施例的目的和技术方案更加清楚，下面将结合本发明实施例，对此实施例中对本发明中的技术方案进行清楚、完整地描述，显然，所描述实施例是本发明一部分实施例，而非全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
参见图1，本发明实施例提供一种位姿、安装角的矩阵李群估计方法，下面将对具体实施步骤进行详细地描述。
步骤1：获取载体的初始姿态、初始速度和初始位置，并将其作为惯性导航系统的初始起算数据；所述姿态包括载体的横滚角、俯仰角和航向角；
其中，所述载体的姿态为速度为/>位置为/>其中，/>表示载体坐标系相对于世界坐标系的旋转矩阵，/>表示载体坐标系相对于世界坐标系的速度向量在世界坐标系中的投影，/>表示载体坐标系相对于世界坐标系的位置向量在世界坐标系中的投影；e表示世界坐标系，b表示载体坐标系。
步骤2：根据IMU模块输出的比力和角速率，进行惯性导航机械编排，将上一时刻的姿态、速度和位置更新为当前时刻的姿态、速度和位置；
其中，所述惯性导航机械编排的步骤为：利用上一时刻的姿态、速度和位置以及IMU输出的比力和角速度信息，根据导航微分方程解算出这一时刻的姿态、速度和位置，包括姿态更新、速度更新和位置更新；其中，解算后的姿态更新、速度更新和位置更新的公式为：
其中，下标k表示当前时刻，下标k-1表示上一时刻；tk表示当前时刻的时间，tk-1表示上一时刻的时间；表示当前时刻的姿态四元数，它与当前时刻的姿态/>可以相互转换；/>表示上一时刻的姿态四元数，它与上一时刻的姿态/>可以相互转换；/>表示更新时间间隔内由于地球自转导致的e系的姿态变化四元数，/>ξk表示更新时间间隔内地球自转的等效旋转矢量，/> 表示地球自转角速度；/>表示更新时间间隔内载体坐标系变化的姿态变化四元数，φk表示当前时刻相对于上一时刻载体坐标系的等效旋转矢量，Δθk和Δθk-1分别表示当前和上一时刻的角增量；/>表示当前时刻的速度；/>表示上一时刻的速度；/>表示由比力引起的速度增量在e系下的投影， 表示e系更新的姿态旋转矩阵，/>可通过转换/>求解，Δθk和Δθk-1分别表示当前和上一时刻的角增量，/>和/>分别表示当前时刻和上一时刻载体坐标系下的速度增量；/>表示由重力加速度和哥氏加速度引起的速度增量在e系下的投影，ge表示地球重力，/>表示地球自转角速度；表示当前时刻的位置；/>表示上一时刻的位置。
步骤3：将导航系统的姿态、速度和位置构建为一个矩阵李群；在矩阵李群上定义导航系统的误差；根据定义的导航系统的误差得到导航系统的姿态误差、速度误差和位置误差；在矩阵李群上定义IMU安装角误差；并将矩阵李群上的姿态误差、速度误差、位置误差以及IMU的器件误差、矩阵李群上定义的IMU安装角误差和里程计尺度因子误差构建为导航系统的误差状态向量；
步骤3所述的导航系统的姿态、速度和位置所构成的矩阵李群为：
其中，03×1表示大小为3×1的零向量。
步骤3所述的在矩阵李群上定义的导航系统的误差为：
其中，η为在矩阵李群上定义的导航系统的误差；χ为导航系统状态的真实值；为导航系统状态的估计值；
步骤3所述的定义在矩阵李群上的导航系统的姿态误差为：
其中，φ为矩阵李群上定义的导航系统的姿态误差； 表示真实的载体姿态；/>表示估计的载体姿态；I3×3表示大小为3×3的单位矩阵，φ×表示由三维向量φ生成的反对称矩阵。
步骤3所述的定义在矩阵李群上的导航系统的速度误差为：
其中，Jρv为矩阵李群上定义的导航系统的速度误差；表示从e系到b系的姿态旋转矩阵，/> 表示真实的载体姿态；/>表示真实的载体速度；/>表示估计的载体速度；
步骤3所述的定义在矩阵李群上的导航系统的位置误差为：
其中，Jρr为矩阵李群上定义的导航系统的位置误差； 表示真实的载体姿态；/>表示真实的载体位置；/>表示估计的载体位置；
步骤3所述的定义在矩阵李群上的IMU安装角误差为：
其中，为矩阵李群上定义的IMU安装角误差；/> 表示真实的载体到里程计的旋转矩阵，即真实的里程计安装角；/>表示估计的里程计安装角；
优选地，步骤3所述导航系统的误差状态向量可以通过一个状态向量x来表示，具体如下：
其中，φ为矩阵李群上的姿态误差，Jρv为矩阵李群上的速度误差，Jρr为矩阵李群上的位置误差，δbg为IMU的陀螺仪零偏改正项，δba为IMU的加速度计零偏改正项，δs为里程计尺度因子误差，为矩阵李群上定义的IMU安装角误差。
步骤4：根据步骤2得到的当前时刻的姿态、速度和位置和步骤3定义的姿态误差、速度误差和位置误差构建姿态误差方程、速度误差方程和位置误差方程，对IMU的器件误差、IMU安装角误差和里程计尺度因子误差进行建模，并据此构建卡尔曼滤波的误差状态微分方程；
优选地，本发明给出步骤4矩阵李群上姿态误差方程、速度误差方程和位置误差方程的具体推导过程，具体如下：
姿态误差状态方程推导过程如下：
且
故有
其中，表示载体坐标系相对于惯性坐标系的角速度在载体坐标系的投影；δbg表示IMU陀螺仪的零偏改正项；wg表示IMU陀螺仪的测量白噪声；推导过程中忽略了地球自转角速度/>的误差以及二阶小量(φ×)(δbg+g)；/>表示等效旋转矢量φ的微分，/>表示真值姿态矩阵/>的微分，/>表示估计的姿态矩阵的微分，/>表示变量的时间微分；
速度误差状态方程推导过程如下：
其中，表示载体坐标系相对于惯性坐标系的角速度在载体坐标系的投影；/>表示比力；δba表示IMU加速度计的零偏改正项；wa表示IMU加速度计的测量白噪声；推导过程中忽略了二阶小量(φ×)(δba+a)；由于/>在局部变化很小，因此忽略了/>而和/>的值都很小，因此/>也被视作小量忽略了；
位置误差状态方程推导过程如下：
其中，表示载体坐标系相对于惯性坐标系的角速度在载体坐标系的投影；由于和/>的值都很小，因此/>被视作小量忽略了；
优选地，IMU的陀螺仪零偏改正项、加速度计零偏改正项可以建模为一阶高斯马尔科夫过程，其相关时间分别为τg和τa，一阶高斯马尔科夫过程驱动白噪声分别为和/>IMU安装角误差和里程计尺度因子误差可以建模为常值，其驱动白噪声分别为/>和ηs；这样，卡尔曼滤波的误差状态微分方程可以表示为
其中，是误差状态的微分，F是线性误差动力学矩阵，G是过程噪声雅克比矩阵，w是白噪声向量，
步骤5：如果当前时刻存在GNSS观测值或里程计观测值，根据GNSS提供的位置观测值、速度观测值和里程计提供的速度观测值，构建对应的量测方程；
优选地，本发明给出步骤5中GNSS的位置量测方程、速度量测方程和里程计的速度量测方程的具体推导过程，具体如下：
GNSS位置量测方程推导如下：
其中，表示通过惯性导航机械编排得到的GNSS相位中心位置，/>表示通过GNSS测量得到的GNSS相位中心位置；/>表示GNSS相位中心到载体坐标系的杆臂；表示GNSS的位置观测噪声向量；
GNSS速度量测方程推导如下：
其中，表示通过惯性导航机械编排得到的GNSS相位中心速度，/>表示通过GNSS测量得到的GNSS相位中心速度；/>表示GNSS相位中心到载体坐标系的杆臂；表示GNSS的速度观测噪声向量；推导过程中忽略了二阶小项/>
里程计速度量测方程推导如下：
其中，表示通过惯性导航机械编排得到的里程计速度，/>表示通过里程计测量得到的里程计速度；/>表示里程计到载体坐标系的杆臂；/>表示里程计的速度观测噪声向量；diag()表示对角矩阵；推导过程中忽略了二阶小项和/>
优选地，量测方程可以表示为：
δz＝Hx+V
其中，δz是测量残差，V是观测噪声向量，/>H是测量雅克比矩阵，/>
其中，是GNSS位置观测残差，/>是GNSS速度观测残差，/>是里程计速度观测残差，/>是GNSS的位置观测噪声向量，/>是GNSS的速度观测噪声向量，是里程计的速度观测噪声向量，/>是GNSS位置观测雅克比矩阵，/>是GNSS速度观测雅克比矩阵，/>是里程计观测雅克比矩阵，
其中若记 和/>建模为高斯白噪声，其协方差矩阵分别为/>和/>可以证明如果取各向同性的观测噪声协方差矩阵并用/>和/>代替/>和/>则组合导航的结果不变，因此，可以使用与状态无关的/>和/>作为卡尔曼滤波的观测矩阵。
步骤6：根据卡尔曼滤波的公式，进行卡尔曼滤波的状态预测；如果当前时刻存在GNSS观测值或里程计观测值，则根据卡尔曼滤波的公式进行量测更新，并根据量测更新得到误差状态量对步骤2得到的当前时刻的姿态、速度和位置以及IMU安装角进行修正；
步骤6所述卡尔曼滤波的状态预测公式为：
其中，表示k-1时刻的状态向量，Pk-1表示k-1时刻的系统状态协方差矩阵，表示预测的状态向量，Pk/k-1表示预测的系统状态协方差矩阵，Φk/k-1表示k-1时刻到k时刻的状态转移矩阵，Qk-1表示k-1时刻的系统噪声协方差矩阵；
步骤6所述卡尔曼滤波的量测更新公式为：
其中，其中，表示k时刻的状态向量，Pk表示k时刻的系统状态协方差矩阵，表示预测的状态向量，Pk/k-1表示预测的系统状态协方差矩阵，Hk表示观测矩阵，Rk表示观测噪声协方差矩阵，Zk表示观测向量，Kk表示卡尔曼增益矩阵，I表示单位矩阵；
步骤6所述的对当前时刻的姿态、速度和位置以及IMU安装角的修正公式为：
其中，和/>表示更新前后的姿态，/>和/>表示更新前后的速度，/>和/>表示更新前后的位置，/>和/>表示更新前后的IMU安装角；
步骤7：进入下一个时刻后，重复步骤2、步骤3、步骤4、步骤5、步骤6，以获得载体每一时刻的姿态、速度和位置。
具体实施时，本发明技术方案提出的方法可由本领域技术人员采用计算机软件技术实现自动运行流程，实现方法的系统装置例如存储本发明技术方案相应计算机程序的计算机可读存储介质以及包括运行相应计算机程序的计算机设备，也应当在本发明的保护范围内。
在一些可能的实施例中，提供一种位姿、安装角的矩阵李群估计系统，包括处理器和存储器，存储器用于存储程序指令，处理器用于调用存储器中的存储指令执行如上所述的一种位姿、安装角的矩阵李群估计方法。
在一些可能的实施例中，提供一种位姿、安装角的矩阵李群估计系统，包括可读存储介质，所述可读存储介质上存储有计算机程序，所述计算机程序执行时，实现如上所述的一种位姿、安装角的矩阵李群估计方法。
以上的实施例仅用以说明本发明的技术方案，而非对其进行限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (8)

1.一种位姿、安装角的矩阵李群估计方法，包括获取载体的初始姿态、初始速度和初始位置，并将其作为惯性导航系统的初始起算数据；所述姿态包括载体的横滚角、俯仰角和航向角；根据IMU模块输出的比力和角速率，进行惯性导航机械编排，将上一时刻的姿态、速度和位置更新为当前时刻的姿态、速度和位置；其特征在于：进行以下步骤，
根据IMU模块输出的比力和角速率，进行惯性导航机械编排，将上一时刻的姿态、速度和位置更新为当前时刻的姿态、速度和位置；
将导航系统的姿态、速度和位置构建为一个矩阵李群；在矩阵李群上定义导航系统的误差；根据定义的导航系统的误差得到导航系统的姿态误差、速度误差和位置误差；在矩阵李群上定义IMU安装角误差；并将矩阵李群上的姿态误差、速度误差、位置误差以及IMU的器件误差、矩阵李群上定义的IMU安装角误差和里程计尺度因子误差构建为导航系统的误差状态向量；
根据当前时刻的姿态、速度和位置和姿态误差、速度误差和位置误差构建姿态误差方程、速度误差方程和位置误差方程，对IMU的器件误差、IMU安装角误差和里程计尺度因子误差进行建模，并据此构建卡尔曼滤波的误差状态微分方程；
如果当前时刻存在GNSS观测值或里程计观测值，根据GNSS提供的位置观测值、速度观测值和里程计提供的速度观测值，构建对应的量测方程；
根据卡尔曼滤波的公式，进行卡尔曼滤波的状态预测；如果当前时刻存在GNSS观测值或里程计观测值，则根据卡尔曼滤波的公式进行量测更新，并根据量测更新得到误差状态量对当前时刻的姿态、速度和位置以及IMU安装角进行修正；
进入下一个时刻后，重复以上处理以获得载体每一时刻的姿态、速度和位置。
2.根据权利要求1所述的一种位姿、安装角的矩阵李群估计方法，其特征在于：所述载体的姿态为速度为/>位置为/>其中，/>表示载体坐标系相对于世界坐标系的旋转矩阵，/>表示载体坐标系相对于世界坐标系的速度向量在世界坐标系中的投影，/>表示载体坐标系相对于世界坐标系的位置向量在世界坐标系中的投影；e表示世界坐标系，b表示载体坐标系。
3.根据权利要求2所述的一种位姿、安装角的矩阵李群估计方法，其特征在于：
导航系统的姿态、速度和位置所构成的矩阵李群为：
其中，03×1表示大小为3×1的零向量；
在矩阵李群上定义的导航系统的误差为：
其中，η为在矩阵李群上定义的导航系统的误差；χ为导航系统状态的真实值；为导航系统状态的估计值；
定义在矩阵李群上的导航系统的姿态误差为：
其中，φ为矩阵李群上定义的导航系统的姿态误差； 表示真实的载体姿态；/>表示估计的载体姿态；I3×3表示大小为3×3的单位矩阵，φ×表示由三维向量φ生成的反对称矩阵；
定义在矩阵李群上的导航系统的速度误差为：
其中，Jρv为矩阵李群上定义的导航系统的速度误差；表示从e系到b系的姿态旋转矩阵，/> 表示真实的载体姿态；/>表示真实的载体速度；/>表示估计的载体速度；
定义在矩阵李群上的导航系统的位置误差为：
其中，Jρr为矩阵李群上定义的导航系统的位置误差； 表示真实的载体姿态；/>表示真实的载体位置；/>表示估计的载体位置；
定义在矩阵李群上的IMU安装角误差为：
其中，为矩阵李群上定义的IMU安装角误差；/> 表示真实的载体到里程计的旋转矩阵，即真实的里程计安装角；/>表示估计的里程计安装角；
导航系统的误差状态向量的元素包括矩阵李群上的姿态误差φ、矩阵李群上的速度误差Jρv、矩阵李群上的位置误差Jρr、IMU的陀螺仪零偏改正项δbg、IMU的加速度计零偏改正项δba、矩阵李群上定义的IMU安装角误差和里程计尺度因子误差δs。
4.根据权利要求3所述的一种位姿、安装角的矩阵李群估计方法，其特征在于：所述姿态误差状态方程为：
其中，表示载体坐标系相对于惯性坐标系的角速度在载体坐标系的投影；δbg表示IMU陀螺仪的零偏改正项；wg表示IMU陀螺仪的测量白噪声；/>表示等效旋转矢量φ的微分；
所述速度误差状态方程为：
其中，表示载体坐标系相对于惯性坐标系的角速度在载体坐标系的投影；/>表示比力；δba表示IMU加速度计的零偏改正项；wa表示IMU加速度计的测量白噪声；
所述位置误差状态方程为：
其中，表示载体坐标系相对于惯性坐标系的角速度在载体坐标系的投影。
5.根据权利要求4所述的一种位姿、安装角的矩阵李群估计方法，其特征在于：
所述的量测方程包括GNSS位置量测方程、GNSS速度量测方程、里程计速度量测方程；
所述GNSS位置量测方程为：
其中，表示通过惯性导航机械编排得到的GNSS相位中心位置，/>表示通过GNSS测量得到的GNSS相位中心位置；/>表示GNSS相位中心到载体坐标系的杆臂；/>表示GNSS的位置观测噪声向量；
所述GNSS速度量测方程为：
其中， 表示通过惯性导航机械编排得到的GNSS相位中心速度，/>表示通过GNSS测量得到的GNSS相位中心速度；/>表示GNSS相位中心到载体坐标系的杆臂；/>表示GNSS的速度观测噪声向量；
步骤5所述里程计速度量测方程为：
其中， 表示通过惯性导航机械编排得到的里程计速度，/>表示通过里程计测量得到的里程计速度；/>表示里程计到载体坐标系的杆臂；/>表示里程计的速度观测噪声向量。
6.一种位姿、安装角的矩阵李群估计系统，其特征在于：用于实现如权利要求1-5任一项所述的一种位姿、安装角的矩阵李群估计方法。
7.根据权利要求6所述位姿、安装角的矩阵李群估计系统，其特征在于：包括处理器和存储器，存储器用于存储程序指令，处理器用于调用存储器中的存储指令执行如权利要求1-5任一项所述的一种位姿、安装角的矩阵李群估计方法。
8.根据权利要求6所述位姿、安装角的矩阵李群估计系统，其特征在于：包括可读存储介质，所述可读存储介质上存储有计算机程序，所述计算机程序执行时，实现如权利要求1-5任一项所述的一种位姿、安装角的矩阵李群估计方法。
CN202310673728.XA 2023-06-07 2023-06-07 一种位姿、安装角的矩阵李群估计方法及系统 Pending CN117146806A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310673728.XA CN117146806A (zh) 2023-06-07 2023-06-07 一种位姿、安装角的矩阵李群估计方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310673728.XA CN117146806A (zh) 2023-06-07 2023-06-07 一种位姿、安装角的矩阵李群估计方法及系统

Publications (1)

Publication Number Publication Date
CN117146806A true CN117146806A (zh) 2023-12-01

Family

ID=88883025

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310673728.XA Pending CN117146806A (zh) 2023-06-07 2023-06-07 一种位姿、安装角的矩阵李群估计方法及系统

Country Status (1)

Country Link
CN (1) CN117146806A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117590441A (zh) * 2024-01-16 2024-02-23 广州导远电子科技有限公司 一种完好性保护水平计算方法及相关设备

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117590441A (zh) * 2024-01-16 2024-02-23 广州导远电子科技有限公司 一种完好性保护水平计算方法及相关设备
CN117590441B (zh) * 2024-01-16 2024-04-30 广州导远电子科技有限公司 一种完好性保护水平计算方法及相关设备

Similar Documents

Publication Publication Date Title
CN111811506B (zh) 视觉/惯性里程计组合导航方法、电子设备及存储介质
JP6094026B2 (ja) 姿勢判定方法、位置算出方法及び姿勢判定装置
CN108731670A (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN110887480B (zh) 基于mems传感器的飞行姿态估计方法及系统
CN110715659A (zh) 零速检测方法、行人惯性导航方法、装置及存储介质
JP2012173190A (ja) 測位システム、測位方法
CN117146806A (zh) 一种位姿、安装角的矩阵李群估计方法及系统
CN114002725A (zh) 一种车道线辅助定位方法、装置、电子设备及存储介质
CN114136315B (zh) 一种基于单目视觉辅助惯性组合导航方法及系统
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
CN116007620A (zh) 一种组合导航滤波方法、系统、电子设备及存储介质
CN114964222A (zh) 一种车载imu姿态初始化方法、安装角估计方法及装置
CN115200578A (zh) 基于多项式优化的惯性基导航信息融合方法及系统
CN113483759B (zh) 一种GNSS、INS、Vision组合导航系统完好性保护级计算方法
CN113566850B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备
US11150090B2 (en) Machine learning zero-rate level calibration
CN109506674B (zh) 一种加速度的校正方法及装置
CN116540286B (zh) 基于非完整性约束的多源鲁棒融合定位方法
CN112729332A (zh) 一种基于旋转调制的对准方法
CN108279025B (zh) 一种基于重力信息的光纤陀螺罗经快速精对准方法
CN113916261B (zh) 基于惯性导航优化对准的姿态误差评估方法
CN116222551A (zh) 一种融合多种数据的水下导航方法及装置
CN116380119A (zh) 组合导航的校准方法、装置和系统
CN117537814A (zh) 一种矩阵李群上的无迹卡尔曼滤波初始对准方法及系统
CN117570976B (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