CN113108783B - 一种无人潜航器惯性/多普勒组合导航方法 - Google Patents

一种无人潜航器惯性/多普勒组合导航方法 Download PDF

Info

Publication number
CN113108783B
CN113108783B CN202110502743.9A CN202110502743A CN113108783B CN 113108783 B CN113108783 B CN 113108783B CN 202110502743 A CN202110502743 A CN 202110502743A CN 113108783 B CN113108783 B CN 113108783B
Authority
CN
China
Prior art keywords
coordinate system
error
abscissa
underwater vehicle
representing
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
CN202110502743.9A
Other languages
English (en)
Other versions
CN113108783A (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.)
National University of Defense Technology
Original Assignee
National University of Defense Technology
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 National University of Defense Technology filed Critical National University of Defense Technology
Priority to CN202110502743.9A priority Critical patent/CN113108783B/zh
Publication of CN113108783A publication Critical patent/CN113108783A/zh
Application granted granted Critical
Publication of CN113108783B publication Critical patent/CN113108783B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

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

Landscapes

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

Abstract

本发明属于导航技术领域，公开了一种无人潜航器惯性/多普勒组合导航方法，适用于无人潜航器等水下载体的全纬度导航航行。本发明提出一种无人潜航器惯性/多普勒组合导航方法，以地理坐标系、横坐标系下的组合导航滤波器为基础，建立了系统误差状态及其协方差矩阵在两个导航坐标系之间的转换关系，设计了滤波状态稳定的组合导航滤波器，解决了坐标系转换过程中的滤波状态跳变问题；本发明设计的惯性/多普勒组合导航方法满足了无人潜航器全纬度航行作业任务的需求，具有十分重要的工程意义。

Description

