张延顺 ，郭雅静，黄小娟，李春雨，汪湛清

(1.北京航空航天大学 仪器科学与光电工程学院，北京100191; 2.北京理工大学 自动化学院，北京100081)

航位推算(Dead Reckoning，DR)/水声定位系统组合定位既能抑制DR 系统随着时间增加由误差累积引起的误差发散，又能平滑水声定位系统输出位置的波动［1-2］.在水声定位系统短时间失效的情况下，可以依靠DR 进行导航定位［3-5］.此方法在水下导航系统中得到广泛应用.DR 系统的初始位置精度影响DR 系统性能［6-7］，也影响DR/水声组合定位系统导航数据的性能.在对姿态和速度平稳性有较高要求的水下精确测绘等［8-9］领域，对DR 系统的初始位置精度要求更高.

水下导航信息贫乏［10-13］，通常只能用水声定位系统输出的位置信息作为DR 系统的初始位置.水声定位系统是根据声波在水底应答器和载体接收机间传播的时间来确定距离进而进行定位的，受声速误差、航行噪声和载体摇摆等影响，整体数据的优劣程度不一，表现为数据波动较大［14-15］.所以，需要从一段时间内水声定位系统输出的位置信息中计算提取出DR 系统需要的初始位置.

当水下运载体为静基座状态时，通常采用对一段时间内水声定位系统输出信号求平均值的方法来得到DR 系统的初始位置，此方法在水下载体处于系泊状态时精度较高.在海洋探索和科学考察时，水下运载体通常为运动状态，在运动状态下，应用求平均值法则会引入较大的计算误差.因此，急需解决水下运载体行进过程中确定DR 系统初始位置的问题.基于此背景，本文提出了运动过程中水下运载体DR 系统初始位置确定的方法.该方法综合DR 和水声定位系统的特点，能得到优化的初始位置，可提高DR/水声组合定位系统姿态和速度的稳定性，有助于水下地形测绘、勘探等系统提高成图质量.

1 DR 系统初始位置确定方法

水声定位系统利用声波在水下传播良好的特性，通过检测其传输的时间差进行定位，但是其输出信号随机误差较大，特别是远距离传输时误差更大.目前常用的水声定位系统主要有长基线系统(LBL)、短基线系统(SBL)、超短基线系统(USBL)，本文应用海试实验中的USBL 数据进行研究.某次海试实验初始定位时间(75 s)内的超短基线定位系统USBL 的输出数据如图1 所示.

图1 初始定位时间内水声定位系统输出位置数据Fig.1 Output location data of hydroacoustic system in initial positioning time

由图1 可见，USBL 水声系统输出数据随机性较大，该段时间段内平均位置为(122.400°，21.640 77°)，最大定位误差为88.50 m，最小定位误差为11.98 m.因此以任一点数据作为DR 系统初始值都可能会有较大的误差，但通常只能从一段时间内的水声定位系统数据中提取DR 系统初始值，这就需要针对运载体运动特点采用相应方法确定初始位置.水下运载体高度信息由深度计提供，所以实际应用时本文只考虑二维平面内的初始位置信息，即经度、纬度信息.

1.1 准静态情况的平均值法定位

准静态情况下水下载体姿态有轻微变化，但无线运动.此情况下，对短时间内水声定位系统提供的经度、纬度进行求平均值运算，在一定程度上能减小定位系统的误差.通常采用的方法是在短时间内采集1 组水声定位系统输出值，然后对此数据进行求平均运算，以平均值作为位置的初始值，初始纬度、经度分别为

式中:Lk和λk分别为水声定位系统k 时刻输出的纬度和经度数值.

在静态情况下平均值法能有效抑制水声定位系统的随机误差，但是在运动情况下，则会产生与运动状态相关的位置误差，且位置误差随着运动速度增大而增大，需要研究动态情况下初始位置确定方法.

1.2 最小二乘拟合方法

动态情况是指水下载体具有线运动.动态情况下采用平均值法计算位置初始会产生明显的误差.水下载体以某一速度直线运动时，可采用最小二乘直线拟合方法对水声定位系统提供的经度和纬度信息分别进行拟合，并计算出位置初值.

最小二乘法(又称最小平方法)是一种数学优化技术，它通过最小化误差的平方和寻找数据的最佳函数匹配.利用最小二乘法可以简便地求得1 组未知位置数据，并使得这些求得的位置数据与实际水声定位系统输出的位置数据之间误差的平方和为最小.

