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

一种基于特殊正交和鲁棒不变扩展卡尔曼滤波的dvl标定方法
技术领域
1.本发明属于非理想条件下的导航器件标定技术领域，具体涉及一种基于特殊正交和鲁棒不变扩展卡尔曼滤波的dvl标定方法。

背景技术：

2.捷联惯导/多普勒测速计程仪(sins/dvl)组合导航系统是目前水下航行器最常用的组合导航方法之一。dvl的标定精度会直接影响sins/dvl的组合导航精度。
3.对于一些体积小的水下航行器，其内部空间的限制会使得sins与dvl在安装时无法实现坐标轴的重合，甚至会出现很大的安装偏差，因此标定算法需要能够对大的安装偏差角进行准确计算或估计。此外，在一些复杂情况下，dvl会出现较大的测量离点，例如，dvl发射的声波波束受海洋生物遮挡无法到达海底，或者当水下航行器驶过海沟时，航行器与海床之间的距离超出dvl声波波束范围，亦或者海底存在强吸波材料(如淤泥等)，发出的声波波束不能反射回来。因此，需要发明在大安装偏差角和大dvl测量离点下保证标定精度的鲁棒标定方法。

技术实现要素：

4.本发明的目的在于针对dvl标定问题，提供能够对大安装偏差角进行准确建模的一种基于特殊正交和鲁棒不变扩展卡尔曼滤波的dvl标定方法，保证dvl的标定精度，即能够更准确地计算/估计出sins和dvl间的安装偏差角和dvl的刻度系数误差。
5.一种基于特殊正交和鲁棒不变扩展卡尔曼滤波的dvl标定方法，包括以下步骤：
6.步骤1：定义状态矩阵rk为为表示sins和dvl间安装偏差角；预先设置k＝0时的李代数预测值李代数的协方差矩阵状态矩阵r0、状态矩阵r0的预测值的协方差矩阵设置刻度系数误差阈值μ；
7.步骤2：设置标定总时间k，令k＝1，开始标定；
8.步骤3：获取当前k时刻sins/gps组合导航系统输出的载体速度测量值从地理坐标系到sins载体坐标系的姿态变换矩阵获取当前k时刻多普勒测速计程仪dvl输出的载体速度测量值获取量测噪声方差矩阵nk；
9.步骤4：计算刻度系数误差
10.11.其中，l
dvl
表示dvl和sins间的杆臂，通过测量得到；表示载体坐标系相对于地理坐标系的旋转角速度，地理坐标系的旋转角速度，表示当前k时刻陀螺仪的输出；表示由sins/gps组合导航系统估计的陀螺零偏；表示地球自转角速度在地理坐标系下的投影；表示载体相对于地球的转动角速度在地理坐标系下的投影，利用载体速度和当地地理位置直接获得；
12.步骤5：获取当前k时刻的量测矩阵hk和观测误差矩阵zk；
13.步骤6：通过鲁棒不变扩展卡尔曼滤波器估计k时刻sins和dvl间安装偏差角
14.步骤6.1：初始化迭代次数t＝1；
15.步骤6.2：更新当前迭代次数下的量测噪声方差的辅助参数
[0016][0017][0018][0019][0020][0021][0022]
其中，表示k-1时刻李代数的预测值，初始时刻的李代数预测值为预设给定；表示k-1时刻李代数的协方差阵，初始时刻的李代数的协方差阵为预设给定；fz(
·
)为相似度函数；
[0023]
步骤6.3：判断是否达到最大迭代次数m；若未达到大迭代次数m，则返回步骤6.2继续迭代；否则，输出量测噪声方差的辅助参数
[0024]
步骤6.4：鲁棒不变扩展卡尔曼滤波器的量测更新；
[0025][0026][0027][0028]
步骤7：若且k＞1，则刻度系数误差不更新，令
[0029]
步骤8：若k＜k，则令k＝k+1，返回步骤3；否则，输出刻度系数误差sins和dvl间安装偏差角完成标定。
[0030]
进一步地，所述步骤5中当前k时刻的量测矩阵hk和观测误差矩阵zk的计算方法为：
[0031]hk
＝αk×
[0032][0033][0034][0035]
其中，运算符
“×”
表示向量叉乘运算。
[0036]
进一步地，所述步骤6.2中相似度函数fz(
·
)为：
[0037][0038]
其中，ω为自由度参数，m为量测维数。
[0039]
本发明的有益效果在于：
[0040]
本发明通过李空间到李代数空间的指数映射，得到了线性的误差方程，可以更准确的刻画误差传播规律，避免非线性条件下不准确的雅各比矩阵对滤波估计的影响。本发明通过引入统计相似度量李，设计鲁棒滤波器，抑制了测量离点对标定精度的影响，可以更准确的输出sins和dvl间的安装偏差角及dvl的刻度系数误差。
附图说明
[0041]
图1为本发明的总体流程示意图。
[0042]
图2为本发明的实施例中俯仰安装偏差角的标定误差图。
[0043]
图3为本发明的实施例中航向安装偏差角的标定误差图。
[0044]
图4为本发明的实施例中刻度系数的标定误差图。
[0045]
图5为本发明的总体框架图。
具体实施方式
[0046]
下面结合附图对本发明做进一步描述。
[0047]
本发明在考虑对sins和dvl间的大安装误差角利用特殊正交(李的一种)进行准确建模的同时,针对目前现有的标定算法无法应对测量离点的问题，发明了新的用于特殊正交模型的鲁棒滤波方法。本发明在将特殊正交模型的误差方程指数映射到李代数空间上的同时，引入统计相似度量(ssm)理论，在李代数空间上研究误差传播规律，自适应调节量测噪声协方差阵并计算滤波增益，然后对李空间进行预测信息和量测信息的指数加权运算，得到安装误差的估计结果。本发明可用于非理想条件下的水下航行器标定领域。
[0048]
首先定义常用坐标系，sins载体坐标系采用b表示，“东-北-天”地理坐标系作为导航系采用n表示，惯性坐标系用i表示，dvl框架坐标系用d表示。
[0049]
dvl的速度测量值可以表示为：
[0050][0051]
式中，表示x的实际测量值，sd表示本发明需要计算的刻度系数误差。r为三阶特殊正交由于其单位正交的性质，可以用来表示姿态旋转矩阵。表示从b系到d系的姿态变换矩阵，也是本发明中需要估计的sins和dvl间安装偏差角。表示从n系到b系的姿态变
换矩阵.vn表示导航坐标系下真实的速度。δvd表示dvl的测量噪声。l
dvl
表示dvl和sins间的杆臂，通常可以事先准确测量。是b系相对于n系的旋转角速度。
[0052]
忽略测量噪声，考虑到特殊正交的单位正交性质，不会对合速度的大小产生影响，因此可以对(1)式两边取2-范数，得到：
[0053][0054]
式中，和由sins/gps组合导航系统直接获得。可以按照下式计算:
[0055][0056]
式中，是陀螺仪的输出。表示地球自转角速度在n系下的投影。为由则导航系统估计的陀螺零偏。可以利用载体速度和当地地理位置直接获得。利用(2)式可以得到刻度系数误差的计算公式。
[0057][0058]
离散形式为
[0059][0060]
定义
[0061][0062][0063][0064]
δαk＝δvd(k)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(9)安装偏差角通常是一个常值。因此结合(6)、(7)、(8)和(9)，使用so(3)表示的标定模型为：
[0065][0066]
因此，安装偏差角的标定问题转化为了so(3)的估计问题。
[0067]
由于(10)所示的观测方程为左不变观测，因此定义左不变误差ηk为
[0068][0069]rk
表示真实状态，表示估计状态。
[0070]
由于f(ab)＝af(b)+f(a)b-af(i)b，因此
[0071][0072]
观测误差定义为
[0073][0074]
(12)和(13)构成了李空间上的误差模型。然而，左不变误差ηk仍然是一个矩阵,(13)是一个非线性方程，因此研究其误差传播是不方便的。而ηk可以指数映射到李代数空间上
[0075]
ηk＝exp(ξk×
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(14)
[0076]
ξk是ηk在李代数空间上的对数投影向量。(14)代入(13)进行一阶泰勒展开得
[0077][0078]
由此，令可以得到李代数空间上的误差传播方程
[0079][0080]
利用不变扩展卡尔曼滤波(iekf)可以得到最终状态的估计结果。
[0081][0082][0083][0084]
其中，hk＝αk×
,nk＝cov(vk)，和表示一步预测和预测误差协方差阵，其计算过程如下：
[0085][0086][0087]
初始时刻的状态和其协方差阵人为给定。
[0088]
测量异常值使得dvl测量的速度与真实速度之间存在较大差异。这种差异可能会被标定算法误解为由安装未对准角度和刻度系数误差引起，从而降低标定精度。特别是对于刻度系数误差，其计算精度完全取决于速度信息的测量质量。对于刻度系数误差的计算，dvl标定算法应该具有检测离点的能力。当检测到离点时，应丢弃当前时刻的测量异常值信息，不更新刻度系数误差。对于so(3)表示的dvl标定模型，应研究鲁棒iekf算法。
[0089]
目前，大多数鲁棒滤波器通过自适应调整滤波增益矩阵来减少异常值对滤波精度的影响，该矩阵可以通过改变测量噪声协方差矩阵来调整。当出现测量离点时，应增加测量协方差矩阵以降低滤波器增益，从而降低最终估计结果中离点信息的比重。iekf的思想类似于kf，两者都是通过对预测信息和测量信息进行加权来更新状态。在iekf中，根据李代数模型中的噪声协方差和误差不确定性(误差协方差)传播规律计算权重矩阵(滤波器增益)。因此，在应用(16)估计李代数的同时适应调整测量噪声协方差，通过使用修正后的测量噪声协方差计算滤波器增益可以有效地降低异常值对iekf的影响。同时，可以通过测量噪声协方差矩阵的调整程度来判断是否出现离点，从而判断当前时刻是否更新刻度系数误差。
[0090]
统计相似度度量s(a,b)可以用来表示两个随机向量a和b之间的相似度，定义如下：
[0091]
s(a,b)＝e[f(||a-b||2)]＝∫∫f(||a-b||2)p(a,b)dadb
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(22)
[0092]
式中，||
·
||表示欧氏范数，p(a,b)表示随机向量a和b之间的联合概率密度。f(
·
)表示相似度函数，并且满足以下三个条件。
[0093]
a)f(
·
)是在定义域[0，+∞)内的连续函数。
[0094]
b)f(
·
)是单调递减函数，即
[0095]
c)f(
·
)的二阶导数是非负的，即
[0096]
统计相似度度量符合相似度度量的通常定义。随机向量之间的相似性越高，统计相似度度量就越大。选择f(t)为f(t)＝-t时，ssm表示不同随机向量之间的负均方误差。当选择f(t)为时，ssm表示不同随机向量之间的相关熵。通过选择不同的相似函数，可以实现不同的ssm。
[0097]
假设李代数ξk的预测更新为
[0098][0099][0100]
初始时刻的李代数预测值及其协方差阵人为给定。
[0101]
本发明则构造了反映李代数与预测李代数、量测与预测量测之间ssm的代价函数。
[0102][0103]
和表示量测噪声方差nk和一步预测协方差阵的均方根分解。
[0104]
为了便于求解(25)所示的代价函数，将后验分布近似为高斯分布并利用詹森(jensen)不等式求解上式中代价函数的下界，将其作为一个新的代价函数。
[0105][0106][0107][0108]
求解代价函数的极值的计算公式如下。
[0109][0110]
由于式(29)为非线性方程，因此在本发明中使用高斯-牛顿迭代算法对(29)进行求解。
[0111]
根据高斯-牛顿迭代法，对于非线性方程式(29)，解的数值更新过程为
[0112][0113]
式中，上标t表示第t次迭代。迭代初值设为表示第t次迭代。迭代初值设为表示代价函数的近似海森(hessian)矩阵。
[0114][0115][0116][0117][0118][0119]
(31)-(35)代入(30)，整理得
[0120][0121][0122]
其中和相当于用来调节一步预测协方差阵和量测噪声方差的辅助参数。由状态理论上是一个常值，因此，相似度函数f
x
(t)选择为f
x
(t)＝-0.5t，即采用负均方误差来度量状态与预测状态之间的相似程度，辅助参数不对预测协方差阵进行调节。而dvl波束测量噪声则可能是由野值引起的非高斯噪声，因此相似度函数fz(t)选为ω是自由度参数，m是量测维数，在本发明中为3。
[0123]
可以直接得到估计误差协方差阵
[0124][0125]
(37)式可以重写为
[0126][0127][0128]
当迭代计算完成后，假设最大迭代次数为m，则(41)表示为利用代替(17)中的nk得
[0129][0130]
[0131][0132]
(42)、(43)和(44)构成了鲁棒不变扩展卡尔曼滤波器的量测更新，即为估计的安装偏差变换矩阵由姿态变换矩阵阵和姿态角的变换关系很容易从中获得安装偏差角。
[0133]
鲁棒iekf引入γk去修正量测噪声协方差阵nk。当出现大的测量离点时，残差会很大，使得γk自动减少。特别是当残差趋于无穷大时，γk几乎区域零使得也接近于零。因此，γk的变化可以用来判断是否有离点出现。
[0134]
定义判断阈值为μ。如果γk＜μ，则刻度系数误差不利用当前时刻的速度信息进行更新，即如果γk＞μ，则刻度系数根据公式(5)计算获得。
[0135]
基于上述理论推导过程，如图1所示，本发明在实际进行标定应用时，步骤为：
[0136]
步骤1：定义状态矩阵rk为为表示sins和dvl间安装偏差角；预先设置k＝0时的李代数预测值李代数的协方差矩阵状态矩阵r0、状态矩阵r0的预测值的协方差矩阵设置刻度系数误差阈值μ；
[0137]
步骤2：设置标定总时间k，令k＝1，开始标定；
[0138]
步骤3：获取当前k时刻sins/gps组合导航系统输出的载体速度测量值从地理坐标系到sins载体坐标系的姿态变换矩阵获取当前k时刻多普勒测速计程仪dvl输出的载体速度测量值获取量测噪声方差矩阵nk；
[0139]
步骤4：计算刻度系数误差
[0140][0141]
其中，l
dvl
表示dvl和sins间的杆臂，通过测量得到；表示载体坐标系相对于地理坐标系的旋转角速度，地理坐标系的旋转角速度，表示当前k时刻陀螺仪的输出；表示由sins/gps组合导航系统估计的陀螺零偏；表示地球自转角速度在地理坐标系下的投影；表示载体相对于地球的转动角速度在地理坐标系下的投影，利用载体速度和当地地理位置直接获得；
[0142]
步骤5：计算当前k时刻的量测矩阵hk和观测误差矩阵zk；
[0143]hk
＝αk×
[0144][0145][0146]
[0147]
其中，运算符
“×”
表示向量叉乘运算。
[0148]
步骤6：通过鲁棒不变扩展卡尔曼滤波器估计k时刻sins和dvl间安装偏差角
[0149]
步骤6.1：初始化迭代次数t＝1；
[0150]
步骤6.2：更新当前迭代次数下的量测噪声方差的辅助参数
[0151][0152][0153][0154][0155][0156][0157]
其中，表示k-1时刻李代数的预测值，初始时刻的李代数预测值为预设给定；表示k-1时刻李代数的协方差阵，初始时刻的李代数的协方差阵为预设给定；fz(
·
)为相似度函数，ω为自由度参数，m为量测维数；
[0158]
步骤6.3：判断是否达到最大迭代次数m；若未达到大迭代次数m，则返回步骤6.2继续迭代；否则，输出量测噪声方差的辅助参数
[0159]
步骤6.4：鲁棒不变扩展卡尔曼滤波器的量测更新；
[0160][0161][0162][0163]
步骤7：若且k＞1，则刻度系数误差不更新，令
[0164]
步骤8：若k＜k，则令k＝k+1，返回步骤3；否则，输出刻度系数误差sins和dvl间安装偏差角完成标定。
[0165]
实施例1：
[0166]
初始位置设置为北纬30
°
，东经120
°
。初始速度为2m/s，向东北方向直线航行。陀螺常值漂移设置为0.005
°
/h，加速度计常值零偏设置为100μg。初始的姿态误差设置为[0.01
°
,0.01
°
,0.02
°
]，安装偏差角设置为大角度[75
°
,75
°
,150
°
]，刻度系数误差设置为0.005。dvl的测量噪声设置为：
[0167]
[0168][0169][0170]
w.p.表示发生概率。图2和图3表示不同算法50次蒙特卡洛仿真中俯仰安装偏差角和航向安装偏差角的均方根误差(rmse)。qkf表示基于四元数卡尔曼滤波的标定算法，qnq-p表示基于位置观测和伪牛顿迭代方法的标定算法，iekf表示在本发明所提出的标定模型基础上仅使用不变扩展卡尔曼滤波的标定算法，鲁棒iekf表示在本发明所提出的标定模型基础上使用鲁棒不变扩展卡尔曼滤波的标定算法，也是本发明所最终提出的标定算法。从图中可以看出，在大安装偏差角和大离点条件下，所提出算法对安装偏差的标定误差最小，与其他算法差别明显。图4表示刻度系数误差的标定结果，可以看出所提出的鲁棒标定算法能够保证很好的标定精度。其他算法由于无法对离点信息进行处理，对不正确信息的错误利用使得对刻度系数误差的计算毫无精度可言。
[0171]
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

