CN110388942B - 一种基于角度和速度增量的车载姿态精对准系统 - Google Patents

一种基于角度和速度增量的车载姿态精对准系统 Download PDF

Info

Publication number
CN110388942B
CN110388942B CN201910804008.6A CN201910804008A CN110388942B CN 110388942 B CN110388942 B CN 110388942B CN 201910804008 A CN201910804008 A CN 201910804008A CN 110388942 B CN110388942 B CN 110388942B
Authority
CN
China
Prior art keywords
vehicle
attitude
alignment
state
time
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
CN201910804008.6A
Other languages
English (en)
Other versions
CN110388942A (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 Machinery Equipment Research Institute
Original Assignee
Beijing Machinery Equipment Research Institute
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 Machinery Equipment Research Institute filed Critical Beijing Machinery Equipment Research Institute
Priority to CN201910804008.6A priority Critical patent/CN110388942B/zh
Publication of CN110388942A publication Critical patent/CN110388942A/zh
Application granted granted Critical
Publication of CN110388942B publication Critical patent/CN110388942B/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

一种基于角度和速度增量的车载姿态精对准系统
技术领域
本发明涉及车辆姿态对准技术领域，尤其涉及一种基于角度和速度增量的车载姿态精对准系统。
背景技术
在发动机怠速和乘客上下车时，车辆会发生晃动，现有技术一般需要较长的粗对准时间来计算载体的初始姿态，才能保证精对准过程获得的车辆当前姿态精度较高。
现有技术采用的姿态对准系统一般如下：对光纤陀螺和石英加速度计的原始数据进行补偿后，实时进行粗对准和精对准两个过程，这两个过程顺序进行。但该姿态对准系统获得的对准精度较差，且对准时间较长。
现有技术在精对准过程中一般采用卡尔曼滤波器，但不对车辆姿态对准系统的噪声方差阵进行实时更新，当预设的噪声方差阵与实际噪声方差差别较大时，车辆姿态对准系统的状态估计误差较大，滤波器收敛速度慢，从而导致姿态修正效果较差。
发明内容
鉴于上述的分析，本发明实施例旨在提供一种基于角度和速度增量的车载姿态精对准系统，用以解决现有技术对准精度差、对准时间较长的问题。
一方面，本发明实施例提供了一种基于角度和速度增量的车载姿态精对准系统，包括：
角度增量采集模块，用于实时采集车辆当前三轴偏离原始三轴的角度增量；
速度增量采集模块，用于实时测量车辆沿原始三轴的速度增量；
导航计算模块，用于对所述角度增量和速度增量分别进行误差补偿，根据补偿结果进行粗对准，然后更新车辆姿态对准系统的实时状态，根据更新后获得的失准角、速度误差修正粗对准后车辆的姿态和速度，根据车辆姿态对准系统自适应滤波的滤波增益获得车辆状态估计，判断所述车辆状态估计与车辆当前状态是否相同；不同，则用所述车辆状态估计替代车辆上一时刻状态，再次更新车辆姿态对准系统的实时状态，直到相同为止，完成车辆姿态精对准。
上述技术方案的有益效果如下：通过采集的当前姿态改变实施粗对准，将粗对准结束的姿态结果作为初始姿态，根据对准原理迭代并进行自适应卡尔曼滤波调整滤波增益进行精对准，提高滤波器收敛速度，在保证对准时间的基础上提高了对准精度。
基于上述系统的进一步改进，所述角度增量采集模块包括三轴光纤陀螺，所述速度增量采集模块包括三轴石英加速度计：
所述三轴光纤陀螺，用于实时采集在载体坐标系内车辆当前三轴偏离原始三轴的角度增量；
所述三轴石英加速度计，用于实时采集在载体坐标系内车辆沿原始三轴的速度增量；
所述载体坐标系的中心设置于车辆重心位置，x轴正向为沿车辆横轴向右，y轴正向为沿车辆纵轴向前，z轴正向为沿车辆立轴向上。
上述进一步改进方案的有益效果是：利用光纤陀螺对角度增量进行采集，成本低，体积小、重量轻，加工工艺、采集方式简单，获得的数据准确性高。
进一步，所述导航计算模块进一步包括：
信号解析子模块，用于对接收到的数字电信号进行解析，获得车辆的所述角度增量和速度增量，将其发送至粗对准子模块；
粗对准子模块，用于对所述角度增量和速度增量分别进行误差补偿，根据补偿结果进行捷联解算，实施粗对准，获得粗对准后车辆的姿态和速度发送至精对准子模块；
精对准子模块，用于实时更新车辆姿态对准系统的实时状态，进而修正粗对准后车辆的姿态和速度，根据车辆姿态对准系统自适应滤波的滤波增益获得车辆状态估计，判断所述车辆状态估计与车辆当前状态是否相同，不同，则用所述车辆状态估计替代车辆上一时刻状态，再次更新车辆对准系统的实时状态，直到相同为止，完成车辆姿态精对准。
上述进一步改进方案的有益效果是：通过采集的角度增量和速度增量进行误差补偿，实施粗对准，将粗对准结束的姿态结果作为初始姿态，根据迭代对准原理以及自适应卡尔曼滤波调整滤波增益进行精对准，提高滤波器收敛速度，在保证对准时间的基础上有效提高了对准精度。
进一步，所述粗对准子模块执行如下程序进行车辆姿态粗对准：
连续获取等间隔时间的4组角度增量数据和4组速度增量数据；
通过下面公式对上述角度增量进行4子样圆锥误差补偿，获得补偿后的等效旋转矢量φ(T)
Figure BDA0002183117170000031
式中，Δθ(i)为ti时刻获得的角度增量，i＝1,…,4，T表示连续获取时间。
通过下面公式对上述速度增量进行4子样旋转误差补偿和划桨误差补偿，获得补偿后的速度增量ΔV(T)
Figure BDA0002183117170000041
式中，ΔV(i)为ti时刻获得的速度增量，i＝1,…,4；
根据补偿后的等效旋转矢量φ(T)和速度增量ΔV(T)，通过惯性系对准系统获取车辆的初始姿态矩阵
Figure BDA0002183117170000042
将上述姿态矩阵
Figure BDA0002183117170000043
作为初始姿态，结合补偿后的等效旋转矢量和补偿后的速度增量进行捷联解算，更新车辆的姿态矩阵
Figure BDA0002183117170000044
速度v和位置p，直到捷联解算算法收敛为止；
对上述捷联解算算法收敛后获得的车辆的姿态矩阵
Figure BDA0002183117170000045
进行分析，获得粗对准后车辆的姿态，同时，将上述捷联解算算法收敛获得的车辆的速度作为获得粗对准后车辆的速度，完成车辆粗对准。
上述进一步改进方案的有益效果是：对解算时间内车辆在空间旋转变化引起的角度增量进行了补偿，扩展了车辆在对准过程中的工作适应性。对解算时间内车辆存在周期性的角运动和线运动引起的速度增量进行了补偿，扩展了车辆在对准过程中的工作适应性。粗对准过程采用惯性系对准系统，提高了车辆对准过程中抗角晃动干扰的能力。进行迭代捷联解算的是补偿后的等效旋转矢量和速度增量，有效降低了存储数据的大小。
进一步，所述粗对准子模块进一步执行如下程序获取车辆的初始姿态矩阵
Figure BDA0002183117170000046
通过下面公式获取初始时刻导航坐标系相对于当前时刻导航坐标系的转换矩阵
Figure BDA0002183117170000047
Figure BDA0002183117170000051
式中，ωie为地球自转角速率，t为采样时间，L为车辆所处地理纬度；
通过下面公式获取当前时刻载体坐标系相对于初始时刻载体坐标系的转换矩阵
Figure BDA0002183117170000052
Figure BDA0002183117170000053
式中，φ(T)×为向量φ(T)的反对称阵运算，I为单位阵；
通过下面公式获取初始时刻载体坐标系相对于初始时刻导航坐标系的转换矩阵
Figure BDA0002183117170000054
Figure BDA0002183117170000055
其中
Figure BDA0002183117170000056
Figure BDA0002183117170000057
式中，
Figure BDA0002183117170000058
为当前解算时刻相对初始时刻导航坐标系速度，
Figure BDA0002183117170000059
为上一解算时刻相对初始时刻导航坐标系速度，
Figure BDA00021831171700000510
初值为0，
Figure BDA00021831171700000511
为当前解算时刻相对初始时刻导航坐标系速度，
Figure BDA00021831171700000512
为当前解算时刻相对初始时刻载体坐标系速度，
Figure BDA00021831171700000513
为上一解算时刻相对初始时刻载体坐标系速度，
Figure BDA00021831171700000514
为车辆上一解算时刻的姿态矩阵，初值为0；
根据上述
Figure BDA00021831171700000515
通过下面公式获取车辆的初始姿态矩阵
Figure BDA00021831171700000516
Figure BDA0002183117170000061
上述进一步改进方案的有益效果是：粗对准过程采用惯性系对准系统，提高了车辆对准过程中抗角晃动干扰的能力。
进一步，所述粗对准子模块进一步执行如下程序更新车辆的姿态矩阵
Figure BDA0002183117170000062
速度v和位置p，进一步包括如下步骤：
将上述姿态矩阵
Figure BDA0002183117170000063
作为初始姿态，通过下面公式更新车辆的姿态矩阵
Figure BDA0002183117170000064
Figure BDA0002183117170000065
其中
Figure BDA0002183117170000066
Figure BDA0002183117170000067
Figure BDA0002183117170000068
Figure BDA0002183117170000069
Figure BDA00021831171700000610
Figure BDA00021831171700000611
Figure BDA00021831171700000612
式中，t为采样时间，T为导航解算时间，
Figure BDA00021831171700000613
为向量
Figure BDA00021831171700000614
的反对称阵运算，vN为当前时刻导航坐标系速度的北向分量，vE为当前时刻导航坐标系速度的东向分量，Re为地球椭圆长半轴，RM为子午圈主曲率半径，RN为卯酉圈主曲率半径，ωie为地球自转角速率，L和h分别为地理纬度和地理高度，
Figure BDA00021831171700000615
分别为tm和tm-1时刻的姿态矩阵，m＝2，…，e为地球椭圆偏心率；
将
Figure BDA0002183117170000071
与
Figure BDA0002183117170000072
差值的绝对值与预设阈值进行比较，如果所述差值的绝对值大于等于预设阈值，重复上述步骤，直到所述差值的绝对值小于预设阈值为止，完成车辆的姿态矩阵
Figure BDA0002183117170000073
的更新；
设置粗对准的初始速度
Figure BDA0002183117170000074
为0，通过下面公式更新车辆的当前时刻速度
Figure BDA0002183117170000075
Figure BDA0002183117170000076
式中，
Figure BDA0002183117170000077
分别表示tm、tm-1时刻的导航坐标系速度，m＝2,…，gn为重力加速度，
Figure BDA0002183117170000078
表示tm-1时刻的姿态矩阵，T为导航解算时间，ΔV(T)为补偿后的速度增量；
将
Figure BDA0002183117170000079
与
Figure BDA00021831171700000710
差值的绝对值与预设阈值进行比较，如果所述差值的绝对值大于等于预设阈值，重复上述步骤，直到所述差值的绝对值小于预设阈值为止，完成车辆的速度v更新；
通过GPS获得车辆初始位置坐标p1，通过下面公式更新车辆的当前时刻位置坐标pm
Figure BDA00021831171700000711
其中
Figure BDA00021831171700000712
RMh＝RM+h
RNh＝RN+h
式中，pm、pm-1分别表示tm-1、tm时刻的位置坐标，m＝2,…，，L为车辆所处地理纬度，
Figure BDA0002183117170000081
分别表示tm、tm-1时刻的导航坐标系速度，RM为子午圈主曲率半径，RN为卯酉圈主曲率半径，h为车辆所处地理高度；
将pm与pm-1差值的绝对值与预设阈值进行比较，如果所述差值的绝对值大于等于预设阈值，重复上述步骤，直到所述差值的绝对值小于预设阈值，完成车辆的位置p更新。
上述进一步改进方案的有益效果是：进行迭代捷联解算的是补偿后的等效旋转矢量和速度增量，与使用直接存储的角度增量和速度增量相比，能够有效节省存储空间。
进一步，所述精对准子模块执行如下程序进行车辆姿态精对准：
考虑系统当前噪声，建立车辆姿态对准系统的状态向量X，获得对应的状态均方误差矩阵P；所述X包含失准角φ、速度误差δv；
根据建立的状态向量X，获取车辆姿态对准系统上一时刻对应的状态向量Xk-1，进而得出上一时刻状态均方根误差矩阵Pk-1、当前时刻对应的状态向量Xk，以及当前时刻状态均方根误差矩阵Pk；
根据车辆姿态对准系统上一时刻对应的状态向量Xk-1以及Pk-1，进行状态一步预测，获得状态预测向量Xk/k-1以及状态预测矩阵Pk/k-1；
根据预测向量Xk/k-1中的失准角φ、速度误差δv进一步修正车辆的姿态矩阵
Figure BDA0002183117170000082
速度v；
根据车辆姿态对准系统自适应滤波的滤波增益获得车辆姿态对准系统状态估计
Figure BDA0002183117170000083
状态均方误差估计
Figure BDA0002183117170000084
判断所述
Figure BDA0002183117170000085
与车辆姿态对准系统当前状态Xk、Pk是否相同，如果二者有一个不同，则用所述
Figure BDA0002183117170000086
替代Xk-1、Pk-1，再次更新车辆姿态对准系统的实时状态，直到二者相同为止，完成车辆姿态精对准。
上述进一步改进方案的有益效果是：通过对车辆姿态对准系统的状态向量X和状态均方误差矩阵P不断迭代，修正车辆的姿态矩阵
Figure BDA0002183117170000091
速度v，进而使得对准更加精确。
进一步，所述状态向量X的元素包括：东向失准角φE、北向失准角φN，天向失准角φU，东向速度误差δvE、北向速度误差δvN、东向陀螺零漂εE，北向陀螺零漂εN、天向陀螺零漂εU、东向加速度计零偏
Figure BDA0002183117170000092
北向加速度计零偏
Figure BDA0002183117170000093
所述状态均方误差矩阵P为
Figure BDA0002183117170000094
所述精对准子模块通过下面公式获取车辆姿态对准系统当前时刻对应的状态向量Xk
Xk＝Φk/k-1Xk-1+Qk
其中
Φk/k-1＝I+F(tk-1)T
Figure BDA0002183117170000095
Qk＝q(tk-1)T
Figure BDA0002183117170000101
式中，()×为向量的反对称阵运算，
Figure BDA0002183117170000102
为更新后车辆的姿态矩阵，T为导航解算时间，ωdb为陀螺随机常值，ωeb为加速度计随机常值，gn为重力加速度；
所述精对准子模块通过下面公式获得状态预测向量Xk/k-1以及状态预测矩阵Pk/k-1
Xk/k-1＝Φk/k-1Xk-1
Figure BDA0002183117170000103
式中，Xk/k-1为系统的状态一步预测向量，Pk/k-1为系统的状态均方误差一步预测矩阵；
所述精对准子模块通过下面公式修正车辆的姿态矩阵
Figure BDA0002183117170000104
速度v
Figure BDA0002183117170000105
v'＝v-aδv
式中，
Figure BDA0002183117170000106
为修正后的姿态矩阵，
Figure BDA0002183117170000107
为粗对准获得的姿态矩阵，v'为修正后的速度，v为粗对准获得的速度，a为修正系数，φ为向量[φE φN φU]T，δv为向量[δvE δvN 0]T。
上述进一步改进方案的有益效果是：通过对车辆姿态对准系统的状态向量X和状态均方误差矩阵P不断迭代，修正车辆的姿态矩阵
Figure BDA0002183117170000108
速度v，进而使得对准更加精确。
进一步，所述精对准子模块执行如下程序获取自适应滤波的滤波增益：
通过下面公式获取滤波增益对应的当前时刻的系统噪声方差Rk
Figure BDA0002183117170000111
其中
Figure BDA0002183117170000112
Zk/k-1＝H Xk/k-1+Vk
Figure BDA0002183117170000113
Figure BDA0002183117170000114
式中，Rk-1为上一时刻的系统噪声方差，βk为当前时刻的自适应滤波系数，βk-1为上一时刻的自适应滤波系数，β0＝1，b为渐消因子，Zk/k-1为量测预测误差矩阵，Vk-1为系统上一时刻的量测噪声向量，Vk为系统当前时刻的量测噪声向量，V1＝[0.5,0.5,2.0]T，k＝2,…，TH为系统的量测更新时间；
根据所述Rk，通过下面公式获取车辆姿态对准系统自适应滤波的滤波增益Kk
Kk＝Pk/k-1HT(H Pk/k-1HT+Rk)-1
上述进一步改进方案的有益效果是：通过自适应滤波不断修正系统噪声方差Rk，能够获得准确的滤波增益Kk，进而提高系统滤波效果。
进一步，所述精对准子模块通过下面公式获得车辆姿态校准系统的状态向量估计
Figure BDA0002183117170000115
Figure BDA0002183117170000116
式中，Zk为系统当前时刻的量测向量，等于粗对准捷联解算获得的速度
Figure BDA0002183117170000117
所述精对准子模块通过下面公式获得上述状态向量估计的均方误差矩阵估计
Figure BDA0002183117170000118
Figure BDA0002183117170000121
上述进一步改进方案的有益效果是：通过将状态向量估计与当前状态进行比较，以及将均方误差矩阵估计与均方误差矩阵进行比较，能够充分保证精对准系统的正确性，是获得的对准结果符合实际情况、精确、可靠。
本发明中，上述各技术方案之间还可以相互组合，以实现更多的优选组合方案。本发明的其他特征和优点将在随后的说明书中阐述，并且，部分优点可从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过说明书以及附图中所特别指出的内容中来实现和获得。
附图说明
附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。
图1为本发明实施例1系统连接示意图；
图2为本发明实施例2系统连接示意图。
具体实施方式
下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理，并非用于限定本发明的范围。
首先介绍本发明公式推导涉及的坐标系定义。
1)惯性坐标系(i系)：原点为地球中心，xi、yi轴在地球赤道平面内，xi轴正向指向春分点(赤道面与黄道面的交线再与天球相交的交点之一)，zi轴为地球自转轴，正向指向地球北极。
2)地球坐标系(e系)：原点为地球中心，xe、ye轴在地球赤道平面内，xe轴正向指向本初子午线，ze轴为地球自转轴，正向指向北极。地球坐标系与地球固联，相对于惯性坐标系以地球自转角速率运动。
3)导航坐标系(n系)：原点为载体(车辆)重心，xn轴正向指东，yn轴正向指北，zn轴正向指天。
4)载体坐标系(b系)：原点为载体重心，xb轴沿载体横轴向右，yb轴沿载体纵轴向前，zb轴沿载体立轴向上。
实施例1
本发明的一个具体实施例，公开了一种基于角度和速度增量的车载姿态精对准系统，如图1所示，包括角度增量采集模块、速度增量采集模块和导航计算模块。角度增量采集模块、速度增量采集模块的输出端与导航计算模块输入端连接。
角度增量采集模块，用于实时采集车辆当前三轴偏离原始三轴的角度增量，将其传输至导航计算模块。
速度增量采集模块，用于实时测量车辆沿原始三轴的速度增量，将其传输至导航计算模块。
导航计算模块，用于对所述角度增量和速度增量分别进行误差补偿，根据补偿结果进行粗对准，然后更新车辆姿态对准系统的实时状态，根据更新后获得的失准角、速度误差修正粗对准后车辆的姿态和速度，根据车辆姿态对准系统自适应滤波的滤波增益获得车辆状态估计，判断所述车辆状态估计与车辆当前状态是否相同；不同，则用所述车辆状态估计替代车辆上一时刻状态，再次更新车辆姿态对准系统的实时状态，直到相同为止，完成车辆姿态精对准。
实施时，所述导航计算模块分三个过程实现车辆姿态对准：
第一个过程：获取车辆当前姿态改变量，包括当前三轴偏离原始三轴的角度增量，以及沿原始三轴的速度增量。
第二个过程：执行粗对准，对上述角度增量和速度增量分别进行误差补偿，根据补偿后的等效旋转矢量和速度增量实施粗对准，得到粗对准后车辆的姿态和速度。
第三个过程：执行精对准，更新车辆姿态对准系统的实时状态，根据更新后获得的失准角、速度误差修正粗对准后车辆的姿态和速度。根据车辆姿态对准系统自适应滤波的滤波增益获得车辆状态估计，判断所述车辆状态估计与车辆当前状态是否相同；不同，则用所述车辆状态估计替代车辆上一时刻状态，再次更新车辆姿态对准系统的实时状态，直到相同为止，完成车辆姿态精对准。
与现有技术相比，通过采集的当前姿态改变实施粗对准，将粗对准结束的姿态结果作为初始姿态，根据对准原理迭代并进行自适应卡尔曼滤波调整滤波增益进行精对准，提高滤波器收敛速度，在保证对准时间的基础上提高了对准精度。
实施例2
在实施例1的基础上进行优化，该系统还包括I/F信号转换模块，如图2所示，其介于速度增量采集模块和导航计算模块之间。
I/F信号转换模块，用于将速度增量采集模块输出的电信号从模拟量转换成数字量，传输至导航计算模块。
优选地，所述角度增量采集模块包括三轴光纤陀螺。三轴光纤陀螺，用于实时采集在载体坐标系内车辆当前三轴偏离原始三轴的角度增量，将其转换成数字电信号传输至导航计算模块。
优选地，所述速度增量采集模块包括三轴石英加速度计。三轴石英加速度计，用于实时采集在载体坐标系内车辆沿原始三轴的速度增量，将其转换成模拟电信号传输至I/F信号转换器，转换成数字电信号后传输至导航计算模块。
需说明的是，上述载体坐标系的中心设置于车辆重心位置，x轴正向为沿车辆横轴向右，y轴正向为沿车辆纵轴向前，z轴正向为沿车辆立轴向上。
优选地，导航计算模块进一步包括依次连接的信号解析子模块、粗对准子模块、精对准子模块。
信号解析子模块，用于对接收到的数字电信号进行解析，获得车辆的所述角度增量和速度增量，将其发送至粗对准子模块。
粗对准子模块，用于对所述角度增量和速度增量分别进行误差补偿，根据补偿结果进行捷联解算，实施粗对准，获得粗对准后车辆的姿态和速度发送至精对准子模块。
精对准子模块，用于实时更新车辆姿态对准系统的实时状态，进而修正粗对准后车辆的姿态和速度，根据车辆姿态对准系统自适应滤波的滤波增益获得车辆状态估计，判断所述车辆状态估计与车辆当前状态是否相同，不同，则用所述车辆状态估计替代车辆上一时刻状态，再次更新车辆对准系统的实时状态，直到相同为止，完成车辆姿态精对准。
优选地，粗对准子模块执行如下程序进行车辆姿态粗对准：
S1.连续获取等间隔时间的4组角度增量数据和4组速度增量数据；
S2.通过下面公式对上述角度增量数据进行4子样圆锥误差补偿，获得补偿后的等效旋转矢量φ(T)
Figure BDA0002183117170000161
式中，Δθ(i)为ti时刻获得的角度增量数据，i＝1,…,4，T表示连续获取时间。
S3.通过下面公式对上述速度增量数据进行4子样旋转误差补偿和划桨误差补偿，获得补偿后的速度增量ΔV(T)
Figure BDA0002183117170000162
式中，ΔV(i)为ti时刻获得的速度增量数据，i＝1,…,4。
具体地，
Figure BDA0002183117170000163
为4子样旋转误差补偿。
Figure BDA0002183117170000164
为划桨误差补偿。
S4.根据补偿后的等效旋转矢量φ(T)和速度增量ΔV(T)，通过惯性系对准系统获取车辆的初始姿态矩阵
Figure BDA0002183117170000165
S5.将上述姿态矩阵
Figure BDA0002183117170000166
作为初始姿态，结合补偿后的等效旋转矢量和补偿后的速度增量进行捷联解算，更新车辆的姿态矩阵
Figure BDA0002183117170000167
速度v和位置p，直到捷联解算算法收敛为止；
S6.对上述捷联解算算法收敛后获得的车辆的姿态矩阵
Figure BDA0002183117170000168
进行分析，获得粗对准后车辆的姿态，同时，将上述捷联解算算法收敛获得的车辆的速度作为获得粗对准后车辆的速度，完成车辆粗对准。
优选地，步骤S4中，粗对准子模块进一步执行如下程序获取车辆的初始姿态矩阵
Figure BDA0002183117170000171
S41.以车辆重心为中心，指向东为x轴正向，指向北为y轴正向，指向上为z轴正向，建立导航坐标系。
S42.通过下面公式获取初始时刻导航坐标系相对于当前时刻导航坐标系的转换矩阵
Figure BDA0002183117170000172
Figure BDA0002183117170000173
式中，ωie为地球自转角速率，t为采样时间，L为车辆所处地理纬度。
S43.通过下面公式获取当前时刻载体坐标系相对于初始时刻载体坐标系的转换矩阵
Figure BDA0002183117170000174
Figure BDA0002183117170000175
式中，φ(T)×为向量φ(T)的反对称阵运算，I为单位阵。
S44.通过下面公式获取初始时刻载体坐标系相对于初始时刻导航坐标系的转换矩阵
Figure BDA0002183117170000176
Figure BDA0002183117170000177
其中
Figure BDA0002183117170000178
Figure BDA0002183117170000179
式中，
Figure BDA0002183117170000181
为当前解算时刻相对初始时刻导航坐标系速度(即在导航坐标系下的车辆速度，本领域技术人员能够理解)，
Figure BDA0002183117170000182
为上一解算时刻相对初始时刻导航坐标系速度，
Figure BDA0002183117170000183
初值为0，
Figure BDA0002183117170000184
为当前解算时刻相对初始时刻导航坐标系速度，
Figure BDA0002183117170000185
为当前解算时刻相对初始时刻载体坐标系速度(即在载体坐标系下的车辆速度，本领域技术人员能够理解)，
Figure BDA0002183117170000186
为上一解算时刻相对初始时刻载体坐标系速度，
Figure BDA0002183117170000187
为车辆上一解算时刻的姿态矩阵，初值为0。
S45.根据上述
Figure BDA0002183117170000188
通过下面公式获取车辆的初始姿态矩阵
Figure BDA0002183117170000189
Figure BDA00021831171700001810
优选地，步骤S5中，粗对准子模块进一步执行如下程序进行捷联解算更新车辆的姿态矩阵
Figure BDA00021831171700001811
速度v和位置p：
S51.将上述姿态矩阵
Figure BDA00021831171700001812
作为初始姿态，通过下面公式更新车辆的姿态矩阵
Figure BDA00021831171700001813
Figure BDA00021831171700001814
其中
Figure BDA00021831171700001815
Figure BDA00021831171700001816
Figure BDA00021831171700001817
Figure BDA00021831171700001818
Figure BDA00021831171700001819
Figure BDA0002183117170000191
Figure BDA0002183117170000192
式中，t为采样时间，T为导航解算时间，
Figure BDA0002183117170000193
为向量
Figure BDA0002183117170000194
的反对称阵运算，vN为当前时刻导航坐标系速度的北向分量，vE为当前时刻导航坐标系速度的东向分量，Re为地球椭圆长半轴，RM为子午圈主曲率半径，RN为卯酉圈主曲率半径，ωie为地球自转角速率，L和h分别为地理纬度和地理高度，
Figure BDA0002183117170000195
分别为tm和tm-1时刻的姿态矩阵，m＝2，…，e为地球椭圆偏心率。
具体地，e为地球椭圆偏心率，通过下面公式获得e
Figure BDA0002183117170000196
式中，f为椭圆扁率，取值1/298.257。
S52.将
Figure BDA0002183117170000197
与
Figure BDA0002183117170000198
差值的绝对值与预设阈值进行比较，如果所述差值的绝对值大于等于预设阈值，重复上述步骤，直到所述差值的绝对值小于预设阈值为止，完成车辆的姿态矩阵
Figure BDA0002183117170000199
的更新。
S53.设置粗对准的初始速度
Figure BDA00021831171700001910
为0，通过下面公式更新车辆的当前时刻速度
Figure BDA00021831171700001911
Figure BDA00021831171700001912
式中，
Figure BDA00021831171700001913
分别表示tm、tm-1时刻导航坐标系速度，m＝2,…，gn为重力加速度，
Figure BDA00021831171700001914
表示tm-1时刻的姿态矩阵，T为导航解算时间，ΔV(T)为补偿后的速度增量。
S54.将
Figure BDA00021831171700001915
与
Figure BDA00021831171700001916
差值的绝对值与预设阈值进行比较，如果所述差值的绝对值大于等于预设阈值，重复上述步骤，直到所述差值的绝对值小于预设阈值为止，完成车辆的速度v更新。
S55.通过GPS获得车辆初始位置坐标p1，通过下面公式更新车辆的当前时刻位置坐标pm
Figure BDA0002183117170000201
其中
Figure BDA0002183117170000202
RMh＝RM+h
RNh＝RN+h
式中，pm、pm-1分别表示tm-1、tm时刻的位置坐标，m＝2,…，，L为车辆所处地理纬度，
Figure BDA0002183117170000203
分别表示tm、tm-1时刻的导航坐标系速度，RM为子午圈主曲率半径，RN为卯酉圈主曲率半径，h为车辆所处地理高度。
S56.将pm与pm-1差值的绝对值与预设阈值进行比较，如果所述差值的绝对值大于等于预设阈值，重复上述步骤，直到所述差值的绝对值小于预设阈值，完成车辆的位置p更新。
优选地，步骤S6中，通过下面公式获得粗对准后车辆的姿态θ、γ、ψ
Figure BDA0002183117170000204
Figure BDA0002183117170000205
优选地，所述精对准子模块执行如下程序进行车辆姿态精对准：
S7.考虑系统当前噪声，建立车辆姿态对准系统的状态向量X，获得对应的状态均方误差矩阵P；所述X包含失准角φ、速度误差δv；
S8.根据建立的状态向量X，获取车辆姿态对准系统上一时刻对应的状态向量Xk-1，进而得出上一时刻状态均方根误差矩阵Pk-1、当前时刻对应的状态向量Xk，以及当前时刻状态均方根误差矩阵Pk；
S9.根据车辆姿态对准系统上一时刻对应的状态向量Xk-1以及Pk-1，进行状态一步预测，获得状态预测向量Xk/k-1以及状态预测矩阵Pk/k-1；
S10.根据预测向量Xk/k-1中的失准角φ、速度误差δv进一步修正车辆的姿态矩阵
Figure BDA0002183117170000211
速度v；
S11.根据车辆姿态对准系统自适应滤波的滤波增益获得车辆姿态对准系统状态估计
Figure BDA0002183117170000212
状态均方误差估计
Figure BDA0002183117170000213
判断所述
Figure BDA0002183117170000214
与车辆姿态对准系统当前状态Xk、Pk是否相同，如果二者有一个不同，则用所述
Figure BDA0002183117170000215
Figure BDA0002183117170000216
替代Xk-1、Pk-1，再次更新车辆姿态对准系统的实时状态，直到二者相同为止，完成车辆姿态精对准。
优选地，步骤S7中，所述状态向量X的元素包括：东向失准角φE、北向失准角φN，天向失准角φU，东向速度误差δvE、北向速度误差δvN、东向陀螺零漂εE，北向陀螺零漂εN、天向陀螺零漂εU、东向加速度计零偏
Figure BDA0002183117170000217
北向加速度计零偏
Figure BDA0002183117170000218
所述状态向量X采用如下形式
Figure BDA0002183117170000219
所述状态均方误差矩阵P为
Figure BDA0002183117170000221
优选地，步骤S8中，精对准子模块通过下面公式获取车辆姿态对准系统当前时刻对应的状态向量Xk，进而根据上面P可得到Pk、Pk-1。
Xk＝Φk/k-1Xk-1+Qk (15)
其中
Φk/k-1＝I+F(tk-1)T
Figure BDA0002183117170000222
Qk＝q(tk-1)T
Figure BDA0002183117170000223
式中，Xk-1为车辆姿态对准系统上一时刻对应的状态向量，Xk车辆姿态对准系统当前时刻对应的状态向量，()×为向量的反对称阵运算，
Figure BDA0002183117170000224
为更新后车辆的姿态矩阵，T为导航解算时间，ωdb为陀螺随机常值，ωeb为加速度计随机常值，gn为重力加速度。
优选地，步骤S9中，精对准子模块通过下面公式获得状态预测向量Xk/k-1以及状态预测矩阵Pk/k-1
Xk/k-1＝Φk/k-1Xk-1 (16)
Figure BDA0002183117170000231
式中，Xk/k-1也称系统的状态一步预测向量，Pk/k-1也称系统的状态均方误差一步预测矩阵。
优选地，步骤S10中，精对准子模块通过下面公式修正车辆的姿态矩阵
Figure BDA0002183117170000232
速度v
Figure BDA0002183117170000233
v'＝v-aδv (18)
式中，
Figure BDA0002183117170000234
为修正后的姿态矩阵，
Figure BDA0002183117170000235
为粗对准获得的姿态矩阵，v'为修正后的速度，v为粗对准获得的速度，a为修正系数，φ为向量[φE φN φU]T，δv为向量[δvE δvN 0]T。
优选地，步骤S11中，所述精对准子模块执行如下程序获取自适应滤波的滤波增益：
S111.通过下面公式获取滤波增益对应的当前时刻的系统噪声方差Rk
Figure BDA0002183117170000236
其中
Figure BDA0002183117170000237
Zk/k-1＝H Xk/k-1+Vk
Figure BDA0002183117170000238
Figure BDA0002183117170000239
式中，Rk-1为上一时刻的系统噪声方差，βk为当前时刻的自适应滤波系数，βk-1为上一时刻的自适应滤波系数，β0＝1，b为渐消因子，Zk/k-1为量测预测误差矩阵，Vk-1为系统上一时刻的量测噪声向量，Vk为系统当前时刻的量测噪声向量，V1＝[0.5,0.5,2.0]T，k＝2,…，TH为系统的量测更新时间；
S112.根据所述Rk，通过下面公式获取车辆姿态对准系统自适应滤波的滤波增益Kk
Kk＝Pk/k-1HT(H Pk/k-1HT+Rk)-1 (20)
优选地，步骤S11中，精对准子模块通过下面公式获得车辆姿态校准系统的状态向量估计
Figure BDA0002183117170000241
Figure BDA0002183117170000242
式中，Zk为系统当前时刻的量测向量，等于粗对准捷联解算获得的速度
Figure BDA0002183117170000243
优选地，步骤S11中，精对准子模块通过下面公式获得上述状态向量估计的均方误差矩阵估计
Figure BDA0002183117170000244
Figure BDA0002183117170000245
与实施例1相比，本实施例提供的系统将惯性系对准系统获得的姿态结果作为初始姿态，利用4子样圆锥误差补偿获得的等效旋转矢量、4子样旋转误差补偿和划桨误差补偿获得的速度增量进行迭代对准。并且，精对准过程采用自适应卡尔曼滤波不断修正系统噪声方差Rk，使得最终对准结果能够精确、可靠、快速获得。
本领域技术人员可以理解，实现上述实施例系统的全部或部分流程，可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于计算机可读存储介质中。其中，所述计算机可读存储介质为磁盘、光盘、只读存储记忆体或随机存储记忆体等。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

