基于横坐标系的捷联惯性导航系统/多普勒速度仪极区组合导航算法

张福斌，马朋，王智辉

（西北工业大学航海学院，陕西西安710072）

基于横坐标系的捷联惯性导航系统/多普勒速度仪极区组合导航算法

张福斌，马朋，王智辉

（西北工业大学航海学院，陕西西安710072）

针对传统指北方位的惯性导航力学编排在高纬度地区因地理经线快速收敛，建立相对于经线的航向越来越困难，以及在地理极点存在奇异值等问题，结合自主水下航行器在导航过程中对自主性、导航精度等需要，提出了基于横坐标系的捷联惯性导航系统/多普勒速度仪极区组合导航方案。给出了横地球坐标系、横地理坐标系等定义以及与常规导航坐标系的转换关系；通过类比常用的指北方位惯性导航力学编排推导了基于横坐标系的惯性导航力学编排；设计了适用于极区的捷联惯性导航系统/多普勒速度仪组合导航卡尔曼滤波算法；对设计的组合导航系统进行了仿真分析。结果表明，该导航方案能有效抑制方位失准角的增长，导航定位精度可满足自主水下航行器极区导航的要求。

控制科学与技术；组合导航；极区；横坐标系；捷联惯性导航系统

0 引言

南极的淡水、蛋白质、矿产资源丰富，北极的能源、海洋生物资源丰富，同时南、北极还是地球上生物生存环境的重要调节器。自主水下航行器（AUV）作为海洋水文环境、海底地形地貌等探测的重要工具，在极地科学考察任务中扮演着重要角色，例如我国即将自主建造第一艘极地科学考察破冰船具有装备AUV的支撑平台。

在高纬度地区，由于水平磁力线变弱及南北半球均存在磁场异常区域，使得基于地磁导航的传感器定向误差大、自主性较差。由于地球自转角速度矢量和重力矢量的夹角随纬度的升高越来越小，使得电罗经的航向测量误差越来越大。故捷联惯性导航系统（SINS）成为AUV导航传感器首选。但当前SINS普遍使用的指北方位、游动方位和自由方位力学编排均是相对经线建立航向，随着纬度升高，经线逐渐收敛到极点，航向确定变得困难甚至失去了意义。

为了解决这一问题，文献［1-3］提出了基于格网导航力学编排的惯性导航算法原理、SINS空中对准、惯性/天文组合导航算法；文献［4-5］给出了横坐标系的概念及定义方法，文献［6-9］在此基础上提出了SINS基于横坐标系的极区导航方法。

目前，SINS/多普勒速度仪（DVL）组合导航方案在中、低纬度地区被广泛用于AUV水下高精度自主导航，本文在已有成果的基础上，提出了一种适用于AUV极区水下导航的SINS/DVL组合导航算法，并通过AUV的典型运动轨迹对该方案的导航性能进行了分析、验证。

1 基于横坐标系的惯性导航

横坐标系是横地球坐标系和横地理坐标系的统称，其沿用传统导航坐标系关于经度、纬度、航向等的定义模式［10］，以180°经线与赤道平面的交点为伪北极点，以0°经线与赤道平面的交点为伪南极点，以90°E/90°W经线圈作为伪赤道［11］，如图1所示。在横坐标系中，地理的南极点、北极点不再是经线的汇集点，从而避免了AUV在极区相对传统经线定向的困难。

图1 横地球坐标系Fig.1 Transverse earth coordinate frame

1.1 横地球坐标系

横地球坐标系用Oxeyeze表示，由地球坐标系Oxeyeze绕Oye轴顺时针旋转90°得到，其Oye轴与原Oye轴重合，Oxe轴与原Oze轴重合，Oze轴为原Oxe轴的负向、过伪北极点；Oxeze平面为伪本初子午面，Oxeye平面为伪赤道平面，两个平面的交线为过地球的南、北极点的直线。从地球坐标系Oxeyeze到横地球坐标系Oxeyeze的坐标转换矩阵为

1.2 横地理坐标系

用横经度λ、横纬度L表示点P在横地球坐标系中的位置，横经度λ是通过点P的伪子午面与伪本初子午面之间的夹角，横纬度L是点P与地心连线与伪赤道平面之间的夹角。以点P为原点的横地理坐标系Oxgygzg与传统地理坐标系Oxgygzg的定义方法相同，平面Oxgyg为过点P的水平面，Oyg轴指向伪北极，Ozg轴指向天向，Oxg轴、Oyg轴、Ozg轴构成右手坐标系。

