CN110006456B - 一种检测车对准方法、装置和设备 - Google Patents

一种检测车对准方法、装置和设备 Download PDF

Info

Publication number
CN110006456B
CN110006456B CN201910333072.0A CN201910333072A CN110006456B CN 110006456 B CN110006456 B CN 110006456B CN 201910333072 A CN201910333072 A CN 201910333072A CN 110006456 B CN110006456 B CN 110006456B
Authority
CN
China
Prior art keywords
angular velocity
coordinate system
vehicle
detection vehicle
attitude
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
CN201910333072.0A
Other languages
English (en)
Other versions
CN110006456A (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 Starneto Technology Corp ltd
Original Assignee
Beijing Starneto Technology Corp 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 Starneto Technology Corp ltd filed Critical Beijing Starneto Technology Corp ltd
Priority to CN201910333072.0A priority Critical patent/CN110006456B/zh
Publication of CN110006456A publication Critical patent/CN110006456A/zh
Application granted granted Critical
Publication of CN110006456B publication Critical patent/CN110006456B/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)

Abstract

本申请提供了一种检测车对准方法、装置和设备，该方法包括：根据检测车的位置信息及预设信息，确定第一角速度、第二角速度、第三角速度，其中，第一角速度为检测车的载体坐标系相对于惯性坐标系的角速度，第二角速度为地球的自转角速度在惯性坐标系中的投影角速度，第三角速度为检测车的地理坐标系相对于地球坐标系的角速度；将所述第一角速度、所述第二角速度、所述第三角速度作为预设的四元数姿态角确定模型输入，确定所述检测车的姿态角；输出所述检测车的姿态角。

Description

