SINS/DR组合导航（二） - 走航式声学多普勒流速仪底跟踪失效可能的原因是

前言

SINS/DVL（多普勒测速仪）组合导航是水下导航中最常用的方式之一，参考国防科技大学吕兆鹏的硕士论文，推导SINS/DVL组合导航相关公式。

DVL测速度系统误差模型

DVL自身测量坐标系为 m\begin{equation}m\end{equation} 系，惯导坐标系为 b\begin{equation}b\end{equation} 系，则DVL速度在导航系 n\begin{equation}n\end{equation} 系的投影为：

VDVLn=CbnCmbVDVLm\begin{equation}\mathbf{V}_{D V L}^{n}=\mathbf{C}_{b}^{n} \mathbf{C}_{m}^{b} \mathbf{V}_{D V L}^{m}\end{equation}

VDVLm\begin{equation}\mathbf{V}_{D V L}^{m}\end{equation} 是DVL在 m\begin{equation}m\end{equation} 系的真实数据，无噪声。

设DVL的刻度因子误差为 δk\begin{equation}\delta k\end{equation} ， δk\begin{equation}\delta k\end{equation} 为小量，一般在 1%\begin{equation}1 \%\end{equation} 甚至更小，当然，此参数越小，也就越贵。DVL测速方程表示如下：

( H# ~& y0 u4 t! _' Q2 n1 l

V~DVLm=(1+δk)VDVLm\begin{equation}\tilde{\mathbf{V}}_{D V L}^{m}=(1+\delta k) \mathbf{V}_{D V L}^{m}\end{equation}\\

设惯导和DVL之间的安装误差为 η=[αβγ]T\begin{equation}\eta=\left[\begin{array}{lll} \alpha & \beta & \gamma \end{array}\right]^{T}\end{equation} ，安装误差一般量级在数度，并且可以看成常值，所以有如下公式：

C~mb=(I−η×)Cmb\begin{equation}\tilde{\mathbf{C}}_{m}^{b}=(\mathbf{I}-\eta \times) \mathbf{C}_{m}^{b}\end{equation}\\

2 ^" O3 h% U2 V5 z1 R, P8 h/ _

同理，姿态角同样存在误差，准确的应该称为失准角，公式如下：

~' P- N' p3 V# d& j

C~bn=(I−Ψ×)Cbn\begin{equation}\tilde{\mathbf{C}}_{b}^{n}=(\mathbf{I}-\Psi \times) \mathbf{C}_{b}^{n}\end{equation}

对于光纤惯导系统来说，失准角是变化的，特别是方位角，一般单位是 °/h\begin{equation}°/h\end{equation} ，但是如果陀螺本身精度较高，则在短时间内（数小时内），方位失准角也可以看作是常值。

dvl在导航坐标系下的实际投影速度，即是有噪声的。

- f* l9 a5 J8 h

V~DVLn=C~bnC~mbV~DVLm\begin{equation}\tilde{\mathbf{V}}_{D V L}^{n}=\tilde{\mathbf{C}}_{b}^{n} \tilde{\mathbf{C}}_{m}^{b} \tilde{\mathbf{V}}_{D V L}^{m}\end{equation}\\

代入第一个公式，可以得到如下公式：

0 Y/ G, @# g3 f. k) A

V~DVLn=[I−Ψ×]Cbn(I−η×)Cmb(1+δk)VDVLm\begin{equation}\tilde{\mathbf{V}}_{D V L}^{n}=[\mathbf{I}-\Psi \times] \mathbf{C}_{b}^{n}(\mathbf{I}-\mathbf{\eta} \times) \mathbf{C}_{m}^{b}(1+\delta k) \mathbf{V}_{D V L}^{m}\end{equation}\\

, s7 I) i2 W9 G& X! ?4 m' r; B

省略上面公式中的二阶小量可得:

=CbnCmbVDVLm−(ψ×)CbnCmbVDVLm+Cbn(−η×)CmbVDVLm+δkCbnCmbVDVLm\begin{equation}=C_{b}^{n} C_{m}^{b} V_{D V L}^{m}-(\psi \times) C_{b}^{n} C_{m}^{b} V_{D V L}^{m}+C_{b}^{n}(-\eta \times) C_{m}^{b} V_{D V L}^{m}+\delta k C_{b}^{n} C_{m}^{b} V_{D V L}^{m}\end{equation}

1 b# `: d/ I8 l) V

接着化简如下：