一种无人潜航器惯性/多普勒组合导航方法
技术领域
本发明属于导航技术领域，涉及惯性/多普勒组合导航方法，特别涉及一种无人潜航器惯性/多普勒组合导航方法，适用于无人潜航器等水下载体的全纬度导航航行。
背景技术
无人潜航器具备机动灵活、部署方便、安全性高的特点，能够执行一些载人潜航器难以完成的任务，近年来得到了各工业强国的大力发展。无人潜航器具有很高的自主性，其搭载的传感器具备导航、感知、自主决策等能力，使其在环境测量、测绘作业等场景下的作用越来越重要。根据现有认知，对无人潜航器而言，自主导航能力是决定其水下生存能力最为关键的一环之一。目前，无人潜航器通常搭载了惯性导航系统(INS)、多普勒测速仪(DVL)、深度计等导航传感器，借助惯性基组合导航系统提供的高精度姿态、速度、位置信息，为无人潜航器水下作业提供空间、时间基准信息。
近年来，随着全球变暖，两极地区冰层逐渐融化，各大国对两极地区的科考任务逐渐增多，纷纷抢占在两极地区的话语权。作为一种有效的测量手段，无人潜航器在两极科考过程中逐渐担负起了重要作用。为了保证无人潜航器在高纬度地区的导航能力，需要着重考虑惯性/多普勒组合导航系统在极区的导航算法设计。近年来，国内外许多机构、专家学者纷纷投入了很大的成本和精力对此问题研究。然而，目前的研究基本延续了中低纬度导航算法设计的思路，只是单纯在高纬度地区进行导航算法设计，忽视了导航过程的连续性。对于很多大中型无人潜航器而言，其水下航行时，需要执行全纬度作业任务，即存在从中低纬度向高纬度航行或者是从高纬度向中低纬度航行的任务场景。高纬度地区可以在格网坐标系或者横坐标系下进行惯导算法机械编排设计和组合导航算法设计，而在中低纬度时一般在地理坐标系下进行设计。横坐标系由于算法设计简单，与航海海图配合较为方便，在航海高纬度导航算法设计时应用较多。对于惯性/多普勒组合导航滤波器而言，如果直接从地理坐标系“硬切换”至横坐标系，将不可避免的出现滤波震荡问题，需要长时间才能完成滤波稳定，这极大影响了无人潜航器的导航精度，对于执行精密测绘任务十分不利，而这正是现有研究忽视的地方。
本发明针对目前存在的问题，提出一种无人潜航器惯性/多普勒组合导航方法，以地理坐标系、横坐标系下的组合导航滤波器为基础，建立了系统误差状态及其协方差矩阵在两个导航坐标系之间的转换关系，设计了滤波状态稳定的组合导航滤波器，解决了坐标系转换过程中的滤波状态跳变问题；本发明设计的惯性/多普勒组合导航满足了无人潜航器全纬度航行作业任务的需求，具有十分重要的工程意义。
发明内容
本发明要解决的技术问题就在于：提供完全自主的全纬度导航方案，解决无人潜航器跨纬度航行过程中不同导航坐标系转换导致的滤波不稳定问题，实现系统误差状态的平滑过渡，提高导航精度，为无人潜航器等水下载体的全纬度导航航行提供技术支撑。
为解决上述技术问题，本发明提出的解决方案为：
一种无人潜航器惯性/多普勒组合导航方法，包括以下步骤：
(1)确定无人潜航器在高纬度航行区域的导航坐标系及位置表示方式，包括如下步骤：
(1.1)确定无人潜航器高纬度地区航行时的导航坐标系，高纬度地区导航坐标系确定为横坐标系；首先将地球坐标系e绕其xe轴旋转-90°，进而绕第一次旋转之后中间坐标系的z′e轴旋转-90°得到横地球坐标系e′；即以90°E经线与赤道平面的交点为伪北极点，以90°W经线与赤道平面的交点为伪南极点，以0°/180°经线圈作为伪赤道，其中，横坐标系t的定义为：yt轴指向伪北极点，zt轴垂直于当地水平面，xt轴与yt轴、zt轴构成右手正交坐标系；
将横地球坐标系e′与地球坐标系e之间的方向余弦矩阵
Figure BDA00030570692200000215
表示为：
Figure BDA0003057069220000021
(1.2)将无人潜航器在横坐标系中位置表示为(Lt，λt，h)，其中，Lt表示横纬度，λt表示横经度，h表示无人潜航器距离水平面的高度；
(2)确定无人潜航器在横坐标系下的更新方程，包括姿态更新方程、速度更新方程、位置更新方程，具体实施如下：
(2.1)确定横坐标系下的姿态更新方程为：
Figure BDA0003057069220000022
其中，
Figure BDA0003057069220000023
表示横坐标系相对于载体坐标系b的方向余弦矩阵，
Figure BDA0003057069220000024
表示载体坐标系相对于惯性坐标系i的旋转角速度，
Figure BDA0003057069220000025
表示横坐标系相对于惯性坐标系的旋转角速度；
(2.2)确定横坐标系下的速度vt的更新方程为：
Figure BDA0003057069220000026
其中，
Figure BDA0003057069220000027
Figure BDA0003057069220000028
Figure BDA0003057069220000029
式中，fb表示载体坐标系下表示的比力，gt表示横坐标系下表示的重力矢量，
Figure BDA00030570692200000210
表示横坐标系相对于惯性坐标系的旋转角速度在横坐标系下的投影，
Figure BDA00030570692200000211
表示横地球坐标系相对于惯性坐标系的旋转角速度在横坐标系下的投影，
Figure BDA00030570692200000212
表示横坐标系相对于横地球坐标系的旋转角速度在横坐标系下的投影，ωie表示地球旋转角速度，Re为地球半径，
Figure BDA00030570692200000213
表示横坐标系下的东向速度，
Figure BDA00030570692200000214
表示横坐标系下的北向速度；
(2.3)确定横坐标系下的位置更新方程为：
Figure BDA0003057069220000031
Figure BDA0003057069220000032
Figure BDA0003057069220000033
式中，
Figure BDA0003057069220000034
表示横坐标系下的垂向速度；
(3)确定无人潜航器在横坐标系下的姿态误差方程、速度误差方程、位置误差方程，具体实施如下：
确定姿态误差φt的方程如下：
Figure BDA0003057069220000035
其中，
Figure BDA0003057069220000036
表示横坐标系相对于惯性坐标系的旋转角速度误差，
Figure BDA0003057069220000037
表示载体坐标系b相对于惯性坐标系的旋转角速度误差；
确定速度误差δvt的方程如下：
Figure BDA0003057069220000038
其中，ft表示横坐标系下表示的比力，，
Figure BDA0003057069220000039
表示横地球坐标系相对于惯性坐标系的旋转角速度误差，
Figure BDA00030570692200000310
表示横坐标系相对于横地球坐标系的旋转角速度误差，δfb表示比力误差；
确定位置误差的方程如下：
Figure BDA00030570692200000311
式中，δLt,δλt,δh分别表示横纬度误差、横经度误差、高度误差，
Figure BDA00030570692200000312
分别表示横坐标系下表示的东向速度误差、北向速度误差、垂向速度误差；
(4)确定无人潜航器导航参数在地理坐标系与横坐标系之间的转换关系并进行转换，导航参数的转换包括姿态转换、速度转换、位置转换；
其中，无人潜航器姿态参数在地理坐标系、横坐标系之间的转换关系为：
Figure BDA00030570692200000313
Figure BDA00030570692200000314
式中，
Figure BDA00030570692200000315
表示地理坐标系n与载体坐标系b之间的方向余弦矩阵，
Figure BDA00030570692200000316
表示横坐标系与地理坐标系之间的方向余弦矩阵，
Figure BDA0003057069220000041
表示地理坐标系与横坐标系之间的方向余弦矩阵，
Figure BDA0003057069220000042
与
Figure BDA0003057069220000043
互为转置矩阵，且有：
Figure BDA0003057069220000044
Figure BDA0003057069220000045
式中，
Figure BDA0003057069220000046
表示地球坐标系与地理坐标系之间的方向余弦矩阵，
Figure BDA0003057069220000047
表示横坐标系与横地球坐标系之间的方向余弦矩阵，L、λ分别表示地理坐标系下的纬度、经度；
无人潜航器速度参数在地理坐标系、横坐标系之间的转换关系为：
Figure BDA0003057069220000048
Figure BDA0003057069220000049
式中，vn表示地理坐标系下表示的速度；
无人潜航器位置参数在横坐标系、地理坐标系之间的转换关系为：
Lt＝arcsin(cosLsinλ)L＝arcsin(cosLtcosλt)
Figure BDA00030570692200000410
高度h在横坐标系、地理坐标系中保持不变；
(5)完成INS/DVL组合导航滤波器在地理坐标系与横坐标系之间的转换，其中INS/DVL组合导航滤波器采用闭环反馈校正方式，具体实施如下：
(5.1)分别确定地理坐标系与横坐标系下的系统误差状态为：
地理坐标系下的系统误差状态xn(t)为
Figure BDA00030570692200000411
横坐标系下的系统误差状态xt(t)为
Figure BDA00030570692200000412
其中，
Figure BDA00030570692200000413
分别表示地理坐标系下表示的东向、北向、垂向姿态误差，
Figure BDA00030570692200000414
分别表示横坐标系下表示的东向、北向、垂向姿态误差，
Figure BDA00030570692200000415
分别表示地理坐标系下表示的东向、北向、垂向速度误差，δL,δλ分别表示地理坐标系下的纬度、经度误差，
Figure BDA00030570692200000416
分别表示x、y、z轴向陀螺常值零偏，
Figure BDA00030570692200000417
分别表示x、y、z轴向加表常值零偏，k,δΨ,δθ分别表示DVL的标度因数误差、方位安装误差角、俯仰安装误差角；
(5.2)分别确定姿态误差、速度误差、位置误差在地理坐标系与横坐标系下间的转换关系为：
首先确定地理坐标系下姿态误差φn与横坐标系下姿态误差φt之间的转换关系
Figure BDA0003057069220000051
式中，
Figure BDA0003057069220000052
Figure BDA0003057069220000053
其次确定地理坐标系下速度误差δvn与横坐标系下速度误差δvt之间的转换关系
Figure BDA0003057069220000054
进而确定横坐标系下表示的纬度误差δLt、经度误差δλt、高度误差δh与地理坐标系下表示的纬度误差δL、经度误差δλ、高度误差δh的转换关系
Figure BDA0003057069220000055
陀螺常值零偏
Figure BDA0003057069220000056
加表常值零偏
Figure BDA0003057069220000057
DVL的标度因数误差、方位安装误差角、俯仰安装误差角k,δΨ,δθ、高度误差δh在地理坐标系与横坐标系下保持不变；
确定横坐标系下的系统误差状态xt(t)与地理坐标系下的系统误差状态xn(t)之间的转换关系如下：
xt(t)＝Φxn(t),xn(t)＝Φ-1xt(t)
其中，Φ为转换系数矩阵，并且根据φt与φn之间的转换关系，δvt与δvn之间的转换关系，δLt,δλt与δL、δλ之间的转换关系，并考虑高度误差δh、陀螺常值零偏
Figure BDA0003057069220000058
加表常值零偏
Figure BDA0003057069220000059
DVL的标度因数误差、方位安装误差角、俯仰安装误差角k,δΨ,δθ在地理坐标系与横坐标系下的不变性进行确定；
(5.3)根据步骤(5.2)，确定地理坐标系下系统误差状态协方差矩阵Pn(t)与横坐标系下系统误差状态协方差矩阵Pt(t)的转换关系：
Figure BDA0003057069220000061
式中，
Figure BDA0003057069220000062
表示横坐标系下表示的系统误差状态估计值，
Figure BDA0003057069220000063
表示地理坐标系下表示的系统误差状态估计值；
(5.4)当无人潜航器在中纬度、高纬度地区跨纬度航行时，闭环反馈INS/DVL组合导航滤波器完成在地理坐标系与横坐标系之间的协方差矩阵转换，转换方式按照步骤(5.3)所述，鉴于系统误差状态在每次闭环反馈校正后为0，其不需要转换。
进一步的，若无人潜航器接收到GNSS定位信息时，利用GNSS位置点信息完成对所述步骤(4)中
Figure BDA0003057069220000064
或
Figure BDA0003057069220000065
的装订更新。
进一步的，所述步骤(5)中导航参数在地理坐标系与横坐标系之间转换时基于转换时刻的纬度阈值判断，且地理坐标系转换到横坐标系、横坐标系转换到地理坐标系两种情况下的阈值设定不同。
进一步的，所述步骤(5)中INS/DVL组合导航滤波器的观测量为DVL速度信息，即在中低纬度的观测量为地理坐标系下速度误差δvn，在高纬度的观测量为横坐标系下速度误差δvt。
进一步的，所述步骤(5)中INS/DVL组合导航滤波器的观测量为DVL速度信息和深度计提供的深度信息，即在中低纬度的观测量为地理坐标系下速度误差δvn、δh，在高纬度的观测量为横坐标系下速度误差δvt、δh。
通过以上步骤可以实现无人潜航器惯性/多普勒全纬度组合导航，保证全球范围内的连续全自主导航定位，不会出现导航滤波器震荡问题。
与现有技术相比，本发明的优点在于：
(1)本发明解决了无人潜航器等水下载体全纬度航行过程中，由于导航坐标系转换带来的组合导航滤波器震荡问题，有效提高导航精度，为高精度测绘作业任务提供精确的空间、时间基准。
(2)本发明设计滤波器具备完全自主性，实现了全自主导航定位。
附图说明
图1为本发明方法的流程示意图。
具体实施方式
以下将结合说明书附图和具体实施例对本发明作进一步详细说明。
如图1所示，一种无人潜航器惯性/多普勒组合导航方法，包括以下步骤：
(1)确定无人潜航器在高纬度航行区域的导航坐标系及位置表示方式，包括如下步骤：
(1.1)确定无人潜航器高纬度地区航行时的导航坐标系，高纬度地区导航坐标系确定为横坐标系；首先将地球坐标系e绕其xe轴旋转-90°，进而绕第一次旋转之后中间坐标系的ze′轴旋转-90°得到横地球坐标系e′；即以90°E经线与赤道平面的交点为伪北极点，以90°W经线与赤道平面的交点为伪南极点，以0°/180°经线圈作为伪赤道，其中，横坐标系t的定义为：yt轴指向伪北极点，zt轴垂直于当地水平面，xt轴与yt轴、zt轴构成右手正交坐标系；
将横地球坐标系e′与地球坐标系e之间的方向余弦矩阵
Figure BDA0003057069220000071
表示为：
Figure BDA0003057069220000072
(1.2)将无人潜航器在横坐标系中位置表示为(Lt，λt，h)，其中，Lt表示横纬度，λt表示横经度，h表示无人潜航器距离水平面的高度；
(2)确定无人潜航器在横坐标系下的更新方程，包括姿态更新方程、速度更新方程、位置更新方程，具体实施如下：
(2.1)确定横坐标系下的姿态更新方程为：
Figure BDA0003057069220000073
其中，
Figure BDA0003057069220000074
表示横坐标系相对于载体坐标系b的方向余弦矩阵，
Figure BDA0003057069220000075
表示载体坐标系相对于惯性坐标系i的旋转角速度，
Figure BDA0003057069220000076
表示横坐标系相对于惯性坐标系的旋转角速度；
(2.2)确定横坐标系下的速度vt的更新方程为：
Figure BDA0003057069220000077
其中，
Figure BDA0003057069220000078
Figure BDA0003057069220000079
Figure BDA00030570692200000710
式中，fb表示载体坐标系下表示的比力，gt表示横坐标系下表示的重力矢量，
Figure BDA00030570692200000711
表示横坐标系相对于惯性坐标系的旋转角速度在横坐标系下的投影，
Figure BDA00030570692200000712
表示横地球坐标系相对于惯性坐标系的旋转角速度在横坐标系下的投影，
Figure BDA00030570692200000713
表示横坐标系相对于横地球坐标系的旋转角速度在横坐标系下的投影，ωie表示地球旋转角速度，Re为地球半径，
Figure BDA00030570692200000714
表示横坐标系下的东向速度，
Figure BDA00030570692200000715
表示横坐标系下的北向速度；
(2.3)确定横坐标系下的位置更新方程为：
Figure BDA0003057069220000081
Figure BDA0003057069220000082
Figure BDA0003057069220000083
式中，
Figure BDA0003057069220000084
表示横坐标系下的垂向速度；
(3)确定无人潜航器在横坐标系下的姿态误差方程、速度误差方程、位置误差方程，具体实施如下：
确定姿态误差φt的方程如下：
Figure BDA0003057069220000085
其中，
Figure BDA0003057069220000086
表示横坐标系相对于惯性坐标系的旋转角速度误差，
Figure BDA0003057069220000087
表示载体坐标系b相对于惯性坐标系的旋转角速度误差；
确定速度误差δvt的方程如下：
Figure BDA0003057069220000088
其中，ft表示横坐标系下表示的比力，，
Figure BDA0003057069220000089
表示横地球坐标系相对于惯性坐标系的旋转角速度误差，
Figure BDA00030570692200000810
表示横坐标系相对于横地球坐标系的旋转角速度误差，δfb表示比力误差；
确定位置误差的方程如下：
Figure BDA00030570692200000811
式中，δLt,δλt,δh分别表示横纬度误差、横经度误差、高度误差，
Figure BDA00030570692200000812
分别表示横坐标系下表示的东向速度误差、北向速度误差、垂向速度误差；
(4)确定无人潜航器导航参数在地理坐标系与横坐标系之间的转换关系并进行转换，导航参数的转换包括姿态转换、速度转换、位置转换；
其中，无人潜航器姿态参数在地理坐标系、横坐标系之间的转换关系为：
Figure BDA00030570692200000813
Figure BDA00030570692200000814
式中，
Figure BDA00030570692200000815
表示地理坐标系n与载体坐标系b之间的方向余弦矩阵，
Figure BDA00030570692200000816
表示横坐标系与地理坐标系之间的方向余弦矩阵，
Figure BDA0003057069220000091
表示地理坐标系与横坐标系之间的方向余弦矩阵，
Figure BDA0003057069220000092
与
Figure BDA0003057069220000093
互为转置矩阵，且有：
Figure BDA0003057069220000094
Figure BDA0003057069220000095
式中，
Figure BDA0003057069220000096
表示地球坐标系与地理坐标系之间的方向余弦矩阵，
Figure BDA0003057069220000097
表示横坐标系与横地球坐标系之间的方向余弦矩阵，L、λ分别表示地理坐标系下的纬度、经度；
无人潜航器速度参数在地理坐标系、横坐标系之间的转换关系为：
Figure BDA0003057069220000098
Figure BDA0003057069220000099
式中，vn表示地理坐标系下表示的速度；
无人潜航器位置参数在横坐标系、地理坐标系之间的转换关系为：
Lt＝arcsin(cosLsinλ)L＝arcsin(cosLtcosλt)
Figure BDA00030570692200000910
高度h在横坐标系、地理坐标系中保持不变；
(5)完成INS/DVL组合导航滤波器在地理坐标系与横坐标系之间的转换，其中INS/DVL组合导航滤波器采用闭环反馈校正方式，具体实施如下：
(5.1)分别确定地理坐标系与横坐标系下的系统误差状态为：
地理坐标系下的系统误差状态xn(t)为
Figure BDA00030570692200000911
横坐标系下的系统误差状态xt(t)为
Figure BDA00030570692200000912
其中，
Figure BDA00030570692200000913
分别表示地理坐标系下表示的东向、北向、垂向姿态误差，
Figure BDA00030570692200000914
分别表示横坐标系下表示的东向、北向、垂向姿态误差，
Figure BDA00030570692200000915
分别表示地理坐标系下表示的东向、北向、垂向速度误差，δL,δλ分别表示地理坐标系下的纬度、经度误差，
Figure BDA00030570692200000916
分别表示x、y、z轴向陀螺常值零偏，
Figure BDA00030570692200000917
分别表示x、y、z轴向加表常值零偏，k,δΨ,δθ分别表示DVL的标度因数误差、方位安装误差角、俯仰安装误差角；
(5.2)分别确定姿态误差、速度误差、位置误差在地理坐标系与横坐标系下间的转换关系为：
首先确定地理坐标系下姿态误差φn与横坐标系下姿态误差φt之间的转换关系
Figure BDA0003057069220000101
式中，
Figure BDA0003057069220000102
Figure BDA0003057069220000103
其次确定地理坐标系下速度误差δvn与横坐标系下速度误差δvt之间的转换关系
Figure BDA0003057069220000104
进而确定横坐标系下表示的纬度误差δLt、经度误差δλt、高度误差δh与地理坐标系下表示的纬度误差δL、经度误差δλ、高度误差δh的转换关系
Figure BDA0003057069220000105
陀螺常值零偏
Figure BDA0003057069220000106
加表常值零偏
Figure BDA0003057069220000107
DVL的标度因数误差、方位安装误差角、俯仰安装误差角k,δΨ,δθ、高度误差δh在地理坐标系与横坐标系下保持不变；
确定横坐标系下的系统误差状态xt(t)与地理坐标系下的系统误差状态xn(t)之间的转换关系如下：
xt(t)＝Φxn(t),xn(t)＝Φ-1xt(t)
其中，Φ为转换系数矩阵，并且根据φt与φn之间的转换关系，δvt与δvn之间的转换关系，δLt,δλt与δL、δλ之间的转换关系，并考虑高度误差δh、陀螺常值零偏
Figure BDA0003057069220000108
加表常值零偏
Figure BDA0003057069220000109
DVL的标度因数误差、方位安装误差角、俯仰安装误差角k,δΨ,δθ在地理坐标系与横坐标系下的不变性进行确定；
(5.3)根据步骤(5.2)，确定地理坐标系下系统误差状态协方差矩阵Pn(t)与横坐标系下系统误差状态协方差矩阵Pt(t)的转换关系：
Figure BDA0003057069220000111
式中，
Figure BDA0003057069220000112
表示横坐标系下表示的系统误差状态估计值，
Figure BDA0003057069220000113
表示地理坐标系下表示的系统误差状态估计值；
(5.4)当无人潜航器在中纬度、高纬度地区跨纬度航行时，闭环反馈INS/DVL组合导航滤波器完成在地理坐标系与横坐标系之间的协方差矩阵转换，转换方式按照步骤(5.3)所述，鉴于系统误差状态在每次闭环反馈校正后为0，其不需要转换。
进一步的，若无人潜航器接收到GNSS定位信息时，利用GNSS位置点信息完成对所述步骤(4)中
Figure BDA0003057069220000114
或
Figure BDA0003057069220000115
的装订更新。
进一步的，所述步骤(5)中导航参数在地理坐标系与横坐标系之间转换时基于转换时刻的纬度阈值判断，且地理坐标系转换到横坐标系、横坐标系转换到地理坐标系两种情况下的阈值设定不同。
进一步的，所述步骤(5)中INS/DVL组合导航滤波器的观测量为DVL速度信息，即在中低纬度的观测量为地理坐标系下速度误差δvn，在高纬度的观测量为横坐标系下速度误差δvt。
进一步的，所述步骤(5)中INS/DVL组合导航滤波器的观测量为DVL速度信息和深度计提供的深度信息，即在中低纬度的观测量为地理坐标系下速度误差δvn、δh，在高纬度的观测量为横坐标系下速度误差δvt、δh。
以上所述仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (5)

