基于条件数的多自主水下航行器协同定位系统可观测度分析

马朋，张福斌，徐德民，刘书强

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

基于条件数的多自主水下航行器协同定位系统可观测度分析

马朋，张福斌，徐德民，刘书强

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

针对基于移动长基线的多自主水下航行器（MAUV）协同定位系统，建立了MAUV的2D运动学模型与距离量测方程。利用基于Lie导数的非线性系统可观测性理论，得到协同定位系统可观测矩阵及MAUV编队的不可观测航行路径。利用奇异值分解（SVD）理论获得系统可观测矩阵的条件数，建立了协同定位系统可观测度的评价函数，并在不同MAUV航行队形对系统可观测度进行仿真分析。结果表明，系统可观测度随着MAUV航行队形的不同而变化，从而为通过变换队形来提高协同定位性能提供了有效的参考依据。

信息处理技术；多自主水下航行器；协同定位；条件数；可观测度

0　引言

可观测性的概念是Kalman在20世纪60年代初为解决线性系统的问题而引入，其反映了系统利用有限时间内的观测量确定系统状态的能力［1］。通常使用可观测矩阵的秩来判断系统的可观测性，若可观测矩阵满秩，则系统是完全可观测的，否则是不完全可观测的。

在多自主水下航行器（MAUV）协同定位过程中，定位系统的可观测性直接关系到系统状态量能否被估计得出，因此对协同定位系统进行可观测性分析具有重要的意义。文献［2-3］在基于单信标测距的自主水下航行器（AUV）协同定位系统中，利用线性时变系统可观测性理论分析了协同定位系统的可观测性，并给出几种AUV在2D和3D运动学模型下的不可观测航行路径。文献［4］利用基于Lie导数的非线性系统可观测性理论，分析了多机器人协同定位系统的可观测性，得出满足定位系统局部弱可观测的条件。文献［5-6］讨论了基于单领航者的MAUV协同定位系统的可观测性，并给出几种可有效提高系统可观测性的MAUV航行路径。文献［7-9］同样利用Lie导数分别讨论了运动控制输入量对多机器人和MAUV协同定位系统可观测性的影响。

无论对于线性系统还是非线性系统，在定性地得到系统是否可观测结论的前提下，往往还需要定量地分析系统及系统状态的可观测度。可观测度反映了观测模型对系统状态变化的敏感程度，与系统状态估计所能达到的精度密切相关［10］。主要的分析方法有，利用估计误差协方差矩阵的特征值大小衡量可观测性的强弱［11］和利用可观测矩阵的条件数来分析系统可观测度［12-13］。目前，这两种方法已广泛应用于惯性导航［14-15］及航空航天导航［16-17］系统性能分析中。

在基于移动长基线（MLBL）的MAUV协同定位过程中［18］，MAUV航行路径与编队队形的变化将造成系统量测向量结构的变化，从而会对协同定位系统的可观测性产生影响。为了定量地衡量分析这种影响，本文在利用Lie导数分析得到系统非线性可观测矩阵的基础上，依据可观测矩阵的条件数建立了定位系统可观测度的评价函数，并分析讨论了协同定位系统可观测度在不同MAUV队形下的变化，为MAUV协同定位过程中的航行队形选择提供参考依据。

1　MAUV协同定位系统与模型

MAUV协同定位是在MAUV协作基础上提出和发展的一种新的定位方式，系统中各AUV在未知环境中互为信标，利用水声通信技术，交换共享各自位置与状态信息，并与相对位置观测量进行融合，从而获得更高的整体定位精度。

在基于移动长基线的MAUV协同定位系统中，利用两个领航AUV组成移动长基线，其中领航AUV1和AUV2均装备高精度导航设备与深度传感器，跟随AUV装备低精度导航设备与深度传感器，领航AUV1和AUV2利用水声信号以单向广播方式定时发送航行状态信息（包括时间、经度、纬度、深度、航向、速度等），如图1所示。跟随AUV则利用接收到的水声信号解算与领航AUV间相对位置关系，依靠相应的定位算法，实现自身位置状态的实时更新。

图1　MAUV协同定位系统Fig.1 Cooperative localization system of MAUV

由于AUV深度信息可由深度传感器直接测得，并且测量精度较高，为简化运动学模型，可不考虑AUV深度，将3D运动模型通过AUV间的深度差在水平面内投影转化为2D运动学模型。