Claims (7)

1.一种基于角度和速度增量的车载姿态精对准系统，其特征在于，包括：
角度增量采集模块，用于实时采集车辆当前三轴偏离原始三轴的角度增量；
速度增量采集模块，用于实时测量车辆沿原始三轴的速度增量；
导航计算模块，用于对所述角度增量和速度增量分别进行误差补偿，根据补偿结果进行粗对准，然后更新车辆姿态对准系统的实时状态，根据更新后获得的失准角、速度误差修正粗对准后车辆的姿态和速度，根据车辆姿态对准系统自适应滤波的滤波增益获得车辆状态估计，判断所述车辆状态估计与车辆当前状态是否相同；不同，则用所述车辆状态估计替代车辆上一时刻状态，再次更新车辆姿态对准系统的实时状态，直到相同为止，完成车辆姿态精对准；
所述导航计算模块进一步包括：
信号解析子模块，用于对接收到的数字电信号进行解析，获得车辆的所述角度增量和速度增量，将其发送至粗对准子模块；
粗对准子模块，用于对所述角度增量和速度增量分别进行误差补偿，根据补偿结果进行捷联解算，实施粗对准，获得粗对准后车辆的姿态和速度发送至精对准子模块；
精对准子模块，用于实时更新车辆姿态对准系统的实时状态，进而修正粗对准后车辆的姿态和速度，根据车辆姿态对准系统自适应滤波的滤波增益获得车辆状态估计，判断所述车辆状态估计与车辆当前状态是否相同，不同，则用所述车辆状态估计替代车辆上一时刻状态，再次更新车辆对准系统的实时状态，直到相同为止，完成车辆姿态精对准；
进一步，所述粗对准子模块执行如下程序进行车辆姿态粗对准：
连续获取等间隔时间的4组角度增量数据和4组速度增量数据；
对上述角度增量数据进行4子样圆锥误差补偿，获得补偿后的等效旋转矢量φ(T)；
对上述速度增量数据进行4子样旋转误差补偿和划桨误差补偿，获得补偿后的速度增量ΔV(T)；
根据补偿后的等效旋转矢量φ(T)和速度增量ΔV(T)，通过惯性系对准系统获取车辆的初始姿态矩阵
Figure FDA0003027137200000021
将上述姿态矩阵
Figure FDA0003027137200000022
作为初始姿态，结合补偿后的等效旋转矢量和补偿后的速度增量进行捷联解算，更新车辆的姿态矩阵
Figure FDA0003027137200000023
速度v和位置p，直到捷联解算算法收敛为止；
对上述捷联解算算法收敛后获得的车辆的姿态矩阵
Figure FDA0003027137200000024
进行分析，获得粗对准后车辆的姿态，同时，将上述捷联解算算法收敛获得的车辆的速度作为获得粗对准后车辆的速度，完成车辆粗对准；
进一步，所述精对准子模块执行如下程序进行车辆姿态精对准：
考虑系统当前噪声，建立车辆姿态对准系统的状态向量X，获得对应的状态均方误差矩阵P；所述X包含失准角φ、速度误差δv；
根据建立的状态向量X，获取车辆姿态对准系统上一时刻对应的状态向量Xk-1，进而得出上一时刻状态均方根误差矩阵Pk-1、当前时刻对应的状态向量Xk，以及当前时刻状态均方根误差矩阵Pk；
根据车辆姿态对准系统上一时刻对应的状态向量Xk-1以及Pk-1，进行状态一步预测，获得状态预测向量Xk/k-1以及状态预测矩阵Pk/k-1；
根据预测向量Xk/k-1中的失准角φ、速度误差δv进一步修正车辆的姿态矩阵
Figure FDA0003027137200000025
速度v；
根据车辆姿态对准系统自适应滤波的滤波增益获得车辆姿态对准系统状态估计
Figure FDA0003027137200000031
状态均方误差估计
Figure FDA0003027137200000032
判断所述
Figure FDA0003027137200000033
与车辆姿态对准系统当前状态Xk、Pk是否相同，如果二者有一个不同，则用所述
Figure FDA0003027137200000034
替代Xk-1、Pk-1，再次更新车辆姿态对准系统的实时状态，直到二者相同为止，完成车辆姿态精对准。
2.根据权利要求1所述的基于角度和速度增量的车载姿态精对准系统，其特征在于，所述角度增量采集模块包括三轴光纤陀螺，所述速度增量采集模块包括三轴石英加速度计；
所述三轴光纤陀螺，用于实时采集在载体坐标系内车辆当前三轴偏离原始三轴的角度增量；
所述三轴石英加速度计，用于实时采集在载体坐标系内车辆沿原始三轴的速度增量；
所述载体坐标系的中心设置于车辆重心位置，x轴正向为沿车辆横轴向右，y轴正向为沿车辆纵轴向前，z轴正向为沿车辆立轴向上。
3.根据权利要求1或2所述的基于角度和速度增量的车载姿态精对准系统，其特征在于，所述粗对准子模块进一步执行如下程序获取车辆的初始姿态矩阵
Figure FDA0003027137200000035
以车辆重心为中心，指向东为x轴正向，指向北为y轴正向，指向上为z轴正向，建立导航坐标系；
通过下面公式获取初始时刻导航坐标系相对于当前时刻导航坐标系的转换矩阵
Figure FDA0003027137200000036
Figure FDA0003027137200000037
式中，ωie为地球自转角速率，t为采样时间，L为车辆所处地理纬度；
通过下面公式获取当前时刻载体坐标系相对于初始时刻载体坐标系的转换矩阵
Figure FDA0003027137200000041
Figure FDA0003027137200000042
式中，φ(T)×为向量φ(T)的反对称阵运算，I为单位阵；
通过下面公式获取初始时刻载体坐标系相对于初始时刻导航坐标系的转换矩阵
Figure FDA0003027137200000043
Figure FDA0003027137200000044
其中
Figure FDA0003027137200000045
Figure FDA0003027137200000046
式中，
Figure FDA0003027137200000047
为当前解算时刻相对初始时刻导航坐标系速度，
Figure FDA0003027137200000048
为上一解算时刻相对初始时刻导航坐标系速度，
Figure FDA0003027137200000049
初值为0，
Figure FDA00030271372000000410
为当前解算时刻相对初始时刻载体坐标系速度，
Figure FDA00030271372000000411
为上一解算时刻相对初始时刻载体坐标系速度，
Figure FDA00030271372000000412
为车辆上一解算时刻的姿态矩阵，初值为0；
根据上述
Figure FDA00030271372000000413
通过下面公式获取车辆的初始姿态矩阵
Figure FDA00030271372000000414
Figure FDA00030271372000000415
4.根据权利要求3所述的基于角度和速度增量的车载姿态精对准系统，其特征在于，所述粗对准子模块进一步执行如下程序更新车辆的姿态矩阵
Figure FDA00030271372000000416
速度v和位置p：
将上述姿态矩阵
Figure FDA0003027137200000051
作为初始姿态，通过下面公式更新车辆的姿态矩阵
Figure FDA0003027137200000052
Figure FDA0003027137200000053
其中
Figure FDA0003027137200000054
Figure FDA0003027137200000055
Figure FDA0003027137200000056
Figure FDA0003027137200000057
Figure FDA0003027137200000058
Figure FDA0003027137200000059
Figure FDA00030271372000000510
式中，t为采样时间，T为导航解算时间，
Figure FDA00030271372000000511
为向量
Figure FDA00030271372000000512
的反对称阵运算，vN为当前时刻导航坐标系速度的北向分量，vE为当前时刻导航坐标系速度的东向分量，Re为地球椭圆长半轴，RM为子午圈主曲率半径，RN为卯酉圈主曲率半径，ωie为地球自转角速率，L和h分别为地理纬度和地理高度，
Figure FDA00030271372000000513
分别为tm和tm-1时刻的姿态矩阵，m＝2，…，e为地球椭圆偏心率；
将
Figure FDA00030271372000000514
与
Figure FDA00030271372000000515
差值的绝对值与预设阈值进行比较，如果所述差值的绝对值大于等于预设阈值，重复上述步骤，直到所述差值的绝对值小于预设阈值为止，完成车辆的姿态矩阵
Figure FDA00030271372000000516
的更新；
设置粗对准的初始速度
Figure FDA00030271372000000517
为0，通过下面公式更新车辆的当前时刻速度
Figure FDA0003027137200000061
Figure FDA0003027137200000062
式中，
Figure FDA0003027137200000063
分别表示tm、tm-1时刻的导航坐标系速度，m＝2,…，gn为重力加速度，
Figure FDA0003027137200000064
表示tm-1时刻的姿态矩阵，T为导航解算时间，ΔV(T)为补偿后的速度增量；
将
Figure FDA0003027137200000065
与
Figure FDA0003027137200000066
差值的绝对值与预设阈值进行比较，如果所述差值的绝对值大于等于预设阈值，重复上述步骤，直到所述差值的绝对值小于预设阈值为止，完成车辆的速度v更新；
通过GPS获得车辆初始位置坐标p1，通过下面公式更新车辆的当前时刻位置坐标pm
Figure FDA0003027137200000067
其中
Figure FDA0003027137200000068
RMh＝RM+h
RNh＝RN+h
式中，pm、pm-1分别表示tm-1、tm时刻的位置坐标，m＝2,…，L为车辆所处地理纬度，
Figure FDA0003027137200000069
分别表示tm、tm-1时刻的导航坐标系速度，RM为子午圈主曲率半径，RN为卯酉圈主曲率半径，h为车辆所处地理高度；
将pm与pm-1差值的绝对值与预设阈值进行比较，如果所述差值的绝对值大于等于预设阈值，重复上述步骤，直到所述差值的绝对值小于预设阈值，完成车辆的位置p更新。
5.根据权利要求1-2、4之一所述的基于角度和速度增量的车载姿态精对准系统，其特征在于，所述状态向量X的元素包括：东向失准角φE、北向失准角φN，天向失准角φU，东向速度误差δvE、北向速度误差δvN、东向陀螺零漂εE，北向陀螺零漂εN、天向陀螺零漂εU、东向加速度计零偏
Figure FDA0003027137200000071
北向加速度计零偏
Figure FDA0003027137200000072
所述状态均方误差矩阵P为
Figure FDA0003027137200000073
所述精对准子模块通过下面公式获取车辆姿态对准系统当前时刻对应的状态向量Xk
Xk＝Φk/k-1Xk-1+Qk
其中
Φk/k-1＝I+F(tk-1)T
Figure FDA0003027137200000074
Qk＝q(tk-1)T
Figure FDA0003027137200000075
式中，()×为向量的反对称阵运算，
Figure FDA0003027137200000076
为更新后车辆的姿态矩阵，T为导航解算时间，ωdb为陀螺随机常值，ωeb为加速度计随机常值，gn为重力加速度，I为单位阵，ωie为地球自转角速率；
所述精对准子模块通过下面公式获得状态预测向量Xk/k-1以及状态预测矩阵Pk/k-1
Xk/k-1＝Φk/k-1Xk-1
Figure FDA0003027137200000081
式中，Xk/k-1为系统的状态一步预测向量，Pk/k-1为系统的状态均方误差一步预测矩阵；
所述精对准子模块通过下面公式修正车辆的姿态矩阵
Figure FDA0003027137200000082
速度v
Figure FDA0003027137200000083
v'＝v-aδv
式中，
Figure FDA0003027137200000084
为修正后的姿态矩阵，
Figure FDA0003027137200000085
为粗对准获得的姿态矩阵，v'为修正后的速度，v为粗对准获得的速度，a为修正系数，φ为向量[φE φN φU]T，δv为向量[δvE δvN 0]T。
6.根据权利要求5所述的基于角度和速度增量的车载姿态精对准系统，其特征在于，所述精对准子模块执行如下程序获取自适应滤波的滤波增益：
通过下面公式获取滤波增益对应的当前时刻的系统噪声方差Rk
Figure FDA0003027137200000086
其中
Figure FDA0003027137200000087
Zk/k-1＝H Xk/k-1+Vk
Figure FDA0003027137200000088
Figure FDA0003027137200000091
式中，Rk-1为上一时刻的系统噪声方差，βk为当前时刻的自适应滤波系数，βk-1为上一时刻的自适应滤波系数，β0＝1，b为渐消因子，Zk/k-1为量测预测误差矩阵，Vk-1为系统上一时刻的量测噪声向量，Vk为系统当前时刻的量测噪声向量，V1＝[0.5,0.5,2.0]T，k＝2,…，TH为系统的量测更新时间；
根据所述Rk，通过下面公式获取车辆姿态对准系统自适应滤波的滤波增益Kk
Kk＝Pk/k-1HT(HPk/k-1HT+Rk)-1。
7.根据权利要求6所述的基于角度和速度增量的车载姿态精对准系统，其特征在于，所述精对准子模块通过下面公式获得车辆姿态校准系统的状态向量估计
Figure FDA0003027137200000092
Figure FDA0003027137200000093
式中，Zk为系统当前时刻的量测向量，等于粗对准捷联解算获得的速度
Figure FDA0003027137200000094
所述精对准子模块通过下面公式获得上述状态向量估计的均方误差矩阵估计
Figure FDA0003027137200000095
Figure FDA0003027137200000096
判断所述估计
Figure FDA0003027137200000097
与车辆当前状态Xk、Pk是否相同，如果二者有一个不同，用所述
Figure FDA0003027137200000098
估计替代Xk-1、Pk-1，再次更新车辆姿态对准系统的实时状态，直到二者相同为止，完成车辆姿态精对准。
CN201910804008.6A 2019-08-28 2019-08-28 一种基于角度和速度增量的车载姿态精对准系统 Active CN110388942B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910804008.6A CN110388942B (zh) 2019-08-28 2019-08-28 一种基于角度和速度增量的车载姿态精对准系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910804008.6A CN110388942B (zh) 2019-08-28 2019-08-28 一种基于角度和速度增量的车载姿态精对准系统