一种检测车对准方法、装置和设备
技术领域
本申请涉及数据处理技术领域，具体而言，涉及一种检测车对准方法、装置和设备。
背景技术
基于惯性导航的轨道检测车可以提供高精度、快速、连续的轨道参数测量。惯性导航初始对准的精度是决定轨检车位置姿态系统精度的重要因素，对于地面静基座对准，粗对准和精对准是对准的两个主要阶段。但粗对准受惯性器件精度限制，且以静态为前提，实际应用中对准精度极低；而基于卡尔曼滤波精对准虽然目标是提高对准精度，但所需时间长，鲁棒性差。
发明内容
有鉴于此，本申请的目的在于提供一种检测车对准方法、装置和设备，用于解决现有技术中对检测车对准效率低的问题。
第一方面，本申请实施例提供了一种检测车对准方法，该方法包括：
根据检测车的位置信息及预设信息，确定第一角速度、第二角速度、第三角速度，其中，第一角速度为检测车的载体坐标系相对于惯性坐标系的角速度，第二角速度为地球的自转角速度在惯性坐标系中的投影角速度，第三角速度为检测车的地理坐标系相对于地球坐标系的角速度；
将所述第一角速度、所述第二角速度、所述第三角速度作为预设的姿态角确定模型输入，确定所述检测车的姿态角；
输出所述检测车的姿态角。
可选地，根据检测车的位置信息及预设信息，确定第二角速度，包括：
通过以下公式计算第二角速度
Figure GDA0002952974280000021
Figure GDA0002952974280000022
其中，
Figure GDA0002952974280000023
为地球的自转角速度在惯性坐标系中的投影角速度，L为检测车的纬度，ωie为地球的自转角速度。
可选地，根据检测车的位置信息及预设信息，确定第三角速度，包括通过以下公式计算第三角速度
Figure GDA0002952974280000024
Figure GDA0002952974280000025
其中，
Figure GDA0002952974280000026
为检测车的地理坐标系相对于地球坐标系的角速度，VE为检测车在地理坐标系中的东向速度，VN为检测车在地理坐标系中的北向速度，RM为沿子午圈的主曲率半径，RN为卯酉圈的主曲率半径，H为检测车的高度，L为检测车的纬度。
可选地，所述将所述第一角速度、所述第二角速度、所述第三角速度作为预设的四元数姿态角确定模型输入，确定所述检测车的姿态角，包括：
通过以下公式计算所述检测车的姿态角：
Figure GDA0002952974280000031
Figure GDA0002952974280000032
L(q)＝qTKq-λ(qTq-1)
(K+λI)q＝0
Figure GDA0002952974280000033
Figure GDA0002952974280000034
Figure GDA0002952974280000035
Figure GDA0002952974280000036
其中，
Figure GDA0002952974280000037
Figure GDA0002952974280000038
q＝q0+q1i+q2j+q3k
Figure GDA0002952974280000039
Figure GDA00029529742800000310
其中，
Figure GDA00029529742800000311
为初始姿态转换矩阵，Cn b为导航坐标系到载体坐标系的姿态转换矩阵，β(t)为第一中间变量，α(t)为第二中间变量，χn(0,t)为检测车的地理坐标系相对于地球坐标系的运动速度
Figure GDA00029529742800000312
的函数；vn为检测车在地理坐标系的速度；
Figure GDA00029529742800000313
为地球的自转角速度在惯性坐标系中的投影角速度；
Figure GDA0002952974280000041
为检测车的地理坐标系相对于地球坐标系的角速度；gn为重力加速度在地理坐标系下投影，其中，gn＝[0 0 -g]T；χb(0,t)为第一角速度
Figure GDA0002952974280000042
的函数；
Figure GDA0002952974280000043
为χn(0,t)的转置，fb为检测车的比力矢量，q为姿态四元数；qT为姿态四元数的转置，L(q)为导航坐标系中加速度与零加速度差值的均方差统计值，
Figure GDA0002952974280000044
为第三中间变量，
Figure GDA0002952974280000045
为第四中间变量，K为为有关于
Figure GDA0002952974280000046
和
Figure GDA0002952974280000047
的矩阵，λ为矩阵K的特征值，q0、q1、q2、q3分别为构成四元数q的实数，b为载体坐标系，n为导航坐标系，θ为检测车的姿态角中的俯仰角，r为检测车的姿态角中的横滚角，
Figure GDA0002952974280000048
为检测车的姿态角中的航向角。
可选地，该方法还包括：
对所述检测车的姿态角进行校准。
第二方面，本申请实施例提供了一种检测车对准装置，该装置包括：
第一确定模块，用于根据检测车的位置信息及预设信息，确定第一角速度、第二角速度、第三角速度，其中，第一角速度为检测车的载体坐标系相对于惯性坐标系的角速度，第二角速度为地球的自转角速度在惯性坐标系中的投影角速度，第三角速度为检测车的地理坐标系相对于地球坐标系的角速度；
第二确定模块，用于将所述第一角速度、所述第二角速度、所述第三角速度作为预设的四元数姿态角确定模型输入，确定所述检测车的姿态角；
输出模块，用于输出所述检测车的姿态角。
可选地，所述第一确定模块具体用于：
通过以下公式计算第二角速度
Figure GDA0002952974280000051
Figure GDA0002952974280000052
其中，
Figure GDA0002952974280000053
为地球的自转角速度在惯性坐标系中的投影角速度，L为检测车的纬度，ωie为地球的自转角速度。
可选地，所述第一确定模块具体用于：
通过以下公式计算第三角速度
Figure GDA0002952974280000054
Figure GDA0002952974280000055
其中，
Figure GDA0002952974280000056
为检测车的地理坐标系相对于地球坐标系的角速度，VE为检测车在地理坐标系中的东向速度，VN为检测车在地理坐标系中的北向速度，RM为沿子午圈的主曲率半径，RN为卯酉圈的主曲率半径，H为检测车的高度，L为检测车的纬度。
可选地，所述第二确定模块具体用于：
通过以下公式计算所述检测车的姿态角：
Figure GDA0002952974280000057
Figure GDA0002952974280000058
L(q)＝qTKq-λ(qTq-1)
(K+λI)q＝0
Figure GDA0002952974280000061
Figure GDA0002952974280000062
Figure GDA0002952974280000063
Figure GDA0002952974280000064
其中，
Figure GDA0002952974280000065
Figure GDA0002952974280000066
q＝q0+q1i+q2j+q3k
Figure GDA0002952974280000067
Figure GDA0002952974280000068
其中，
Figure GDA0002952974280000069
为初始姿态转换矩阵，Cn b为导航坐标系到载体坐标系的姿态转换矩阵，β(t)为第一中间变量，α(t)为第二中间变量，χn(0,t)为检测车的地理坐标系相对于地球坐标系的运动速度
Figure GDA00029529742800000610
的函数；vn为检测车在地理坐标系的速度；
Figure GDA00029529742800000611
为地球的自转角速度在惯性坐标系中的投影角速度；
Figure GDA00029529742800000612
为检测车的地理坐标系相对于地球坐标系的角速度；gn为重力加速度在地理坐标系下投影，其中，gn＝[0 0 -g]T；χb(0,t)为第一角速度
Figure GDA00029529742800000613
的函数；
Figure GDA00029529742800000614
为χn(0,t)的转置，fb为检测车的比力矢量，q为姿态四元数；qT为姿态四元数的转置，L(q)为导航坐标系中加速度与零加速度差值的均方差统计值，
Figure GDA00029529742800000615
为第三中间变量，
Figure GDA00029529742800000616
为第四中间变量，K为有关于
Figure GDA0002952974280000071
和
Figure GDA0002952974280000072
的矩阵，λ为矩阵K的特征值，q0、q1、q2、q3分别为构成四元数q的实数，b为载体坐标系，n为导航坐标系，θ为检测车的姿态角中的俯仰角，r为检测车的姿态角中的横滚角，
Figure GDA0002952974280000073
为检测车的姿态角中的航向角。
第三方面，本申请实施例提供了一种计算机设备包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述方法的步骤。
本申请实施例提供的一种检测车对准方法，根据检测车的位置信息及预设信息，确定第一角速度、第二角速度、第三角速度，其中，第一角速度为检测车的载体坐标系相对于惯性坐标系的角速度，第二角速度为地球的自转角速度在惯性坐标系中的投影角速度，第三角速度为检测车的地理坐标系相对于地球坐标系的角速度；将所述第一角速度、所述第二角速度、所述第三角速度作为预设的四元数姿态角确定模型输入，确定所述检测车的姿态角。本申请通过基于四元数的姿态角确定模型计算检测车的姿态角，具有强鲁棒性、快速性和高精度的优点，提高了对准精度，进而提高了轨检车姿态解算精度。
为使本申请的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。
附图说明
为了更清楚地说明本申请实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
图1为本申请实施例提供的一种检测车对准方法的流程示意图；
图2为本申请实施例提供的一种检测车对准装置的第一种结构示意图；
图3为本申请实施例提供的一种检测车对准装置的第二种结构示意图；
图4为本申请实施例提供的一种计算机设备400的结构示意图。
具体实施方式
为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。
本申请实施例提供了一种检测车对准方法，如图1所示，该方法包括以下步骤：
S110，根据检测车的位置信息及预设信息，确定第一角速度、第二角速度、第三角速度，其中，第一角速度为检测车的载体坐标系相对于惯性坐标系的角速度，第二角速度为地球的自转角速度在惯性坐标系中的投影角速度，第三角速度为检测车的地理坐标系相对于地球坐标系的角速度；
这里，检测车一般为轨道检测车，位置信息包括检测车所处的纬度、高度、检测车在东向的速度VE、检测车在北向的速度VN、检测车在天向的速度VU、预设信息包括地球子午圈的主曲率半径RM、地球卯酉圈的主曲率半径RN，地球自转角速度ωie、地球椭球长半轴Re＝6378137m、地球椭球长半轴Re＝6378137m、地球引力矢量Gn、地球半径Rn、重力加速度gn等，其中，重力加速度在导航系下投影为gn＝[0 0 -g]T。
本申请在位置姿态系统测量过程中，通常选取本地地理系作为导航坐标系。
在具体实施中，第一角速度为检测车相对于惯性坐标系的角速度
Figure GDA0002952974280000091
一般由惯性导航系统直接量测获得。
通过以下公式计算第二角速度
Figure GDA0002952974280000092
Figure GDA0002952974280000093
其中，
Figure GDA0002952974280000094
为地球的自转角速度在惯性坐标系中的投影角速度，L为检测车的纬度，ωie为地球的自转角速度。
通过以下公式计算第三角速度
Figure GDA0002952974280000095
Figure GDA0002952974280000096
其中，
Figure GDA0002952974280000097
为检测车的地理坐标系相对于地球坐标系的角速度，VE为检测车在地理坐标系中的东向速度，VN为检测车在地理坐标系中的北向速度，RM为地球子午圈的主曲率半径，RN为地球卯酉圈的主曲率半径，H为检测车的高度，该高度一般为检测车作业时所处的当地的高度，L为检测车的纬度，该纬度一般为作业时所处的当地的纬度。
S120，将所述第一角速度、所述第二角速度、所述第三角速度作为预设的四元数姿态角确定模型输入，确定所述检测车的姿态角，这里确定的姿态角一般为检测车的初始姿态角；
这里，检测车的姿态角包括检测车的俯仰角θ、横滚角r、航向角
Figure GDA0002952974280000101
通过以下公式计算所述检测车的姿态角：
Figure GDA0002952974280000102
Figure GDA0002952974280000103
L(q)＝qTKq-λ(qTq-1)
(K+λI)q＝0
Figure GDA0002952974280000104
Figure GDA0002952974280000105
Figure GDA0002952974280000106
Figure GDA0002952974280000107
其中，
Figure GDA0002952974280000108
Figure GDA0002952974280000109
q＝q0+q1i+q2j+q3k
其中，
Figure GDA00029529742800001010
为初始姿态转换矩阵，Cn b为导航坐标系到载体坐标系的姿态转换矩阵，β(t)为第一中间变量，α(t)为第二中间变量，χn(0,t)为检测车的地理坐标系相对于地球坐标系的运动速度
Figure GDA00029529742800001011
的函数；vn为检测车在地理坐标系的速度；
Figure GDA0002952974280000111
为地球的自转角速度在惯性坐标系中的投影角速度；
Figure GDA0002952974280000112
为检测车的地理坐标系相对于地球坐标系的角速度；gn为重力加速度在地理坐标系下投影，其中，gn＝[0 0 -g]T；χb(0,t)为第一角速度
Figure GDA0002952974280000113
的函数；
Figure GDA0002952974280000114
为χn(0,t)的转置，fb为检测车的比力矢量，q为姿态四元数；qT为姿态四元数的转置，L(q)为导航坐标系中加速度与零加速度差值的均方差统计值，
Figure GDA0002952974280000115
为第三中间变量，
Figure GDA0002952974280000116
为第四中间变量，K为有关于
Figure GDA0002952974280000117
和
Figure GDA0002952974280000118
的矩阵，λ为矩阵K的特征值，q0、q1、q2、q3分别为构成四元数q的实数，b为载体坐标系，n为导航坐标系，θ为检测车的姿态角中的俯仰角，r为检测车的姿态角中的横滚角，
Figure GDA0002952974280000119
为检测车的姿态角中的航向角。
以下为计算检测车的姿态角的计算公式的推导原理：
RM＝Re(1-2e+3e sin2L)
RN＝Re(1+e sin2L)
其中，RM为地球子午圈的主曲率半径；RN为地球卯酉圈的主曲率半径；e为地球扁率，e＝1/298.257；Re为地球椭球长半轴，Re＝6378137m；
基础表达式为：
Figure GDA00029529742800001110
其中，β(t)和α(t)分别表达如下：
Figure GDA00029529742800001111
Figure GDA00029529742800001112
其中，χb(0,t)为
Figure GDA0002952974280000121
的函数，χn(0,t)为
Figure GDA0002952974280000122
的函数，vn为导航坐标系的速度，静态对准中该速度为0；
Figure GDA0002952974280000123
表示导航坐标系系相对于地球坐标系的运动速度，静态对准过程中认为该值为地球自转角速度；fb表示直接量测获得的比力矢量，一般是由物体三轴姿态角(角速率、加速度)测量装置(Inertial measurement unit，IMU)直接量测获得的。
为了便于计算，将公式1改为四元数运算的形式为：
Figure GDA0002952974280000124
其中，q为姿态四元数，q*为q的转置。
姿态四元数q满足：
Figure GDA0002952974280000125
其中，
Figure GDA0002952974280000126
为第三中间变量，
Figure GDA0002952974280000127
为第四中间变量；
其中，K阵定义如下：
Figure GDA0002952974280000128
基于公式6对公式(5)处理后得到的公式如下：
Figure GDA0002952974280000129
同时，q满足：qTq＝1(8)
为了将上述条件(公式8)融入运算中，使解算方便，应用拉格朗日乘子法定义L(q)如下：
L(q)＝qTKq-λ(qTq-1)(9)
其中，L(q)为导航中加速度与零加速度差值的均方差统计值，λ为K矩阵的特征值。
因此，当L(q)取得最小值时，应符合如下条件
Figure GDA0002952974280000131
其中，I为单位矩阵。
Figure GDA0002952974280000132
由于式(11)是四元数自然属性，只需将式(10)重新整理可得：
(K+λI)q＝0(12)
L(q)表示导航中加速度与零加速度差值的均方差统计值，而K阵的最小特征值λmin，即表示L(q)所能达到的最小值。因此，当q取对应于K阵最小特征值λmin的特征向量
Figure GDA0002952974280000133
时，L(q)取得最小值λmin，此时，
Figure GDA0002952974280000134
即为所求姿态四元数。
姿态转换矩阵可以表示为：
Figure GDA0002952974280000135
式中：q为上述
Figure GDA0002952974280000136
其中，q＝q0+q1i+q2j+q3k，
Figure GDA0002952974280000137
继而根据公式13求得初始姿态角俯仰角θ、横滚角r、航向角
Figure GDA0002952974280000138
为如下表达式。
Figure GDA0002952974280000139
Figure GDA00029529742800001310
Figure GDA0002952974280000141
其中，θ为检测车的俯仰角、r为检测车的横滚角、
Figure GDA0002952974280000142
为检测车的航向角。
另外，公式1的推导过程如下：
在传统捷联解算算法中，通常选取地理系作为导航系。在该系中，IMU姿态微分方程如下：
Figure GDA0002952974280000143
其中，
Figure GDA0002952974280000144
表示由
Figure GDA0002952974280000145
更新的姿态矩阵，
Figure GDA0002952974280000146
表示载体坐标系相对于导航坐标系的角速度，
Figure GDA0002952974280000147
表示载体坐标系相对于惯性坐标系的角速度，该量由IMU直接量测获得；
Figure GDA0002952974280000148
表示地球自转角速度在惯性系中的投影，
Figure GDA0002952974280000149
表示导航系相对于地球系的转动角速度。
导航坐标系下的速度微分方程如下：
Figure GDA00029529742800001410
其中，
Figure GDA00029529742800001411
为导航坐标系下的速度。
由于
Figure GDA00029529742800001412
和
Figure GDA00029529742800001413
可以表示为：
Figure GDA00029529742800001414
Figure GDA00029529742800001415
其中，χb(0,t)为
Figure GDA00029529742800001416
的函数，χn(0,t)为
Figure GDA00029529742800001417
的函数。
因此，
Figure GDA00029529742800001418
可以表示为：
Figure GDA00029529742800001419
将上式代入速度微分方程，可得公式1：
Figure GDA0002952974280000151
公式5的推导原理如下：
公式(4)中姿态四元数定义为q＝[s η]，由标量部分s和矢量部分η组成，与
Figure GDA0002952974280000152
关系表示如下：
Figure GDA0002952974280000153
根据四元数相乘法则如下：
Figure GDA0002952974280000154
其中，s2为四元数q2的标量部分，η2为四元数q2的矢量部分，s1为四元数q1的标量部分，η1为四元数q1的矢量部分。
其中，
Figure GDA0002952974280000155
和
Figure GDA0002952974280000156
定义为
Figure GDA0002952974280000157
公式23可以转化为：
Figure GDA0002952974280000158
继而转化为：
Figure GDA0002952974280000159
姿态四元数q满足：
Figure GDA00029529742800001510
上述算法可以实现对准，但是，观察β(t)表达式，由于每时刻真实
Figure GDA0002952974280000161
不可知，故算法假设静基座对准下
Figure GDA0002952974280000162
始终为零。
S130，输出所述检测车的姿态角(初始姿态角)。
在输出检测车的姿态角之前还包括：
对所述检测车的姿态角(初始姿态角)进行校准。
在具体实施中，通过反馈校验对检测车的姿态角(初始姿态角)进行校准，现有技术中已有详细的介绍，此处不再进行过多说明。
本申请实施例提供的一种检测车对准方法，根据检测车的位置信息及预设信息，确定第一角速度、第二角速度、第三角速度，其中，第一角速度为检测车的载体坐标系相对于惯性坐标系的角速度，第二角速度为地球的自转角速度在惯性坐标系中的投影角速度，第三角速度为检测车的地理坐标系相对于地球坐标系的角速度；将所述第一角速度、所述第二角速度、所述第三角速度作为预设的四元数姿态角确定模型输入，确定所述检测车的姿态角。本申请通过基于四元数的姿态角确定模型计算检测车的姿态角，具有强鲁棒性、快速性和高精度的优点，提高了对准精度，进而提高了轨检车姿态解算精度。
本申请实施例提供了一种检测车对准装置，如图2所示。该装置包括：
第一确定模块21，用于用于根据检测车的位置信息及预设信息，确定第一角速度、第二角速度、第三角速度，其中，第一角速度为检测车的载体坐标系相对于惯性坐标系的角速度，第二角速度为地球的自转角速度在惯性坐标系中的投影角速度，第三角速度为检测车的地理坐标系相对于地球坐标系的角速度；
第二确定模块22，用于将所述第一角速度、所述第二角速度、所述第三角速度作为预设的四元数姿态角确定模型输入，确定所述检测车的姿态角；
输出模块23，用于输出所述检测车的姿态角。
可选地，所述第一确定模块具体用于：
通过以下公式计算第二角速度
Figure GDA0002952974280000171
Figure GDA0002952974280000172
其中，
Figure GDA0002952974280000173
为地球的自转角速度在惯性坐标系中的投影角速度，L为检测车的纬度，ωie为地球的自转角速度。
可选地，所述第一确定模块具体用于：
通过以下公式计算第三角速度
Figure GDA0002952974280000174
Figure GDA0002952974280000175
其中，
Figure GDA0002952974280000176
为检测车的地理坐标系相对于地球坐标系的角速度，VE为检测车在地理坐标系中的东向速度，VN为检测车在地理坐标系中的北向速度，RM为沿子午圈的主曲率半径，RN为卯酉圈的主曲率半径，H为检测车的高度，该高度一般为检测车作业时所处的当地的高度，L为检测车作的纬度，该纬度一般为检测车作业时所处的当地的纬度。
可选地，所述第二确定模块22具体用于：
通过以下公式计算所述检测车的姿态角：
Figure GDA0002952974280000181
Figure GDA0002952974280000182
L(q)＝qTKq-λ(qTq-1)
(K+λI)q＝0
Figure GDA0002952974280000183
Figure GDA0002952974280000184
Figure GDA0002952974280000185
Figure GDA0002952974280000186
其中，
Figure GDA0002952974280000187
Figure GDA0002952974280000188
q＝q0+q1i+q2j+q3k
其中，
Figure GDA0002952974280000189
为初始姿态转换矩阵，Cn b为导航坐标系到载体坐标系的姿态转换矩阵，β(t)为第一中间变量，α(t)为第二中间变量，χn(0,t)为检测车的地理坐标系相对于地球坐标系的运动速度
Figure GDA00029529742800001810
的函数；vn为检测车在地理坐标系的速度；
Figure GDA00029529742800001811
为地球的自转角速度在惯性坐标系中的投影角速度；
Figure GDA00029529742800001812
为检测车的地理坐标系相对于地球坐标系的角速度；gn为重力加速度在地理坐标系下投影，其中，gn＝[0 0 -g]T；χb(0,t)为第一角速度
Figure GDA00029529742800001813
的函数；fb为检测车的比力矢量，q为姿态四元数；qT为姿态四元数的转置，L(q)为导航坐标系中加速度与零加速度差值的均方差统计值，K为关于
Figure GDA0002952974280000191
和
Figure GDA0002952974280000192
的矩阵的L(q)的矩阵，λ为矩阵K的特征值，q0、q1、q2、q3分别为构成四元数q的实数，b为载体坐标系，n为导航坐标系，θ为检测车的姿态角中的俯仰角，r为检测车的姿态角中的横滚角，
Figure GDA0002952974280000193
为检测车的姿态角中的航向角。
本申请实施例还提供了一种检测车对准装置，如图3所示，该装置与图2中的装置相比，还包括：校准模块24，所述校准模块24用于：
对所述检测车的姿态角进行校准。
如图4所示，本申请一实施例提供了一种计算机设备400，用于执行图1中检测车对准方法，该设备包括存储器401、处理器402及存储在该存储器401上并可在该处理器402上运行的计算机程序，其中，上述处理器402执行上述计算机程序时实现上述检测车对准方法的步骤。
具体地，上述存储器401和处理器402能够为通用的存储器和处理器，这里不做具体限定，当处理器402运行存储器401存储的计算机程序时，能够执行上述检测车对准方法，用于解决现有技术中对检测车对准效率低的问题，根据检测车的位置信息及预设信息，确定第一角速度、第二角速度、第三角速度，其中，第一角速度为检测车的载体坐标系相对于惯性坐标系的角速度，第二角速度为地球的自转角速度在惯性坐标系中的投影角速度，第三角速度为检测车的地理坐标系相对于地球坐标系的角速度；将所述第一角速度、所述第二角速度、所述第三角速度作为预设的四元数姿态角确定模型输入，确定所述检测车的姿态角。本申请通过基于四元数的姿态角确定模型计算检测车的姿态角，具有强鲁棒性、快速性和高精度的优点，提高了对准精度，进而提高了轨检车姿态解算精度。
对应于图1中的检测车对准方法，本申请实施例还提供了一种计算机可读存储介质，该计算机可读存储介质上存储有计算机程序，该计算机程序被处理器运行时执行上述检测车对准方法的步骤。
具体地，该存储介质能够为通用的存储介质，如移动磁盘、硬盘等，该存储介质上的计算机程序被运行时，能够执行上述检测车对准方法，用于解决现有技术中对检测车对准效率低的问题，根据检测车的位置信息及预设信息，确定第一角速度、第二角速度、第三角速度，其中，第一角速度为检测车的载体坐标系相对于惯性坐标系的角速度，第二角速度为地球的自转角速度在惯性坐标系中的投影角速度，第三角速度为检测车的地理坐标系相对于地球坐标系的角速度；将所述第一角速度、所述第二角速度、所述第三角速度作为预设的四元数姿态角确定模型输入，确定所述检测车的姿态角。本申请通过基于四元数的姿态角确定模型计算检测车的姿态角，具有强鲁棒性、快速性和高精度的优点，提高了对准精度，进而提高了轨检车姿态解算精度。
在本申请所提供的实施例中，应该理解到，所揭露装置和方法，可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，又例如，多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些通信接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本申请提供的实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。
所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释，此外，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
最后应说明的是：以上所述实施例，仅为本申请的具体实施方式，用以说明本申请的技术方案，而非对其限制，本申请的保护范围并不局限于此，尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化，或者对其中部分技术特征进行等同替换；而这些修改、变化或者替换，并不使相应技术方案的本质脱离本申请实施例技术方案的精神和范围。都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应所述以权利要求的保护范围为准。