定义t时刻跟随AUV的状态向量为X（t）=［x（t），y（t），φ（t）］T，假定AUV做定深航行，此时跟随AUV的2D运动学模型为

式中：u1（t）为AUV航行速度；φ（t）为AUV航向角。

定义t时刻领航AUV的状态向量分别为X1（t）=［x1（t），y1（t），φ1（t）］T，X2（t）=［x2（t），y2（t），φ2（t）］T，则协同定位系统的距离量测方程为

2　MAUV协同定位系统可观测性

为了便于分析MAUV协同定位系统的可观测性，利用Lie导数求解非线性系统的可观测矩阵，选取量测向量函数

结合AUV非线性运动模型（1）式，则量测函数hk（X）沿向量f的各阶Lie导数为

式中：f=［cos（φ（t））sin（φ（t））0］T；Δ为梯度运算函数，定义为

根据微分几何理论［19］，非线性系统的可观测矩阵为

从而，利用（3）式～（6）式可得MAUV协同定位系统可观测矩阵为

假定领航、跟随AUV以定速航行，且跟随AUV的航向角速度为0.当满足以下3种情况时，系统可观测矩阵O1不满秩，即rank（O1）＜3，此时MAUV协同定位系统为不可观测的。

分析这3种情况，可以发现，当领航、跟随AUV以相同航向角沿同一直线航行时，协同定位系统的两量测向量共线，从而使得可观测矩阵（7）式行向量线性相关，系统可观测矩阵O1变为奇异矩阵。此时，MAUV协同定位系统是不可观测的。

3　MAUV协同定位系统可观测度分析

3.1条件数与可观测度

设有线性方程组Ax=b，其中A为系数矩阵，b为已知向量，x为待求解向量。假定系数矩阵A和向量b存在小扰动δA和δb，此时解向量x扰动为x+δx，则扰动后的方程组为

对（8）式两端取范数可证明得［20］：当δA=0，δb≠0时，有；当δA≠0，δb=0时，有

定义κ（A）=‖A-1‖‖A‖为矩阵A的条件数，可以看出条件数κ（A）反映了解向量x随系统扰动变化的敏感程度，是衡量线性方程数值稳定性的重要指标，矩阵的条件数越大，矩阵越接近病态。在矩阵理论中，利用奇异值分解（SVD）法求解条件数可得

式中：σmax（A）和σmin（A）分别为矩阵A的最大奇异值和最小奇异值。

考虑（1）式和（3）式组成的非线性协同定位系统，由于其观测空间是由（4）式生成的包含状态变量和观测变量在内的最小线性空间，且关于Lie导数是封闭的，可以利用（9）式定义非线性系统的可观测度为

因而，根据矩阵条件数的定义知0≤C（O）≤1，可以说可观测度C（O）越接近于0，矩阵O将越趋于病态，当C（O）=0时，矩阵O为奇异矩阵，系统不可观测。反之，C（O）越接近于1，系统扰动对解向量误差的影响越小，系统状态估计效果越好。

3.2协同定位系统可观测度

根据上文研究可知，量测向量结构的改变将引起协同定位系统可观测矩阵奇异性的变化，进而影响系统可观测度的变化，而造成系统量测向量结构改变的主要因素则包括AUV间相对位置关系与航向角。

为了便于分析说明这些影响，以领航AUV1为坐标原点，两领航AUV组成的基线为横坐标，基线距离固定为l，且将领航与跟随AUV位置用极坐标表示，如图2所示。

图2　协同定位系统坐标转换示意图Fig.2 Coordinate transformation of cooperative localization system

结合上文定义，将AUV位置从直角坐标转换到极坐标下的关系为

式中：l为领航AUV间的距离，为固定常值；ρ为跟随AUV到领航AUV1间的距离；θ为跟随AUV与领航AUV1连线与领航AUV基线间的夹角。

定义l与ρ有如下比例关系：

则系统可观测矩阵（7）式可表示为

从而，MAUV协同定位系统可观测度为

3.3仿真研究

利用Matlab对MAUV协同定位系统的可观测度进行仿真研究，并分两种情况进行讨论分析。

1）假设领航与跟随AUV航向角固定且相等，两领航AUV间的距离为l=500，夹角θ的变化范围为-180°～180°，比值λ从0开始逐渐增大。从而，协同定位系统可观测度C（O1）的变化如图3所示。