水声定位系统输出1 组纬度、经度数据分别为

分别对纬度L、经度λ 进行最小二乘拟合，假设最小二乘拟合直线函数为

分别计算拟合系数a1、a2、b1和b2使得计算得出的纬度、经度与实测的纬度、经度离差的平方和最小:

将计算得到的a1、a2、b1和b2代入式(3)，即可得到拟合直线.如果后续工作为实时定位解算，应以拟合直线的末尾点为该时间点以后DR 的起始点.本文所研究的实验数据为离线后处理，为了对全部数据进行定位解算研究，以拟合直线的初始点为全部数据DR 系统的初始位置.

采用最小二乘法确定初始位置后，能在一定程度上提高载体运动状态情况下初始位置的计算精度.但最小二乘法拟合出的零阶和一阶系数是在对1 组数据整体误差最小情况下计算出来的.当水声定位数据输出误差正态性不好时，如前段数据误差正偏大，而后段数据负偏大情况下最小二乘法拟合出曲线的一阶系数会偏小，这会导致初始位置计算偏大，且该方法计算得出的运动轨迹具有随机性，不同区段内计算得出的运动轨迹偏差较大.因此，在实用中此方法还需要改进.

1.3 轨迹平移法

水声位置数据分布影响拟合的运动轨迹，其误差分布的非正态性将导致最小二乘拟合直线的斜率偏离真实值，从而引起初始位置计算误差.为解决此问题，本文依据DR 系统短期精度高、数据平滑的特点，提出利用轨迹平移法计算初始位置的方案.轨迹平移法原理如图2 所示.

图2 轨迹平移法原理Fig.2 Principle of trajectory translation method

首先以水声定位系统USBL 数据初始点坐标为初始位置，利用罗经提供的姿态信息和多普勒速度计程仪(Doppler Velocity Log，DVL)提供的速度信息，进行DR 得到水下运载体在初始定位阶段的运行轨迹.罗经输出的姿态信息在短时间内精度较高，所以用DR 得到的位置数据能真实反映水下载体的运动趋势和运动轨迹的形状，此运动趋势与真实运动轨迹相比误差较小.但由于水声定位系统提供的初始位置具有随机性，所以应用DR 系统计算的轨迹相对真实轨迹在位置上是有偏移的，但将DR 计算得到的轨迹进行平移则会得到与真实轨迹比较接近的运行轨迹.为得到轨迹的准确初始位置，本文在水声定位系统数据和DR 系统数据拟合直线距离平方和最小准则下，对DR 系统计算出的轨迹进行平移，直至水声定位系统位置数据到该直线距离平方和最小的判断准则实现后，提取得到的轨迹即为在统计意义下与真实轨迹最接近的轨迹.而该轨迹的初始点即为所求的初始位置.轨迹平移法确定初始位置算法流程图如图3 所示.

利用轨迹平移法确定初始位置方法充分利用了DR 导航系统短期精度高和水声定位系统误差不发散的特点，进一步减小了初始的位置误差.

图3 轨迹平移法算法流程图Fig.3 Flow diagram of trajectory translation method

2 实验研究

2.1 实验条件

本文在理论研究的基础上，利用一次深度为4 700 m 左右的深拖系统海试实验数据对第1 节初始位置确定方法进行了实验验证与分析.实验数据包括约20 min 的罗经、DVL 以及水声定位系统USBL 的原始实际实验数据，其中罗经和DVL数据用于进行DR，USBL 数据用于定位.数据中罗经为OCTANS，航向动态精度为0.1°/cos L，航向静态误差为0.05°/cos L;DVL 对底测速误差为0.5%V，V 为速度;USBL 误差为0.5%S，S 为位移.海试实验中，不同传感器输出数据频率不同，需要进行时间对准和匹配，所以定位结果分析中统一取计算周期为15 s.

2.2 初始位置确定方法实验

图4 USBL 原始位置数据Fig.4 Raw position data of USBL

