CN113670335A - 基于dvl辅助和向量截断化k矩阵的水下载体初始对准方法 - Google Patents

基于dvl辅助和向量截断化k矩阵的水下载体初始对准方法 Download PDF

Info

Publication number
CN113670335A
CN113670335A CN202110947453.5A CN202110947453A CN113670335A CN 113670335 A CN113670335 A CN 113670335A CN 202110947453 A CN202110947453 A CN 202110947453A CN 113670335 A CN113670335 A CN 113670335A
Authority
CN
China
Prior art keywords
matrix
vector
representing
time
state
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.)
Granted
Application number
CN202110947453.5A
Other languages
English (en)
Other versions
CN113670335B (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.)
Hohai University HHU
Original Assignee
Hohai University HHU
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 Hohai University HHU filed Critical Hohai University HHU
Priority to CN202110947453.5A priority Critical patent/CN113670335B/zh
Publication of CN113670335A publication Critical patent/CN113670335A/zh
Application granted granted Critical
Publication of CN113670335B publication Critical patent/CN113670335B/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)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种基于多普勒计程仪(Doppler Velocity Log，DVL)辅助和向量截断化K矩阵的水下载体初始对准方法，首先获取各传感器数据，并对数据预处理；根据传感器数据计算观测矢量和参考矢量，建立K矩阵的状态矩阵方程和量测矩阵方程；利用K矩阵的零迹性和对称性，将状态矩阵方程和量测矩阵方程向量化后再截断；然后利用卡尔曼滤波估计最优K矩阵；从最优估计K矩阵中提取出最优四元数，将其转换成姿态矩阵方程；最后利用姿态矩阵链式方程，得到当前时刻的姿态矩阵，解算出失准角，完成初始对准过程。本发明打破了传统的初始对准框架，不再区分粗对准和精对准过程，也不再区分大失准角以及小失准角，改进了初始对准的收敛速度和精度。

Description