技术特征：

1.一种基于特殊正交和鲁棒不变扩展卡尔曼滤波的dvl标定方法，其特征在于，包括以下步骤：步骤1：定义状态矩阵r
k
为为表示sins和dvl间安装偏差角；预先设置k＝0时的李代数预测值李代数的协方差矩阵状态矩阵r0、状态矩阵r0的预测值的协方差矩阵设置刻度系数误差阈值μ；步骤2：设置标定总时间k，令k＝1，开始标定；步骤3：获取当前k时刻sins/gps组合导航系统输出的载体速度测量值从地理坐标系到sins载体坐标系的姿态变换矩阵获取当前k时刻多普勒测速计程仪dvl输出的载体速度测量值获取量测噪声方差矩阵n
k
；步骤4：计算刻度系数误差步骤4：计算刻度系数误差其中，l
dvl
表示dvl和sins间的杆臂，通过测量得到；表示载体坐标系相对于地理坐标系的旋转角速度，坐标系的旋转角速度，表示当前k时刻陀螺仪的输出；表示由sins/gps组合导航系统估计的陀螺零偏；表示地球自转角速度在地理坐标系下的投影；表示载体相对于地球的转动角速度在地理坐标系下的投影，利用载体速度和当地地理位置直接获得；步骤5：获取当前k时刻的量测矩阵h
k
和观测误差矩阵z
k
；步骤6：通过鲁棒不变扩展卡尔曼滤波器估计k时刻sins和dvl间安装偏差角步骤6.1：初始化迭代次数t＝1；步骤6.2：更新当前迭代次数下的量测噪声方差的辅助参数步骤6.2：更新当前迭代次数下的量测噪声方差的辅助参数步骤6.2：更新当前迭代次数下的量测噪声方差的辅助参数步骤6.2：更新当前迭代次数下的量测噪声方差的辅助参数步骤6.2：更新当前迭代次数下的量测噪声方差的辅助参数步骤6.2：更新当前迭代次数下的量测噪声方差的辅助参数步骤6.2：更新当前迭代次数下的量测噪声方差的辅助参数其中，表示k-1时刻李代数的预测值，初始时刻的李代数预测值为预设给定；表示k-1时刻李代数的协方差阵，初始时刻的李代数的协方差阵为预
设给定；f
z
(
·
)为相似度函数；步骤6.3：判断是否达到最大迭代次数m；若未达到大迭代次数m，则返回步骤6.2继续迭代；否则，输出量测噪声方差的辅助参数步骤6.4：鲁棒不变扩展卡尔曼滤波器的量测更新；步骤6.4：鲁棒不变扩展卡尔曼滤波器的量测更新；步骤6.4：鲁棒不变扩展卡尔曼滤波器的量测更新；步骤7：若且k＞1，则刻度系数误差不更新，令步骤8：若k＜k，则令k＝k+1，返回步骤3；否则，输出刻度系数误差sins和dvl间安装偏差角完成标定。2.根据权利要求1所述的一种基于特殊正交和鲁棒不变扩展卡尔曼滤波的dvl标定方法，其特征在于：所述步骤5中当前k时刻的量测矩阵h
k
和观测误差矩阵z
k
的计算方法为：h
k
＝α
k
×××
其中，运算符
“×”
表示向量叉乘运算。3.根据权利要求1所述的一种基于特殊正交和鲁棒不变扩展卡尔曼滤波的dvl标定方法，其特征在于：所述步骤6.2中相似度函数f
z
(
·
)为：其中，ω为自由度参数，m为量测维数。

技术总结

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