1.一种无人潜航器惯性/多普勒组合导航方法，其特征在于，包括以下步骤：
(1)确定无人潜航器在高纬度航行区域的导航坐标系及位置表示方式，包括如下步骤：
(1.1)确定无人潜航器高纬度地区航行时的导航坐标系，高纬度地区导航坐标系确定为横坐标系；首先将地球坐标系e绕其xe轴旋转-90°，进而绕第一次旋转之后中间坐标系的ze′轴旋转-90°得到横地球坐标系e′；即以90°E经线与赤道平面的交点为伪北极点，以90°W经线与赤道平面的交点为伪南极点，以0°/180°经线圈作为伪赤道，其中，横坐标系t的定义为：yt轴指向伪北极点，zt轴垂直于当地水平面，xt轴与yt轴、zt轴构成右手正交坐标系；
将横地球坐标系e′与地球坐标系e之间的方向余弦矩阵
Figure FDA0003057069210000011
表示为：
Figure FDA0003057069210000012
(1.2)将无人潜航器在横坐标系中位置表示为(Lt，λt，h)，其中，Lt表示横纬度，λt表示横经度，h表示无人潜航器距离水平面的高度；
(2)确定无人潜航器在横坐标系下的更新方程，包括姿态更新方程、速度更新方程、位置更新方程，具体实施如下：
(2.1)确定横坐标系下的姿态更新方程为：
Figure FDA0003057069210000013
其中，
Figure FDA0003057069210000014
表示横坐标系相对于载体坐标系b的方向余弦矩阵，
Figure FDA0003057069210000015
表示载体坐标系相对于惯性坐标系i的旋转角速度，
Figure FDA0003057069210000016
表示横坐标系相对于惯性坐标系的旋转角速度；
(2.2)确定横坐标系下的速度vt的更新方程为：
Figure FDA0003057069210000017
其中，
Figure FDA0003057069210000018
Figure FDA0003057069210000019
Figure FDA00030570692100000110
式中，fb表示载体坐标系下表示的比力，gt表示横坐标系下表示的重力矢量，
Figure FDA00030570692100000111
表示横坐标系相对于惯性坐标系的旋转角速度在横坐标系下的投影，
Figure FDA00030570692100000112
表示横地球坐标系相对于惯性坐标系的旋转角速度在横坐标系下的投影，
Figure FDA00030570692100000113
表示横坐标系相对于横地球坐标系的旋转角速度在横坐标系下的投影，ωie表示地球旋转角速度，Re为地球半径，
Figure FDA00030570692100000114
表示横坐标系下的东向速度，
Figure FDA00030570692100000115
表示横坐标系下的北向速度；
(2.3)确定横坐标系下的位置更新方程为：
Figure FDA0003057069210000021
Figure FDA0003057069210000022
Figure FDA0003057069210000023
式中，
Figure FDA0003057069210000024
表示横坐标系下的垂向速度；
(3)确定无人潜航器在横坐标系下的姿态误差方程、速度误差方程、位置误差方程，具体实施如下：
确定姿态误差φt的方程如下：
Figure FDA0003057069210000025
其中，
Figure FDA0003057069210000026
表示横坐标系相对于惯性坐标系的旋转角速度误差，
Figure FDA0003057069210000027
示载体坐标系b相对于惯性坐标系的旋转角速度误差；
确定速度误差δvt的方程如下：
Figure FDA0003057069210000028
其中，ft表示横坐标系下表示的比力，，
Figure FDA0003057069210000029
表示横地球坐标系相对于惯性坐标系的旋转角速度误差，
Figure FDA00030570692100000210
表示横坐标系相对于横地球坐标系的旋转角速度误差，δfb表示比力误差；
确定位置误差的方程如下：
Figure FDA00030570692100000211
式中，δLt,δλt,δh分别表示横纬度误差、横经度误差、高度误差，
Figure FDA00030570692100000212
分别表示横坐标系下表示的东向速度误差、北向速度误差、垂向速度误差；
(4)确定无人潜航器导航参数在地理坐标系与横坐标系之间的转换关系并进行转换，导航参数的转换包括姿态转换、速度转换、位置转换；
其中，无人潜航器姿态参数在地理坐标系、横坐标系之间的转换关系为：
Figure FDA00030570692100000213
Figure FDA00030570692100000214
式中，
Figure FDA00030570692100000215
表示地理坐标系n与载体坐标系b之间的方向余弦矩阵，
Figure FDA00030570692100000216
表示横坐标系与地理坐标系之间的方向余弦矩阵，
Figure FDA0003057069210000031
表示地理坐标系与横坐标系之间的方向余弦矩阵，
Figure FDA0003057069210000032
与
Figure FDA0003057069210000033
互为转置矩阵，且有：
Figure FDA0003057069210000034
Figure FDA0003057069210000035
式中，
Figure FDA0003057069210000036
表示地球坐标系与地理坐标系之间的方向余弦矩阵，
Figure FDA0003057069210000037
表示横坐标系与横地球坐标系之间的方向余弦矩阵，L、λ分别表示地理坐标系下的纬度、经度；
无人潜航器速度参数在地理坐标系、横坐标系之间的转换关系为：
Figure FDA0003057069210000038
Figure FDA0003057069210000039
式中，vn表示地理坐标系下表示的速度；
无人潜航器位置参数在横坐标系、地理坐标系之间的转换关系为：
Lt＝arcsin(cos L sinλ) L＝arcsin(cos Ltcosλt)
Figure FDA00030570692100000310
高度h在横坐标系、地理坐标系中保持不变；
(5)完成INS/DVL组合导航滤波器在地理坐标系与横坐标系之间的转换，其中INS/DVL组合导航滤波器采用闭环反馈校正方式，具体实施如下：
(5.1)分别确定地理坐标系与横坐标系下的系统误差状态为：
地理坐标系下的系统误差状态xn(t)为
Figure FDA00030570692100000311
横坐标系下的系统误差状态xt(t)为
Figure FDA00030570692100000312
其中，
Figure FDA00030570692100000313
分别表示地理坐标系下表示的东向、北向、垂向姿态误差，
Figure FDA00030570692100000314
分别表示横坐标系下表示的东向、北向、垂向姿态误差，
Figure FDA00030570692100000315
别表示地理坐标系下表示的东向、北向、垂向速度误差，δL,δλ分别表示地理坐标系下的纬度、经度误差，
Figure FDA00030570692100000316
分别表示x、y、z轴向陀螺常值零偏，
Figure FDA00030570692100000317
分别表示x、y、z轴向加表常值零偏，k,δΨ,δθ分别表示DVL的标度因数误差、方位安装误差角、俯仰安装误差角；
(5.2)分别确定姿态误差、速度误差、位置误差在地理坐标系与横坐标系下间的转换关系为：
首先确定地理坐标系下姿态误差φn与横坐标系下姿态误差φt之间的转换关系
Figure FDA0003057069210000041
式中，
Figure FDA0003057069210000042
Figure FDA0003057069210000043
其次确定地理坐标系下速度误差δvn与横坐标系下速度误差δvt之间的转换关系
Figure FDA0003057069210000044
进而确定横坐标系下表示的纬度误差δLt、经度误差δλt、高度误差δh与地理坐标系下表示的纬度误差δL、经度误差δλ、高度误差δh的转换关系
Figure FDA0003057069210000045
陀螺常值零偏
Figure FDA0003057069210000046
加表常值零偏
Figure FDA0003057069210000047
DVL的标度因数误差、方位安装误差角、俯仰安装误差角k,δΨ,δθ、高度误差δh在地理坐标系与横坐标系下保持不变；
确定横坐标系下的系统误差状态xt(t)与地理坐标系下的系统误差状态xn(t)之间的转换关系如下：
xt(t)＝Φxn(t),xn(t)＝Φ-1xt(t)
其中，Φ为转换系数矩阵，并且根据φt与φn之间的转换关系，δvt与δvn之间的转换关系，δLt,δλt与δL、δλ之间的转换关系，并考虑高度误差δh、陀螺常值零偏
Figure FDA0003057069210000048
加表常值零偏
Figure FDA0003057069210000049
DVL的标度因数误差、方位安装误差角、俯仰安装误差角k,δΨ,δθ在地理坐标系与横坐标系下的不变性进行确定；
(5.3)根据步骤(5.2)，确定地理坐标系下系统误差状态协方差矩阵Pn(t)与横坐标系下系统误差状态协方差矩阵Pt(t)的转换关系：
Figure FDA0003057069210000051
Pn(t)＝Φ-1Pt(t)Φ-T
式中，
Figure FDA0003057069210000052
示横坐标系下表示的系统误差状态估计值，
Figure FDA0003057069210000053
表示地理坐标系下表示的系统误差状态估计值；
(5.4)当无人潜航器在中纬度、高纬度地区跨纬度航行时，闭环反馈INS/DVL组合导航滤波器完成在地理坐标系与横坐标系之间的协方差矩阵转换，转换方式按照步骤(5.3)所述，鉴于系统误差状态在每次闭环反馈校正后为0，其不需要转换。
2.如权利要求1所述的一种无人潜航器惯性/多普勒组合导航方法，其特征在于，若无人潜航器接收到GNSS定位信息时，利用GNSS位置点信息完成对所述步骤(4)中Cn e或Ct e′的装订更新。
3.如权利要求1所述的一种无人潜航器惯性/多普勒组合导航方法，其特征在于，所述步骤(5)中导航参数在地理坐标系与横坐标系之间转换时基于转换时刻的纬度阈值判断，且地理坐标系转换到横坐标系、横坐标系转换到地理坐标系两种情况下的阈值设定不同。
4.如权利要求1所述的一种无人潜航器惯性/多普勒组合导航方法，其特征在于，所述步骤(5)中INS/DVL组合导航滤波器的观测量为DVL速度信息，即在中低纬度的观测量为地理坐标系下速度误差δvn，在高纬度的观测量为横坐标系下速度误差δvt。
5.如权利要求1所述的一种无人潜航器惯性/多普勒组合导航方法，其特征在于，所述步骤(5)中INS/DVL组合导航滤波器的观测量为DVL速度信息和深度计提供的深度信息，即在中低纬度的观测量为地理坐标系下速度误差δvn、δh，在高纬度的观测量为横坐标系下速度误差δvt、δh。
CN202110502743.9A 2021-05-09 2021-05-09 一种无人潜航器惯性/多普勒组合导航方法 Active CN113108783B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110502743.9A CN113108783B (zh) 2021-05-09 2021-05-09 一种无人潜航器惯性/多普勒组合导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110502743.9A CN113108783B (zh) 2021-05-09 2021-05-09 一种无人潜航器惯性/多普勒组合导航方法