Claims (8)

1.一种检测车对准方法，其特征在于，该方法包括：
根据检测车的位置信息及预设信息，确定第一角速度、第二角速度、第三角速度，其中，第一角速度为检测车的载体坐标系相对于惯性坐标系的角速度，第二角速度为地球的自转角速度在惯性坐标系中的投影角速度，第三角速度为检测车的地理坐标系相对于地球坐标系的角速度；
将所述第一角速度、所述第二角速度、所述第三角速度作为预设的姿态角确定模型输入，确定所述检测车的姿态角；
输出所述检测车的姿态角；
将所述第一角速度、所述第二角速度、所述第三角速度作为预设的四元数姿态角确定模型输入，确定所述检测车的姿态角，包括：
通过以下公式计算所述检测车的姿态角：
Figure FDA0002995500060000011
β(t)＝q*α(t)*qT
L(q)＝qTKq-λ(qTq-1)
(K+λI)q＝0
Figure FDA0002995500060000012
Figure FDA0002995500060000013
Figure FDA0002995500060000014
Figure FDA0002995500060000015
其中，
Figure FDA0002995500060000021
Figure FDA0002995500060000022
q＝q0+q1i+q2j+q3k
Figure FDA0002995500060000023
Figure FDA0002995500060000024
其中，
Figure FDA0002995500060000025
为初始姿态转换矩阵，Cn b为导航坐标系到载体坐标系的姿态转换矩阵，β(t)为第一中间变量，α(t)为第二中间变量，χn(0,t)为检测车的地理坐标系相对于地球坐标系的运动速度
Figure FDA0002995500060000026
的函数，vn为检测车在地理坐标系的速度，
Figure FDA0002995500060000027
为检测车在地理坐标系的速度的一阶导数；
Figure FDA0002995500060000028
为地球的自转角速度在惯性坐标系中的投影角速度；
Figure FDA0002995500060000029
为检测车的地理坐标系相对于地球坐标系的角速度，gn为重力加速度在地理坐标系下投影，其中，gn＝[0 0 -g]T，χb(0,t)为第一角速度
Figure FDA00029955000600000210
的函数，
Figure FDA00029955000600000211
为χn(0,t)的转置，fb为检测车的比力矢量，q为姿态四元数，qT为姿态四元数的转置，L(q)为导航坐标系中加速度与零加速度差值的均方差统计值，
Figure FDA00029955000600000212
为第三中间变量，
Figure FDA00029955000600000213
为第四中间变量，K为有关于
Figure FDA00029955000600000214
和
Figure FDA00029955000600000215
的矩阵，λ为矩阵K的特征值，q0、q1、q2、q3分别为构成四元数q的实数，b为载体坐标系，n为导航坐标系，θ为检测车的姿态角中的俯仰角，r为检测车的姿态角中的横滚角，
Figure FDA00029955000600000216
为检测车的姿态角中的航向角。
2.如权利要求1所述的方法，其特征在于，根据检测车的位置信息及预设信息，确定第二角速度，包括：
通过以下公式计算第二角速度
Figure FDA0002995500060000031
Figure FDA0002995500060000032
其中，
Figure FDA0002995500060000033
为地球的自转角速度在惯性坐标系中的投影角速度，L为检测车的纬度，ωie为地球的自转角速度。
3.如权利要求1所述的方法，其特征在于，根据检测车的位置信息及预设信息，确定第三角速度，包括
通过以下公式计算第三角速度
Figure FDA0002995500060000034
Figure FDA0002995500060000035
其中，
Figure FDA0002995500060000036
为检测车的地理坐标系相对于地球坐标系的角速度，VE为检测车在地理坐标系中的东向速度，VN为检测车在地理坐标系中的北向速度，RM为沿子午圈的主曲率半径，RN为卯酉圈的主曲率半径，H为检测车的高度，L为检测车的纬度。
4.如权利要求1所述的方法，其特征在于，该方法还包括：
对所述检测车的姿态角进行校准。
5.一种检测车对准装置，其特征在于，该装置包括：
第一确定模块，用于根据检测车的位置信息及预设信息，确定第一角速度、第二角速度、第三角速度，其中，第一角速度为检测车的载体坐标系相对于惯性坐标系的角速度，第二角速度为地球的自转角速度在惯性坐标系中的投影角速度，第三角速度为检测车的地理坐标系相对于地球坐标系的角速度；
第二确定模块，用于将所述第一角速度、所述第二角速度、所述第三角速度作为预设的四元数姿态角确定模型输入，确定所述检测车的姿态角；
输出模块，用于输出所述检测车的姿态角；
第二确定模块，用于通过以下公式计算所述检测车的姿态角：
Figure FDA0002995500060000041
β(t)＝q*α(t)*qT
L(q)＝qTKq-λ(qTq-1)
(K+λI)q＝0
Figure FDA0002995500060000042
Figure FDA0002995500060000043
Figure FDA0002995500060000044
Figure FDA0002995500060000045
其中，
Figure FDA0002995500060000046
Figure FDA0002995500060000047
q＝q0+q1i+q2j+q3k
Figure FDA0002995500060000048
Figure FDA0002995500060000051
其中，
Figure FDA0002995500060000052
为初始姿态转换矩阵，Cn b为导航坐标系到载体坐标系的姿态转换矩阵，β(t)为第一中间变量，α(t)为第二中间变量，χn(0,t)为检测车的地理坐标系相对于地球坐标系的运动速度
Figure FDA0002995500060000053
的函数；vn为检测车在地理坐标系的速度；
Figure FDA0002995500060000054
为地球的自转角速度在惯性坐标系中的投影角速度；
Figure FDA0002995500060000055
为检测车的地理坐标系相对于地球坐标系的角速度；gn为重力加速度在地理坐标系下投影，其中，gn＝[0 0 -g]T；χb(0,t)为第一角速度
Figure FDA0002995500060000056
的函数；
Figure FDA0002995500060000057
为χn(0,t)的转置，fb为检测车的比力矢量，q为姿态四元数，qT为姿态四元数的转置，L(q)为导航坐标系中加速度与零加速度差值的均方差统计值，
Figure FDA0002995500060000058
为第三中间变量，
Figure FDA0002995500060000059
为第四中间变量，K为有关于
Figure FDA00029955000600000510
和
Figure FDA00029955000600000511
的矩阵，λ为矩阵K的特征值，q0、q1、q2、q3分别为构成四元数q的实数，b为载体坐标系，n为导航坐标系，θ为检测车的姿态角中的俯仰角，r为检测车的姿态角中的横滚角，
Figure FDA00029955000600000512
为检测车的姿态角中的航向角。
6.如权利要求5所述的装置，其特征在于，所述第一确定模块具体用于：
通过以下公式计算第二角速度
Figure FDA00029955000600000513
Figure FDA00029955000600000514
其中，
Figure FDA00029955000600000515
为地球的自转角速度在惯性坐标系中的投影角速度，L为检测车的纬度，ωie为地球的自转角速度。
7.如权利要求5所述的装置，其特征在于，所述第一确定模块具体用于：
通过以下公式计算第三角速度
Figure FDA0002995500060000061
Figure FDA0002995500060000062
其中，
Figure FDA0002995500060000063
为检测车的地理坐标系相对于地球坐标系的角速度，VE为检测车在地理坐标系中的东向速度，VN为检测车在地理坐标系中的北向速度，RM为沿子午圈的主曲率半径，RN为卯酉圈的主曲率半径，H为检测车的高度，L为检测车的纬度。
8.一种计算机设备包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现上述权利要求1至4任一项所述的方法的步骤。
CN201910333072.0A 2019-04-24 2019-04-24 一种检测车对准方法、装置和设备 Active CN110006456B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910333072.0A CN110006456B (zh) 2019-04-24 2019-04-24 一种检测车对准方法、装置和设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910333072.0A CN110006456B (zh) 2019-04-24 2019-04-24 一种检测车对准方法、装置和设备

