CN115371705A - 一种基于特殊正交群和鲁棒不变扩展卡尔曼滤波的dvl标定方法 - Google Patents

一种基于特殊正交群和鲁棒不变扩展卡尔曼滤波的dvl标定方法 Download PDF

Info

Publication number
CN115371705A
CN115371705A CN202211056699.4A CN202211056699A CN115371705A CN 115371705 A CN115371705 A CN 115371705A CN 202211056699 A CN202211056699 A CN 202211056699A CN 115371705 A CN115371705 A CN 115371705A
Authority
CN
China
Prior art keywords
dvl
matrix
measurement
sins
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.)
Pending
Application number
CN202211056699.4A
Other languages
English (en)
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.)
Harbin Engineering University
Original Assignee
Harbin Engineering University
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 Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN202211056699.4A priority Critical patent/CN115371705A/zh
Publication of CN115371705A publication Critical patent/CN115371705A/zh
Pending legal-status Critical Current

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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Data Mining & Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Pure & Applied Mathematics (AREA)
  • Computational Mathematics (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Operations Research (AREA)
  • Manufacturing & Machinery (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computing Systems (AREA)
  • Navigation (AREA)

Abstract

本发明属于非理想条件下的导航器件标定技术领域，具体涉及一种基于特殊正交群和鲁棒不变扩展卡尔曼滤波的DVL标定方法。本发明通过李群空间到李代数空间的指数映射，得到了线性的误差方程，可以更准确的刻画误差传播规律，避免非线性条件下不准确的雅各比矩阵对滤波估计的影响。本发明通过引入统计相似度量李群，设计鲁棒滤波器，抑制了测量离群点对标定精度的影响，可以更准确的输出SINS和DVL间的安装偏差角及DVL的刻度系数误差。

Description

一种基于特殊正交群和鲁棒不变扩展卡尔曼滤波的DVL标定 方法
技术领域
本发明属于非理想条件下的导航器件标定技术领域，具体涉及一种基于特殊正交群和鲁棒不变扩展卡尔曼滤波的DVL标定方法。
背景技术
捷联惯导/多普勒测速计程仪(SINS/DVL)组合导航系统是目前水下航行器最常用的组合导航方法之一。DVL的标定精度会直接影响SINS/DVL的组合导航精度。
对于一些体积小的水下航行器，其内部空间的限制会使得SINS与DVL在安装时无法实现坐标轴的重合，甚至会出现很大的安装偏差，因此标定算法需要能够对大的安装偏差角进行准确计算或估计。此外，在一些复杂情况下，DVL会出现较大的测量离群点，例如，DVL发射的声波波束受海洋生物遮挡无法到达海底，或者当水下航行器驶过海沟时，航行器与海床之间的距离超出DVL声波波束范围，亦或者海底存在强吸波材料(如淤泥等)，发出的声波波束不能反射回来。因此，需要发明在大安装偏差角和大DVL测量离群点下保证标定精度的鲁棒标定方法。
发明内容
本发明的目的在于针对DVL标定问题，提供能够对大安装偏差角进行准确建模的一种基于特殊正交群和鲁棒不变扩展卡尔曼滤波的DVL标定方法，保证DVL的标定精度，即能够更准确地计算/估计出SINS和DVL间的安装偏差角和DVL的刻度系数误差。
一种基于特殊正交群和鲁棒不变扩展卡尔曼滤波的DVL标定方法，包括以下步骤：
步骤1：定义状态矩阵Rk为
Figure BDA0003825166820000011
Figure BDA0003825166820000012
表示SINS和DVL间安装偏差角；预先设置k＝0时的李代数预测值
Figure BDA0003825166820000013
李代数
Figure BDA0003825166820000014
的协方差矩阵
Figure BDA0003825166820000015
状态矩阵R0、状态矩阵R0的预测值
Figure BDA0003825166820000016
的协方差矩阵
Figure BDA0003825166820000017
设置刻度系数误差阈值μ；
步骤2：设置标定总时间K，令k＝1，开始标定；
步骤3：获取当前k时刻SINS/GPS组合导航系统输出的载体速度测量值
Figure BDA0003825166820000018
从地理坐标系到SINS载体坐标系的姿态变换矩阵
Figure BDA0003825166820000019
获取当前k时刻多普勒测速计程仪DVL输出的载体速度测量值
Figure BDA00038251668200000110
获取量测噪声方差矩阵Nk；
步骤4：计算刻度系数误差
Figure BDA00038251668200000111
Figure BDA0003825166820000021
其中，lDVL表示DVL和SINS间的杆臂，通过测量得到；
Figure BDA0003825166820000022
表示载体坐标系相对于地理坐标系的旋转角速度，
Figure BDA0003825166820000023
Figure BDA0003825166820000024
表示当前k时刻陀螺仪的输出；
Figure BDA0003825166820000025
表示由SINS/GPS组合导航系统估计的陀螺零偏；
Figure BDA0003825166820000026
表示地球自转角速度在地理坐标系下的投影；
Figure BDA0003825166820000027
表示载体相对于地球的转动角速度在地理坐标系下的投影，利用载体速度和当地地理位置直接获得；
步骤5：获取当前k时刻的量测矩阵Hk和观测误差矩阵Zk；
步骤6：通过鲁棒不变扩展卡尔曼滤波器估计k时刻SINS和DVL间安装偏差角
Figure BDA0003825166820000028
步骤6.1：初始化迭代次数t＝1；
步骤6.2：更新当前迭代次数下的量测噪声方差的辅助参数
Figure BDA0003825166820000029
Figure BDA00038251668200000210
Figure BDA00038251668200000211
Figure BDA00038251668200000212
Figure BDA00038251668200000213
Figure BDA00038251668200000214
Figure BDA00038251668200000215
其中，
Figure BDA00038251668200000216
表示k-1时刻李代数的预测值，初始时刻的李代数预测值
Figure BDA00038251668200000217
为预设给定；
Figure BDA00038251668200000218
表示k-1时刻李代数
Figure BDA00038251668200000219
的协方差阵，初始时刻的李代数
Figure BDA00038251668200000220
的协方差阵
Figure BDA00038251668200000221
为预设给定；fz(·)为相似度函数；
步骤6.3：判断是否达到最大迭代次数M；若未达到大迭代次数M，则返回步骤6.2继续迭代；否则，输出量测噪声方差的辅助参数
Figure BDA00038251668200000222
步骤6.4：鲁棒不变扩展卡尔曼滤波器的量测更新；
Figure BDA00038251668200000223
Figure BDA0003825166820000031
Figure BDA0003825166820000032
步骤7：若
Figure BDA0003825166820000033
且k＞1，则刻度系数误差不更新，令
Figure BDA0003825166820000034
步骤8：若k＜K，则令k＝k+1，返回步骤3；否则，输出刻度系数误差
Figure BDA0003825166820000035
SINS和DVL间安装偏差角
Figure BDA0003825166820000036
完成标定。
进一步地，所述步骤5中当前k时刻的量测矩阵Hk和观测误差矩阵Zk的计算方法为：
Hk＝αk×
Figure BDA0003825166820000037
Figure BDA0003825166820000038
Figure BDA0003825166820000039
其中，运算符“×”表示向量叉乘运算。
进一步地，所述步骤6.2中相似度函数fz(·)为：
Figure BDA00038251668200000310
其中，ω为自由度参数，m为量测维数。
本发明的有益效果在于：
本发明通过李群空间到李代数空间的指数映射，得到了线性的误差方程，可以更准确的刻画误差传播规律，避免非线性条件下不准确的雅各比矩阵对滤波估计的影响。本发明通过引入统计相似度量李群，设计鲁棒滤波器，抑制了测量离群点对标定精度的影响，可以更准确的输出SINS和DVL间的安装偏差角及DVL的刻度系数误差。
附图说明
图1为本发明的总体流程示意图。
图2为本发明的实施例中俯仰安装偏差角的标定误差图。
图3为本发明的实施例中航向安装偏差角的标定误差图。
图4为本发明的实施例中刻度系数的标定误差图。
图5为本发明的总体框架图。
具体实施方式
下面结合附图对本发明做进一步描述。
本发明在考虑对SINS和DVL间的大安装误差角利用特殊正交群(李群的一种)进行准确建模的同时,针对目前现有的标定算法无法应对测量离群点的问题，发明了新的用于特殊正交群模型的鲁棒滤波方法。本发明在将特殊正交群模型的误差方程指数映射到李代数空间上的同时，引入统计相似度量(SSM)理论，在李代数空间上研究误差传播规律，自适应调节量测噪声协方差阵并计算滤波增益，然后对李群空间进行预测信息和量测信息的指数加权运算，得到安装误差的估计结果。本发明可用于非理想条件下的水下航行器标定领域。
首先定义常用坐标系，SINS载体坐标系采用b表示，“东-北-天”地理坐标系作为导航系采用n表示，惯性坐标系用i表示，DVL框架坐标系用d表示。
DVL的速度测量值可以表示为：
Figure BDA0003825166820000041
式中，
Figure BDA0003825166820000042
表示x的实际测量值，sd表示本发明需要计算的刻度系数误差。R为三阶特殊正交群由于其单位正交的性质，可以用来表示姿态旋转矩阵。
Figure BDA0003825166820000043
表示从b系到d系的姿态变换矩阵，也是本发明中需要估计的SINS和DVL间安装偏差角。
Figure BDA0003825166820000044
表示从n系到b系的姿态变换矩阵.vn表示导航坐标系下真实的速度。δvd表示DVL的测量噪声。lDVL表示DVL和SINS间的杆臂，通常可以事先准确测量。
Figure BDA0003825166820000045
是b系相对于n系的旋转角速度。
忽略测量噪声，考虑到特殊正交群的单位正交性质，不会对合速度的大小产生影响，因此可以对(1)式两边取2-范数，得到：
Figure BDA0003825166820000046
式中，
Figure BDA0003825166820000047
和
Figure BDA0003825166820000048
由SINS/GPS组合导航系统直接获得。
Figure BDA0003825166820000049
可以按照下式计算:
Figure BDA00038251668200000410
式中，
Figure BDA00038251668200000411
是陀螺仪的输出。
Figure BDA00038251668200000412
表示地球自转角速度在n系下的投影。
Figure BDA00038251668200000413
为由则导航系统估计的陀螺零偏。
Figure BDA00038251668200000414
可以利用载体速度和当地地理位置直接获得。利用(2)式可以得到刻度系数误差的计算公式。
Figure BDA00038251668200000415
离散形式为
Figure BDA0003825166820000051
定义
Figure BDA0003825166820000052
Figure BDA0003825166820000053
Figure BDA0003825166820000054
δαk＝δvd(k) (9)安装偏差角通常是一个常值。因此结合(6)、(7)、(8)和(9)，使用SO(3)表示的标定模型为：
Figure BDA0003825166820000055
因此，安装偏差角的标定问题转化为了SO(3)的估计问题。
由于(10)所示的观测方程为左不变观测，因此定义左不变误差ηk为
Figure BDA0003825166820000056
Rk表示真实状态，
Figure BDA0003825166820000057
表示估计状态。
由于f(ab)＝af(b)+f(a)b-af(I)b，
Figure BDA0003825166820000058
因此
Figure BDA0003825166820000059
观测误差定义为
Figure BDA00038251668200000510
(12)和(13)构成了李群空间上的误差模型。然而，左不变误差ηk仍然是一个矩阵,(13)是一个非线性方程，因此研究其误差传播是不方便的。而ηk可以指数映射到李代数空间上
ηk＝exp(ξk×) (14)
ξk是ηk在李代数空间上的对数投影向量。(14)代入(13)进行一阶泰勒展开得
Figure BDA00038251668200000511
由此，令
Figure BDA00038251668200000512
可以得到李代数空间上的误差传播方程
Figure BDA0003825166820000061
利用不变扩展卡尔曼滤波(IEKF)可以得到最终状态的估计结果。
Figure BDA0003825166820000062
Figure BDA0003825166820000063
Figure BDA0003825166820000064
其中，Hk＝αk×,Nk＝cov(Vk)，
Figure BDA0003825166820000065
和
Figure BDA0003825166820000066
表示一步预测和预测误差协方差阵，其计算过程如下：
Figure BDA0003825166820000067
Figure BDA0003825166820000068
初始时刻的状态
Figure BDA0003825166820000069
和其协方差阵
Figure BDA00038251668200000610
人为给定。
测量异常值使得DVL测量的速度与真实速度之间存在较大差异。这种差异可能会被标定算法误解为由安装未对准角度和刻度系数误差引起，从而降低标定精度。特别是对于刻度系数误差，其计算精度完全取决于速度信息的测量质量。对于刻度系数误差的计算，DVL标定算法应该具有检测离群点的能力。当检测到离群点时，应丢弃当前时刻的测量异常值信息，不更新刻度系数误差。对于SO(3)表示的DVL标定模型，应研究鲁棒IEKF算法。
目前，大多数鲁棒滤波器通过自适应调整滤波增益矩阵来减少异常值对滤波精度的影响，该矩阵可以通过改变测量噪声协方差矩阵来调整。当出现测量离群点时，应增加测量协方差矩阵以降低滤波器增益，从而降低最终估计结果中离群点信息的比重。IEKF的思想类似于KF，两者都是通过对预测信息和测量信息进行加权来更新状态。在IEKF中，根据李代数模型中的噪声协方差和误差不确定性(误差协方差)传播规律计算权重矩阵(滤波器增益)。因此，在应用(16)估计李代数的同时适应调整测量噪声协方差，通过使用修正后的测量噪声协方差计算滤波器增益可以有效地降低异常值对IEKF的影响。同时，可以通过测量噪声协方差矩阵的调整程度来判断是否出现离群点，从而判断当前时刻是否更新刻度系数误差。
统计相似度度量s(a,b)可以用来表示两个随机向量a和b之间的相似度，定义如下：
s(a,b)＝E[f(||a-b||2)]＝∫∫f(||a-b||2)p(a,b)dadb (22)
式中，||·||表示欧氏范数，p(a,b)表示随机向量a和b之间的联合概率密度。f(·)表示相似度函数，并且满足以下三个条件。
a)f(·)是在定义域[0，+∞)内的连续函数。
b)f(·)是单调递减函数，即
Figure BDA0003825166820000071
c)f(·)的二阶导数是非负的，即
Figure BDA0003825166820000072
统计相似度度量符合相似度度量的通常定义。随机向量之间的相似性越高，统计相似度度量就越大。选择f(t)为f(t)＝-t时，SSM表示不同随机向量之间的负均方误差。当选择f(t)为
Figure BDA0003825166820000073
时，SSM表示不同随机向量之间的相关熵。通过选择不同的相似函数，可以实现不同的SSM。
假设李代数ξk的预测更新为
Figure BDA0003825166820000074
Figure BDA0003825166820000075
初始时刻的李代数预测值
Figure BDA0003825166820000076
及其协方差阵
Figure BDA0003825166820000077
人为给定。
本发明则构造了反映李代数与预测李代数、量测与预测量测之间SSM的代价函数。
Figure BDA0003825166820000078
Figure BDA0003825166820000079
和
Figure BDA00038251668200000710
表示量测噪声方差Nk和一步预测协方差阵
Figure BDA00038251668200000711
的均方根分解。
为了便于求解(25)所示的代价函数，将后验分布近似为高斯分布并利用詹森(Jensen)不等式求解上式中代价函数的下界，将其作为一个新的代价函数。
Figure BDA00038251668200000712
Figure BDA00038251668200000713
Figure BDA00038251668200000714
求解代价函数的极值的计算公式如下。
Figure BDA00038251668200000715
由于式(29)为非线性方程，因此在本发明中使用高斯-牛顿迭代算法对(29)进行求解。
根据高斯-牛顿迭代法，对于非线性方程式(29)，解的数值更新过程为
Figure BDA0003825166820000081
式中，上标t表示第t次迭代。迭代初值设为
Figure BDA0003825166820000082
Figure BDA0003825166820000083
表示代价函数
Figure BDA0003825166820000084
的近似海森(Hessian)矩阵。
Figure BDA0003825166820000085
Figure BDA0003825166820000086
Figure BDA0003825166820000087
Figure BDA0003825166820000088
Figure BDA0003825166820000089
(31)-(35)代入(30)，整理得
Figure BDA00038251668200000810
Figure BDA00038251668200000811
其中
Figure BDA00038251668200000812
和
Figure BDA00038251668200000813
相当于用来调节一步预测协方差阵和量测噪声方差的辅助参数。由状态理论上是一个常值，因此，相似度函数fx(t)选择为fx(t)＝-0.5t，即采用负均方误差来度量状态与预测状态之间的相似程度，辅助参数
Figure BDA00038251668200000814
不对预测协方差阵进行调节。而DVL波束测量噪声则可能是由野值引起的非高斯噪声，因此相似度函数fz(t)选为
Figure BDA00038251668200000815
ω是自由度参数，m是量测维数，在本发明中为3。
可以直接得到估计误差协方差阵
Figure BDA00038251668200000816
(37)式可以重写为
Figure BDA00038251668200000817
Figure BDA00038251668200000818
当迭代计算完成后，假设最大迭代次数为M，则(41)表示为
Figure BDA0003825166820000091
利用
Figure BDA0003825166820000092
代替(17)中的Nk得
Figure BDA0003825166820000093
Figure BDA0003825166820000094
Figure BDA0003825166820000095
(42)、(43)和(44)构成了鲁棒不变扩展卡尔曼滤波器的量测更新，
Figure BDA0003825166820000096
即为估计的安装偏差变换矩阵
Figure BDA0003825166820000097
由姿态变换矩阵阵和姿态角的变换关系很容易从
Figure BDA0003825166820000098
中获得安装偏差角。
鲁棒IEKF引入γk去修正量测噪声协方差阵Nk。当出现大的测量离群点时，残差
Figure BDA0003825166820000099
会很大，使得γk自动减少。特别是当残差趋于无穷大时，γk几乎区域零使得
Figure BDA00038251668200000910
也接近于零。因此，γk的变化可以用来判断是否有离群点出现。
定义判断阈值为μ。如果γk＜μ，则刻度系数误差不利用当前时刻的速度信息进行更新，即
Figure BDA00038251668200000911
如果γk＞μ，则刻度系数根据公式(5)计算获得。
基于上述理论推导过程，如图1所示，本发明在实际进行标定应用时，步骤为：
步骤1：定义状态矩阵Rk为
Figure BDA00038251668200000912
Figure BDA00038251668200000913
表示SINS和DVL间安装偏差角；预先设置k＝0时的李代数预测值
Figure BDA00038251668200000914
李代数
Figure BDA00038251668200000915
的协方差矩阵
Figure BDA00038251668200000916
状态矩阵R0、状态矩阵R0的预测值
Figure BDA00038251668200000917
的协方差矩阵
Figure BDA00038251668200000918
设置刻度系数误差阈值μ；
步骤2：设置标定总时间K，令k＝1，开始标定；
步骤3：获取当前k时刻SINS/GPS组合导航系统输出的载体速度测量值
Figure BDA00038251668200000919
从地理坐标系到SINS载体坐标系的姿态变换矩阵
Figure BDA00038251668200000920
获取当前k时刻多普勒测速计程仪DVL输出的载体速度测量值
Figure BDA00038251668200000921
获取量测噪声方差矩阵Nk；
步骤4：计算刻度系数误差
Figure BDA00038251668200000922
Figure BDA00038251668200000923
其中，lDVL表示DVL和SINS间的杆臂，通过测量得到；
Figure BDA00038251668200000924
表示载体坐标系相对于地理坐标系的旋转角速度，
Figure BDA0003825166820000101
Figure BDA0003825166820000102
表示当前k时刻陀螺仪的输出；
Figure BDA0003825166820000103
表示由SINS/GPS组合导航系统估计的陀螺零偏；
Figure BDA0003825166820000104
表示地球自转角速度在地理坐标系下的投影；
Figure BDA0003825166820000105
表示载体相对于地球的转动角速度在地理坐标系下的投影，利用载体速度和当地地理位置直接获得；
步骤5：计算当前k时刻的量测矩阵Hk和观测误差矩阵Zk；
Hk＝αk×
Figure BDA0003825166820000106
Figure BDA0003825166820000107
Figure BDA0003825166820000108
其中，运算符“×”表示向量叉乘运算。
步骤6：通过鲁棒不变扩展卡尔曼滤波器估计k时刻SINS和DVL间安装偏差角
Figure BDA0003825166820000109
步骤6.1：初始化迭代次数t＝1；
步骤6.2：更新当前迭代次数下的量测噪声方差的辅助参数
Figure BDA00038251668200001010
Figure BDA00038251668200001011
Figure BDA00038251668200001012
Figure BDA00038251668200001013
Figure BDA00038251668200001014
Figure BDA00038251668200001015
Figure BDA00038251668200001016
其中，
Figure BDA00038251668200001017
表示k-1时刻李代数的预测值，初始时刻的李代数预测值
Figure BDA00038251668200001018
为预设给定；
Figure BDA00038251668200001019
表示k-1时刻李代数
Figure BDA00038251668200001020
的协方差阵，初始时刻的李代数
Figure BDA00038251668200001021
的协方差阵
Figure BDA00038251668200001022
为预设给定；fz(·)为相似度函数，
Figure BDA00038251668200001023
ω为自由度参数，m为量测维数；
步骤6.3：判断是否达到最大迭代次数M；若未达到大迭代次数M，则返回步骤6.2继续迭代；否则，输出量测噪声方差的辅助参数
Figure BDA00038251668200001024
步骤6.4：鲁棒不变扩展卡尔曼滤波器的量测更新；
Figure BDA0003825166820000111
Figure BDA0003825166820000112
Figure BDA0003825166820000113
步骤7：若
Figure BDA0003825166820000114
且k＞1，则刻度系数误差不更新，令
Figure BDA0003825166820000115
步骤8：若k＜K，则令k＝k+1，返回步骤3；否则，输出刻度系数误差
Figure BDA0003825166820000116
SINS和DVL间安装偏差角
Figure BDA0003825166820000117
完成标定。
实施例1：
初始位置设置为北纬30°，东经120°。初始速度为2m/s，向东北方向直线航行。陀螺常值漂移设置为0.005°/h，加速度计常值零偏设置为100μg。初始的姿态误差设置为[0.01°,0.01°,0.02°]，安装偏差角设置为大角度[75°,75°,150°]，刻度系数误差设置为0.005。DVL的测量噪声设置为：
Figure BDA0003825166820000118
Figure BDA0003825166820000119
Figure BDA00038251668200001110
w.p.表示发生概率。图2和图3表示不同算法50次蒙特卡洛仿真中俯仰安装偏差角和航向安装偏差角的均方根误差(RMSE)。QKF表示基于四元数卡尔曼滤波的标定算法，QNQ-P表示基于位置观测和伪牛顿迭代方法的标定算法，IEKF表示在本发明所提出的标定模型基础上仅使用不变扩展卡尔曼滤波的标定算法，鲁棒IEKF表示在本发明所提出的标定模型基础上使用鲁棒不变扩展卡尔曼滤波的标定算法，也是本发明所最终提出的标定算法。从图中可以看出，在大安装偏差角和大离群点条件下，所提出算法对安装偏差的标定误差最小，与其他算法差别明显。图4表示刻度系数误差的标定结果，可以看出所提出的鲁棒标定算法能够保证很好的标定精度。其他算法由于无法对离群点信息进行处理，对不正确信息的错误利用使得对刻度系数误差的计算毫无精度可言。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.一种基于特殊正交群和鲁棒不变扩展卡尔曼滤波的DVL标定方法，其特征在于，包括以下步骤：
步骤1：定义状态矩阵Rk为
Figure FDA0003825166810000011
Figure FDA0003825166810000012
表示SINS和DVL间安装偏差角；预先设置k＝0时的李代数预测值
Figure FDA0003825166810000013
李代数
Figure FDA0003825166810000014
的协方差矩阵
Figure FDA0003825166810000015
状态矩阵R0、状态矩阵R0的预测值
Figure FDA0003825166810000016
的协方差矩阵
Figure FDA0003825166810000017
设置刻度系数误差阈值μ；
步骤2：设置标定总时间K，令k＝1，开始标定；
步骤3：获取当前k时刻SINS/GPS组合导航系统输出的载体速度测量值
Figure FDA0003825166810000018
从地理坐标系到SINS载体坐标系的姿态变换矩阵
Figure FDA0003825166810000019
获取当前k时刻多普勒测速计程仪DVL输出的载体速度测量值
Figure FDA00038251668100000110
获取量测噪声方差矩阵Nk；
步骤4：计算刻度系数误差
Figure FDA00038251668100000111
Figure FDA00038251668100000112
其中，lDVL表示DVL和SINS间的杆臂，通过测量得到；
Figure FDA00038251668100000113
表示载体坐标系相对于地理坐标系的旋转角速度，
Figure FDA00038251668100000114
Figure FDA00038251668100000115
表示当前k时刻陀螺仪的输出；
Figure FDA00038251668100000116
表示由SINS/GPS组合导航系统估计的陀螺零偏；
Figure FDA00038251668100000117
表示地球自转角速度在地理坐标系下的投影；
Figure FDA00038251668100000118
表示载体相对于地球的转动角速度在地理坐标系下的投影，利用载体速度和当地地理位置直接获得；
步骤5：获取当前k时刻的量测矩阵Hk和观测误差矩阵Zk；
步骤6：通过鲁棒不变扩展卡尔曼滤波器估计k时刻SINS和DVL间安装偏差角
Figure FDA00038251668100000119
步骤6.1：初始化迭代次数t＝1；
步骤6.2：更新当前迭代次数下的量测噪声方差的辅助参数
Figure FDA00038251668100000120
Figure FDA00038251668100000121
Figure FDA00038251668100000122
Figure FDA00038251668100000123
Figure FDA0003825166810000021
Figure FDA0003825166810000022
Figure FDA0003825166810000023
其中，
Figure FDA0003825166810000024
表示k-1时刻李代数的预测值，初始时刻的李代数预测值
Figure FDA0003825166810000025
为预设给定；
Figure FDA0003825166810000026
表示k-1时刻李代数
Figure FDA0003825166810000027
的协方差阵，初始时刻的李代数
Figure FDA0003825166810000028
的协方差阵
Figure FDA0003825166810000029
为预设给定；fz(·)为相似度函数；
步骤6.3：判断是否达到最大迭代次数M；若未达到大迭代次数M，则返回步骤6.2继续迭代；否则，输出量测噪声方差的辅助参数
Figure FDA00038251668100000210
步骤6.4：鲁棒不变扩展卡尔曼滤波器的量测更新；
Figure FDA00038251668100000211
Figure FDA00038251668100000212
Figure FDA00038251668100000213
步骤7：若
Figure FDA00038251668100000214
且k＞1，则刻度系数误差不更新，令
Figure FDA00038251668100000215
步骤8：若k＜K，则令k＝k+1，返回步骤3；否则，输出刻度系数误差
Figure FDA00038251668100000216
SINS和DVL间安装偏差角
Figure FDA00038251668100000217
完成标定。
2.根据权利要求1所述的一种基于特殊正交群和鲁棒不变扩展卡尔曼滤波的DVL标定方法，其特征在于：所述步骤5中当前k时刻的量测矩阵Hk和观测误差矩阵Zk的计算方法为：
Hk＝αk×
Figure FDA00038251668100000218
Figure FDA00038251668100000219
Figure FDA00038251668100000220
其中，运算符“×”表示向量叉乘运算。
3.根据权利要求1所述的一种基于特殊正交群和鲁棒不变扩展卡尔曼滤波的DVL标定方法，其特征在于：所述步骤6.2中相似度函数fz(·)为：
Figure FDA0003825166810000031
其中，ω为自由度参数，m为量测维数。
CN202211056699.4A 2022-08-31 2022-08-31 一种基于特殊正交群和鲁棒不变扩展卡尔曼滤波的dvl标定方法 Pending CN115371705A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211056699.4A CN115371705A (zh) 2022-08-31 2022-08-31 一种基于特殊正交群和鲁棒不变扩展卡尔曼滤波的dvl标定方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211056699.4A CN115371705A (zh) 2022-08-31 2022-08-31 一种基于特殊正交群和鲁棒不变扩展卡尔曼滤波的dvl标定方法