Publications (2)

Publication Number Publication Date
CN113108783A CN113108783A (zh) 2021-07-13
CN113108783B true CN113108783B (zh) 2022-06-14

Family

ID=76721378

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110502743.9A Active CN113108783B (zh) 2021-05-09 2021-05-09 一种无人潜航器惯性/多普勒组合导航方法

Country Status (1)

Country Link
CN (1) CN113108783B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114018255B (zh) * 2021-11-03 2023-06-27 湖南国天电子科技有限公司 一种水下滑翔机的智能组合导航方法、系统、设备和介质
CN115031727B (zh) * 2022-03-31 2023-06-20 哈尔滨工程大学 一种基于状态变换的多普勒辅助捷联惯导系统初始对准方法
CN115200574B (zh) * 2022-07-03 2024-04-19 中国人民解放军国防科技大学 一种地球椭球模型下的极区横向组合导航方法
CN115060274B (zh) * 2022-08-17 2022-11-18 南开大学 一种水下一体式自主导航装置及其初始对准方法

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107543545A (zh) * 2017-10-30 2018-01-05 中国人民解放军国防科技大学 极区双航海惯性导航系统定位信息融合方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9341718B2 (en) * 2012-09-07 2016-05-17 Honeywell International Inc. Method and system for providing integrity for hybrid attitude and true heading

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107543545A (zh) * 2017-10-30 2018-01-05 中国人民解放军国防科技大学 极区双航海惯性导航系统定位信息融合方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Transfer alignment method for SINS based on reverse navigation solution and data fusion;Sun Jin 等;《Journal of Chinese Inertial Technology》;20151231;第23卷(第6期);第727-732页 *
航海多惯导协同定位与误差参数估计;王林;《中国博士学位论文全文数据库工程科技Ⅱ辑》;20200215(第02期);第C032-36页 *