Publications (2)

Publication Number Publication Date
CN110006456A CN110006456A (zh) 2019-07-12
CN110006456B true CN110006456B (zh) 2021-05-14

Family

ID=67173811

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910333072.0A Active CN110006456B (zh) 2019-04-24 2019-04-24 一种检测车对准方法、装置和设备

Country Status (1)

Country Link
CN (1) CN110006456B (zh)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1908584A (zh) * 2006-08-23 2007-02-07 北京航空航天大学 一种捷联惯性导航系统初始姿态确定方法
KR20190001832A (ko) * 2017-06-28 2019-01-07 국방과학연구소 시간지연 적응 보상기능을 갖는 관성항법장치 및 그의 급속 초기정렬 방법
CN109211230A (zh) * 2018-09-07 2019-01-15 东南大学 一种基于牛顿迭代法的炮弹姿态和加速度计常值误差估计方法
CN109579870A (zh) * 2017-04-25 2019-04-05 广州市泰斗鑫信息科技有限公司 捷联惯导系统的自动对准方法和组合导航装置
CN109596144A (zh) * 2018-12-10 2019-04-09 苏州大学 Gnss位置辅助sins行进间初始对准方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104748763B (zh) * 2015-03-19 2017-11-28 北京航天自动控制研究所 适用于车载晃动的捷联惯组快速对准方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1908584A (zh) * 2006-08-23 2007-02-07 北京航空航天大学 一种捷联惯性导航系统初始姿态确定方法
CN109579870A (zh) * 2017-04-25 2019-04-05 广州市泰斗鑫信息科技有限公司 捷联惯导系统的自动对准方法和组合导航装置
KR20190001832A (ko) * 2017-06-28 2019-01-07 국방과학연구소 시간지연 적응 보상기능을 갖는 관성항법장치 및 그의 급속 초기정렬 방법
CN109211230A (zh) * 2018-09-07 2019-01-15 东南大学 一种基于牛顿迭代法的炮弹姿态和加速度计常值误差估计方法
CN109596144A (zh) * 2018-12-10 2019-04-09 苏州大学 Gnss位置辅助sins行进间初始对准方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
INS/CCD系统四元数中心差分姿态估计算法;丁国强等;《华中科技大学学报》;20140930;第42卷(第9期);第19-23页 *