Publications (1)

Publication Number Publication Date
CN115371705A true CN115371705A (zh) 2022-11-22

Family

ID=84069195

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211056699.4A Pending CN115371705A (zh) 2022-08-31 2022-08-31 一种基于特殊正交群和鲁棒不变扩展卡尔曼滤波的dvl标定方法

Country Status (1)

Country Link
CN (1) CN115371705A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116772903A (zh) * 2023-08-16 2023-09-19 河海大学 基于迭代ekf的sins/usbl安装角估计方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116772903A (zh) * 2023-08-16 2023-09-19 河海大学 基于迭代ekf的sins/usbl安装角估计方法
CN116772903B (zh) * 2023-08-16 2023-10-20 河海大学 基于迭代ekf的sins/usbl安装角估计方法

Similar Documents

Publication Publication Date Title
CN109324330B (zh) 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
US20230204793A1 (en) Gnss receiver protection levels
CN112525218B (zh) 一种ins/dvl组合导航系统鲁棒智能协同校准方法
JP5237723B2 (ja) 動的に較正されるセンサデータと、ナビゲーションシステム内の繰り返し拡張カルマンフィルタとを使用する、ジャイロコンパスの整合用のシステム及び方法
CN110823217B (zh) 一种基于自适应联邦强跟踪滤波的组合导航容错方法
Jo et al. GPS-bias correction for precise localization of autonomous vehicles
WO2023045357A1 (zh) 一种基于统计相似度量的组合导航鲁棒滤波方法
CN108594272B (zh) 一种基于鲁棒卡尔曼滤波的抗欺骗干扰组合导航方法
US20030036847A1 (en) Position and heading error-correction method and apparatus for vehicle navigation systems
EP1837627A2 (en) Methods and systems for implementing an iterated extended kalman filter within a navigation system
EP1657567A2 (en) Multipath height finding method
CN110794409B (zh) 一种可估计未知有效声速的水下单信标定位方法
Luo et al. A new robust Kalman filter for SINS/DVL integrated navigation system
CN107063245B (zh) 一种基于5阶ssrckf的sins/dvl组合导航滤波方法
US8209117B2 (en) Method and system for latitude adaptive navigation quality estimation
CN110567455B (zh) 一种求积更新容积卡尔曼滤波的紧组合导航方法
CN110749891B (zh) 一种可估计未知有效声速的自适应水下单信标定位方法
WO2016203744A1 (ja) 測位装置
KR101470081B1 (ko) 이동 정보 결정 장치, 수신기 및 그에 의한 방법
CN110779519A (zh) 一种具有全局收敛性的水下航行器单信标定位方法
CN115371705A (zh) 一种基于特殊正交群和鲁棒不变扩展卡尔曼滤波的dvl标定方法
KR101502721B1 (ko) 적응형 상호작용 다중모델 추정기를 이용한 정밀 위치정보 제공 방법 및 장치
Chu et al. Performance comparison of tight and loose INS-Camera integration
Dektor et al. Robust adaptive terrain-relative navigation
CN113093092A (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