Sins/dvl组合中dvl方位安装误差估计方法
【技术领域】
[0001] 本发明涉及导航领域，具体涉及一种SINS/DVL组合中DVL方位安装误差估计方 法。
【背景技术】
[0002] 对于基于积分工作方式的惯性导航系统（Strapdown Inertial Navi gat ion，SINS)而言，其定位误差随着时间累积，难以满足长时间的高精度定位要求，多 普勒测速仪（Doppler Velocity Log，DVL)由于能够提供高精度连续速度信息，是常用的 辅助导航设备。将DVL与SINS组合，从而抑制定位误差的发散。DVL的安装偏差角是影响 SINS/DVL组合定位精度的主要因素，为了提高SINS/DVL组合导航系统的定位精度，需要准 确标定捷联惯性器件与多普勒测速仪之间的安装偏差。目前，对方位安装误差标定通常利 用外部GPS基准信息，通过比较GPS的轨迹和推算轨迹得到方位安装误差的标定值。然而， 这种方法破坏掉了水下航行器的隐蔽性。
【发明内容】
[0003] 发明目的：为了克服上述问题，本发明在航行器处于水下时，利用DVL的速度辅助 信息，完成DVL方位安装误差的估计并校正。在保证SINS/DVL组合定位精度的同时提高了 隐蔽性。
[0004] 技术方案：一种SINS/DVL组合中DVL方位安装误差估计方法，包括以下步骤：
[0005] (1)选择SINS速度误差、姿态误差、位置误差、加速度计零偏、陀螺仪零偏、DVL刻 度系数误差、DVL与SINS中頂U的方位安装误差作为状态向量，构建系统状态方程；选择 SINS载体系速度与DVL安装坐标系速度差值作为量测向量，构建系统量测方程；
[0006] (2)由Kalman滤波器作为SINS/DVL信息融合滤波器；
[0007] (3)选择转弯运动以获取SINS/DVL方位安装误差的最佳可观测度，即最佳估计效 果。
[0008] 进一步地，所述步骤（1)具体包括如下步骤：
[0009] 1)选取SINS的误差状态为：速度误差δν = [δνΕ δνΝ]τ，姿态误差φ = [φΕ ΦΝ Φυ]Τ，位置误差Sp = [δ? δ λ]τ，加速度计零偏Ρ = [?7, pj，陀螺仪零偏ε = [εχ ε y ε JT，共计12维；并将DVL刻度系数误差δ K = [ δ Kx δ Ky]T和DVL与MJ的ζ轴 方向的方位安装误差％扩充为状态向量；则SINS/DVL组合导航系统模型的状态向量为15 维：
[0010]
⑴
[0011] 其中，δνΕ、δν#别为SINS的东向、北向速度误差；Φ Ε、ΦΝ、别是东向、北 向、天向失准角；s L、δ λ分别为炜度、经度误差；RR分别为x、y轴方向的加速度计零 偏；ε χ、ε y、ε z分别为x、y、z轴方向的陀螺仪零偏；δ K χ、δ Ky分别为x、y轴方向的DVL 刻度系数误差；
[0012] 将DVL刻度系数误差、DVL与頂U的方位安装误差认为是常值，即：
[0013]
(2)
[0014] 其中，政_是Xdvl的导数，砝^是δκχ的导数，滅： >:是SKy的导数，喪是％的导数；[0015] 根据SINS/DVL误差模型，可得SINS/DVL的线性状态方程：
[0016]
[0017]
[0018]
[0019] 式中，F(t)为系统的状态转移矩阵；WsinsS状态向量的过程噪声矩阵;SINS 过程噪声矩阵中速度误差矩阵;W41为SINS过程噪声矩阵中失准角矩阵;w p为SINS过程噪 声矩阵中位置误差矩阵；为SV的导数j为Φ的导数；为SP的导数；^为^?的导 数d为ε的导数；为S K的导数；失为A的导数；
[0025] 其中，《ie、R与L分别为地球自转角速度、地球半径以及载体所在地理炜度；f E、fN 与fu为加速度测量值在导航系η中沿东向、北向与天向轴的投影；C为载体系b向导航系 η的转换矩阵；％与Vn分别为SINS的东向与北向速度；C；(MD为矩阵中第M行第 N列的元素；
[0026] 2)以SINS载体系速度与DVL安装坐标系速度差值作为量测向量，并构建系统量测 方程；
[0027] 考虑DVL刻度系数误差和随机测暈误差，DVL测量值为：
[0028]
(5)
[0029] 式中，上标m表示DVL安装坐标系；!表示DVL在m系中的真实值，δ Vdl为测量 噪声；i = X，y为DVL的水平安装方向；匕了;为DVL的测量值；
[0030] 在载体系b下，SINS的速度计算值为：
[0031]
(6::)
[0032] 式中，为SINS速度真实值；JFJvi为SINS解算的速度误差；d为SINS在导航 系η中解算的速度(G s+Mk.)在b系的投影值；C::为η'系到η系的转换矩阵，且有：
[0033] m = /+#χ (7)
[0034] 式中，η'为计算导航坐标系；
[0035] 将式（7)代入式（6)得到：
[0036]
(8)
[0037] 式中，为SINS在导航系η中的速度真实值Gk在载体系b中的投影；
[0038] 由于DVL与SINS间存在安装误差角P , SINS载体系速度与DVL安装坐标系速度 之间存在如下关系：
[0039]
Cf)
[0040] 式中，r为DVL安装坐标系m向载体系b的转换矩阵。*为包含方位安装误差轉 在内的安装误差角向量，忽略水平安装误差，有：P=P 〇死]%
[0041] 从而有：
[0042] (10)
[0043] 根据式（8~10)，有：[0044]
QO
[0045] 式中，δ Vd* DVL 测量噪声，δ V 測 δ V di，i = X，y ;仏即 = W ;
[0046] 式（11)即为SINS/DVL的量测方程，写成矩阵形式：
[0047] Z = HX+V (12)
[0048] 式中Z = V为量测噪声（一般用零均值白噪声序列描述）； H为量测矩阵：
[0049]
[0050] 式中，匕与匕为SINS速度在X与y轴的投影。
[0051] 进一步地，所述的步骤2)具体包括：
[0052]
(14)
[0053] 式中，Kk为增益矩阵；Pk为状态向量的协方差矩阵；R kS量测噪声序列协方差 矩阵；Qk为过程噪声序列Wk的协方差矩阵；氧为状态向量估计值；爲为状态向量一步预 测值；Ptk :为状态向量的协方差矩阵一步预测值；Φ i为状态转移矩阵，即式⑶中F⑴ 阵的离散形式；Fk i为过程噪声转移矩阵。
[0054] 进一步地，所述的步骤3)具体包括：
[0055] 求取线性时变系统的总可观测矩阵的表达式如下：
[0056]
(15)
[0057] 式中，泛为连续系统在第j时间段的可观测矩阵，其形式为：
[0058]
(16)
[0059] 其中，p = 15 ;p_l为矩阵Fj的乘方次数；1为连续系统在第j时间段的量测矩阵； Fj为第j时间段的状态转移矩阵；
[0060] 利用列主元高斯消元法将式（16)转换为上三角矩阵，并再进一步化为对角矩阵 得到状态的可观测性情况，并根据对角矩阵对角元素的绝对值得到SINS/DVL组合导航系 统在转弯运动下的可观测度。
[0061] 有益效果：1)补偿过程中，不需要外部GPS参考辅助导航信息，保证了隐蔽性；2) 在SINS与DVL组合过程中，不需要将DVL速度转换至导航坐标系；3)根据可观测性分析结 果，可以选择改善DVL方位安装误差估计的可行方案。
【附图说明】
[0062] 图1为本发明使用的DVL方位安装误差补偿方案示意图；
[0063] 图2为本发明DVL方位安装误差估计图；
【具体实施方式】
[0064] 本发明针对DVL辅助SINS构成的水下组合导航系统，选择SINS速度误差、姿态误 差、位置误差、加速度计零偏、陀螺仪零偏、DVL刻度系数误差、DVL与SINS中頂U的方位安 装误差作为状态向量，构建系统状态方程；选择SINS载体系速度与DVL安装坐标系速度差 值作为量测向量，构建系统量测方程；由Kalman滤波器作为SINS/DVL信息融合滤波器；选 择转弯运动以获取SINS/DVL方位安装误差的最佳可观测度，即最佳估计效果。
[0065] 下面结合附图对本发明实施方法做更详细地描述：
[0066] 一种SINS/DVL组合中DVL方位安装误差估计方法，选择SINS速度误差、姿态误 差、位置误差、加速度计零偏、陀螺仪零偏、DVL刻度系数误差、DVL与SINS中頂U的方位安 装误差作为状态向量，构建系统状态方程；选择SINS载体系速度与DVL安装坐标系速度差 值作为量测向量，构建系统量测方程。具体包括如下步骤：
[0067] 1)选取SINS的误差状态为：速度误差δν = [δνΕ δνΝ]τ，姿态误差φ = [φΕ ΦΝ ΦυΓ，位置误差Sp= [δ? δ λ]τ，加速度计零偏Ρ = 陀螺仪零偏ε = [εχ ε y ε JT，共计12维；并将DVL刻度系数误差δ K = [ δ Kx δ Ky]T和DVL与MJ的ζ轴 方向的方位安装误差P扩充为状态向量；则SINS/DVL组合导航系统模型的状态向量为15 维：
[0068] (])
[0069] 其中，δ VE、δ %分别为SINS的东向、北向速度误差；Φ Ε、ΦΝ、伞^分别是东向、北 向、天向失准角；S L、δ λ分别为炜度、经度误差；K、: K分别为x、y轴方向的加速度计零 偏；ε χ、ε y、ε z分别为x、y、z轴方向的陀螺仪零偏；δ K χ、δ Ky分别为x、y轴方向的DVL 刻度系数误差；
[0070] 将DVL刻度系数误差、DVL与頂U的方位安装误差认为是常值，即：
[0071] (2)
[0072] 其中，Xim是V的导数，成是δΚ#导数，是δΚ^导数，<是化的导数； [0073] 根据SINS/DVL误差模型，可得SINS/DVL的线性状态方程： LziN 丄 UdU；?丄；7LM n ^ * 1 w u/ ?
[0074]
[0075] 上式写成矩阵形式为：
[0076]
[0077] 式中，F(t)为系统的状态转移矩阵；WsinsS状态向量的过程噪声矩阵;w vS SINS 过程噪声矩阵中速度误差矩阵;W41为SINS过程噪声矩阵中失准角矩阵;w p为SINS过程噪 声矩阵中位置误差矩阵；
[0083] 其中，《ie、R与L分别为地球自转角速度、地球半径以及载体所在地理炜度；f E、fN 与fu为加速度测量值在导航系η中沿东向、北向与天向轴的投影；?'为载体系b向导航系 η的转换矩阵；％与V及别为SINS的东向与北向速度；
[0084] 2)以SINS载体系速度与DVL安装坐标系速度差值作为量测向量，并构建系统量测 方程；
[0085] 考虑DVL刻度系数误差和随机测量误差，DVL测量值为：
[0086] Ka +SV111, i = x,y (5)
[0087] 式中，上标m表示DVL安装坐标系；表示DVL在m系中的真实值，δ Vdl为测量 噪声；i = X，y为DVL的水平安装方向