应用本文方法在第2.1 节介绍的实验条件下开展初始位置确定方法的实验研究，由于水下定位没有绝对的参考基准，所以只能用水下航行器搭载导航系统来计算运行轨迹基准.USBL 原始位置数据如图4 所示，其中前75 s(T1时间段，T1为初始定位时间)数据用于求取初始位置.图4中水下航行器直线行驶的数据轨迹为直线，但存在波动，这是由于水声定位原理和行驶过程中非理想因素的影响造成的，但这并没有影响水下航行器直线行驶的趋势.采样点间隔为15 s，为减小非理想因素对运行轨迹的影响，本文采用卡尔曼滤波方法对轨迹数据进行事后处理，得到图5、图6中的水下航行器运行轨迹R，R 为直线.R 比较真实地反映了水下航行器的运行方向，依据T1时间段内的R 确定的初始点是最准确的.但是实际航行中无法事先获得此直线，本文利用后处理方法获得此直线，只用于对本文方法计算的T1时间段内运行轨迹进行评价.也就是说，将R 作为本文方法评价的基准，换句话说就是利用与R 最接近的直线确定的初始位置是最准确的.

本文分别采用平均值法、最小二乘法和轨迹平移法，利用在图4 所示数据中T1时间段的数据来研究初始位置确定方法.应用不同方法在T1时间段内获得的运动轨迹和全部时间段内的后处理轨迹参考基准R 如图5 所示.

图5 初始位置确定与全部数据的数据参考基准Fig.5 Determination of initial position and data reference of all data

为清晰观察T1时间段内不同方法确定的运行轨迹，截取T1时间段内不同方法获得的运动轨迹如图6 所示.图6 中，直线R 为经后处理获得的直线参考基准，曲线1 为利用平均值法获得的运行轨迹，曲线2 为利用本文提出的轨迹平移法计算确定的运行轨迹，曲线3 为对USBL 数据进行最小二乘拟合获得的运行轨迹.从图6 中可以看出，以USBL 在T1时间段内的平均值为运行轨迹，受USBL 水声定位系统数据波动的影响，引入计算误差;对USBL 数据进行最小二乘拟合获得的运行轨迹，受USBL 水声定位系统数据误差分布非正态性影响，经度和纬度轨迹方向偏离直线R，且计算得出的运动轨迹具有随机性;轨迹平移法利用DR 系统输出的位置信息和USBL 水声定位系统的位置信息进行曲线拟合和平移，得到新拟合曲线的斜率和距离更加接近真实运动轨迹.因此本文利用轨迹平移法确定的拟合曲线的起始点作为全部数据DR 的起始位置，这样有助于提高DR 的精度和组合导航的性能.

图6 不同方法对应的初始运动轨迹Fig.6 Initial trajectory corresponding with different methods

不同方法确定的初始定位时间(T1时间段)内的运行轨迹与直线R 作对比的误差曲线如图7所示.图7 中曲线1 为平均值法确定的运行轨迹误差曲线;曲线2 为本文提出的轨迹平移法确定的运行轨迹误差曲线;曲线3 为USBL 最小二乘拟合方法确定的运行轨迹误差.

图7 不同方法确定运行轨迹误差Fig.7 Error of initial trajectory corresponding with different methods

采用平均值法、最小二乘法和本文方法确定的初始位置(经度、纬度)分别是:(122.399 81°，21.640 77°)、(122.399 52°，21.640 32°)、(122.399 68°，21.640 72°).采用以上3 种方法确定的初始位置与由通过直线R 确定的初始位置间的距离误差分别为:39.35、30.68、23.68 m.以其中采用本文提出的轨迹平移法得到的初始位置误差最小，分别是平均值法和最小二乘拟合方法的60.2%和77.2%.由以上结果可知，轨迹平移法确定初始位置误差最小，以此方法确定的DR系统精度有所提高，此方法可减小DR 误差提高组合导航系统性能.

3 结 论

本文综合研究水下运载体动基座直线航行条件下的DR/水声定位系统输出数据的特点，提出了一种DR 轨迹平移法确定最优初始位置的方法，经实验验证表明:

1)本文提出的运动过程中水下运载体DR系统初始位置确定方法，利用水声定位系统提供的位置信息和DR 得到的方向信息计算得到优化的初始位置，该方法确定的初始位置误差最小，分别是平均值法和最小二乘拟合方法的60.2%和77.2%.

2)利用本文方法进行初始位置的确定，提高了初始时段DR 定位方法的准确程度.

3)提高DR 的基础上，也提高了组合导航的性能，有助于水下地形测绘、勘探等系统提高成图质量.

为使本文方法适应海试实验在线定位，仍需优化算法，提高其实时性.

References)

［1］ 王淑炜，张延顺.基于罗经/DVL/水声定位系统的水下组合导航方法研究［J］.海洋技术学报，2014，33(1):19-23.