参照地球坐标系Oxeyeze到地理坐标系Oxgygzg的坐标转换矩阵，直接写出横地球坐标系Oxeyeze到横地理坐标系Oxgygzg的坐标转换矩阵为

故由地球坐标系Oxeyeze到横地理坐标系的坐标转换矩阵可由下式求得

设伪北向与真北向之间的夹角为ϑ，即除极点外，指向北极点的轴Oyg绕ozg轴（或Ozg轴）顺时针旋转ϑ角（北偏东为正）后与轴Oyg重合，故从地理坐标系Oxgygzg到横地理坐标系Oxgygzg的坐标转换矩阵为

则由地球坐标系Oxeyeze到横地理坐标系的坐标转换矩阵为

1.3 基于横坐标系的力学编排

以横地理坐标系Oxgygzg作为导航坐标系Oxnynzn，其力学编排和误差方程可借鉴指北方位惯性导航系统力学编排和误差方程［10］形式得到，二者的主要区别体现在地球自转角速度矢量ωie在横地理坐标系Oxgygzg与在地理坐标系Oxgygzg各坐标轴投影分量的不同。

1.3.1 姿态微分方程

用四元数Q表示刚体旋转运动学方程

1.3.2 速度微分方程

比力方程在横坐标系下表示为

1.3.3 位置微分方程

横纬度、横经度、深度的微分方程分别为

1.3.4 姿态误差方程

假设SINS中陀螺的刻度系数误差和安装误差角已补偿，姿态误差方程的矢量形式为

1.3.5 速度误差方程

假设SINS中加速度计的刻度系数误差和安装误差角已经补偿，速度误差方程的矢量形式为

设加速度Δ计的误差δfb由随机常值和零均值高斯白噪声w两部分组成，即

1.3.6 位置误差方程

位置误差方程的分量形式为

2 极区组合导航

假设AUV导航系统由SINS、DVL、全球卫星定位系统（GPS）、深度传感器组成，组合导航系统的工作流程如下:

1）借助母船导航设备或岸基辅助导航设备完成SINS的初始对准；

2）组合导航系统切换到SINS/GPS以水平位置差值为量测量的组合导航状态，AUV吊放入水；

3）AUV处于水面漂浮状态，启动DVL测量AUV相对于海底或其下方一定深度水层的三维速度，组合导航系统切换到SINS/GPS/DVL以水平位置差值、三维速度差值为量测量的组合导航状态，由于深度测量精度较高，一般可达到深度传感器满量程的万分之一，为了简化，组合导航过程中AUV的航行深度采用深度传感器测量值，并忽略深度测量误差的影响；

4）AUV下潜进行水中航行，组合导航系统切换到SINS/DVL以三维速度差值为量测量的组合导航状态；

5）待AUV执行任务结束，上浮到水面，组合导航系统切换到SINS/GPS/DVL以水平位置差值、三维速度差值为量测量的组合导航状态，等待回收。

考虑到关于极区SINS初始对准已有研究［1-2］，AUV吊放入水和水面漂浮时间一般较短，引起的导航误差变化通常较小。而AUV水下航行过程中导航精度对其完成任务影响很大，且未见基于横坐标系的SINS/DVL组合导航算法，故这里仅研究基于横坐标系的SINS/DVL组合导航Kalman滤波算法。

2.1 系统状态方程

基于AUV的航行深度已利用深度传感器获得较为精确的测量，故不考虑SINS的深度误差，选取SINS的姿态误差、速度误差、水平位置误差Δ和，陀螺和加速度计的随机常值零位偏移εb和b作为状态变量，即

将（8）式、（9）式、（10）式去掉深度误差有关项，整理可得系统状态方程为

2.2 系统量测方程

DVL输出测量值为其仪表坐标系下的三维速度，假定SINS与DVL之间的安装偏差已充分补偿，即可认为SINS的载体坐标系与DVL的仪表坐标系重合。以美国RDI公司工作频率300 kHz的DVL为例，当AUV距海底高程小于200 m时，DVL既可测得AUV相对于海底的绝对速度，也可测得AUV相对于其下方某一水层的速度。当AUV距海底高程大于200 m时，DVL仅可测得AUV相对于其下方某一水层的速度。

假设AUV距海底高程小于200 m，DVL的测量误差为零均值的高斯白噪声wD，其测量输出为其载体系坐标系下的三维速度，即式中:vbD为载体系下AUV的实际速度。

利用SINS输出的姿态角将DVL输出的载体系下AUV的三维速度转换到横导航坐标系下，即