=VDVLn−(ψ×)VDVLn+Cbn(−η×)VDVLb+δkVDVLn\begin{equation}=V_{D V L}^{n}-(\psi \times) V_{D V L}^{n}+C_{b}^{n}(-\eta \times) V_{D V L}^{b}+\delta k V_{D V L}^{n}\end{equation}\\

* P" x2 a; ]4 l2 k0 b1 t/ b4 w1 H7 {

因为公式中的状态向量不能写成 ×\begin{equation}\times\end{equation} （叉乘）的形式，所以接着化简如下：

3 Z' {/ e9 J1 {( r/ d: `+ b

=VDVLn+(VDVLn×)ψ+Cbn(−η×)VDVLb+δkVDVLLn\begin{equation}=V_{D V L}^{n}+\left(V_{D V L}^{n} \times\right) \psi+C_{b}^{n}(-\eta \times) V_{D V L}^{b}+\delta k V_{D V L L}^{n}\end{equation}\\

: z$ C" B. Y% O& R

上公式中把 ψ\begin{equation}\psi\end{equation} 的叉乘符号去掉了，接下来也要把 η\begin{equation}\eta\end{equation} 的叉乘符号去掉，有如下公式成立：

( z1 @/ k. m0 [6 M

Cbn(−η×)VDVLb=VDVLn×Cbnη\begin{equation}C_{b}^{n}(-\eta \times) V_{D V L}^{b}=V_{D V L}^{n} \times C_{b}^{n} \eta\end{equation}

7 v f( X4 t, q& A: w2 E8 n

上述公式暂时不知道如何推导得到的，但是已经用matlab验证过了，是成立的。

所以最终的推导结果为：

=VDVLn+(VDVLn×)ψ+VDVLn×Cbnη+δkVDVLn\begin{equation}=V_{D V L}^{n}+\left(V_{D V L}^{n} \times\right) \psi+V_{D V L}^{n} \times C_{b}^{n} \eta+\delta k V_{D V L}^{n}\end{equation}

所以DVL的测速误差在 nn 系中可以表示如下：

" M( w: H+ H- N8 S% l

δVDVLn=V~DVLn−VDVLn=VDVLn×ψ+VDVLn×Cbnη+δkVDVLn\begin{equation}\delta V_{D V L}^{n}=\tilde{V}_{D V L}^{n}-V_{D V L}^{n}=V_{D V L}^{n} \times \psi+V_{D V L}^{n} \times C_{b}^{n} \eta+\delta k V_{D V L}^{n}\end{equation}

上式即为考虑了姿态失准角，安装偏角以及DVL刻度因子误差的DVL测速度误差模型。

/ \4 ]( \* A5 w+ q4 o5 z: v0 G

假设不考虑姿态失准角，即使用成熟的商用姿态解算算法，则公式如下：

4 |2 s) Y! U t+ ~

δVDVLn=VDVLn×Cbnη+δkVDVLn\begin{equation}\delta V_{D V L}^{n}=V_{D V L}^{n} \times C_{b}^{n} \eta+\delta k V_{D V L}^{n}\end{equation}

; @- d* E1 z' F2 H( `

2. SINS/DVL组合导航方案

?8 M( o" T6 Q% I6 Z* h. U0 D }

SINS/DVL组合导航方案框图如下所示：

SINS/DVL 组合导航原理框图

组合导航系统误差状态量选取如下：

X=[ψEψNψUδVEδVNβδk]T\begin{equation}X=\left[\begin{array}{lllllll} \psi_{E} & \psi_{N} & \psi_{U} & \delta V_{E} & \delta V_{N} & \beta & \delta k \end{array}\right]^{\mathrm{T}}\end{equation}

_7 |+ o$ o: p

对于SINS系统不考虑导航系天向速度，天向通道有水压深度计，仅考虑三个姿态失准角，东向、北向速度。由于SINS和DVL之间的滚转角和俯仰角误差对定位的影响较小，所以这里的安装偏差只考虑偏航角误差。减少系统维数，有利于减少工程量，也降低了滤波器发散的可能性。这里的状态变量维度为 77 ，如果不考虑姿态失准角，维度为 44 ,如下所示：

X=[δVEδVNβδk]T\begin{equation}X=\left[\delta V_{E} \quad \delta V_{N} \quad \beta \quad \delta k\right]^{\mathrm{T}}\end{equation}\\

首先偏航安装误差角 β\begin{equation}\beta\end{equation} 和DVL刻度因子 δk\begin{equation}\delta k\end{equation} 为常值，表示如下：

β˙=0,δk˙=0\begin{equation}\dot{\beta}=0, \quad \delta \dot{k}=0\end{equation}

当为 77 维时，系统状态方程表示如下：

X˙=[ASINS05×202×7]X+[M103×302×3M202×6]w\begin{equation}\dot{\mathbf{X}}=\left[\begin{array}{cc} \mathbf{A}_{SI N S} & \mathbf{0}_{5 \times 2} \\ \mathbf{0}_{2 \times 7} \end{array}\right] \mathbf{X}+\left[\begin{array}{cc} \mathbf{M}_{1} & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{2 \times 3} & \mathbf{M}_{2} \\ \mathbf{0}_{2 \times 6} \end{array}\right] \mathbf{w}\end{equation}

- y+ ]* c) G' }- g: r: M9 C

其中，

ASINS=[A11A12A21A22]\begin{equation}\mathbf{A}_{S I N S}=\left[\begin{array}{ll} \mathbf{A}_{11} & \mathbf{A}_{12} \\ \mathbf{A}_{21} & \mathbf{A}_{22} \end{array}\right]\end{equation}

+ ^7 B5 j, l3 e0 I- }