基于DVL辅助和向量截断化K矩阵的水下载体初始对准方法
技术领域
本发明涉及导航系统初对准技术领域中，基于Wahba问题的最优K矩阵姿态估计方法，尤其涉及一种基于DVL辅助和向量截断化K矩阵的水下载体初始对准方法。
背景技术
海洋观测是认识海洋的基本手段，是海洋经济开发、环境保护和权益维护的基础。实施“透明海洋”战略，加强海洋观测技术装备开发，加快海洋观测体系建设，是目前海洋科技创新的一个重要方向。水下自主潜航器(Autonomous Underwater Vehicle,AUV)作为海洋观测技术的重要装备，以其隐蔽性好、可代替人类执行危险任务等优点，受到研究人员广泛关注。
目前，水下载体包含有水下自主潜航器(Autonomous Underwater Vehicle,AUV)、遥控无人潜水器ROV，以及混合水下无人载体HUV。其中，AUV作为水下载体在执行各种水下任务时，要求导航、制导与控制系统之间能够相互协调实现正常运转，导航技术是决定AUV技术水平的一个特别重要标志。显然，要达到定位的目的，需要已知运载体的初始速度和位置，初始对准精度的高低很大程度上决定了导航的精度。捷联惯性导航系统初始对准分为粗对准和精对准两部分，其中精对准需要在粗对准的基础上完成。
基于惯性系的姿态确定(Attitude Determination,AD)方法能够提高水下载体的初始对准性能，目前，采用姿态确定来进行初始对准时，有两种方法：一种是确定姿态矩阵(DCM)，然而，由于惯性测量单元(IMU)测量的观测矢量加速度包含随机噪声，该方法并没有获得良好的粗对准性能。另一种解决AD问题的方法是确定相应的姿态四元数。众多学者在此基础上提出了多种确定姿态四元数的方法，其中D.Choukroun等人研究的Optimal-REQUEST算法具有鲁棒性强，计算精度高等优点，是四元数的最优递归时变估计。但Optimal-REQUEST算法依赖于保守的估计性能指标和标量增益来估计K矩阵，而标量增益的设定很大程度上决定了算法的精度。
因此，如何通过具体的方法来对水下载体进行快速的初始对准，进而克服现有技术中的不足之处，成为亟需解决的技术问题。
发明内容
发明目的：在对水下载体进行初始对准时，为克服Optimal-REQUEST算法依赖于一个保守的估计性能指标和标量增益来估计K矩阵的缺点，本发明提供了一种基于DVL辅助和向量截断化K矩阵的水下载体初始对准方法，将K矩阵的矩阵状态空间方程进行向量化，并利用K矩阵元素之间的线性相关性截断得到状态向量，在其基础上应用卡尔曼滤波来完成对水下载体的初始对准过程，解决了量测矢量存在噪声影响时初始对准问题。
技术方案：本发明基于DVL辅助和向量截断化K矩阵的水下载体初始对准方法包括以下步骤：
(1)定义解算所需的参考标准坐标系；
(2)获取惯性传感器实时数据以及DVL实时数据；
(3)根据载体的比力方程
Figure BDA0003217217940000021
将比力方程两边同时积分，再忽略小量得到如下表达式：
Figure BDA0003217217940000022
进而构造观测矢量ξ和参考矢量r；然后根据步骤(2)获取的惯性传感器实时数据和DVL实时数据，来解算当前时刻的观测矢量ξ和参考矢量r；
观测矢量ξ和参考矢量r的构造方法如式(1)至式(4)所示：
(3.1)构造观测矢量ξ：
Figure BDA0003217217940000023
Figure BDA0003217217940000024
式中，Vb(0)表示初始时刻DVL采集到的速度矢量，Vb(t)表示t时刻DVL采集到的速度矢量；
Figure BDA0003217217940000025
表示载体坐标系b从初始时刻到t时刻的姿态转移矩阵，b0表示载体坐标系b的初始时刻；fb表示在载体坐标系中测量到的比力矢量；
Figure BDA0003217217940000026
表示地球自转角速率在导航坐标系中的投影；×表示求该向量的反对称矩阵；ξ为规范化后的观测矢量；β表示直接由比力方程得到的未规范化的观测矢量；
(3.2)构建参考矢量r：
Figure BDA0003217217940000027
Figure BDA0003217217940000028
式中，
Figure BDA0003217217940000029
表示从初始时刻到t时刻的导航坐标系n的姿态转移矩阵；n0表示导航坐标系n的初始时刻；gn表示当地重力矢量；r为规范化后的参考矢量；α表示由比力方程得到的未规范化的参考矢量；
(4)利用步骤(3)中解算的观测矢量ξ和参考矢量r来构建向量截断化后的K矩阵，写出截断化后的K矩阵对应的状态方程和量测方程，并采用卡尔曼滤波的方法对K矩阵进行更新；具体过程为：
在滤波开始前，利用步骤(3)解算的观测矢量ξ和参考矢量r来构建K矩阵，并利用K矩阵的零迹性和对称性将K矩阵向量化后再截断成9维的状态向量初值x0；
卡尔曼滤波开始后，步骤(3)解算的观测矢量ξ和参考矢量r用来构建δK矩阵，同样利用δK矩阵的零迹性和对称性将δK矩阵向量化后再截断成9维的量测向量y，用于对K矩阵进行更新；
(5)从步骤(4)中更新后的K矩阵中提取最优四元数，将最优四元数转化成姿态矩阵方程，再利用姿态矩阵链式方程解算出当前时刻的姿态矩阵，从中计算出失准角，完成初始对准过程。
步骤(2)中，惯性传感器实时数据包括水下自主潜航器的加速度和角速率信息，DVL实时数据包括水下自主潜航器的线速度信息。
步骤(4)的具体过程为：
(4.1)构造k时刻K矩阵：
Figure BDA0003217217940000031
Figure BDA0003217217940000032
式中，ai为标量权重系数，规定
Figure BDA0003217217940000033
tr(■)表示对矩阵求迹，即tr(Bk)表示对Bk求迹；T表示对矩阵求转置；ξi表示k时刻第i个观测矢量；ri表示k时刻第i个参考矢量；Bk表示由ξi和ri构成的三维矩阵；Sk表示由ξi和ri构成的三维矩阵；zk表示由ξi和ri构成的三维向量；σk表示矩阵Bk的迹；Kk表示k时刻的K矩阵；I3表示3×3的单位矩阵；
(4.2)利用K矩阵的零迹性和对称性，将K矩阵向量化后再截断成9维状态向量：
Figure BDA0003217217940000041
式中，
Figure BDA0003217217940000042
表示Kk矩阵的第i行第j列的元素，i＝1～3；j＝1～4；xk代表k时刻的状态向量；
(4.3)构造k+1时刻的δKk+1矩阵：
Figure BDA0003217217940000043
Figure BDA0003217217940000044
式中，tr(■)表示对矩阵求迹；T表示对矩阵求转置；δKk+1代表k+1时刻的δK矩阵；ξk+1表示k+1时刻单个观测矢量；rk+1表示k+1时刻单个参考矢量；Bk+1表示由ξk+1和rk+1构成的三维矩阵；Sk+1表示由ξk+1和rk+1构成的三维矩阵；zk+1表示由ξk+1和rk+1构成的三维向量；σk+1表示矩阵Bk+1的迹；rk+1 T代表rk+1的转置；Bk+1 T代表Bk+1的转置；
(4.4)利用δKk+1的零迹性与对称性，将δKk+1矩阵向量截断化成9维量测向量：
Figure BDA0003217217940000045
式中，
Figure BDA0003217217940000046
表示δKk+1矩阵的第i行第j列的元素，i＝1～3；j＝1～4；yk代表k时刻的量测向量；
(4.5)建立状态方程模型和量测方程模型
状态方程表达式如下：
Figure BDA0003217217940000047
式中，xk为k时刻9×1的状态向量；xk+1为k+1时刻9×1的状态向量；Ψk为状态转移矩阵，对于初始对准过程而言，简化成Ψk＝I9；
Figure BDA0003217217940000048
为高斯白噪声；Γk为9×3状态相关的噪声增益矩阵；
量测方程表达式如下：
Figure BDA0003217217940000051
Figure BDA0003217217940000052
式中，xk+1为k+1时刻9×1的状态向量；yk+1为k+1时刻9×1的量测向量；
Figure BDA0003217217940000053
为量测向量相关的量测噪声矩阵；其中，ai为k+1时刻第i个观测矢量的权重系数，规定
Figure BDA0003217217940000054
Figure BDA0003217217940000055
表示k+1时刻第i个观测矢量误差；
Figure BDA0003217217940000056
表示k+1时刻第i个9×3的噪声增益矩阵；
(4.6)进行卡尔曼滤波，更新K矩阵
初始化：
Figure BDA0003217217940000057
时间更新；
Figure BDA0003217217940000058
Figure BDA0003217217940000059
式中，Ψk表示k时刻状态转移矩阵；
Figure BDA00032172179400000510
表示k时刻最优状态估计；
Figure BDA00032172179400000511
表示k+1时刻状态一步预测；Pk/k表示k时刻状态误差协方差矩阵；Pk+1/k表示k+1时刻状态误差协方差矩阵一步预测；
Figure BDA00032172179400000512
表示系统噪声方程阵；
Figure BDA00032172179400000513
表示初始时刻的状态向量，y0表示初始时刻的量测向量；Γk为9×3状态相关的噪声增益矩阵；
量测更新：
Gk+1＝Pk+1/k(Pk+1/k+Rk+1)-1 (20)
Figure BDA00032172179400000514
Figure BDA00032172179400000515
式中，
Figure BDA00032172179400000516
表示k+1时刻状态一步预测；yk+1表示k+1时刻系统量测；Pk+1/k表示k+1时刻状态误差协方差矩阵一步预测；Gk+1表示k+1时刻的卡尔曼增益；I9为9维的单位阵；
Figure BDA00032172179400000517
表示k+1时刻对状态向量x的估计值；Pk+1/k+1表示k+1时刻对协方差矩阵的估计值；Gk+1 T表示k+1时刻的卡尔曼增益的转置；Rk+1表示k+1时刻量测噪声方差阵，其具体定义为：
Figure BDA0003217217940000061
其中，ai为k+1时刻第i个观测矢量的权重系数，规定
Figure BDA0003217217940000062
Figure BDA0003217217940000063
表示k+1时刻第i个观测矢量误差；
Figure BDA0003217217940000064
表示k+1时刻第i个9×3的噪声增益矩阵，定义为：
Figure BDA0003217217940000065
式中，
Figure BDA0003217217940000066
代表rk+1向量中的第1个元素，
Figure BDA0003217217940000067
代表rk+1向量中的第2个元素，
Figure BDA0003217217940000068
代表rk+1向量中的第3个元素。
步骤(4.2)中，利用K矩阵的零迹性和对称性，将K矩阵向量化后再截断成9维状态向量，即将K矩阵的上三角矩阵的元素分别提取出来，按照行数列数由小到大的顺序构成状态向量。
步骤(4.4)中，利用δKk+1的零迹性与对称性，将δKk+1矩阵向量截断化成9维量测向量，即将δKk+1矩阵的上三角矩阵的元素分别提取出来，按照行数列数由小到大的顺序构成量测向量。
步骤(4.5)中，该Γk为9×3状态相关的噪声增益矩阵，定义如下：
Figure BDA0003217217940000071
式中，Δt表示采样时间；
Figure BDA0003217217940000072
表示k时刻xk状态向量的第i个元素，i＝1～9。
步骤(4.5)中，
Figure BDA0003217217940000073
为k+1时刻第i个9×3的噪声增益矩阵，定义如下：
Figure BDA0003217217940000074
式中，
Figure BDA0003217217940000075
代表rk+1向量中的第1个元素，
Figure BDA0003217217940000076
代表rk+1向量中的第2个元素，
Figure BDA0003217217940000077
代表rk+1向量中的第3个元素。
步骤(4.6)中，Γk为9×3状态相关的噪声增益矩阵，具体表达式见下：
Figure BDA0003217217940000081
式中，Δt表示采样时间；xki表示k时刻9维xk状态向量的第i个元素，i＝1～9。
步骤(5)的具体过程如下：
(5.1)将卡尔曼滤波后得到的状态向量重写回K矩阵形式；
Figure BDA0003217217940000082
Figure BDA0003217217940000083
式中，
Figure BDA0003217217940000084
为滤波后的状态向量最佳估计值
Figure BDA0003217217940000085
对应的第i个元素，i＝1～10；
(5.2)基于Wahba问题求解K矩阵最大特征值，所述K矩阵最大特征值对应的特征向量即为最优四元数，从更新后的K矩阵提取最优四元数的计算方法为：
Figure BDA0003217217940000086
Figure BDA0003217217940000087
式中，
Figure BDA0003217217940000088
表示最优四元数中的矢量部分；
Figure BDA0003217217940000089
表示最优四元数；
Figure BDA00032172179400000810
与
Figure BDA00032172179400000811
为k时刻从最优估计K矩阵
Figure BDA00032172179400000812
中按照K矩阵定义提取的最优估计，具体提取方法为：
Figure BDA00032172179400000813
为步骤5中计算得到的K矩阵中的第4行第4列元素，
Figure BDA00032172179400000814
为步骤5中计算得到的K矩阵中第4个列向量对应的前3个元素；
Figure BDA0003217217940000091
的计算公式为
Figure BDA0003217217940000092
其中K3×3表示K矩阵左上角的3维方阵；
(5.3)将最优四元数转换成初始时刻姿态矩阵
Figure BDA0003217217940000093
Figure BDA0003217217940000094
其中，q0表示四元数的第1个元素；q1表示四元数的第2个元素；q2表示四元数的第3个元素；q3表示四元数的第4个元素；
Figure BDA0003217217940000095
Cij表示初始时刻姿态矩阵
Figure BDA0003217217940000096
第i行第j列的元素，i＝1～3，j＝1～3；
(5.4)利用矩阵链式乘法求得当前时刻的姿态矩阵，实时解算姿态角：
姿态矩阵链式乘法表达式为：
Figure BDA0003217217940000097
式中，
Figure BDA0003217217940000098
表示t时刻导航坐标系n到载体坐标系b的姿态转移矩阵；
Figure BDA0003217217940000099
表示从0时刻到t时刻，载体坐标系b的姿态转移矩阵；
Figure BDA00032172179400000910
表示从0时刻到t时刻，导航坐标系n的姿态转移矩阵；
Figure BDA00032172179400000911
表示初始时刻0时刻时，导航坐标系n到载体坐标系b的姿态转移矩阵。
假设载体的方位角为ψ，俯仰角为θ，倾斜角为γ，实时解算姿态角得：
Figure BDA00032172179400000912
(5.5)：求取姿态角的均值，所述均值为最终的失准角，进而完成初始对准过程。
步骤(5.5)中，当实时解算的姿态角曲线收敛后，求取收敛稳定时间内姿态角的均值。
工作原理：本发明基于多普勒计程仪(DopplerVelocityLog，DVL)辅助和向量截断化K矩阵的水下载体初始对准方法，首先获取各传感器数据，并对数据预处理；根据传感器数据计算观测矢量和参考矢量，建立K矩阵的状态矩阵方程和量测矩阵方程；利用K矩阵的零迹性和对称性，将状态矩阵方程和量测矩阵方程向量化后再截断；然后利用卡尔曼滤波估计最优K矩阵；接着从最优估计K矩阵中提取出最优四元数，将最优四元数转换成姿态矩阵方程；最后利用姿态矩阵链式方程，得到当前时刻的姿态矩阵，从中解算出失准角，完成初始对准过程。在估计过程中最大程度的保留了估计四元数的单位范数特性，在受益于卡尔曼滤波的最优概率基础的同时，避免了卡尔曼滤波固有的线性化和四元数归一化的缺陷。
有益效果：与现有技术相比，本发明具有以下优点：
(1)本发明基于Wahba问题并利用最优K矩阵进行姿态估计，将状态矩阵模型向量化后再进行截断，采用卡尔曼滤波方法来解决量测矢量存在噪声影响时初始对准问题，弥补了Optimal-REQUEST算法最优增益为标量，以及估计代价函数较为保守这两大缺点。
(2)本发明打破了传统的初始对准框架，不再区分粗对准和精对准过程，也不再区分大失准角以及小失准角，能够适用于各类复杂环境中。
(3)本发明引入了DVL外部设备辅助，实现了载体在水下动态初始对准。
(4)本发明中的滤波算法使估计四元数的单位范数特性在估计过程中得到了最优的保留，并且在受益于卡尔曼滤波的最优概率基础的同时，避免了卡尔曼滤波固有的线性化和四元数归一化的传统陷阱，改进了初始对准精度。
(5)本发明将向量化K矩阵的卡尔曼滤波器维数通过K矩阵的零迹特征和对称性截断成9维，减少了滤波算法的计算量，加快了初始对准的收敛速度。
附图说明
图1为本发明基于DVL辅助和向量截断化K矩阵的水下载体初始对准方法整体流程示意图；
图2为图1中利用Kalman滤波估计最优K矩阵和解算最优四元数的流程图。
具体实施方式
实施例：
本发明以AUV为例，采用基于DVL辅助和向量截断化K矩阵的水下载体初始对准方法来实施具体过程，首先定义解算所需的参考标准坐标系，获取惯性传感器数据以及DVL实时数据，并对数据预处理，计算出观测矢量ξ和参考矢量r；建立系统的矩阵状态方程和量测方程；利用K矩阵零迹性和对称性，将矩阵状态方程和量测方程向量化后再截断；接着采用卡尔曼滤波进行滤波，得到最优估计的K矩阵；最后将最优四元数从最优估计K矩阵中提取出来，利用姿态矩阵链式法则得到当前姿态矩阵，进而计算出失准角，以完成初始对准过程。
如图1所示，本发明基于DVL辅助和向量截断化K矩阵的水下载体初始对准方法具体包括以下步骤：
步骤(1)：定义解算所需的参考标准坐标系；
步骤(2)：获取惯性传感器实时数据以及DVL实时数据，惯性传感器实时数据包括水下自主潜航器的加速度和角速率信息，DVL实时数据包括DVL采集到的水下自主潜航器的线速度信息；其中，该线速度对应比力方程的Vb，具体见步骤(3)中比力方程的公式，进而使观测矢量更加准确；
步骤(3)：根据载体的比力方程
Figure BDA0003217217940000111
将比力方程两边同时积分，再忽略小量得到如下表达式，
Figure BDA0003217217940000112
进而定义观测矢量ξ和参考矢量r；然后根据步骤(2)获取的惯性传感器实时数据和DVL实时数据，来解算当前时刻的观测矢量ξ和参考矢量r；从上述内容可知，该比力方程是为了构建观测矢量和参考矢量，是本发明中水下载体初始对准的基础；观测矢量ξ和参考矢量r的构造方法如式(1)至式(4)所示：
(3.1)构建观测矢量ξ：
Figure BDA0003217217940000113
Figure BDA0003217217940000114
式中，Vb(0)表示初始时刻DVL采集到的速度矢量，Vb(t)表示t时刻DVL采集到的速度矢量；
Figure BDA0003217217940000115
表示载体坐标系b从初始时刻到t时刻的姿态转移矩阵；b0表示载体坐标系b的初始时刻；fb表示在载体坐标系中测量到的比力矢量；
Figure BDA0003217217940000116
表示地球自转角速率在导航坐标系中的投影；×表示求该向量的反对称矩阵；ξ为规范化后的观测矢量；β表示未规范化的观测矢量；
(3.2)构建参考矢量r：
Figure BDA0003217217940000117
Figure BDA0003217217940000118
式中，
Figure BDA0003217217940000119
表示从初始时刻到t时刻的导航坐标系n的姿态转移矩阵；n0表示导航坐标系n的初始时刻；gn表示当地重力矢量；r为规范化后的参考矢量；α表示由比力方程得到的未规范化的参考矢量；
步骤(4)：利用步骤(3)中解算的观测矢量ξ和参考矢量r来构建向量截断化后的K矩阵，写出截断化后的K矩阵对应的状态方程和量测方程，并采用卡尔曼滤波的方法对K矩阵进行更新；具体过程为：
在卡尔曼滤波开始前，利用步骤(3)解算的观测矢量ξ和参考矢量r来构建K矩阵，并利用K矩阵的零迹性和对称性将K矩阵向量化后再截断成9维的状态向量初值x0。
卡尔曼滤波开始后，步骤(3)解算的观测矢量ξ和参考矢量r用来构建δK矩阵，同样利用δK矩阵的零迹性和对称性将δK矩阵向量化后再截断成9维的量测向量y，用于对K矩阵进行更新；具体过程为：
(4.1)构造k时刻K矩阵：
Figure BDA0003217217940000121
Figure BDA0003217217940000122
式中，ai为标量权重系数，规定
Figure BDA0003217217940000123
tr(■)表示对矩阵求迹；即tr(Bk)表示对Bk求迹；T表示对矩阵求转置；ξi表示k时刻第i个观测矢量；ri表示k时刻第i个参考矢量；Bk表示由ξi和ri构成的三维矩阵；Sk表示由ξi和ri构成的三维矩阵；zk表示由ξi和ri构成的三维向量；σk表示矩阵Bk的迹；Kk表示k时刻的K矩阵；I3表示3×3的单位矩阵；
(4.2)利用K矩阵的零迹性和对称性，将K矩阵向量化后再截断成9维状态向量：即将K矩阵的上三角矩阵的元素分别提取出来，按照行数列数由小到大的顺序构成状态向量：
Figure BDA0003217217940000124
式中，
Figure BDA0003217217940000125
表示Kk矩阵的第i行第j列的元素，i＝1～3；j＝1～4，ij为11，12,13,14，22,23,24,33,34九种组合；xk代表k时刻的状态向量。
(4.3)构造k+1时刻的δKk+1矩阵：
Figure BDA0003217217940000131
Figure BDA0003217217940000132
式中，tr(·)表示对矩阵求迹；T表示对矩阵求转置；δKk+1代表k+1时刻的δK矩阵；ξk+1表示k+1时刻单个观测矢量；rk+1表示k+1时刻单个参考矢量；Bk+1表示由ξk+1和rk+1构成的三维矩阵；Sk+1表示由ξk+1和rk+1构成的三维矩阵；zk+1表示由ξk+1和rk+1构成的三维向量；σk+1表示矩阵Bk+1的迹；rk+1 T代表rk+1的转置；Bk+1 T代表Bk+1的转置；
(4.4)利用δKk+1的零迹性与对称性，将δKk+1矩阵向量截断化成9维量测向量，即将δKk+1矩阵的上三角矩阵的元素分别提取出来，按照行数列数由小到大的顺序构成量测向量；
Figure BDA0003217217940000133
式中，
Figure BDA0003217217940000134
表示δKk+1矩阵的第i行第j列的元素，i＝1～3；j＝1～4；ij为11，12,13,14，22,23,24,33,34九种组合；yk代表k时刻的量测向量；
(4.5)建立状态方程模型和量测方程模型
状态方程表达式如下：
Figure BDA0003217217940000135
式中，xk为k时刻9×1的状态向量；xk+1为k+1时刻9×1的状态向量；Ψk为状态转移矩阵，对于初始对准过程而言，简化成Ψk＝I9；
Figure BDA0003217217940000136
为高斯白噪声；Γk为9×3状态相关的噪声增益矩阵，定义如下：
Figure BDA0003217217940000141
式中，Δt表示采样时间；
Figure BDA0003217217940000142
表示k时刻xk状态向量的第i个元素，i＝1～9；量测方程表达式如下：
Figure BDA0003217217940000143
Figure BDA0003217217940000144
式中，xk+1为k+1时刻9×1的状态向量；yk+1为k+1时刻9×1的量测向量；
Figure BDA0003217217940000145
为量测向量相关的量测噪声矩阵；其中，ai为k+1时刻第i个观测矢量的权重系数，规定
Figure BDA0003217217940000146
Figure BDA0003217217940000147
表示k+1时刻第i个观测矢量误差；
Figure BDA0003217217940000148
表示k+1时刻第i个9×3的噪声增益矩阵，定义如下：
Figure BDA0003217217940000149
式中，
Figure BDA00032172179400001410
代表rk+1向量中的第1个元素，
Figure BDA00032172179400001411
代表rk+1向量中的第2个元素，
Figure BDA00032172179400001412
代表rk+1向量中的第3个元素；
(4.6)进行卡尔曼滤波，更新K矩阵
初始化：
Figure BDA0003217217940000151
时间更新；
Figure BDA0003217217940000152
Figure BDA0003217217940000153
式中，Ψk表示k时刻状态转移矩阵；
Figure BDA0003217217940000154
表示k时刻最优状态估计；
Figure BDA0003217217940000155
表示k+1时刻状态一步预测；Pk/k表示k时刻状态误差协方差矩阵；Pk+1/k表示k+1时刻状态误差协方差矩阵一步预测；
Figure BDA0003217217940000156
表示系统噪声方程阵；
Figure BDA0003217217940000157
表示初始时刻的状态向量，y0表示初始时刻的量测向量；Γk为9×3状态相关的噪声增益矩阵，具体表达式见下：
Figure BDA0003217217940000158
式中，Δt表示采样时间；xki表示k时刻9维xk状态向量的第i个元素，i＝1～9；
量测更新：
Gk+1＝Pk+1/k(Pk+1/k+Rk+1)-1(20)
Figure BDA0003217217940000159
Figure BDA00032172179400001510
式中，
Figure BDA00032172179400001511
表示k+1时刻状态一步预测；yk+1表示k+1时刻系统量测；Pk+1/k表示k+1时刻状态误差协方差矩阵一步预测；Gk+1表示k+1时刻的卡尔曼增益；I9为9维的单位阵；
Figure BDA0003217217940000161
表示k+1时刻对状态向量x的估计值；Pk+1/k+1表示k+1时刻对协方差矩阵的估计值；Gk+1 T表示k+1时刻的卡尔曼增益的转置；Rk+1表示k+1时刻量测噪声方差阵，其具体定义为：
Figure BDA0003217217940000162
其中，ai为k+1时刻第i个量测向量的权重系数，规定
Figure BDA0003217217940000163
Figure BDA0003217217940000164
表示k+1时刻第i个观测矢量误差；
Figure BDA0003217217940000165
为k+1时刻第i个9×3的噪声增益矩阵，定义为：
Figure BDA0003217217940000166
式中，
Figure BDA0003217217940000167
代表rk+1向量中的第1个元素，
Figure BDA0003217217940000168
代表rk+1向量中的第2个元素，
Figure BDA0003217217940000169
代表rk+1向量中的第3个元素；
步骤(5)：从步骤(4)中更新后的K矩阵中提取最优四元数，将最优四元数转化成姿态矩阵方程，再利用姿态矩阵链式方程解算出当前时刻的姿态矩阵，从中计算出失准角，完成初始对准过程；具体过程如下：
步骤(5.1)：将卡尔曼滤波后得到的状态向量重写回K矩阵形式；
Figure BDA00032172179400001610
Figure BDA00032172179400001611
式中，
Figure BDA0003217217940000171
为滤波后的状态向量最佳估计值
Figure BDA0003217217940000172
对应的第i个元素，i＝1～10；
步骤(5.2)：基于Wahba问题利用最优K矩阵进行姿态估计，其中，Wahba问题是在载体坐标系的参考向量和观测向量中，用数字或矢量确定载体姿态的问题，该问题的最优解应当使损失函数达到最小，求解最小损失函数的方法为构建K矩阵，求解K矩阵最大特征值，该最大特征值对应的特征向量即为最优四元数，从更新后的K矩阵提取最优四元数的计算方法为：
Figure BDA0003217217940000173
Figure BDA0003217217940000174
式中，
Figure BDA0003217217940000175
表示最优四元数中的矢量部分；
Figure BDA0003217217940000176
表示最优四元数；
Figure BDA0003217217940000177
与
Figure BDA0003217217940000178
为k时刻从最优估计K矩阵
Figure BDA0003217217940000179
中按照K矩阵定义提取的最优估计，具体提取方法为：
Figure BDA00032172179400001710
为步骤5中计算得到的K矩阵中的第4行第4列元素，
Figure BDA00032172179400001711
为步骤5中计算得到的K矩阵中第4个列向量对应的前3个元素；
Figure BDA00032172179400001712
的计算公式为
Figure BDA00032172179400001713
其中K3×3表示K矩阵左上角的3维方阵；
步骤(5.3)：将最优四元数转换成初始时刻姿态矩阵
Figure BDA00032172179400001714
Figure BDA00032172179400001715
其中，q0表示四元数的第1个元素；q1表示四元数的第2个元素；q2表示四元数的第3个元素；q3表示四元数的第4个元素。
Figure BDA00032172179400001716
Cij表示初始时刻姿态矩阵
Figure BDA00032172179400001717
第i行第j列的元素，i＝1～3，j＝1～3；具体为，C11表示初始时刻姿态矩阵
Figure BDA00032172179400001718
的第1行第1列的元素，C12表示初始时刻姿态矩阵
Figure BDA00032172179400001719
的第1行第2列的元素，C13表示初始时刻姿态矩阵
Figure BDA00032172179400001720
的第1行第3列的元素；C21表示初始时刻姿态矩阵
Figure BDA00032172179400001721
的第2行第1列的元素，C22表示初始时刻姿态矩阵
Figure BDA00032172179400001722
的第2行第2列的元素，C23表示初始时刻姿态矩阵
Figure BDA00032172179400001723
的第2行第3列的元素；C31表示初始时刻姿态矩阵
Figure BDA00032172179400001724
的第3行第1列的元素，C32表示初始时刻姿态矩阵
Figure BDA0003217217940000181
的第3行第2列的元素，C33表示初始时刻姿态矩阵
Figure BDA0003217217940000182
的第3行第3列的元素。
步骤(5.4)：利用矩阵链式乘法求得当前时刻的姿态矩阵，实时解算姿态角：
姿态矩阵链式乘法表达式为：
Figure BDA0003217217940000183
式中，
Figure BDA0003217217940000184
表示t时刻导航坐标系n到载体坐标系b的姿态转移矩阵；
Figure BDA0003217217940000185
表示从0时刻到t时刻，载体坐标系b的姿态转移矩阵；
Figure BDA0003217217940000186
表示从0时刻到t时刻，导航坐标系n的姿态转移矩阵；
Figure BDA0003217217940000187
表示初始时刻0时刻时，导航坐标系n到载体坐标系b的姿态转移矩阵。
假设载体的方位角为ψ，俯仰角为θ，倾斜角为γ，实时解算姿态角得：
Figure BDA0003217217940000188
步骤(5.5)：当实时解算的姿态角曲线收敛后，求取收敛时间内姿态角的均值，该均值即为最终的失准角，进而完成初始对准过程。
通过以上方法，本发明在需要快速对准的环境下快速获取较准确的初始对准失准角。