Publications (2)

Publication Number Publication Date
CN110388942A CN110388942A (zh) 2019-10-29
CN110388942B true CN110388942B (zh) 2021-09-10

Family

ID=68289503

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910804008.6A Active CN110388942B (zh) 2019-08-28 2019-08-28 一种基于角度和速度增量的车载姿态精对准系统

Country Status (1)

Country Link
CN (1) CN110388942B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111351483B (zh) * 2020-03-31 2021-12-07 北京控制工程研究所 一种递归多子样大动态惯性导航方法
CN112697166B (zh) * 2020-11-04 2023-06-06 河北汉光重工有限责任公司 一种运动状态下的捷联惯导系统自对准方法
CN117848389B (zh) * 2024-03-08 2024-05-17 浙江航天润博测控技术有限公司 导航对准方法、导航设备及导航系统

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102519450A (zh) * 2011-12-12 2012-06-27 东南大学 一种用于水下滑翔器的组合导航装置及方法
CN102620748A (zh) * 2012-03-22 2012-08-01 东南大学 捷联惯导系统晃动基座条件下杆臂效应的估计和补偿方法
CN102679978A (zh) * 2012-05-14 2012-09-19 北京理工大学 一种旋转式捷联惯性导航系统静基座初始对准方法
CN106705992A (zh) * 2015-11-12 2017-05-24 北京自动化控制设备研究所 一种双轴光纤惯导系统快速自标定自对准方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105203129B (zh) * 2015-10-13 2019-05-07 上海华测导航技术股份有限公司 一种惯导装置初始对准方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102519450A (zh) * 2011-12-12 2012-06-27 东南大学 一种用于水下滑翔器的组合导航装置及方法
CN102620748A (zh) * 2012-03-22 2012-08-01 东南大学 捷联惯导系统晃动基座条件下杆臂效应的估计和补偿方法
CN102679978A (zh) * 2012-05-14 2012-09-19 北京理工大学 一种旋转式捷联惯性导航系统静基座初始对准方法
CN106705992A (zh) * 2015-11-12 2017-05-24 北京自动化控制设备研究所 一种双轴光纤惯导系统快速自标定自对准方法