从图3中可以看出，无论AUV航向角为0°还是90°，协同定位系统可观测度分布趋势相同。当θ≈45°，时，定位系统可观测度C（O1）最大。此时根据三角形几何理论可证明，跟随AUV与两领航AUV连线间的夹角β≈45°.从而可知，在AUV航向角固定的情况下，当跟随AUV处于以两领航AUV连线为直径的圆上且到两领航AUV间距离相等时，协同定位系统可观测度最高。

图3　协同定位系统可观测度分布图Fig.3 Distribution diagram of observabilities of cooperative localization system

2）假设领航与跟随AUV相对位置关系固定，航向角时刻保持相同，且跟随AUV与两领航AUV间距离相等，两领航AUV间的距离为l=500，航向角φ的变化范围为-120°～120°。从而，协同定位系统可观测度C（O1）的变化如图4和表1所示。

图4　协同定位系统可观测度变化曲线Fig.4 Variational curves of observability of cooperative localization system

表1　协同定位系统可观测度对比Tab.1 Comparison of observabilities for cooperative localization system

表1所示为协同定位系统可观测度随夹角θ的变化，假定领航与跟随AUV航向角相同且固定，则在-45°～45°的变化范围内，夹角θ越大，协同定位系统可观测度越高。图4所示为协同定位系统可观测度随航向角φ的变化，从图中可以看出，在领航与跟随AUV间相对位置固定的情况下，协同定位系统可观测度随航向角变化的趋势相同，随着AUV航行方向与领航AUV基线间的夹角增大，协同定位系统可观测度增高，并在±90°处达到最大。而当AUV的航向角φ=0°，夹角θ=0°时，如图4（a）中圆圈所标示，协同定位系统可观测度为0，此时系统是不可观测的，此结论与上文利用可观测矩阵的秩所得分析结果相同。

4　结论

本文通过建立基于移动长基线的MAUV协同定位系统模型，利用Lie导数求解获得了定位系统非线性可观测矩阵，并证明得当领航、跟随AUV以相同航向角沿同一直线进行定速航行时，MAUV协同定位系统是不可观测的。在此基础上，利用SVD理论获得定位系统可观测矩阵的条件数，建立了系统可观测度的评价函数，通过系列仿真得到系统可观测度的分布图，并讨论了协同定位系统可观测度在不同MAUV队形下的变化，从而实现了对定位系统可观测性的定量分析。

（

）

［1］邢光谦.量测系统的能观度和状态估计精度［J］.自动化学报，1985，11（2）：152-158. XING Guang-qian.Degree of observability and accuracy of state estimate for general measurement system［J］.Acta Automatica Sinica，1985，11（2）：152-158.（in Chinese）

［2］Gadre A，Stilwell D.Underwater navigation in the presence of unknown currents based on range measurements from a single location［C］∥Proceedings of the 2005 American Control Conference.Portland.US：IEEE，2005：656-661.

［3］Lee P，Jun B，Lim Y.Review on underwater navigation system based on range measurements from one reference［C］∥OCEANS 2008-MTS/IEEE KobeTechno-Ocean.Kobe，Japan：MTS/ IEEE，2008：1-5.

［4］Zhou X，Roumeliotis S.Robot-to-robot relative pose estimation from range measurements［J］.IEEE Transactions on Robots，2008，24（6）：1379-1393.

［5］Antonelli G，Arrichiello F，Chiaverini S，et al.Observability analysis of relative localization for AUVs based on ranging and depth measurements［C］∥2010 IEEE International Conference on Robotics and Automation（ICRA）.San Francisco，US：IEEE，2010：4276-4281.

［6］Antonelli G，Caiti A，Calare V，et al.Designing behaviors to improve observability for relative localization of AUVs［C］∥IEEE International Conference on Robotics and Auto-mation（ICRA）. Anchorage，US：IEEE，2010：4270-4275.

［7］Martinelli A，Siegwart R.Observability analysis for mobile robot localization［C］∥2005 IEEE/RSJ International Conference on Intelligent Robots and Systems.Anchorage，US：IEEE，2005：1471-1476.

［8］房新鹏，严卫生，李俊兵.基于距离量测的主从式AUV协同定位系统可观测性研究［J］.西北工业大学学报，2012，30（4）：547-552. FANG Xin-peng，YAN Wei-sheng，LI Jun-bing.An effective observability analysis for the leader-follower autonomous underwater vehicles（AUVs）cooperative localization based on range measurements［J］.Journal of Northwestern Polytechnical University，2012，30（4）：547-552.（in Chinese）