Claims (10)

1.一种基于DVL辅助和向量截断化K矩阵的水下载体初始对准方法，其特征在于：所述方法包括以下步骤：
(1)定义解算所需的参考标准坐标系；
(2)获取惯性传感器实时数据以及DVL实时数据；
(3)根据载体的比力方程
Figure FDA0003217217930000011
将比力方程两边同时积分，再忽略小量得到如下表达式：
Figure FDA0003217217930000012
进而构造观测矢量ξ和参考矢量r；然后根据步骤(2)获取的惯性传感器实时数据和DVL实时数据，来解算当前时刻的观测矢量ξ和参考矢量r；
所述观测矢量ξ和参考矢量r的构造方法如式(1)至式(4)所示：
(3.1)构造观测矢量ξ：
Figure FDA0003217217930000013
Figure FDA0003217217930000014
式中，Vb(0)表示初始时刻DVL采集到的速度矢量，Vb(t)表示t时刻DVL采集到的速度矢量；
Figure FDA0003217217930000015
表示载体坐标系b从初始时刻到t时刻的姿态转移矩阵，b0表示载体坐标系b的初始时刻；fb表示在载体坐标系中测量到的比力矢量；
Figure FDA0003217217930000016
表示地球自转角速率在导航坐标系中的投影；×表示求该向量的反对称矩阵；ξ为规范化后的观测矢量；β表示直接由比力方程得到的未规范化的观测矢量；
(3.2)构建参考矢量r：
Figure FDA0003217217930000017
Figure FDA0003217217930000018
式中，
Figure FDA0003217217930000019
表示从初始时刻到t时刻的导航坐标系n的姿态转移矩阵；n0表示导航坐标系n的初始时刻；gn表示当地重力矢量；r为规范化后的参考矢量；α表示由比力方程得到的未规范化的参考矢量；
(4)利用步骤(3)中解算的观测矢量ξ和参考矢量r来构建向量截断化后的K矩阵，写出截断化后的K矩阵对应的状态方程和量测方程，并采用卡尔曼滤波的方法对K矩阵进行更新；具体过程为：
在卡尔曼滤波开始前，利用步骤(3)解算的观测矢量ξ和参考矢量r来构建K矩阵，并利用K矩阵的零迹性和对称性将K矩阵向量化后再截断成9维的状态向量初值x0；
卡尔曼滤波开始后，步骤(3)解算的观测矢量ξ和参考矢量r用来构建δK矩阵，同样利用δK矩阵的零迹性和对称性将δK矩阵向量化后再截断成9维的量测向量y，用于对K矩阵进行更新；
(5)从步骤(4)中更新后的K矩阵中提取最优四元数，将最优四元数转化成姿态矩阵方程，再利用姿态矩阵链式方程解算出当前时刻的姿态矩阵，从中计算出失准角，完成初始对准过程。
2.根据权利要求1所述的基于DVL辅助和向量截断化K矩阵的水下载体初始对准方法，其特征在于：步骤(2)中，所述惯性传感器实时数据包括水下自主潜航器的加速度和角速率信息，DVL实时数据包括水下自主潜航器的线速度信息。
3.根据权利要求1所述的基于DVL辅助和向量截断化K矩阵的水下载体初始对准方法，其特征在于：步骤(4)的具体过程为：
(4.1)构造k时刻K矩阵：
Figure FDA0003217217930000021
Figure FDA0003217217930000022
式中，ai为标量权重系数，规定
Figure FDA0003217217930000023
tr(■)表示对矩阵求迹，即tr(Bk)表示对Bk求迹；T表示对矩阵求转置；ξi表示k时刻第i个观测矢量；ri表示k时刻第i个参考矢量；Bk表示由ξi和ri构成的三维矩阵；Sk表示由ξi和ri构成的三维矩阵；zk表示由ξi和ri构成的三维向量；σk表示矩阵Bk的迹；Kk表示k时刻的K矩阵；I3表示3×3的单位矩阵；
(4.2)利用K矩阵的零迹性和对称性，将K矩阵向量化后再截断成9维状态向量：
Figure FDA0003217217930000031
式中，
Figure FDA0003217217930000032
表示Kk矩阵的第i行第j列的元素，i＝1～3；j＝1～4；xk代表k时刻的状态向量；
(4.3)构造k+1时刻的δKk+1矩阵：
Figure FDA0003217217930000033
Figure FDA0003217217930000034
式中，tr(■)表示对矩阵求迹；T表示对矩阵求转置；δKk+1代表k+1时刻的δK矩阵；ξk+1表示k+1时刻单个观测矢量；rk+1表示k+1时刻单个参考矢量；Bk+1表示由ξk+1和rk+1构成的三维矩阵；Sk+1表示由ξk+1和rk+1构成的三维矩阵；zk+1表示由ξk+1和rk+1构成的三维向量；σk+1表示矩阵Bk+1的迹；rk+1 T代表rk+1的转置；Bk+1 T代表Bk+1的转置；
(4.4)利用δKk+1的零迹性与对称性，将δKk+1矩阵向量截断化成9维量测向量：
Figure FDA0003217217930000035
式中，
Figure FDA0003217217930000036
表示δKk+1矩阵的第i行第j列的元素，i＝1～3；j＝1～4；yk代表k时刻的量测向量；
(4.5)建立状态方程模型和量测方程模型
状态方程表达式如下：
xk+1＝Ψkxk+Γkζk (11)
式中，xk为k时刻9×1的状态向量；xk+1为k+1时刻9×1的状态向量；Ψk为状态转移矩阵，对于初始对准过程而言，简化成Ψk＝I9；ζk为高斯白噪声；Γk为9×3状态相关的噪声增益矩阵；
量测方程表达式如下：
Figure FDA0003217217930000041
Figure FDA0003217217930000042
式中，xk+1为k+1时刻9×1的状态向量；yk+1为k+1时刻9×1的量测向量；
Figure FDA0003217217930000043
为量测向量相关的量测噪声矩阵；其中，ai为k+1时刻第i个观测矢量的权重系数，规定
Figure FDA0003217217930000044
Figure FDA0003217217930000045
表示k+1时刻第i个观测矢量误差；
Figure FDA0003217217930000046
表示k+1时刻第i个9×3的噪声增益矩阵；
(4.6)进行卡尔曼滤波，更新K矩阵
初始化：
Figure FDA0003217217930000047
时间更新；
Figure FDA0003217217930000048
Figure FDA00032172179300000417
式中，Ψk表示k时刻状态转移矩阵；
Figure FDA0003217217930000049
表示k时刻最优状态估计；
Figure FDA00032172179300000410
表示k+1时刻状态一步预测；Pk/k表示k时刻状态误差协方差矩阵；Pk+1/k表示k+1时刻状态误差协方差矩阵一步预测；
Figure FDA00032172179300000411
表示系统噪声方程阵；
Figure FDA00032172179300000412
表示初始时刻的状态向量，y0表示初始时刻的量测向量；Γk为9×3状态相关的噪声增益矩阵；
量测更新：
Gk+1＝Pk+1/k(Pk+1/k+Rk+1)-1 (20)
Figure FDA00032172179300000413
Figure FDA00032172179300000418
式中，
Figure FDA00032172179300000414
表示k+1时刻状态一步预测；yk+1表示k+1时刻系统量测；Pk+1/k表示k+1时刻状态误差协方差矩阵一步预测；Gk+1表示k+1时刻的卡尔曼增益；I9为9维的单位阵；
Figure FDA00032172179300000415
表示k+1时刻对状态向量x的估计值；Pk+1/k+1表示k+1时刻对协方差矩阵的估计值；
Figure FDA00032172179300000416
表示k+1时刻的卡尔曼增益的转置；Rk+1表示k+1时刻量测噪声方差阵，其具体定义为：
Figure FDA0003217217930000051
其中，ai为k+1时刻第i个观测矢量的权重系数，规定
Figure FDA0003217217930000052
Figure FDA0003217217930000053
表示k+1时刻第i个观测矢量误差；
Figure FDA0003217217930000054
表示k+1时刻第i个9×3的噪声增益矩阵，定义为：
Figure FDA0003217217930000055
式中，
Figure FDA0003217217930000056
代表rk+1向量中的第1个元素，
Figure FDA0003217217930000057
代表rk+1向量中的第2个元素，
Figure FDA0003217217930000058
代表rk+1向量中的第3个元素。
4.根据权利要求1所述的基于DVL辅助和向量截断化K矩阵的水下载体初始对准方法，其特征在于：步骤(4.2)中，利用K矩阵的零迹性和对称性，将K矩阵向量化后再截断成9维状态向量，即将K矩阵的上三角矩阵的元素分别提取出来，按照行数列数由小到大的顺序构成状态向量。
5.根据权利要求1所述的基于DVL辅助和向量截断化K矩阵的水下载体初始对准方法，其特征在于：步骤(4.4)中，利用δKk+1的零迹性与对称性，将δKk+1矩阵向量截断化成9维量测向量，即将δKk+1矩阵的上三角矩阵的元素分别提取出来，按照行数列数由小到大的顺序构成量测向量。
6.根据权利要求1所述的基于DVL辅助和向量截断化K矩阵的水下载体初始对准方法，其特征在于：步骤(4.5)中，所述Γk为9×3状态相关的噪声增益矩阵，定义如下：
Figure FDA0003217217930000061
式中，Δt表示采样时间；
Figure FDA0003217217930000062
表示k时刻xk状态向量的第i个元素，i＝1～9。
7.根据权利要求1所述的基于DVL辅助和向量截断化K矩阵的水下载体初始对准方法，其特征在于：步骤(4.5)中，
Figure FDA0003217217930000063
为k+1时刻第i个9×3的噪声增益矩阵，定义如下：
Figure FDA0003217217930000064
式中，
Figure FDA0003217217930000065
代表rk+1向量中的第1个元素，
Figure FDA0003217217930000066
代表rk+1向量中的第2个元素，
Figure FDA0003217217930000067
代表rk+1向量中的第3个元素。
8.根据权利要求1所述的基于DVL辅助和向量截断化K矩阵的水下载体初始对准方法，其特征在于：步骤(4.6)中，Γk为9×3状态相关的噪声增益矩阵，具体表达式见下：
Figure FDA0003217217930000071
式中，Δt表示采样时间；xki表示k时刻9维xk状态向量的第i个元素，i＝1～9。
9.根据权利要求1所述的基于DVL辅助和向量截断化K矩阵的水下载体初始对准方法，其特征在于：所述步骤(5)具体过程如下：
(5.1)将卡尔曼滤波后得到的状态向量重写回K矩阵形式；
Figure FDA0003217217930000072
Figure FDA0003217217930000073
式中，
Figure FDA0003217217930000074
为滤波后的状态向量最佳估计值
Figure FDA0003217217930000075
对应的第i个元素，i＝1～10；
(5.2)基于Wahba问题求解K矩阵最大特征值，所述K矩阵最大特征值对应的特征向量即为最优四元数，从更新后的K矩阵提取最优四元数的计算方法为：
Figure FDA0003217217930000076
Figure FDA0003217217930000077
式中，
Figure FDA0003217217930000078
表示最优四元数中的矢量部分；
Figure FDA0003217217930000079
表示最优四元数；
Figure FDA00032172179300000710
与
Figure FDA00032172179300000711
为k时刻从最优估计K矩阵
Figure FDA00032172179300000712
中按照K矩阵定义提取的最优估计，具体提取方法为：
Figure FDA0003217217930000081
为步骤5中计算得到的K矩阵中的第4行第4列元素，
Figure FDA0003217217930000082
为步骤5中计算得到的K矩阵中第4个列向量对应的前3个元素；
Figure FDA0003217217930000083
的计算公式为
Figure FDA0003217217930000084
其中K3×3表示K矩阵左上角的3维方阵；
(5.3)将最优四元数转换成初始时刻姿态矩阵
Figure FDA0003217217930000085
Figure FDA0003217217930000086
其中，q0表示四元数的第1个元素；q1表示四元数的第2个元素；q2表示四元数的第3个元素；q3表示四元数的第4个元素；
Figure FDA0003217217930000087
Cij表示初始时刻姿态矩阵
Figure FDA0003217217930000088
第i行第j列的元素，i＝1～3，j＝1～3；
(5.4)利用矩阵链式乘法求得当前时刻的姿态矩阵，实时解算姿态角：
姿态矩阵链式乘法表达式为：
Figure FDA0003217217930000089
式中，
Figure FDA00032172179300000810
表示t时刻导航坐标系n到载体坐标系b的姿态转移矩阵；
Figure FDA00032172179300000811
表示从0时刻到t时刻，载体坐标系b的姿态转移矩阵；
Figure FDA00032172179300000812
表示从0时刻到t时刻，导航坐标系n的姿态转移矩阵；
Figure FDA00032172179300000813
表示初始时刻0时刻时，导航坐标系n到载体坐标系b的姿态转移矩阵。
假设载体的方位角为ψ，俯仰角为θ，倾斜角为γ，实时解算姿态角得：
Figure FDA00032172179300000814
(5.5)：求取姿态角的均值，所述均值为最终的失准角，进而完成初始对准过程。
10.根据权利要求9所述的基于DVL辅助和向量截断化K矩阵的水下载体初始对准方法，其特征在于：步骤(5.5)中，当实时解算的姿态角曲线收敛后，求取收敛时间内姿态角的均值。
CN202110947453.5A 2021-08-18 2021-08-18 基于dvl辅助和向量截断化k矩阵的水下载体初始对准方法 Active CN113670335B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110947453.5A CN113670335B (zh) 2021-08-18 2021-08-18 基于dvl辅助和向量截断化k矩阵的水下载体初始对准方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110947453.5A CN113670335B (zh) 2021-08-18 2021-08-18 基于dvl辅助和向量截断化k矩阵的水下载体初始对准方法