A11\begin{equation}A_{11}\end{equation} :失准角导数和失准角之间的关系；

. T; ?( H, ^* ]

A12\begin{equation}A_{12}\end{equation} :失准角导数和导航系速度误差的关系；

. f: z7 r- f& Z% W( Y

A21\begin{equation}A_{21}\end{equation} :导航系速度误差导数和失准角之间的关系；

/ P8 {- @/ t, t4 Z! O5 D7 E

A22\begin{equation}A_{22}\end{equation} :导航系速度误差导数和导航系速度误差之间的关系

当维度为 44 时，失准角为 00 ，A11\begin{equation}A_{11}\end{equation} 、 A12\begin{equation}A_{12}\end{equation} ， A21\begin{equation}A_{21}\end{equation} 都为零矩阵。上述矩阵都可以在一般介绍惯性导航的书中找到，这里只给出 A21\begin{equation}A_{21}\end{equation} 和 A22\begin{equation}A_{22}\end{equation} 。

A21=[0−funfnnfun0−fen]\begin{equation}A_{21}=\left[\begin{array}{ccc} 0 & -f_{u}^{n} & f_{n}^{n} \\ f_{u}^{n} & 0 & -f_{e}^{n} \end{array}\right]\end{equation}

这里的 fn\begin{equation}f^{n}\end{equation} 表示为导航系下的比力。

; c; }. p) {# ^, D$ w! N6 m8 M

δv˙n=Mvvvn\begin{equation}\delta \dot{v}^{n}=M_{v v} v^{n}\end{equation}\\

: e, `% K9 H( t( c+ g( }2 V9 |

Mvv=(vn×)Mav−[(2ωien+ωenn)×]\begin{equation}M_{v v}=\left(v^{n} \times\right) M_{a v}-\left[\left(2 \omega_{i e}^{n}+\omega_{e n}^{n}\right) \times\right]\end{equation}

上式即为导航系速度误差导数和导航系速度误差之间的关系。

其中 Mav\begin{equation}M_{a v}\end{equation} 公式如下：

Mav=[0−1/RMh01/RNh00tan⁡L/RNh00]\begin{equation}M_{a v}=\left[\begin{array}{ccc} 0 & -1 / R_{M h} & 0 \\ 1 / R_{N h} & 0 & 0 \\ \tan L / R_{N h} & 0 & 0 \end{array}\right]\end{equation}\\

$ A2 y% ]8 F/ G

ww 为系统噪声，前3个为陀螺仪角度随机游走噪声，后面是加速度计速率随机游走噪声，这里不考虑加速度计 ZZ 轴噪声。

M1=−Cbn\begin{equation}\mathbf{M}_{1}=-\mathbf{C}_{b}^{n}\end{equation}

( ~/ X6 R1 G5 W7 e! U: t5 M

M2=[C11C12C21C22]\begin{equation}M_{2}=\left[\begin{array}{ll} C_{11} & C_{12} \\ C_{21} & C_{22} \end{array}\right]\end{equation}\\

因为涉及到陀螺仪噪声和加速度计噪声从载体坐标系到导航系的转换，所以才有了 M1\begin{equation}\mathbf{M}_{1}\end{equation}

3 n) m2 H q6 J4 ^

和 M2\begin{equation}M_{2}\end{equation} 。

4 \8 O2 N3 x; S

3. SINS/DVL 组合导航系统量测方程

观测量是SINS速度和DVL测量得到的速度在导航系下的速度之差：

V~SINSn−V~DVLn=(Vn+δVSINSn)−(Vn+δVDVLn)=δVSINSn−δVDVLn\begin{equation}\begin{aligned} \tilde{\mathbf{V}}_{SIN S}^{n}-\tilde{\mathbf{V}}_{D V L}^{n} &=\left(\mathbf{V}^{n}+\delta \mathbf{V}_{SIN S}^{n}\right)-\left(\mathbf{V}^{n}+\delta \mathbf{V}_{D V L}^{n}\right) \\ &=\delta \mathbf{V}_{S I N S}^{n}-\delta \mathbf{V}_{D V L}^{n} \end{aligned}\end{equation}\\

将DVL测速误差模型代入其中可得：

V~SINSn−V~DVLn=δVSINSn−VDVLn×Ψ−(VDVLn×)Cbnη−KVDVLn\begin{equation}\tilde{\mathbf{V}}_{S I N S}^{n}-\tilde{\mathbf{V}}_{D V L}^{n}=\delta \mathbf{V}_{S I N S}^{n}-\mathbf{V}_{D V L}^{n} \times \Psi-\left(\mathbf{V}_{D V_{L}}^{n} \times\right) \mathbf{C}_{b}^{n} \mathbf{\eta}-K \mathbf{V}_{D V L}^{n}\end{equation}

δVSINSn\begin{equation}\delta \mathbf{V}_{S I N S}^{n}\end{equation} 表示和惯导速度误差之间的关系；−VDVLn×Ψ\begin{equation}-\mathbf{V}_{D V L}^{n} \times \Psi\end{equation} 表示和惯导失准角之间的关系；−(VDVLn×)Cbnη\begin{equation}-\left(\mathbf{V}_{D V_{L}}^{n} \times\right) \mathbf{C}_{b}^{n} \eta\end{equation} 表示和安装误差角之间的关系；−KVDVLn\begin{equation}-K \mathbf{V}_{D V L}^{n}\end{equation} 表示和DVL刻度因子之间的关系；

所以系统的测量方程为

Z=[100001](V~SINSn−V~DVLn)=HX(t)+v(t)\begin{equation}\begin{aligned} \mathbf{Z} &=\left[\begin{array}{lll} 1 & 0 & 0 \\ 0 & 0 & 1 \end{array}\right]\left(\tilde{\mathbf{V}}_{S I N S}^{n}-\tilde{\mathbf{V}}_{D V L}^{n}\right) \\ &=\mathbf{H} \mathbf{X}(t)+\mathbf{v}(t) \end{aligned}\end{equation}

/ x) p+ [/ h# L1 U4 ]8 m

量测矩阵为：

H=[M1,I2×2,M2,M3]\begin{equation}H=\left[M_{1}, I_{2 \times 2}, M_{2}, M_{3}\right]\end{equation}

上式中， M1\begin{equation}M_{1}\end{equation} 表示为观测量和失准角之间的关系，因为不考虑天向速度，所以只取 −VDVLn\begin{equation}-\mathbf{V}_{D V L}^{n}\end{equation} 矩阵中的前两行，公式如下：

" y: b- o, N' D) n) V4 P3 S

M1=[0VDVLUn−VDVLNn−VDVLUn0VDVLEn]\begin{equation}M_{1}=\left[\begin{array}{ccc} 0 & V_{D V L U}^{n} & -V_{D V L N}^{n} \\ -V_{D V L U}^{n} & 0 & V_{D V L E}^{n} \end{array}\right]\end{equation}

I2×2\begin{equation}I_{2 \times 2}\end{equation} 表示和惯导速度误差之间的关系；

M2\begin{equation}M_{2}\end{equation} 表示和安装误差偏航角误差之间的关系， M2\begin{equation}M_{2}\end{equation} 的推导可以参考如下Matlab代码，不容易出错。

+ Y ^& X7 C7 u2 ~3 i

% sins/dvl组合导航观测量和安装误差角之间的关系 syms V_e V_n V_u syms c11 c12 c13 c21 c22 c23 c31 c32 c33 0 \, P7 f4 H0 P" S5 B6 z8 f+ h & I- ^$ c* T( d8 ?4 O2 E cross_mat = [ 0, -V_u, V_n; , v) ^6 Y, `& }/ ?% h1 k/ s V_u, 0, -V_e; -V_n, V_e, 0]; / Y% }2 {# ]5 m) E6 v cbn = [ c11, c12, c13; d$ t) {9 _ }# c/ _. n c21, c22, c23; $ \9 D0 D" _6 f- a7 F$ v9 R0 p" P c31, c32,c33]; ( ?9 `, g- H& n5 n" X mount_part = -cross_mat*cbn; : R$ P$ `/ H$ d6 j# p, ? % 结果如下： 1 _$ e7 G- @( j1 P8 G % [ V_u*c21 - V_n*c31, V_u*c22 - V_n*c32, V_u*c23 - V_n*c33] % [ V_e*c31 - V_u*c11, V_e*c32 - V_u*c12, V_e*c33 - V_u*c13] ' b. X" @& u2 U5 X \. N % [ V_n*c11 - V_e*c21, V_n*c12 - V_e*c22, V_n*c13 - V_e*c23] ; e8 o' h5 }3 r3 m' I yaw_mount = mount_part*[0;0;1]; % [ pitch ]================[ 0 ] % [ roll ]================[ 0 ] 0 s" k( `$ u. e) r2 z, s % [ yaw ]================[ yaw ] % 如果不考虑安装滚转角和俯仰角 % x! M& `# I2 j) n: k% F % V_u*c23 - V_n*c33 ========C31========东向 ) U( o. \$ g5 O5 X! T- b % V_e*c33 - V_u*c13 ========C32========北向 % V_n*c13 - V_e*c23 ========C33========天向

- l& R3 r& Z- L

因为没有考虑安装俯仰角和滚转角，并且在状态变量中没有天向速度，所以 M2\begin{equation}M_{2}\end{equation} 如下：

M2=[C23VDVLUn−C33VDVLNnC33VDVLEn−C13VDVLUn]M_{2}=\left[\begin{array}{l} C_{23} V_{D V L U}^{n}-C_{33} V_{D V L N}^{n} \\ C_{33} V_{D V L E}^{n}-C_{13} V_{D V L U}^{n} \end{array}\right]

4 G7 U+ k. R6 h$ I4 u0 a9 H3 p

M3\begin{equation}M_{3}\end{equation} 表示和DVL刻度因子误差之间的关系， M3\begin{equation}M_{3}\end{equation} 公式如下：

0 h0 h+ K/ n1 d

M3=[−VDVEn−VDVLNn]\begin{equation}M_{3}=\left[\begin{array}{c} -V_{D V E}^{n} \\ -V_{D V L N}^{n} \end{array}\right]\end{equation}

1 o" t5 w/ q+ S+ e8 D+ O

4. 考虑洋流的影响

当设备距离海底的距离大于DVL的探底深度时，DVL只能得到相对洋流的速度，只有同时知道了洋流速度信息时，才能得到绝对的速度，一种实时估计洋流速度的思路如下：

! ~; e) }+ B/ C" z/ w4 T( b1 d% `

