CN111337056B - 基于优化的LiDAR运动补偿位置姿态系统对准方法 - Google Patents

基于优化的LiDAR运动补偿位置姿态系统对准方法 Download PDF

Info

Publication number
CN111337056B
CN111337056B CN202010422534.9A CN202010422534A CN111337056B CN 111337056 B CN111337056 B CN 111337056B CN 202010422534 A CN202010422534 A CN 202010422534A CN 111337056 B CN111337056 B CN 111337056B
Authority
CN
China
Prior art keywords
coordinate system
inertial
attitude
quaternion
navigation
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
CN202010422534.9A
Other languages
English (en)
Other versions
CN111337056A (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.)
Beijing Digital Green Earth Technology Co.,Ltd.
Original Assignee
Beijing Greenvalley Technology Co ltd
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 Beijing Greenvalley Technology Co ltd filed Critical Beijing Greenvalley Technology Co ltd
Priority to CN202010422534.9A priority Critical patent/CN111337056B/zh
Publication of CN111337056A publication Critical patent/CN111337056A/zh
Application granted granted Critical
Publication of CN111337056B publication Critical patent/CN111337056B/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
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明涉及基于优化的LiDAR运动补偿位置姿态系统对准方法，建立一个综合利用惯性测量单元敏感信息和GPS量测信息的数学模型，该数学模型能够利用惯性测量单元敏感的运动信息和GPS测量的位置、速度信息进行纯惯性导航解算，并在导航解算中将初始姿态作为一个独立待求量，利用优化方法解算得到某时间点的最优初始姿态信息，使得IMU在该初始姿态下进行纯惯性导航的速度轨迹与GPS速度轨迹之间方差最小。本发明具有对准精度高、速度快、鲁棒性强的优点，可用于惯性导航坐标系统或惯性测量系统进行空中对准。

Description