Publications (2)

Publication Number Publication Date
CN113670335A true CN113670335A (zh) 2021-11-19
CN113670335B CN113670335B (zh) 2023-10-24

Family

ID=78543518

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110947453.5A Active CN113670335B (zh) 2021-08-18 2021-08-18 基于dvl辅助和向量截断化k矩阵的水下载体初始对准方法

Country Status (1)

Country Link
CN (1) CN113670335B (zh)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103759742A (zh) * 2014-01-22 2014-04-30 东南大学 基于模糊自适应控制技术的捷联惯导非线性对准方法
CN105806363A (zh) * 2015-11-16 2016-07-27 东南大学 基于srqkf的sins/dvl水下大失准角对准方法
CN109141475A (zh) * 2018-09-14 2019-01-04 苏州大学 一种dvl辅助sins鲁棒行进间初始对准方法
CN109443378A (zh) * 2018-09-21 2019-03-08 苏州大学 速度辅助行进间回溯初始对准方法
CN110398257A (zh) * 2019-07-17 2019-11-01 哈尔滨工程大学 Gps辅助的sins系统快速动基座初始对准方法
WO2020087845A1 (zh) * 2018-10-30 2020-05-07 东南大学 基于gpr与改进的srckf的sins初始对准方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103759742A (zh) * 2014-01-22 2014-04-30 东南大学 基于模糊自适应控制技术的捷联惯导非线性对准方法
CN105806363A (zh) * 2015-11-16 2016-07-27 东南大学 基于srqkf的sins/dvl水下大失准角对准方法
CN109141475A (zh) * 2018-09-14 2019-01-04 苏州大学 一种dvl辅助sins鲁棒行进间初始对准方法
CN109443378A (zh) * 2018-09-21 2019-03-08 苏州大学 速度辅助行进间回溯初始对准方法
WO2020087845A1 (zh) * 2018-10-30 2020-05-07 东南大学 基于gpr与改进的srckf的sins初始对准方法
CN110398257A (zh) * 2019-07-17 2019-11-01 哈尔滨工程大学 Gps辅助的sins系统快速动基座初始对准方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
LIAM PAULL ET AL.: "AUV Navigation and Localization：A Review", IEEE JOURNAL OF OCEANIC EGINEERING *
李贤义;齐建宇;赵鹏飞;: "SINS/DVL水下初始对准与定位导航", 兵器装备工程学报, no. 04 *