首先在上述的运动预测方程中，增加两个状态变量，另洋流速度在导航坐标系东向，北向的分量分别如下，忽略天向速度：

VCurrentn=[VceVcn]\begin{equation}V_{\text {Current}}^{n}=\left[V_{c e} \quad V_{c n}\right]\end{equation}

! v6 Y" h% |$ K W* h

假设在一段有限时间内，流速的大小和方向是近似不变的，则在运动预测方程中，有：

δV˙ce=0\begin{equation}\delta \dot{V}_{c e}=0\end{equation}

h- ]2 s- y8 {% {" M) y" p

δV˙cn=0\begin{equation}\delta \dot{V}_{c n}=0\end{equation}

而观测量公式如下，需要注意的是 δVDVLn+δVCURn\begin{equation}\delta V_{D V L}^{n}+\delta V_{C U R}^{n}\end{equation} 才是从DVL中观测的水下速度， δVDVLn\begin{equation}\delta V_{D V L}^{n}\end{equation} 和前叙一样，仍然是对底速度。

7 J; d! }" c: W' V0 K

Z=δVSINSn−δVDVLn−δVCURn\begin{equation}Z=\delta V_{S I N S}^{n}-\delta V_{D V L}^{n}-\delta V_{C U R}^{n}\end{equation}

S0 _, A( x% g" n6 v2 _

参考论文

SINS/DVL 组合导航技术研究 国防科技大学 吕召鹏 - f& K) t: K* o' M7 J! R3 K* M6 [ ' w* o$ }4 ~4 |& Q! v, }% ^( v 8 v' A9 n& F3 J3 R/ j& v