［9］Sharma R，Beard R，Taylor C，et al.Graph-based observability analysis of bearing-only cooperative localization［J］.IEEE Transactions on Robots，2012，28（2）：522-529.

［10］常晓华，崔平远，王晓明，等.基于条件数的能观性度量方法及在自主导航系统中的应用［J］.宇航学报，2010，31（5）：1331-1337. CHANG Xiao-hua，CUI Ping-yuan，WANG Xiao-ming，et al.A condition number-based observability analysis method and its application in autonomous navigation system［J］.Journal of Astronautics，2010，31（5）：1331-1337.（in Chinese）

［11］Ham F，Brown R.Observability，eigenvalues，and Kalman filtering［J］.IEEE Transactions on Aerospace and Electronic Systems，1983，19（2）：269-273.

［12］Arrichiello F，Antonelli G，Aguiar A，et al.Observability metric of the relative localization for AUVs based on range and depth measurements：theory and experi-ments［C］∥2011 IEEE/RSJ International ConferenceonIntelligentRobotsandSystems（ICIRS）.San Francisco，US：IEEE/RSJ，2011：3166-3171.

［13］Arrichiello F，Antonelli G，Aguiar A，et al.An observability metric for underwater vehicle localization using range measurements［J］.Sensors，2013，13（12）：16191-16215.

［14］Long R，Qin Y，Jia J.Observable degree analysis of SINS initial alignment based on singular value decomposition［C］∥IEEE International Symposium on Knowledge Acquisition and Modeling Workshop.Wuhan，China：IEEE，2008：444-448.

［15］马艳红，胡军.基于SVD理论的可观测度分析方法的几个反例［J］.中国惯性技术学报，2008，16（4）：448-452. MA Yan-hong，HU Jun.Counterexamples for degree of observability analysis method based on SVD theory［J］.Journal of Chinese Inertial Technology，2008，16（4）：448-452.（in Chinese）

［16］Maessen D，Gill E.Relative state estimation and observability analysis for formation flying satellites［J］.Journal of Guidance，Control，and Dynamics，2012，35（1）：321-326.

［17］Li M，Wang D，Huang X.Study on the observability analysis based on the trace of error covariance matrix for spacecraft autonomous navigation［C］∥IEEE International Conference on Control and Automation（ICCA）.Hangzhou，China：IEEE，2013：95-98.

［18］Vaganay J，Leonard J，Curcio J，et al.Experimental validation of the moving long baseline navigation con-cept［C］∥IEEE/ OES Autonomous Underwater Vehicles.US：IEEE/OES，2004：59-65.

［19］Isidori A.非线性控制系统［M］.第3版.王奔，庄圣贤，译.北京：电子工业出版社，2005. Isidori A.Nonlinear control system［M］.3rd ed.WANG Beng，ZHUANG Sheng-xian，translated.Beijing：Publishing House of Electronics Industry，2005.（in Chinese）

［20］张贤达.矩阵分析与应用［M］.北京：清华大学出版社，2004. ZHANG Xian-da.Matrix analysis and applications［M］.Beijing：Tsinghua University Press，2004.（in Chinese）

Observability Analysis of Cooperative Localization System for MAUV Based on Condition Number

MA Peng，ZHANG Fu-bin，XU De-min，LIU Shu-qiang
（School of Marine Science and Technology，Northwestern Polytechnical University，Xi'an 710072，Shaanxi，China）

For the multiple autonomous underwater vehicle（MAUV）cooperative localization system based on the moving long baseline，the 2D kinematic models and range measurement equations of MAUV are established.The observability theory of nonlinear system based on Lie derivatives is used to derive the observability matrix of the cooperative localization system and describe the unobservable trajectories of MAUV system.The evaluation function for the observability metric of the localization system is given using the condition number of observability matrix obtained from singular value decomposition（SVD）theory.Finally，a series of simulation experiments are designed to analyze the system observability under the different MAUV formations.The results show that the system observability change with the MAUV formation，which provides an effective reference to improve the performance of cooperative localization system by transforming the MAUV formation.

information processing technology；multiple autonomous underwater vehicle；cooperative localization；condition number；observability

TP391

A

1000-1093（2015）01-0138-06

10.3969/j.issn.1000-1093.2015.01.020

2014-03-24

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

马朋（1987—），男，博士研究生。E-mail：sxhy_mapeng@126.com；张福斌（1972—），男，副教授，硕士生导师。E-mail：zhangfb@nwpu.edu.cn