舒旭光
摘 要： 针对水下航行体INS/DVL组合导航系统，提出了基于DVL测速误差预先补偿的卡尔曼滤波方案，介绍了测速误差预先补偿方法和补偿措施。通过试验验证，基于DVL测量速度误差预先补偿的组合导航技术改善了系统滤波收敛特性，提高了导航定位精度。
关键词： 水下航行体； 组合导航； 预先补偿； 卡尔曼滤波
中图分类号： TN966?34 文献标识码： A 文章编号： 1004?373X（2014）13?0139?04
Integrated navigation technology for underwater vehicles based on
pre?compensation of velocity measurement deviation
SHU Xu?guang
（710th Research Institute， Yichang 443000， China）
Abstract： For INS/DVL integrated navigation system in a long?distance underwater vehicle， the scheme of Kalman filtering based on the pre?compensation of velocity measurement deviation generated by DVL is proposed. The methods and measures of pre?compensation of the velocity measurement deviation are introduced. The test validation result shows that the integrated navigation technology based on pre?compensation of the velocity measurement deviation caused by DVL can improve the filtering convergence features and the navigation accuracy of the system.
Keyword： underwater vehicle； integrated navigation； pre?compensation； Kalman filtering
0 引 言
适应于水下的导航技术包括惯性导航技术、多普勒速度仪测量速度的推算导航技术、海底地形匹配导航技术、地磁匹配导航技术、水下图像匹配定位技术等。惯性导航系统（INS）具有自主、不受外界干扰、隐蔽性好、信息全面等优点，已成为主要导航系统[1?2]。但是INS导航误差随着时间累积[3?4]，在水下远程航行体长时间巡航过程中难以保证高精度导航。多普勒速度仪（DVL）通过测量多普勒频移获得航行体的运动速度，具有测速精度高，测速误差不随时间积累的优点[5]，但DVL需要外部的航向和垂直基准信息完成推算导航。水下远程航行体采用INS/DVL组合导航是一种理想的选择[6]，INS/DVL组合导航发挥了INS和DVL各自的优势，克服INS和DVL单个导航系统的不足，实现水下长时间高精度导航定位。
1 组合导航系统的特点
水下远程航行体组合导航系统由激光陀螺INS、相控阵DVL组成。INS通过激光陀螺和石英挠性加速度计测量航行体的角速度和加速度，实现初始对准、惯性导航算法，并接收DVL测量的载体速度，实现组合导航计算，输出离底高度以及经修正的位置、速度、角速度、航向和姿态信息。水下远程航行体惯性制导控制系统根据预先装订的航路点等控制参数，结合INS/DVL组合导航系统和深度传感器提供的信息，完成航路点制导控制和适应海底地形的寻深控制，使航行体准确自航至目标航路点。
激光陀螺的工作原理是利用光程差来测量旋转角速度的Sagnac效应。不同于传统的机电陀螺，激光陀螺没有高速转动的转子，具有动态范围宽、精度高、瞬时启动、环境适应性强、可靠性高、寿命长等优点，是理想的惯性敏感器件。相控阵DVL的最大优点是只需用一个平面基阵进行相控发射和接收，从工作机理上自动消除了水中声速变化这一影响速度测量的因素，不再需要进行声速补偿[7]。在波束宽度和工作频率相同的情况下，平面相控阵的体积是多个独立换能器构成的基阵四分之一；而在波束宽度和基阵尺寸相同的情况下，平面相控阵可用更低的工作频率，因而可达到大得多的工作距离。因此，相控阵多普勒测速技术非常适合水下航行体使用。
2 DVL测速误差预先补偿
根据声学多普勒速度仪的工作原理，在载体存在纵摇和横摇的情况下，多普勒测量速度将产生一定的误差[8]，其相对误差形式如下式：
[δl=（1-cosΔθ）×100%δh=（1-cosΔφ）×100%] （1）
式中：[δl，][δh]分别为纵向、侧向相对测速误差；[Δθ，][Δφ]分别为水下远程航行体纵摇、横摇。
用[ψ，][θ，][φ]分别表示水下远程航行体姿态角中的偏航角、俯仰角、横滚角，按照姿态角的定义[4]，水下远程航行体的纵摇、横摇可由[θ，][φ]计算得到：
[Δθ=θΔφ=π2-arcos（cosθsinφ）] （2）
水下远程航行体纵轴、横轴在水平面上方时，DVL纵向、侧向测量速度相对真实速度增大，反之则减小。根据公式（1），考虑水下远程航行体纵摇[Δθ、]横摇[Δφ]的符号，得到DVL纵向、侧向测量速度的放大系数[kx，][kz：]
[kx=1+sign（Δθ）?（cosΔθ-1）kz=1-sign（Δφ）?（cosΔφ-1）] （3）
利用该放大系数对DVL纵向测量速度[Vdx、]侧向测量速度[Vdz]进行误差补偿，计算消除了因姿态变化引起的误差后的多普勒测量速度[Vdx，][Vdz]：
[Vdx=VdxkxVdz=Vdzkz] （4）
基于DVL测速误差预先补偿方案如下：
相控阵DVL通过波束形成技术形成3°的窄波束进行声信号的收发，因此它对海底工作地形和载体姿态有一定的适应性。在海底地形或者航行体姿态变化较大的情况下，DVL底跟踪速度误差相应增大，采用上述补偿方法对DVL测量速度进行预先补偿难以较好的消除其测速误差。这种情况下，一方面通过加速度门限判决剔除异常速度；另一方面，通过对回波强度和噪声水平的综合判断，将异常速度赋值为-32.767 m/s（无效速度），并设置无效标志输送给INS。
在海底地形或者航行体姿态变化较大的情况下，DVL短时间内（通常为几秒至十几秒）输出无效的速度信息，这时系统选择纯惯性导航。在这一时间内纯惯性导航精度可以保证，这样可以有效避免最终的导航结果受到污染。
在DVL输出的速度信息有效时，系统选择INS/DVL组合，对DVL测量速度进行预先补偿，并且根据航行体姿态变化情况自适应调节滤波噪声系数，保证系统处于最佳方式，提高导航定位精度。
3 INS/DVL组合导航
将INS和DVL进行组合，通过卡尔曼滤波器进行信息融合，并采用反馈校正，利用INS参数误差的估计值反馈到INS内部对INS的速度、位置、姿态、惯性器件误差等参数进行校正，利用DVL参数误差的估计值对DVL的测量速度等进行校正，构成具有高精度、高可靠性、高自主性的功能完善的水下组合导航系统。组合导航系统原理图如图1所示。
图1 INS/DVL组合导航原理图
导航坐标系采用北天东地理坐标系。
取状态向量为：
[X=（?N，?U，?E，δVN，δVE，δλ，δL，εx，εy，εz，δVDx，δVDz，δKDx，δKDz）T。]
其中，[?N，?U，?E]为航向和姿态失准角；[δVN，δVE]为北向、东向速度误差；[δλ，δL]为经度、纬度误差；[，εx，εy，εz]为侧向、纵向和垂向陀螺漂移；[δVDx，δVDz]为DVL前向和侧向测速误差；[δKDx，δKDz]为刻度系数误差。
系统状态方程如下：
[?N=δVER-ωiesinLδL-（ωiesinL+VERtanL）?E-VNR?U+C11εx+C12εy+C13εz?U=tanLRδVE+（ωiecosL+VERsec2L）δL+（ωiecosL+VER）?E+VNR?N+C21εx+C22εy+C23εz?E=-δVNR+（ωiesinL+VERtanL）?N-（ωiecosL+VER）?U+C31εx+C32εy+C33εzδVN=-2（ωiesinL+VERtanL）δVE-（2ωieVEcosL+V2ERsec2L）δL-fUφE+fE?U+?NδVE=VERtanLδVE+（2ωiesinL+VERtanL）δVN+（2ωieVNcosL+VEVNRsec2L）δL+?Eδλ=secLRδVE+VERtanLsecLδL]
[δL=δVNR]
[εx=-1τgεx+wgx]
[εy=-1τgεy+wgy]
[εz=-1τgεz+wgz]
式中：[fN，fU]和[fE]分别为三个加速度计测得的比力在导航坐标系上的分量； [VN，VE，L]和[λ]为惯导系统导航参数输出；[?N]和[?E]为加速度计随机噪声，服从零均值正态分布。
根据DVL工作原理，它测量载体相对海底的速度，测量误差主要有速度偏移误差[δVDi][（i=x，y，z），]刻度系数误差[δKDi][（i=x，y，z），][δVd]用一阶马尔可夫过程表示， [δKDi]为随机常数。
相应误差状态方程为：
[δVDx=-1τdδVDx+wDxδVDz=-1τdδVDz+wDzδKDx=0δKDz=0]
将上述误差状态方程表示如下：
[X=AX+BW] （5）
其中：
[W=000?N?E00wgxwgy wgzwdxwdy00T]
[A=AI7×7?04×3C3×3?07×4…?…?…03×7?Ag3×3?03×4…?…?…04×7?04×3?AD4×4]
[B=I14×14]
5 结 论
针对某水下远程航行体的特点，采用以激光陀螺INS为核心，辅助以相控阵DVL组成的组合导航系统担负水下自主导航定位的任务。本文重点介绍了DVL测量速度预先补偿方法、补偿方案以及INS/DVL组合导航数据融合滤波技术。通过试验验证，基于DVL测量速度预先补偿的组合导航技术改善了系统滤波收敛特性，提高了导航定位精度。
参考文献
[1] CHENG Jian?hua， ZOU Ji?bin， WU Lei， et al. The design of an effective marine inertial navigation system scheme [C]// Proceedings of the International Workshop on Knowledge Disco?very and data Mining. Adelaide，Australia： IEEE Computer Society， 2008： 671?676.
[2] CUREY R K， ASH M E， THIELMAN L O. Proposed IEEE inertial system terminology standard and other inertial sensor standards [C]// Proceedings of IEEE 2004 Position Location Navigation Symposium. Monterey， USA： IEEE， 2004：83?90.
[3] 陈永冰，钟斌.惯性导航原理[M].北京：国防工业出版社，2007.
[4] 张涛.基于惯导及水下声学辅助系统的AUV容错导航技术[J].中国惯性技术学报，2013，21（4）：512?516.
[5] 秦瑞，王顺伟，袁晓峰，等.多普勒测速仪/捷联惯导组合导航技术研究[J].战术导弹技术，2006（6）：68?72.
[6] 邹洪，向大威，景永刚.多普勒计程仪的数据平滑方法[J].声学技术，2008，23（1）：507?510.
[7] 田坦，张殿伦，卢逢春，等.相控阵多普勒测速技术研究[J].哈尔滨工程大学学报，2002，27（4）：80?85.
[8] 阳跃图.船用多普勒计程仪误差分析[J].天津航海，2011（2）：9?10.
利用该放大系数对DVL纵向测量速度[Vdx、]侧向测量速度[Vdz]进行误差补偿，计算消除了因姿态变化引起的误差后的多普勒测量速度[Vdx，][Vdz]：
[Vdx=VdxkxVdz=Vdzkz] （4）
基于DVL测速误差预先补偿方案如下：
相控阵DVL通过波束形成技术形成3°的窄波束进行声信号的收发，因此它对海底工作地形和载体姿态有一定的适应性。在海底地形或者航行体姿态变化较大的情况下，DVL底跟踪速度误差相应增大，采用上述补偿方法对DVL测量速度进行预先补偿难以较好的消除其测速误差。这种情况下，一方面通过加速度门限判决剔除异常速度；另一方面，通过对回波强度和噪声水平的综合判断，将异常速度赋值为-32.767 m/s（无效速度），并设置无效标志输送给INS。
在海底地形或者航行体姿态变化较大的情况下，DVL短时间内（通常为几秒至十几秒）输出无效的速度信息，这时系统选择纯惯性导航。在这一时间内纯惯性导航精度可以保证，这样可以有效避免最终的导航结果受到污染。
在DVL输出的速度信息有效时，系统选择INS/DVL组合，对DVL测量速度进行预先补偿，并且根据航行体姿态变化情况自适应调节滤波噪声系数，保证系统处于最佳方式，提高导航定位精度。
3 INS/DVL组合导航
将INS和DVL进行组合，通过卡尔曼滤波器进行信息融合，并采用反馈校正，利用INS参数误差的估计值反馈到INS内部对INS的速度、位置、姿态、惯性器件误差等参数进行校正，利用DVL参数误差的估计值对DVL的测量速度等进行校正，构成具有高精度、高可靠性、高自主性的功能完善的水下组合导航系统。组合导航系统原理图如图1所示。
图1 INS/DVL组合导航原理图
导航坐标系采用北天东地理坐标系。
取状态向量为：
[X=（?N，?U，?E，δVN，δVE，δλ，δL，εx，εy，εz，δVDx，δVDz，δKDx，δKDz）T。]
其中，[?N，?U，?E]为航向和姿态失准角；[δVN，δVE]为北向、东向速度误差；[δλ，δL]为经度、纬度误差；[，εx，εy，εz]为侧向、纵向和垂向陀螺漂移；[δVDx，δVDz]为DVL前向和侧向测速误差；[δKDx，δKDz]为刻度系数误差。
系统状态方程如下：
[?N=δVER-ωiesinLδL-（ωiesinL+VERtanL）?E-VNR?U+C11εx+C12εy+C13εz?U=tanLRδVE+（ωiecosL+VERsec2L）δL+（ωiecosL+VER）?E+VNR?N+C21εx+C22εy+C23εz?E=-δVNR+（ωiesinL+VERtanL）?N-（ωiecosL+VER）?U+C31εx+C32εy+C33εzδVN=-2（ωiesinL+VERtanL）δVE-（2ωieVEcosL+V2ERsec2L）δL-fUφE+fE?U+?NδVE=VERtanLδVE+（2ωiesinL+VERtanL）δVN+（2ωieVNcosL+VEVNRsec2L）δL+?Eδλ=secLRδVE+VERtanLsecLδL]
[δL=δVNR]
[εx=-1τgεx+wgx]
[εy=-1τgεy+wgy]
[εz=-1τgεz+wgz]
式中：[fN，fU]和[fE]分别为三个加速度计测得的比力在导航坐标系上的分量； [VN，VE，L]和[λ]为惯导系统导航参数输出；[?N]和[?E]为加速度计随机噪声，服从零均值正态分布。
根据DVL工作原理，它测量载体相对海底的速度，测量误差主要有速度偏移误差[δVDi][（i=x，y，z），]刻度系数误差[δKDi][（i=x，y，z），][δVd]用一阶马尔可夫过程表示， [δKDi]为随机常数。
相应误差状态方程为：
[δVDx=-1τdδVDx+wDxδVDz=-1τdδVDz+wDzδKDx=0δKDz=0]
将上述误差状态方程表示如下：
[X=AX+BW] （5）
其中：
[W=000?N?E00wgxwgy wgzwdxwdy00T]
[A=AI7×7?04×3C3×3?07×4…?…?…03×7?Ag3×3?03×4…?…?…04×7?04×3?AD4×4]
[B=I14×14]
5 结 论
针对某水下远程航行体的特点，采用以激光陀螺INS为核心，辅助以相控阵DVL组成的组合导航系统担负水下自主导航定位的任务。本文重点介绍了DVL测量速度预先补偿方法、补偿方案以及INS/DVL组合导航数据融合滤波技术。通过试验验证，基于DVL测量速度预先补偿的组合导航技术改善了系统滤波收敛特性，提高了导航定位精度。
参考文献
[1] CHENG Jian?hua， ZOU Ji?bin， WU Lei， et al. The design of an effective marine inertial navigation system scheme [C]// Proceedings of the International Workshop on Knowledge Disco?very and data Mining. Adelaide，Australia： IEEE Computer Society， 2008： 671?676.
[2] CUREY R K， ASH M E， THIELMAN L O. Proposed IEEE inertial system terminology standard and other inertial sensor standards [C]// Proceedings of IEEE 2004 Position Location Navigation Symposium. Monterey， USA： IEEE， 2004：83?90.
[3] 陈永冰，钟斌.惯性导航原理[M].北京：国防工业出版社，2007.
[4] 张涛.基于惯导及水下声学辅助系统的AUV容错导航技术[J].中国惯性技术学报，2013，21（4）：512?516.
[5] 秦瑞，王顺伟，袁晓峰，等.多普勒测速仪/捷联惯导组合导航技术研究[J].战术导弹技术，2006（6）：68?72.
[6] 邹洪，向大威，景永刚.多普勒计程仪的数据平滑方法[J].声学技术，2008，23（1）：507?510.
[7] 田坦，张殿伦，卢逢春，等.相控阵多普勒测速技术研究[J].哈尔滨工程大学学报，2002，27（4）：80?85.
[8] 阳跃图.船用多普勒计程仪误差分析[J].天津航海，2011（2）：9?10.
利用该放大系数对DVL纵向测量速度[Vdx、]侧向测量速度[Vdz]进行误差补偿，计算消除了因姿态变化引起的误差后的多普勒测量速度[Vdx，][Vdz]：
[Vdx=VdxkxVdz=Vdzkz] （4）
基于DVL测速误差预先补偿方案如下：
相控阵DVL通过波束形成技术形成3°的窄波束进行声信号的收发，因此它对海底工作地形和载体姿态有一定的适应性。在海底地形或者航行体姿态变化较大的情况下，DVL底跟踪速度误差相应增大，采用上述补偿方法对DVL测量速度进行预先补偿难以较好的消除其测速误差。这种情况下，一方面通过加速度门限判决剔除异常速度；另一方面，通过对回波强度和噪声水平的综合判断，将异常速度赋值为-32.767 m/s（无效速度），并设置无效标志输送给INS。
在海底地形或者航行体姿态变化较大的情况下，DVL短时间内（通常为几秒至十几秒）输出无效的速度信息，这时系统选择纯惯性导航。在这一时间内纯惯性导航精度可以保证，这样可以有效避免最终的导航结果受到污染。
在DVL输出的速度信息有效时，系统选择INS/DVL组合，对DVL测量速度进行预先补偿，并且根据航行体姿态变化情况自适应调节滤波噪声系数，保证系统处于最佳方式，提高导航定位精度。
3 INS/DVL组合导航
将INS和DVL进行组合，通过卡尔曼滤波器进行信息融合，并采用反馈校正，利用INS参数误差的估计值反馈到INS内部对INS的速度、位置、姿态、惯性器件误差等参数进行校正，利用DVL参数误差的估计值对DVL的测量速度等进行校正，构成具有高精度、高可靠性、高自主性的功能完善的水下组合导航系统。组合导航系统原理图如图1所示。
图1 INS/DVL组合导航原理图
导航坐标系采用北天东地理坐标系。
取状态向量为：
[X=（?N，?U，?E，δVN，δVE，δλ，δL，εx，εy，εz，δVDx，δVDz，δKDx，δKDz）T。]
其中，[?N，?U，?E]为航向和姿态失准角；[δVN，δVE]为北向、东向速度误差；[δλ，δL]为经度、纬度误差；[，εx，εy，εz]为侧向、纵向和垂向陀螺漂移；[δVDx，δVDz]为DVL前向和侧向测速误差；[δKDx，δKDz]为刻度系数误差。
系统状态方程如下：
[?N=δVER-ωiesinLδL-（ωiesinL+VERtanL）?E-VNR?U+C11εx+C12εy+C13εz?U=tanLRδVE+（ωiecosL+VERsec2L）δL+（ωiecosL+VER）?E+VNR?N+C21εx+C22εy+C23εz?E=-δVNR+（ωiesinL+VERtanL）?N-（ωiecosL+VER）?U+C31εx+C32εy+C33εzδVN=-2（ωiesinL+VERtanL）δVE-（2ωieVEcosL+V2ERsec2L）δL-fUφE+fE?U+?NδVE=VERtanLδVE+（2ωiesinL+VERtanL）δVN+（2ωieVNcosL+VEVNRsec2L）δL+?Eδλ=secLRδVE+VERtanLsecLδL]
[δL=δVNR]
[εx=-1τgεx+wgx]
[εy=-1τgεy+wgy]
[εz=-1τgεz+wgz]
式中：[fN，fU]和[fE]分别为三个加速度计测得的比力在导航坐标系上的分量； [VN，VE，L]和[λ]为惯导系统导航参数输出；[?N]和[?E]为加速度计随机噪声，服从零均值正态分布。
根据DVL工作原理，它测量载体相对海底的速度，测量误差主要有速度偏移误差[δVDi][（i=x，y，z），]刻度系数误差[δKDi][（i=x，y，z），][δVd]用一阶马尔可夫过程表示， [δKDi]为随机常数。
相应误差状态方程为：
[δVDx=-1τdδVDx+wDxδVDz=-1τdδVDz+wDzδKDx=0δKDz=0]
将上述误差状态方程表示如下：
[X=AX+BW] （5）
其中：
[W=000?N?E00wgxwgy wgzwdxwdy00T]
[A=AI7×7?04×3C3×3?07×4…?…?…03×7?Ag3×3?03×4…?…?…04×7?04×3?AD4×4]
[B=I14×14]
5 结 论
针对某水下远程航行体的特点，采用以激光陀螺INS为核心，辅助以相控阵DVL组成的组合导航系统担负水下自主导航定位的任务。本文重点介绍了DVL测量速度预先补偿方法、补偿方案以及INS/DVL组合导航数据融合滤波技术。通过试验验证，基于DVL测量速度预先补偿的组合导航技术改善了系统滤波收敛特性，提高了导航定位精度。
参考文献
[1] CHENG Jian?hua， ZOU Ji?bin， WU Lei， et al. The design of an effective marine inertial navigation system scheme [C]// Proceedings of the International Workshop on Knowledge Disco?very and data Mining. Adelaide，Australia： IEEE Computer Society， 2008： 671?676.
[2] CUREY R K， ASH M E， THIELMAN L O. Proposed IEEE inertial system terminology standard and other inertial sensor standards [C]// Proceedings of IEEE 2004 Position Location Navigation Symposium. Monterey， USA： IEEE， 2004：83?90.
[3] 陈永冰，钟斌.惯性导航原理[M].北京：国防工业出版社，2007.
[4] 张涛.基于惯导及水下声学辅助系统的AUV容错导航技术[J].中国惯性技术学报，2013，21（4）：512?516.
[5] 秦瑞，王顺伟，袁晓峰，等.多普勒测速仪/捷联惯导组合导航技术研究[J].战术导弹技术，2006（6）：68?72.
[6] 邹洪，向大威，景永刚.多普勒计程仪的数据平滑方法[J].声学技术，2008，23（1）：507?510.
[7] 田坦，张殿伦，卢逢春，等.相控阵多普勒测速技术研究[J].哈尔滨工程大学学报，2002，27（4）：80?85.
[8] 阳跃图.船用多普勒计程仪误差分析[J].天津航海，2011（2）：9?10.