Also Published As

Publication number Publication date
CN113670335B (zh) 2023-10-24

Similar Documents

Publication Publication Date Title
CN108731670B (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN106780699B (zh) 一种基于sins/gps和里程计辅助的视觉slam方法
CN109724599B (zh) 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法
CN111486845B (zh) 基于海底地形匹配的auv多策略导航方法
CN104075715B (zh) 一种结合地形和环境特征的水下导航定位方法
CN105806363B (zh) 基于srqkf的sins/dvl水下大失准角对准方法
CN109443379A (zh) 一种深海潜航器的sins/dvl水下抗晃动对准方法
CN110514203B (zh) 一种基于isr-ukf的水下组合导航方法
CN108387236B (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN108362288B (zh) 一种基于无迹卡尔曼滤波的偏振光slam方法
CN111596333B (zh) 一种水下定位导航方法及系统
Huang et al. Variational Bayesian-based filter for inaccurate input in underwater navigation
CN112015086B (zh) 一种欠驱动水面船有限时间路径跟踪输出反馈控制方法
CN113252041B (zh) 一种适用于小型水下机器人的组合导航方法
Ko et al. Particle filter approach for localization of an underwater robot using time difference of arrival
CN109739088B (zh) 一种无人船有限时间收敛状态观测器及其设计方法
CN113739795A (zh) 一种基于偏振光/惯性/视觉组合导航的水下同步定位与建图方法
CN112857398A (zh) 一种系泊状态下舰船的快速初始对准方法和装置
CN108981691B (zh) 一种天空偏振光组合导航在线滤波与平滑方法
CN109813316A (zh) 一种基于地形辅助的水下载体紧组合导航方法
CN103616026A (zh) 一种基于h∞滤波的auv操纵模型辅助捷联惯导组合导航方法
CN113670335B (zh) 基于dvl辅助和向量截断化k矩阵的水下载体初始对准方法
CN111024071A (zh) Gnss辅助的加速度计和陀螺仪常值漂移估算的导航方法及系统
CN113190025B (zh) 一种适用于变结构水下机器人的运动控制方法
CN114111840B (zh) 一种基于组合导航的dvl误差参数在线标定方法

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