Also Published As

Publication number Publication date
CN110006456A (zh) 2019-07-12

Similar Documents

Publication Publication Date Title
CN106990426B (zh) 一种导航方法和导航装置
CN109001787A (zh) 一种姿态角解算与定位的方法及其融合传感器
CN103900565B (zh) 一种基于差分gps的惯导系统姿态获取方法
CN102486377B (zh) 一种光纤陀螺捷联惯导系统初始航向的姿态获取方法
CN105371844B (zh) 一种基于惯性/天文互助的惯性导航系统初始化方法
EP2557394A1 (en) Method and system for processing pulse signals within an interital navigation system
US20180356227A1 (en) Method and apparatus for calculation of angular velocity using acceleration sensor and geomagnetic sensor
CN104764463B (zh) 一种惯性平台调平瞄准误差的自检测方法
CN101563625A (zh) 用于二维和三维精确位置和定向确定的布置和方法
CN103076025B (zh) 一种基于双解算程序的光纤陀螺常值误差标定方法
CN108132053B (zh) 一种行人轨迹构建方法、系统及惯性测量装置
CN103727940B (zh) 基于重力加速度矢量匹配的非线性初始对准方法
CN103557864A (zh) Mems捷联惯导自适应sckf滤波的初始对准方法
CN107728182A (zh) 基于相机辅助的柔性多基线测量方法和装置
CN103940442A (zh) 一种采用加速收敛算法的定位方法及装置
CN103674064B (zh) 捷联惯性导航系统的初始标定方法
EP2930467A1 (en) A system and method for sensing the inclination of a moving platform with respect to gravity
CN112577518A (zh) 一种惯性测量单元标定方法及装置
CN109489661B (zh) 一种卫星初始入轨时陀螺组合常值漂移估计方法
CN112432642A (zh) 一种重力灯塔与惯性导航融合定位方法及系统
CN115932723A (zh) 定位方法、装置、计算机设备、存储介质和程序产品
Lu et al. Improved calibration of IMU biases in analytic coarse alignment for AHRS
CN109827572B (zh) 一种检测车位置预测的方法及装置
CN104154914A (zh) 一种空间稳定型捷联惯导系统初始姿态测量方法
RU2717566C1 (ru) Способ определения погрешностей инерциального блока чувствительных элементов на двухосном поворотном столе

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