Also Published As

Publication number Publication date
CN110388942A (zh) 2019-10-29

Similar Documents

Publication Publication Date Title
CN106871928B (zh) 基于李群滤波的捷联惯性导航初始对准方法
CN111024064B (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN110388941B (zh) 一种基于自适应滤波的车辆姿态对准方法
CN111156987B (zh) 基于残差补偿多速率ckf的惯性/天文组合导航方法
CN110221332B (zh) 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法
CN110388942B (zh) 一种基于角度和速度增量的车载姿态精对准系统
CN110702143B (zh) 基于李群描述的sins捷联惯性导航系统动基座快速初始对准方法
CN110398257A (zh) Gps辅助的sins系统快速动基座初始对准方法
CN111366148B (zh) 适用于机载光电观瞄系统多次观察的目标定位方法
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN110595503B (zh) 基于李群最优估计的sins捷联惯性导航系统晃动基座自对准方法
CN109612460B (zh) 一种基于静止修正的垂线偏差测量方法
CN111156986B (zh) 一种基于抗差自适应ukf的光谱红移自主组合导航方法
CN113503892B (zh) 一种基于里程计和回溯导航的惯导系统动基座初始对准方法
CN110440830A (zh) 动基座下车载捷联惯导系统自对准方法
CN114858189A (zh) 捷联惯导系统陀螺漂移等效补偿方法
CN110285834B (zh) 基于一点位置信息的双惯导系统快速自主重调方法
CN111207773B (zh) 一种用于仿生偏振光导航的姿态无约束优化求解方法
CN116105730A (zh) 基于合作目标卫星甚短弧观测的仅测角光学组合导航方法
CN116222551A (zh) 一种融合多种数据的水下导航方法及装置
CN112229421A (zh) 基于李群最优估计的捷联惯性导航晃动基座粗对准方法
CN112212862A (zh) 一种改进粒子滤波的gps/ins组合导航方法
CN111637896A (zh) 一种基于星历约束辅助的自主天文导航方法
CN112212889A (zh) 基于特殊正交群最优估计的sins捷联惯性导航系统晃动基座粗对准方法
CN110375773B (zh) Mems惯导系统姿态初始化方法

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