Wang S W，Zhang Y S.Research on the underwater integrated navigation method based on compass/DVL/hydroacoustic position system［J］.Journal of Ocean Technology，2014，33(1):19-23(in Chinese).

［2］ Rigby P，Pizarro O，Williams S B.Towards geo-referenced AUV navigation through fusion of USBL and DVL measurements［C］∥Proceedings of OCEANS 2006.Piscataway，NJ:IEEE Press，2006:1-6.

［3］ 冯子龙，刘健，刘开周.AUV 自主导航航位推算算法的研究［J］.机器人，2005，27(2):168-172.

Feng Z L，Liu J，Liu K Z.Dead reckoning method for autonomous navigation of autonomous underwater vehicles［J］.Robot，2005，27(2):168-172(in Chinese)

［4］ Jirawimut R，Ptasinski P，Garaj V，et al.A method for dead reckoning parameter correction in pedestrian navigation system［J］.IEEE Transactions on Instrumentation and Measurement，2003，52(1):209-215.

［5］ 万磊，李璐，刘建成，等.一种基于航位推算的水下机器人导航算法［J］.中国造船，2005，45(4):77-82.

Wan L，Li L，Liu J C，et al.Navigation algorithm based on dead reckoning of automatic underwater vehicle［J］.Shipbuilding of China，2005，45(4):77-82(in Chinese).

［6］ Yang X Y，He H.GPS/DR integrated navigation system based on adaptive robust Kalman filtering［C］∥Proceedings of International Conference on Microwave and Millimeter Wave Technology.Piscataway，NJ:IEEE Press，2008，4:1946-1949.

［7］ Chung H，Ojeda L，Borenstein J.Accurate mobile robot deadreckoning with a precision-calibrated fiber-optic gyroscope［J］.IEEE Transactions on Robotics and Automation，2001，17(1):80-84.

［8］ 朱维庆，朱敏，刘晓东，等.海底微地貌测量系统［J］.海洋测绘，2003，23(3):27-31.

Zhu W Q，Zhu M，Liu X D，et al.The survey system for detailed relief on sea floor［J］.Hydrographic Surveying and Charting 2003，23(3):27-31(in Chinese).

［9］ 朱维庆，刘晓东，张东升，等.高分辨率测深侧扫声纳［J］.海洋技术，2005，24(4):29-35.

Zhu W Q，Liu X D，Zhang D S，et al.High resolution bathymetric sidescan sonar［J］.Ocean Technology，2005，24(4):29-35(in Chinese).

［10］ 孙树民，李悦.浅谈水下定位技术的发展［J］.广东造船，2004(4):19-24.

Sun S M，Li Y.The development of underwater positioning technology［J］.Shipbuilding of Guangdong，2004(4):19-24(in Chinese).

［11］ Kim K，Choi H T，Lee C M.Underwater precise navigation using multiple sensor fusion［C］∥Proceedings of 2013 IEEE International Underwater Technology Symposium(UT).Piscataway，NJ:IEEE Press，2013:1-4.

［12］ 刘晓东，张方生，朱维庆，等.深水声学拖曳系统［J］.海洋测绘，2006，25(6):37-40.

Liu X D，Zhang F S，Zhu W S，et al.Towed deep water acoustics system［J］.Hydrographic Surveying and Charting，2006，25(6):37-40(in Chinese).

［13］ 蒋青吉，肖昌荣，肖波.HiPAP100 水下定位系统在海底摄像中的应用［J］.海洋地质前沿，2013，29(11):62-66.

Jiang Q J，Xiao C R，Xiao B.The application of HiPAP 100 underwater positioning system to the seabed video system survey［J］.Marine Geology Frontiers，2013，29(11):62-66(in Chinese).

［14］ 李守军，包更生，吴水根.水声定位技术的发展现状与展望［J］.海洋技术，2005，24(1):130-135.

Li S J，Bao G S，Wu S G.A practical overview and prospect of acoustic positioning technology［J］.Ocean Technology，2005，24(1):130-135(in Chinese).

［15］ 吴永亭，周兴华，杨龙.水下声学定位系统及其应用［J］.海洋测绘，2003，23(4):18-21.

Wu Y T，Zhou X H，Yang L.Underwater acoustic positioning system and its application［J］.Hydrographic Surveying and Charting，2003，23(4):18-21(in Chinese).