式中:I为单位阵。

将SINS输出的三维速度减去转换到横坐标系下的DVL三维速度作为量测量，建立量测方程

将量测方程写成矩阵形式

式中:

由于wD为零均值的高斯白噪声，故V仍可认为零均值的高斯白噪声，且强度不变。利用Kalman滤波算法，结合（12）式和（15）式对AUV航行姿态、速度及位置进行递推估计。

3 仿真分析

3.1 仿真轨迹及仿真参数设置

仿真轨迹:AUV航行起点为横地理坐标系的（5°N，0°E，0 m），对应的地理坐标系坐标为（85°N，30°E，0 m），初始航向30°、横滚和俯仰角均为0°、深度0 m，速度0 m/s；航行过程分为13个阶段:1）水面静止阶段，持续时间5 s；2）水面加速阶段，航行器纵轴方向加速度0.4 m/s2，持续时间10 s；3）水面匀速直航阶段，持续时间30 s；4）进入下潜阶段，俯仰角速度-0.6°/s，持续时间10 s；5）定角下潜阶段，持续时间50 s；6）改平阶段，俯仰角速度0.6°/s，持续时间10 s；7）定深直航阶段，深度-25 m，持续时间7 035 s；8）进入上爬阶段，俯仰角速度0.6°/s，持续时间10 s；9）定角上爬阶段，持续时间50 s；10）改平阶段，俯仰角速度-0.6°/s，持续时间10 s；11）水面匀速直航阶段，持续时间20 s；12）水面减速阶段，航行器纵轴方向加速度-0.4 m/s2，持续时间10 s；13）水面静止，持续时间5 s.整个过程共7 255 s.

SINS的误差:定义i=x，y，z表示各坐标轴，陀螺的随机常值误差εbi=0.02°/h，零均值高斯白噪声加速度计的随机常值误差，零均值高斯白噪声；DVL的测速误差:零均值高斯白噪声wDi=0.01 m/s；导航初始误差:姿态角误差（0.2°，0.2°，0.2°），位置误差（10 m，10 m，0.2 m），速度误差（0.1 m/s，0.1 m/s，0.1 m/s）；滤波周期为1 s.

图2 姿态误差Fig.2 Orientation errors

3.2 仿真结果

假设AUV航行过程中，DVL全程可获得AUV相对海底的绝对速度，图2～图4为采用Matlab仿真得到SINS/DVL组合导航的姿态误差、速度误差、位置误差。

图3 速度误差Fig.3 Velocity errors

图4 位置误差Fig.4 Position errors

从图2可以看出，SINS/DVL组合导航系统的俯仰和横滚角误差均小于1′，航向角误差的大小基本维持不变，这是因为，一方面AUV运动过程中水平加速度较小，另一方面在极区地球自转角速度的北向分量很小。从图3可以看出，水平速度误差小于0.02 m/s，且基本保持不变，其数值主要受航向误差角和AUV航速影响。从图4可以看出，水平位置误差随时间变化，变化的快慢主要由航向误差角和AUV航速的数值决定，当AUV初始航向误差角较小时，SINS/DVL组合导航系统的自主导航定位精度可以满足一般水下作业的任务需求。

4 结论

本文参考概念易于理解、物理意义明确的传统指北方位惯性导航力学编排方案，提出了一种基于横坐标系的SINS/DVL用于AUV执行极区水下探测任务时自主导航参数计算及滤波方法，并利用AUV作业时一条典型的运动轨迹对其导航性能进行了分析，结果表明:该算法能够较好地满足AUV水下自主导航定位的需求。

另外，本文假设SINS初始失准角较小，而在极区因重力矢量与地球自转角速度矢量夹角小，仅靠SINS自对准很难获得较高的对准精度。因此，研究极区横坐标系下SINS的对准方法是下一步工作的重点。

（References）

［1］ 周琪，秦永元，付强文，等.极区飞行格网惯性导航算法原理［J］.西北工业大学学报，2013，31（2）:210-217. ZHOU Qi，QIN Yong-yuan，FU Qiang-wen，et al.Grid mechanization in inertial navigation system for transpolar aircraft［J］.Journal of Northwestern Polytechnical University，2013，31（2）: 210-217.（in Chinese）

［2］ 吴枫，秦永元，周琪.机载武器极区传递对准算法［J］.中国惯性技术学报，2013，21（2）:141-146. WU Feng，QIN Yong-yuan，ZHOU Qi.Airborne weapon transfer alignment algorithm in polar regions［J］.Journal of Chinese Inertial Technology，2013，21（2）:141-146.（in Chinese）