基于优化的LiDAR运动补偿位置姿态系统对准方法
技术领域
本发明涉及激光雷达空中对准技术领域，尤其涉及基于优化的LiDAR运动补偿位置姿态系统对准方法。
背景技术
位置姿态系统(POS)是高精度航空遥感对地观测领域的一项关键技术，用于补偿成像过程中载机的运动误差。而空中对准是位置姿态系统初始对准技术中的一个重要组成部分，该技术可以帮助位置姿态系统完成紧急任务，提高系统的反应速度，使得位置姿态系统可以在无法完成地面(例如：自然灾害监测，军事任务等)初始对准的情况下实现空中对准。
在空中对准过程中，位置和速度信息往往直接取自GPS信息，因此，所谓空中对准主要指的是姿态对准。与地面静基座对准相比，空中对准往往在较大的机动环境中进行，较大的机动是一种严重的噪声，会严重影响对准精度和姿态收敛速度、甚至会引起对准失败。因此，相对于地面静基座对准，空中对准要困难的多。
目前常用的空中对准方法绝大多数是基于卡尔曼滤波技术，如经典卡尔曼滤波、扩展卡尔曼滤波、粒子滤波、模型预测滤波等。然而，无论是怎样改进的卡尔曼滤波方法，都要求姿态初值和姿态初值的可信度矩阵。姿态初值中的水平姿态往往来自于加速度计的敏感值，姿态初值中的航向姿态往往来自于GPS。空中对准过程中飞机的机动会使得加速度计敏感值算得的水平姿态有较大误差，而较大的初始值误差会引起严重的非线性，甚至滤波发散。因此，传统方法通常要求飞机在空中对准过程中尽量保持匀速直线运动以避免姿态结果出现较大误差。
发明内容
本发明提供了基于优化的LiDAR运动补偿位置姿态系统对准方法，该方法实现了空中对准，克服了对准过程中载机空中机动对对准过程的影响，提高了空中对准的精度和鲁棒性。
本发明提供了基于优化的LiDAR运动补偿位置姿态系统对准方法，包括如下主要操作步骤：
选取惯性坐标系进行姿态和速度的捷联惯导解算公式的更新；
实时采集GPS数据，从中获取实时速度信息和位置信息，将通过GPS数据获取的所述速度信息和所述位置信息代入所述速度的捷联惯导解算公式中；
以纯惯性导航的速度轨迹与GPS速度轨迹在最小二乘原则下重合为条件，求解所述条件下的初始姿态，此求解得到的所述初始姿态即为最优初始姿态。
优选地，作为一种可实施方案；基于优化的LiDAR运动补偿位置姿态系统对准方法，具体包括执行如下操作步骤：
步骤S1：选取惯性空间作为导航解算的参考坐标系空间，在惯性空间中的姿态差分方程为：
Figure GDA0002584691620000021
其中，i是惯性坐标系，b是载体坐标系，
Figure GDA0002584691620000022
是载体坐标系相对于惯性坐标系的姿态矩阵，
Figure GDA0002584691620000023
是载体坐标系下惯性测量单元敏感到的相对于惯性空间的转动角速度，
Figure GDA0002584691620000024
是向量
Figure GDA0002584691620000025
的叉乘矩阵；
经姿态更新解算，
Figure GDA0002584691620000026
为如下形式：
Figure GDA0002584691620000031
其中，χ(0,t)是利用
Figure GDA0002584691620000032
更新解算得到的函数，更新方法采用四元数通用姿态更新方法，t是时间，
Figure GDA0002584691620000033
是载体坐标系相对于惯性坐标系的初始姿态；
同时实时采集GPS数据，从中获取实时速度信息和位置信息，并根据所述速度信息和所述位置信息计算得到
Figure GDA0002584691620000034
其中，
Figure GDA0002584691620000035
是导航坐标系相对于惯性空间的姿态矩阵，
Figure GDA0002584691620000036
是在导航坐标系中表示的导航坐标系相对于地球坐标系的运动速度，
Figure GDA0002584691620000037
是在导航坐标系中表示的载体相对于地球运动产生的等效转速；
步骤S2：在所述惯性空间中的速度更新微分方程如下：
Figure GDA0002584691620000038
其中，
Figure GDA0002584691620000039
是在惯性空间中表示的导航坐标系相对于地球坐标系的运动速度，fi是在惯性空间中表示的惯性测量单元敏感的比力信息，
Figure GDA00025846916200000310
是在惯性空间中表示的地球自转角速度，
Figure GDA00025846916200000311
是在惯性空间中表示的载体相对于地球运动产生的等效转速，
Figure GDA00025846916200000312
是在惯性空间中表示的载体相对于地球的运动速度，gi是在惯性空间中表示的地球重力矢量；
步骤S3：将步骤S2的所述速度更新微分方程变换到惯性坐标系下，即按照所述惯性空间中的速度更新微分方程进行速度积分计算，从而得到改进后速度更新微分方程如下：
Figure GDA00025846916200000313
其中，
Figure GDA00025846916200000314
是导航坐标系相对于惯性空间的姿态矩阵，
Figure GDA00025846916200000315
是导航坐标系相对于惯性空间的姿态矩阵的逆矩阵
Figure GDA00025846916200000316
是在导航坐标系中表示的载体相对于地球运动产生的等效转速，
Figure GDA00025846916200000317
是在导航坐标系中表示的导航坐标系相对于地球坐标系的运动速度，gn是在导航坐标系中表示的地球重力矢量，fb是在载体坐标系中表示的比力矢量，公式4中
Figure GDA0002584691620000041
均以采集GPS数据得到的速度信息和位置信息进行运算；
步骤S4：将步骤S3的所述改进后速度更新微分方程进行处理四元数形式转换处理；即将所述改进后速度更新微分方程以
Figure GDA0002584691620000042
的形式进行转换表示：
Figure GDA0002584691620000043
其中，α(t)和β(t)表示如下：
Figure GDA0002584691620000044
Figure GDA0002584691620000045
步骤S5：将步骤S4中所述α(t)、β(t)和
Figure GDA0002584691620000046
表示为四元数相乘的形式如下：
Figure GDA0002584691620000049
其中，
Figure GDA00025846916200000410
表示四元数相乘运算，q为四元数形式的
Figure GDA0002584691620000047
q*为q的共轭四元数，α(t)′为四元数形式的α(t)，β(t)′为四元数形式的β(t)；
步骤S6：定义四元数的两种运算如下：
Figure GDA0002584691620000048
其中，s为四元数q的标量部分，η为四元数q的矢量部分；
步骤S7：按照最小二乘原则求解步骤S5中的公式8，所求初始姿态四元数q符合以下最优指标：
Figure GDA0002584691620000051
步骤S8：从上述公式9中提取并定义K阵得到如下公式10:
Figure GDA0002584691620000052
步骤S9：求解步骤S8中公式10的K阵特征值和特征向量，其中K阵最小特征值所对应的特征向量四元数即代表最优初始姿态四元数；tf为对准过程结束的时间。
进一步的，在步骤S9之后还包括如下操作步骤：
步骤S10：根据所述最优初始姿态四元数来计算优化初始姿态阵：
Figure GDA0002584691620000053
其中，Tij表示
Figure GDA0002584691620000054
阵的第i行第j列元素，i取值包括1、2、3，j取值包括1、2、3；qk表示最优姿态四元数的第k个分量，k取值包括0、1、2、3；
步骤S11：按照通用惯性导航方法，根据所述最优初始姿态四元数，求解航向角φ、俯仰角θ和横滚角γ。
基于以上技术方案，本发明实施例至少存在如下方面的技术优势：
本发明提供了基于优化的LiDAR运动补偿位置姿态系统对准方法，包括下列运算步骤：
选取惯性坐标系进行姿态和速度的捷联惯导解算公式的更新；实时采集GPS数据，从中获取实时速度信息和位置信息，将通过GPS数据获取的所述速度信息和所述位置信息代入所述速度的捷联惯导解算公式中；以纯惯性导航的速度轨迹与GPS速度轨迹在最小二乘原则下重合为条件，求解所述条件下的初始姿态，此求解得到的所述初始姿态即为最优初始姿态。
在空中对准过程中，惯性测量单元的空中对准主要是姿态对准，准确的空中对准初始姿态结果带来的效果是：在该初始姿态下进行纯惯性导航，其导航位置误差随时间发散的速度变慢，因为与不准确的初始姿态相比较，准确的初始姿态意味着加速度在水平方向的投影误差和地球自转在惯性测量单元本体下的投影误差更小，继而通过二次积分得到的位置误差发散的更加缓慢。因此，本实施例可以以此为原则，在空中对准时，寻求一个最优的初始姿态，使得在该初始姿态下进行纯惯性导航，其位置误差达到最小。因此，本发明提供的基于优化的LiDAR运动补偿位置姿态系统对准方法，其以以纯惯性导航的速度轨迹与GPS速度轨迹在最小二乘原则下重合为条件，求解所述条件下的初始姿态。为了实现方便，选取惯性坐标系进行姿态和速度的捷联惯导解算更新，同时引入GPS信息在捷联惯导解算公式中代替速度信息和位置信息的反馈，最终从速度更新公式中将初始姿态项提取出来，并将所有项转化为四元数形式，通过求四元数矩阵最小特征值对应的特征向量的方法求得最优初始姿态(具体形式为最优初始姿态四元数)。
在本发明应用方法中，建立了以惯性坐标系下的捷联惯导解算为基础的数学模型，该模型利用GPS量测信息(即上述速度信息和位置信息)，将空中对准过程转化为一个求四元数矩阵最小特征值和对应特征向量的过程。该算法下求得的最优初始姿态，使得在该姿态下进行的纯惯性导航的速度轨迹能够与GPS速度轨迹在最小二乘原则下重合，该原则能够保证以该姿态为初始姿态进行惯性导航的导航误差达到最小，符合惯性坐标系统对于初始姿态的要求，能够使初始姿态与真实姿态相比误差最小。
分析上述技术内容可知：本发明实施例采用了与现有基于卡尔曼滤波的对准技术完全不同的一种解析计算的方法，该方法能够解析计算出最优初始姿态，该算法采用的数学模型对于外界的载机机动变化不敏感(即便是机载机动变化量较大，也不会对空中对准的最优初始姿态解析产生影响；因此其改变了信息传输流向，摒弃了受影响较大的位置、速度和姿态信息(在传统的技术方案中，使用的是位置、速度和姿态信息反馈，然而这些信息均来自捷联惯导解算)，转而使用GPS量测信息(即GPS量测的位置信息和速度信息)进行替代，使得初始姿态与导航某时刻的速度之间能够呈线性关系，为通过将速度信息作为观测量识别初始姿态奠定基础)，因此抗扰能力强，克服了现有方法大失准角下容易滤波发散的问题，大大提高了算法的鲁棒性和精度。因此，本发明与传统方案相比，具有精度高、抗扰动的优点。
附图说明
为了更清楚地说明本发明的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对本发明保护范围的限定。在各个附图中，类似的构成部分采用类似的编号。
图1为本发明实施例提供的基于优化的LiDAR运动补偿位置姿态系统对准方法的捷联惯导解算信号流；
图2为本发明实施例提供的基于优化的LiDAR运动补偿位置姿态系统对准方法的主要流程图；
图3为本发明实施例提供的基于优化的LiDAR运动补偿位置姿态系统对准方法中核心算法的流程图。
具体实施方式
本发明提供了基于优化的LiDAR运动补偿位置姿态系统对准方法，可用于位置姿态系统的空中对准，在位置姿态系统无法完成地面初始对准的情况下，计算惯性测量单元(即IMU)在空中的初始姿态。
众所周知，惯性坐标系(简称惯性系)可以用oxiyizi表示。原点是地球中心；xi与yi在地球赤道平面内相互垂直，分别指向相应的恒星,ozi是地球的自转轴。惯性器件(陀螺仪和加速度计)测量得到的物理量是相对于惯性坐标系的。例如，陀螺仪输出的是载体坐标系相对于惯性坐标系(i系)的角速度。另外，在导航中，最重要的两个坐标系是载体坐标系(简称载体系)和导航坐标系(简称导航系)。载体坐标系是以载体为中心，主要作用是处理与传感器直接测得的物理量。导航坐标系可以是地固坐标系、地理坐标系等等，通常使用的是当地水平坐标系。
选取惯性坐标系进行姿态和速度的捷联惯导解算更新，同时引入GPS数据(即GPS量测信息)在捷联惯导解算公式中代替速度信息和位置信息的反馈，最终从速度更新公式中将初始姿态项提取出来，并将所有项转化为四元数形式，通过求四元数矩阵最小特征值对应的特征向量的方法求得最优初始姿态(具体形式为最优初始姿态四元数)。
在上述应用步骤中，求解最优初始姿态，显然是最为重要的技术步骤；
如图1所示，一般来讲，陀螺仪和加速度计的采样信息，首先经过IMU补偿过程转化为载体坐标系下的角速度信息
Figure GDA0002584691620000081
和加速度信息
Figure GDA0002584691620000082
其中角速度信息
Figure GDA0002584691620000083
通过四元数捷联姿态更新运算得到载体的姿态信息
Figure GDA0002584691620000084
载体坐标系下的加速度信息
Figure GDA0002584691620000085
经姿态转化为惯性坐标系下的加速度信息
Figure GDA0002584691620000086
然后利用GPS信息得到导航坐标系与惯性坐标系之间的姿态转化阵
Figure GDA0002584691620000087
利用该姿态转化阵最终得到导航坐标系下的加速度信息，通过积分运算得到速度信息
Figure GDA0002584691620000088
和位置信息。其中，导航坐标系相对于惯性坐标系的运动角速度
Figure GDA0002584691620000089
通过地球自转角速度和运动等效角速度算得，地球自转角速度
Figure GDA00025846916200000810
通过GPS位置信息转化为
Figure GDA00025846916200000811
与运动等效角速度
Figure GDA00025846916200000812
共同得到
Figure GDA00025846916200000813
通过四元数捷联姿态更新运算得到姿态信息。
基于四元数约束最小二乘的空中对准方法,是在通常惯性导航解算原理基础上,改变了信息传输流向，将位置、速度和姿态反馈截断，转而使用GPS的相关信息进行替代，使得初始姿态与导航某时刻的速度之间能够呈线性关系，为通过将速度信息作为观测量识别初始姿态奠定基础。
参见图2，本发明提供了基于优化的LiDAR运动补偿位置姿态系统对准方法，包括如下主要操作步骤：
选取惯性坐标系进行姿态和速度的捷联惯导解算公式的更新；
实时采集GPS数据，从中获取实时速度信息和位置信息，将通过GPS数据获取的所述速度信息和所述位置信息代入所述速度的捷联惯导解算公式中；
以纯惯性导航的速度轨迹与GPS速度轨迹在最小二乘原则下重合为条件，求解所述条件下的初始姿态，此求解得到的所述初始姿态即为最优初始姿态。
本发明的实施例基于优化的LiDAR运动补偿位置姿态系统对准方法，具体包括执行如下操作步骤：
步骤S1：选取惯性空间作为导航解算的参考坐标系空间，在惯性空间中的姿态差分方程为：
Figure GDA0002584691620000091
其中，i是惯性坐标系，b是载体坐标系，
Figure GDA0002584691620000092
是载体坐标系相对于惯性坐标系的姿态矩阵，
Figure GDA0002584691620000093
是载体坐标系下惯性测量单元敏感到的相对于惯性空间的转动角速度(即惯性测量单元敏感获得，得到的相对于惯性空间的转动角速度)，
Figure GDA0002584691620000094
是向量
Figure GDA0002584691620000095
的叉乘矩阵；
经姿态更新解算，
Figure GDA0002584691620000096
为如下形式：
Figure GDA0002584691620000097
其中，χ(0,t)是利用
Figure GDA0002584691620000098
更新解算得到的函数，更新方法采用四元数通用姿态更新方法，t是时间，
Figure GDA0002584691620000099
是载体坐标系相对于惯性坐标系的初始姿态；
同时实时采集GPS数据，从中获取实时速度信息和位置信息，并根据所述速度信息和所述位置信息计算得到
Figure GDA0002584691620000101
其中，是导航坐标系相对于惯性空间的姿态矩阵，
Figure GDA0002584691620000103
是在导航坐标系中表示的导航坐标系相对于地球坐标系的运动速度，
Figure GDA0002584691620000104
是在导航坐标系中表示的载体相对于地球运动产生的等效转速；
步骤S2：在所述惯性空间中的速度更新微分方程如下：
Figure GDA0002584691620000105
其中，
Figure GDA0002584691620000106
是在惯性空间中表示的导航坐标系相对于地球坐标系的运动速度，fi是在惯性空间中表示的惯性测量单元敏感的比力信息，
Figure GDA0002584691620000107
是在惯性空间中表示的地球自转角速度，
Figure GDA0002584691620000108
是在惯性空间中表示的载体相对于地球运动产生的等效转速，
Figure GDA0002584691620000109
是在惯性空间中表示的载体相对于地球的运动速度，gi是在惯性空间中表示的地球重力矢量；
步骤S3：将步骤S2的所述速度更新微分方程变换到惯性坐标系下，即按照所述惯性空间中的速度更新微分方程进行速度积分计算，从而得到改进后速度更新微分方程如下：
Figure GDA00025846916200001010
其中，
Figure GDA00025846916200001011
是导航坐标系相对于惯性空间的姿态矩阵，
Figure GDA00025846916200001012
是导航坐标系相对于惯性空间的姿态矩阵的逆矩阵
Figure GDA00025846916200001013
是在导航坐标系中表示的导航坐标系相对于地球坐标系的运动速度，
Figure GDA00025846916200001014
是在导航坐标系中表示的载体相对于地球运动产生的等效转速，
Figure GDA00025846916200001015
是在导航坐标系中表示的导航坐标系相对于地球坐标系的运动速度，gn是在导航坐标系中表示的地球重力矢量，fb是在载体坐标系中表示的比力矢量，公式4中
Figure GDA00025846916200001016
均以采集GPS数据得到的速度信息和位置信息进行运算；
步骤S4：将步骤S3的所述改进后速度更新微分方程进行处理四元数形式转换处理；即将所述改进后速度更新微分方程以
Figure GDA0002584691620000111
的形式进行转换表示：
Figure GDA0002584691620000112
其中，α(t)和β(t)表示如下：
Figure GDA0002584691620000113
Figure GDA0002584691620000114
(需要说明的是，通过上述公式4(即改进后速度更新微分方程)通过与公式5进行转换表示，便可以得到公式6(即公式6为有关β(t)的表示和公式7(公式7则为有关α(t)的表示))；
步骤S5：将步骤S4中所述α(t)、β(t)和
Figure GDA0002584691620000115
表示为四元数相乘的形式如下：
Figure GDA0002584691620000118
其中，
Figure GDA0002584691620000119
表示四元数相乘运算，q为四元数形式的
Figure GDA0002584691620000116
q*为q的共轭四元数，α(t)′为四元数形式的α(t)，β(t)′为四元数形式的β(t)；
步骤S6：定义四元数的两种运算如下：
Figure GDA0002584691620000117
其中，s为四元数q的标量部分，η为四元数q的矢量部分；
步骤S7：按照最小二乘原则求解步骤S5中的公式8，所求初始姿态四元数q符合以下最优指标：
Figure GDA0002584691620000121
步骤S8：从上述公式8中提取并定义K阵得到如下公式10:
Figure GDA0002584691620000122
步骤S9：求解步骤S8中公式10的K阵特征值和特征向量，其中K阵最小特征值所对应的特征向量四元数即代表最优初始姿态四元数，tf为对准过程结束的时间。
步骤S10：根据所述最优初始姿态四元数来计算优化初始姿态阵：
Figure GDA0002584691620000123
其中，Tij表示
Figure GDA0002584691620000124
阵的第i行第j列元素，i取值包括1、2、3，j取值包括1、2、3；qk表示最优姿态四元数的第k个分量，k取值包括0、1、2、3；
步骤S11：按照通用惯性导航方法，根据所述最优初始姿态四元数，求解航向角φ、俯仰角θ和横滚角γ。
Figure GDA0002584691620000125
θ＝sin-1(T23)
Figure GDA0002584691620000126
其中，航向角φ主值判断如下表
Figure GDA0002584691620000127
Figure GDA0002584691620000131
横滚角γ主值判断如下表
T<sub>33</sub> T<sub>31</sub> γ真值 γ所在象限
+ - γ 第一象限
- - γ+π 第二象限
- + γ+π 第三象限
+ + γ 第四象限
优选地，作为一种可实施方案；关于上述K阵(即算法K)需要另外说明的是，算法
Figure GDA0002584691620000132
在实际计算中需要离散化积分处理为
Figure GDA0002584691620000133
且数值积分计算时间间隔T为两次采集GPS数据之间的时间间隔，n为采样个数，tf为对准过程结束的时间。
优选地，作为一种可实施方案；将初始时刻的地球坐标系定义为惯性坐标系，
Figure GDA0002584691620000134
按照下式计算：
Figure GDA0002584691620000135
其中，
Figure GDA0002584691620000141
为导航坐标系相对于惯性坐标系的转化矩阵，
Figure GDA0002584691620000142
为地球坐标系相对于惯性坐标系的转化矩阵，
Figure GDA0002584691620000143
为导航坐标系相对于地球坐标系的转化矩阵，
Figure GDA0002584691620000144
为地球自转角速度矢量在导航坐标系中的投影，λ(t)为载体所在位置的经度，L(t)为载体所在位置的纬度。
参见图3，综上关于上述应用算法可以通过如下步骤进行总结说明：
选取惯性坐标系进行姿态和速度的捷联惯导解算公式的更新；
实时采集GPS数据，从中获取实时速度信息和位置信息；
惯性坐标系下速度积分公式进行积分运算；
积分项四元数形式转换；
数值积分运算求指标矩阵最优指标；
求最优指标矩阵的特征值和特征向量；
将最小特征值对应的特征向量作为最优初始姿态；
最终从速度更新公式中将初始姿态项提取出来，并将所有项转化为四元数形式，通过求四元数矩阵最小特征值对应的特征向量的方法求得最优初始姿态(具体形式为最优初始姿态四元数)。
综上所述，本发明实施例提供的基于优化的LiDAR运动补偿位置姿态系统对准方法，建立一个惯性测量单元敏感信息和GPS量测信息的数学模型，该数学模型能够利用惯性测量单元敏感的运动信息和GPS测量的位置信息、速度信息进行纯惯性导航解算，并在导航解算中将初始姿态作为一个独立待求量，利用优化方法解算得到某时间点的最优初始姿态信息，使得IMU在该初始姿态下进行纯惯性导航的速度轨迹与GPS速度轨迹之间方差最小。本发明具有对准精度高、速度快、鲁棒性强的优点，可用于惯性导航坐标系统或惯性测量系统进行空中对准。
本发明实施例提供的基于优化的LiDAR运动补偿位置姿态系统对准方法，其抗扰能力强，克服了现有方法大失准角下容易滤波发散的问题，大大提高了算法的鲁棒性和精度，显然本发明实施例其设计的上述应用方案具有显著的技术进步。
对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。
此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。

Claims (4)

1.基于优化的LiDAR运动补偿位置姿态系统对准方法，其特征在于：包括以下步骤：
选取惯性坐标系进行姿态和速度的捷联惯导解算公式的更新；
实时采集GPS数据，从中获取实时速度信息和位置信息，将通过GPS数据获取的所述速度信息和所述位置信息代入所述速度的捷联惯导解算公式中；
以纯惯性导航的速度轨迹与GPS速度轨迹在最小二乘原则下重合为条件，求解所述条件下的初始姿态，此求解得到的所述初始姿态即为最优初始姿态；
具体包括执行如下操作步骤：
步骤S1：选取惯性空间作为导航解算的参考坐标系空间，在惯性空间中的姿态差分方程为：
Figure FDA0002584691610000011
其中，i是惯性坐标系，b是载体坐标系，
Figure FDA0002584691610000012
是载体坐标系相对于惯性坐标系的姿态矩阵，
Figure FDA0002584691610000013
是载体坐标系下惯性测量单元敏感到的相对于惯性空间的转动角速度，
Figure FDA0002584691610000014
是向量
Figure FDA0002584691610000015
的叉乘矩阵；
经姿态更新解算，
Figure FDA0002584691610000016
为如下形式：
Figure FDA0002584691610000017
其中，x(0,t)是利用
Figure FDA0002584691610000018
更新解算得到的函数，更新方法采用四元数通用姿态更新方法，t是时间，
Figure FDA0002584691610000021
是载体坐标系相对于惯性坐标系的初始姿态；
同时实时采集GPS数据，从中获取实时速度信息和位置信息，并根据所述速度信息和所述位置信息计算得到
Figure FDA0002584691610000022
其中，
Figure FDA0002584691610000023
是导航坐标系相对于惯性空间的姿态矩阵，
Figure FDA0002584691610000024
是在导航坐标系中表示的导航坐标系相对于地球坐标系的运动速度，
Figure FDA0002584691610000025
是在导航坐标系中表示的载体相对于地球运动产生的等效转速；
步骤S2：在所述惯性空间中的速度更新微分方程如下：
Figure FDA0002584691610000026
其中，
Figure FDA0002584691610000027
是在惯性空间中表示的导航坐标系相对于地球坐标系的运动速度，fi是在惯性空间中表示的惯性测量单元敏感的比力信息，
Figure FDA0002584691610000028
是在惯性空间中表示的地球自转角速度，
Figure FDA0002584691610000029
是在惯性空间中表示的载体相对于地球运动产生的等效转速，
Figure FDA00025846916100000210
是在惯性空间中表示的载体相对于地球的运动速度，gi是在惯性空间中表示的地球重力矢量；
步骤S3：将步骤S2的所述速度更新微分方程变换到惯性坐标系下，即按照所述惯性空间中的速度更新微分方程进行速度积分计算，从而得到改进后速度更新微分方程如下：
Figure FDA00025846916100000211
其中，
Figure FDA00025846916100000212
是导航坐标系相对于惯性空间的姿态矩阵，
Figure FDA00025846916100000213
是导航坐标系相对于惯性空间的姿态矩阵的逆矩阵
Figure FDA00025846916100000214
Figure FDA00025846916100000215
是在导航坐标系中表示的载体相对于地球运动产生的等效转速，
Figure FDA00025846916100000216
是在导航坐标系中表示的导航坐标系相对于地球坐标系的运动速度，gn是在导航坐标系中表示的地球重力矢量，fb是在载体坐标系中表示的比力矢量，公式4中
Figure FDA00025846916100000217
均以采集GPS数据得到的速度信息和位置信息进行运算；
步骤S4：将步骤S3的所述改进后速度更新微分方程进行处理四元数形式转换处理；即将所述改进后速度更新微分方程以
Figure FDA0002584691610000031
的形式进行转换表示：
Figure FDA0002584691610000032
其中，α(t)和β(t)表示如下：
Figure FDA0002584691610000033
Figure FDA0002584691610000034
步骤S5：将步骤S4中所述α(t)、β(t)和
Figure FDA0002584691610000035
表示为四元数相乘的形式如下：
Figure FDA0002584691610000036
其中，
Figure FDA0002584691610000037
表示四元数相乘运算，q为四元数形式的
Figure FDA0002584691610000038
q*为q的共轭四元数，α(t)'为四元数形式的α(t)，β(t)'为四元数形式的β(t)；
步骤S6：定义四元数的两种运算如下：
Figure FDA0002584691610000039
其中，s为四元数q的标量部分，η为四元数q的矢量部分；
步骤S7：按照最小二乘原则求解步骤S5中的公式8，所求初始姿态四元数q符合以下最优指标：
Figure FDA0002584691610000041
步骤S8：从上述公式9中提取并定义K阵得到如下公式10:
Figure FDA0002584691610000042
步骤S9：求解步骤S8中公式10的K阵特征值和特征向量，其中K阵最小特征值所对应的特征向量四元数即代表最优初始姿态四元数；tf为对准过程结束的时间。
2.根据权利要求1所述的基于优化的LiDAR运动补偿位置姿态系统对准方法，其特征在于，
在步骤S9之后还包括如下操作步骤：
步骤S10：根据所述最优初始姿态四元数来计算优化初始姿态阵：
Figure FDA0002584691610000043
其中，Tij表示
Figure FDA0002584691610000044
阵的第i行第j列元素，i取值包括1、2、3，j取值包括1、2、3；qk表示最优姿态四元数的第k个分量，k取值包括0、1、2、3；
步骤S11：按照通用惯性导航方法，根据所述最优初始姿态四元数，求解航向角φ、俯仰角θ和横滚角γ。
3.根据权利要求2所述的基于优化的LiDAR运动补偿位置姿态系统对准方法，其特征在于:算法
Figure FDA0002584691610000045
在实际计算中需要离散化积分处理为
Figure FDA0002584691610000051
且数值积分计算时间间隔T为两次采集GPS数据之间的时间间隔，n为采样个数，tf为对准过程结束的时间。
4.根据权利要求3所述的基于优化的LiDAR运动补偿位置姿态系统对准方法，其特征在于:将初始时刻的地球坐标系定义为惯性坐标系，
Figure FDA0002584691610000052
按照下式计算：
Figure FDA0002584691610000053
其中，
Figure FDA0002584691610000054
为导航坐标系相对于惯性坐标系的转化矩阵，
Figure FDA0002584691610000055
为地球坐标系相对于惯性坐标系的转化矩阵，
Figure FDA0002584691610000056
为导航坐标系相对于地球坐标系的转化矩阵，
Figure FDA0002584691610000057
为地球自转角速度矢量在导航坐标系中的投影，λ(t)为载体所在位置的经度，L(t)为载体所在位置的纬度。
CN202010422534.9A 2020-05-19 2020-05-19 基于优化的LiDAR运动补偿位置姿态系统对准方法 Active CN111337056B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010422534.9A CN111337056B (zh) 2020-05-19 2020-05-19 基于优化的LiDAR运动补偿位置姿态系统对准方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010422534.9A CN111337056B (zh) 2020-05-19 2020-05-19 基于优化的LiDAR运动补偿位置姿态系统对准方法

Publications (2)

Publication Number Publication Date
CN111337056A CN111337056A (zh) 2020-06-26
CN111337056B true CN111337056B (zh) 2020-08-25

Family

ID=71184978

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010422534.9A Active CN111337056B (zh) 2020-05-19 2020-05-19 基于优化的LiDAR运动补偿位置姿态系统对准方法

Country Status (1)

Country Link
CN (1) CN111337056B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112987054B (zh) * 2021-02-24 2023-03-03 博雅工道（北京）机器人科技有限公司 标定sins/dvl组合导航系统误差的方法和装置
CN113124901B (zh) * 2021-04-01 2022-03-11 中铁第四勘察设计院集团有限公司 位置校正方法及装置、电子设备及存储介质

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104457446A (zh) * 2014-11-28 2015-03-25 北京航天控制仪器研究所 一种自旋制导炮弹的空中自对准方法
CN108709552A (zh) * 2018-04-13 2018-10-26 哈尔滨工业大学 一种基于mems的imu和gps紧组合导航方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103557871B (zh) * 2013-10-22 2016-06-15 北京航空航天大学 一种浮空飞行器捷联惯导空中初始对准方法
CN104820758A (zh) * 2015-05-20 2015-08-05 江苏华豪航海电器有限公司 一种基于奇异值分解的传递对准精度评估可观测性分析方法
CN105115508B (zh) * 2015-08-27 2017-12-22 北京航天控制仪器研究所 基于后数据的旋转制导炮弹快速空中对准方法
WO2019122987A1 (en) * 2017-12-22 2019-06-27 Shakibay Senobari Mohammad Accurate initialization of strapdown inertial navigation system
CN110398257B (zh) * 2019-07-17 2022-08-02 哈尔滨工程大学 Gps辅助的sins系统快速动基座初始对准方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104457446A (zh) * 2014-11-28 2015-03-25 北京航天控制仪器研究所 一种自旋制导炮弹的空中自对准方法
CN108709552A (zh) * 2018-04-13 2018-10-26 哈尔滨工业大学 一种基于mems的imu和gps紧组合导航方法

Also Published As

Publication number Publication date
CN111337056A (zh) 2020-06-26

Similar Documents

Publication Publication Date Title
CN111426318B (zh) 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法
CN111947652B (zh) 一种适用于月球着陆器的惯性/视觉/天文/激光测距组合导航方法
CN107655476B (zh) 基于多信息融合补偿的行人高精度足部导航方法
CN110030994B (zh) 一种基于单目的鲁棒性视觉惯性紧耦合定位方法
CN107289930B (zh) 基于mems惯性测量单元的纯惯性车辆导航方法
CN112505737B (zh) 一种gnss/ins组合导航方法
CN103363992B (zh) 基于梯度下降的四旋翼无人机姿态航向参考系统解算方法
CN108225308A (zh) 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法
Stančić et al. The integration of strap-down INS and GPS based on adaptive error damping
CN108458709B (zh) 基于视觉辅助测量的机载分布式pos数据融合方法和装置
Hansen et al. Nonlinear observer design for GNSS-aided inertial navigation systems with time-delayed GNSS measurements
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN108168548B (zh) 一种通过机器学习算法与模型辅助的行人惯性导航系统和方法
CN110243377B (zh) 一种基于分层式结构的集群飞行器协同导航方法
CN103674021A (zh) 基于捷联惯导与星敏感器的组合导航系统及方法
CN111337056B (zh) 基于优化的LiDAR运动补偿位置姿态系统对准方法
CN111189442B (zh) 基于cepf的无人机多源导航信息状态预测方法
CN101603833A (zh) 稳瞄吊舱的比力差积分匹配传递对准及其组合导航方法
CN112902956A (zh) 一种手持式gnss/mems-ins接收机航向初值获取方法、电子设备、存储介质
Liu et al. Integrated navigation on vehicle based on low-cost SINS/GNSS using deep learning
CN112197765B (zh) 一种实现水下机器人精细导航的方法
CN113008229A (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
Hong et al. In-flight alignment of SDINS under large initial heading error
Wei et al. An improved in-flight alignment method based on backtracking navigation for GPS-aided low cost SINS with short endurance
CN107702718A (zh) 一种基于瞬间可观测度模型的机载pos机动优化方法与装置

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
CP01 Change in the name or title of a patent holder
CP01 Change in the name or title of a patent holder

Address after: Room 2301-2308, third floor, building 2, incubator, Zhongguancun Software Park, Dongbeiwang, Haidian District, Beijing 100094

Patentee after: Beijing Digital Green Earth Technology Co.,Ltd.

Address before: Room 2301-2308, third floor, building 2, incubator, Zhongguancun Software Park, Dongbeiwang, Haidian District, Beijing 100094

Patentee before: BEIJING GREENVALLEY TECHNOLOGY Co.,Ltd.