Also Published As

Publication number Publication date
CN113108783A (zh) 2021-07-13

Similar Documents

Publication Publication Date Title
CN113108783B (zh) 一种无人潜航器惯性/多普勒组合导航方法
CN109211269B (zh) 一种双轴旋转惯导系统姿态角误差标定方法
US9574881B2 (en) Method and system for controlling antenna of mobile communication application system based on double quaternions in MEMS inertial navigation
CN115200574B (zh) 一种地球椭球模型下的极区横向组合导航方法
CN108594283B (zh) Gnss/mems惯性组合导航系统的自由安装方法
CN107806874B (zh) 一种视觉辅助的捷联惯导极区初始对准方法
CN108731674B (zh) 一种基于单轴旋转调制的惯性天文组合导航系统及计算方法
CN108225325B (zh) 基于虚拟球模型的极地横向导航方法
CN108426575A (zh) 用地球椭球模型改进的捷联惯导极地横向导航方法
CN105928515A (zh) 一种无人机导航系统
CN104776847B (zh) 一种适用于水下导航系统单点估计陀螺漂移的方法
JP2002158525A (ja) 衛星追尾用アンテナ制御装置
CN113108787B (zh) 一种长航时惯导/卫星全球组合导航方法
CN113568442B (zh) 一种对星控制系统及方法
CN105928519B (zh) 基于ins惯性导航与gps导航以及磁力计的导航算法
CN109506662B (zh) 一种小天体着陆初始对准方法、其相对导航基准确定方法及装置
CN110986934A (zh) 一体化双轴旋转惯导天文组合导航系统的导航方法及系统
CN105300407B (zh) 一种用于单轴调制激光陀螺惯导系统的海上动态启动方法
CN112833878A (zh) 近地面多源天文自主导航方法
CN113108788B (zh) 一种长航时惯导/天文全球组合导航方法
CN113155125B (zh) 一种大飞机ins/gnss全球组合导航方法
CN113218390B (zh) 基于姿态和恒星高度角融合的旋转惯性天文组合导航方法
CN111964671B (zh) 一种基于双轴旋转调制的惯性天文组合导航系统及方法
CN113985464A (zh) 一种用于车载卫星动中通的零位标定方法及系统
CN113108786B (zh) 考虑飞行可靠性的长航时旋转惯导/卫星组合导航方法

Legal Events

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