［3］ 周琪，秦永元，严恭敏，等.大飞机极区惯性/天文组合导航算法［J］.系统工程与电子技术，2013，35（12）:2559-2565. ZHOU Qi，QIN Yong-yuan，YAN Gong-min，et al.Stellar-inertialnavigation for transpolar large aircraft［J］.Systems Engineering and Electronics，2013，35（12）:2559-2565.（in Chinese）

［4］ Fox W A W.Transverse navigation:an alternative to the grid system［J］.Journal of Navigation，1949，2（1）:25-35.

［5］ Dyer G C.Polar navigation—a new transverse mercator technique［J］.Journal of Navigation，1971，24（4）:484-495.

［6］ 徐晓苏，豆嫚.基于横向地理坐标系的极区惯性导航方法研究［J］.华中科技大学学报:自然科学版，2014，42（12）: 116-121. XU Xiao-su，DOU Man.Inertial navigation algorithm transverse geographic in polar regions based on coordinate system［J］.Journal of Huazhong University of Science and Technology:Natural Science Edition，2014，42（12）:116-121.（in Chinese）

［7］ 李倩，孙枫，奔粤阳，等.基于横坐标系的捷联惯导系统极区导航方法［J］.中国惯性技术学报，2014，22（3）:288-295. LI Qian，SUN Feng，BEN Yue-yang，et al.Polar navigation of strapdown inertial navigation system based on transversal frame in polar region［J］.Journal of Chinese Inertial Technology，2014，22（3）:288-295.（in Chinese）

［8］ 李倩，孙枫，奔粤阳，等.横坐标系捷联惯导系统极区导航及阻尼设计［J］.系统工程与电子技术，2014，36（12）:2496-2503. LI Qian，SUN Feng，BEN Yue-yang，et al.Transversal strapdown INS and damping design in polar region［J］.Systems Engineering and Electronics，2014，36（12）:2496-2503.（in Chinese）

［9］ 姚逸卿，徐晓苏，童金武.极区间接横向惯性导航方法［J］.中国惯性技术学报，2015，23（1）:29-34. YAO Yi-qing，XU Xiao-su，TONG Jin-wu.Indirect transverse inertial navigation algorithm in polar region［J］.Journal of Chinese Inertial Technology，2015，23（1）:29-34.（in Chinese）

［10］ 秦永元.惯性导航［M］.北京:科学出版社，2006. QIN Yong-yuan.Inertial navigation［M］.Beijing:Science Press，2006.（in Chinese）

［11］ 朱启举，秦永元，周琪.极区航空导航综述［J］.测控技术，2014，33（10）:5-8. ZHU Qi-ju，QIN Yong-yuan，ZHOU Qi.Summary of polar air navigation［J］.Measurement and Control Technology，2014，33（10）:5-8.（in Chinese）

SINS/DVL Integrated Navigation Algorithm Based on Transversal Coordinate Frame in Polar Region

ZHANG Fu-bin，MA Peng，WANG Zhi-hui
（School of Marine Science and Technology，Northwestern Polytechnical University，Xi'an 710072，Shaanxi，China）

The traditional north-pointing inertial navigation mechanics arrangement of inertial navigation system is not suitable for using in the polar region due to the rapid convergence of geographical longitude at high latitudes so the establishment of the heading relative to the longitude is more and more difficult，and the singular value in geographic poles exists.Focusing on the above problem and the autonomy and navigation accuracy requirements of autonomous underwater vehicle（AUV）during navigation，a SINS/ DVL integrated navigation scheme based on transversal frame in polar region is proposed.The definition of the transverse earth/geographic coordinate frame and its conversion with conventional navigation frame are presented.The mechanizations of inertial navigation based on transversal frame in polar region are derived by analogy with the traditional north-pointing navigation mechanizations.A SINS/DVL integrated navigation algorithm based on Kalman filter in polar region is designed.The simulation is carried out to analyze the proposed integrated navigation system.The results indicate that the navigation method can restrain effectively the increase in azimuth misalignment angle，and the navigation accuracy can satisfy the navigation requirement of AUV in polar region.

control science and technology；integrated navigation；polar region；transversal frame；strapdown inertial navigation system

U666.1

A

1000-1093（2016）07-1229-07

10.3969/j.issn.1000-1093.2016.07.010

2015-10-14

国家自然科学基金项目（61273333）

张福斌（1972—），男，副教授。E-mail:zhangfb@nwpu.edu.cn