单信标测距AUV水下定位系统观测性分析

严卫生，房新鹏，崔荣鑫

(西北工业大学 航海学院，陕西 西安，710072)

摘要：针对基于单信标测距的自主水下航行器(AUV)定位系统观测性问题，建立AUV三维空间运动学模型，选取实物平台常用的深度、偏航角和俯仰角作为基本观测量辅助距离信息进行水下定位，利用基于Lie导数的非线性系统观测性秩判据分析不同辅助测量值下系统的可观性以及控制输入和运动路径对可观性的影响。研究结果表明：满足定位系统可观测的最简量测组合为距离值和偏航角，含有偏航角的辅助量测组合均使得定位系统可观测，系统控制输入和运动路径的变化会导致可观性发生变化。

关键词：单信标；自主水下航行器；观测性分析；水下定位

中图分类号：TP242.3          文献标志码：A         文章编号：1672-7207(2012)08-3085-06

Observability analysis of single-beacon underwater localization of autonomous underwater vehicle

YAN Wei-sheng, FANG Xin-peng, CUI Rong-xin

(College of Marine Engineering, Northwestern Ploytechnical University, Xi’an 710072, China)

Abstract: Aiming at the observability analysis of autonomous underwater vehicle (AUV) underwater localization system based on the range measurements from a single beacon, an AUV three-dimensional kinematics model was developed. Three common measurements used on real platform, such as depth, heading and pitch, were considered as basic observation values, assisting range measurements for underwater localization. An observability analysis was performed on the system defined with different assist measurements, and then the influence of control inputs and trajectories on the observability property was also performed by nonlinear observability rank condition, which is based on the Lie derivatives. The results indicate that the most simplified measurements combination meeting the system observable is heading and range, and the systems, whose assist measurements containing the heading, are all observable, the change in control inputs and trajectories may make the change in observability property.

Key words: single beacon; autonomous underwater vehicle; observability analysis; underwater localization

小型自主水平航行器(AUV)在民用和军事领域正发挥着越来越重要的作用^[1-3]，但是由于其结构简单、体积和携带的能源有限，不适于使用传统的声学导航和惯性导航方法进行定位。基于单信标测距的水下定位系统能有效降低硬件复杂性和安装复杂性，特别适于解决小型AUV的水下定位问题，引起学者的广泛关注。Lee等^[4]介绍一种基于捷联惯性测量单元的组合导航系统，AUV利用与参考基站间的距离测量值辅助提高IMU-DVL系统定位性能。Casey等^[5]基于扩展卡尔曼滤波(EKF)融合距离值、航向角和相对水流速度，设计水下导航增强系统预测UUV位置及洋流速度。以上研究均基于参考信标位置已知，限制基于信标测距定位系统的应用。针对这一问题，Olson等^[6]提出一种信标位置未知的定位系统，能够同时预测AUV及静止信标的位置。Gao等^[7]将装有高精度导航设备的AUV作为移动参考信标，通过水声通信传输距离信息，利用EKF融合距离信息和低精度AUV传感信息，提高低精度AUV定位精度。定位过程实际上是一个状态预测过程^[8]，研究预测问题的首要前提就是系统可观测。当系统可观测时，测量值能提供足够信息进行状态预测^[9]。可见，可观测性对AUV定位系统研究有着重要意义。Song^[10]利用费歇尔信息矩阵得到二维平面内基于距离量测的目标跟踪系统可观测的充要条件。Roumeliotis等^[11]提出多机器人分布式定位方法，利用外部传感器提供机器人间相对量测，研究指出至少一个机器人有绝对定位能力时系统可观测。Gadre^[12]研究基于单信标测距的水下定位系统，利用线性时变系统(LTI)观测性理论分别对无洋流、存在已知洋流、存在未知洋流时系统的观测性进行研究。Zhou等^[13]研究基于距离量测的机器人相对状态预测问题，利用基于Lie导数的非线性系统观测性秩判据分析系统局部弱观测条件。值得注意的是，当前对AUV水下定位系统观测性的研究都是在观测量已定的基础上展开的，忽略观测量选取以及控制输入变化对定位系统可观性的影响。在AUV定位系统实物设计过程中，更简便的量测组合能有效降低对AUV成本、体积及运算性能等方面的要求，具有重要的理论意义与实用价值。本文选取深度、偏航角和俯仰角作为基本辅助测量值，利用基于Lie导数的非线性系统观测秩判据对选用不同测量值组合时定位系统的观测性进行研究，得到满足系统可观测的最简测量值组合，然后简要分析控制输入及运动轨迹对系统可观性的影响，以便为AUV定位系统实物设计提供参考依据。

1  AUV运动学模型

假设信标固定在某参考位置，以信标参考位置为坐标原点建立直角坐标系，系统原理如图1所示。

图1  AUV运动学模型

Fig.1  Kinematic model of AUV

由原理图建立AUV运动学方程：

      (1)

为分析观测量的选取对定位系统观测性的影响，选取以下测量值辅助距离信息进行水下定位：

(1) h₁(x)=z(t)；

(2) h₂(x)=ψ(t)；

(3) h₃(x)=θ(t)。

2  非线性系统观测性

考虑如下表达形式的非线性系统：

    (2)

其中：x=[x₁，…，x_n]^T，为状态向量；u=[u₁，…，u_l]^T，为输入向量；y=[y₁，…，y_m]^T，为输出向量；y_k=h_k(x)；k=1，…，m。

系统(式(2))是非线性系统，利用LTI系统可观测性判定方法分析其观测性可能会产生错误的结果^[14]，必须利用基于Lie导数的非线性观测秩判据判定观 测性。

2.1  Lie导数

函数h_k(x)的零阶Lie导数为函数本身，即：

             (3)

函数h_k(x)关于f_i的n(n≥1)阶Lie导数为：

 (4)

h_k(x)关于f_i和f_j的d阶混合Lie导数为：

      (5)

则可定义非线性系统观测性矩阵为：

  (6)

2.2  观测性秩判据

若非线性系统(式(2))的观测性矩阵O满秩，即rank(O)=n，则称非线性系统(式(2))局部弱能观^[15]。

3  AUV定位系统观测性

将式(1)改写为式(2)的形式，则：

   (7)

          (8)

          (9)

为研究方便，选取作为距离测量值，利用观测性秩判据分析不同测量值组合辅助距离值定位时系统的观测性。

3.1  观测量为距离测量值

h₀(x)的零阶Lie导数：

     (10)

        (11)

h₀(x)的一阶Lie导数：

   (12)

(13)

h₀(x)的二阶Lie导数：

    (14)

  (15)

 (16)

其中：

(17)

其中：

h₀(x)的三阶Lie导数为：

   (18)

    (19)

(20)

其中：

(21)

其中：

由式(6)建立系统观测性矩阵：

 (22)

rank(O₁)=4＜5，即仅选取与信标间距离作为观测量进行水下定位时系统不可观测。

3.2  深度测量值辅助距离值定位

当深度测量值辅助距离值进行定位时，h(x)=   [h₀  h₁]^T。

              (23)

             (24)

        (25)

     (26)

由式(6)建立系统观测性矩阵：

       (27)

rank(O₂)=4＜5，即深度测量值辅助距离值进行水下定位时系统不可观测。

3.3  偏航角测量值辅助距离值定位

当偏航角测量值辅助距离值进行定位时，h(x)= [h₀  h₂]^T。

             (28)

        (29)

由式(6)建立系统观测性矩阵：

           (30)

有rank(O₃)=5，观测矩阵满秩，即偏航角测量值辅助距离进行水下定位时系统可观测。

3.4  俯仰角测量值辅助距离值定位

当俯仰角测量值辅助距离值进行定位时，h(x)= [h₀  h₃]^T。

             (31)

       (32)

由式(6)建立系统观测性矩阵：

             (33)

有rank(O₄)=4＜5，即俯仰角测量值辅助距离进行水下定位时系统不可观测。

3.5  多个测量值辅助距离值定位

从前面的分析可知，当h(x)=[h₀  h₂]^T时定位系统可观测，即距离值和偏航角能够提供足够信息进行AUV状态预测，由此得到以下推论。

推论1：当多个测量值辅助距离值定位时，若辅助测量值组合中含有偏航角，则水下定位系统均可观测。

证明：

(1) 当时，由以上分析建立系统观测性矩阵：

     (34)

有rank(O₅)=5，观测矩阵满秩，即深度和偏航角辅助距离进行水下定位时系统可观测。

(2) 当时，系统观测性矩阵：

        (35)

有rank(O₆)=5，观测矩阵满秩，即偏航角和俯仰角辅助距离进行水下定位时系统可观测。

(3) 同理，当时，系统观测性矩阵：

   (36)

有rank(O₇)=5，观测矩阵满秩，即深度、偏航角和俯仰角辅助距离值进行水下定位时系统可观测。

(4) 当，即测量值不包含偏航角时，rank(O)＜5，定位系统不可观测。

由以上分析可知：观测量为距离和偏航角是进行单信标水下定位的最简量测组合。

3.6  控制输入对系统观测性的影响

AUV在实际运动过程中，控制输入的变化可能会导致可观测定位系统变得不可观测。因此，需要分析控制输入系统观测性的影响。本文仅简要分析时控制输入对系统观测性的影响。

同时抑制AUV的偏航角速度和俯仰角速度，即令u₂=0，u₃=0。此时，只能利用函数h(x)关于f₁的Lie导数建立系统的观测矩阵：

(37)

当时，rank(O₈)=5，定位系统可观测。

由图1可知：若，则，即AUV在过z轴的平面内直线运动时定位系统不可观测，在设计AUV运动路径时要尽量避免这些轨迹。

4  仿真研究

为验证本文对水下定位系统观测性分析的有效性，选取距离、深度、偏航角和俯仰角作为观测量，令偏航角速度和俯仰角速度均为0，基于EKF分别对AUV在不可观测路径及可观测路径运动时系统的定位性能进行仿真。

4.1  不可观测路径运动

假设AUV从(0,0,0)出发沿直线运动300 s，偏航角和俯仰角分别为45°和60°，航行速度为1 m/s；然后以1 m/s速度定深直线航行300 s，即偏航角和俯仰角分别为45°和0°。AUV初始位置预测误差为(5,5,5)，状态更新频率为1 Hz。图2和3所示分别为AUV在不可观测路径的航行轨迹曲线和定位误差曲线。

4.2  可观测路径运动

假设AUV从(50,50,50)出发沿直线运动300 s，偏航角和俯仰角分别为36°和60°，航行速度为1 m/s，然后以1 m/s的速度定深直线航行300 s，即偏航角和俯仰角分别为36°和0°。AUV初始位置预测误差(10,10,10)，状态更新频率为1 Hz。图4和图5所示分别为AUV在可观测路径的航行轨迹曲线和定位误差曲线。

仿真结果表明：当AUV在不可观测路径运动时，AUV位置预测发散，不能有效跟踪理想运动轨迹；当AUV在可观测路径运动时，系统定位性能较好；当AUV开始运动时，由于初始位置误差以及EKF滤波器初始化系统定位误差较大；当AUV在300 s运动状态变化时，由于侧向漂移定位误差增加较大，在其余运动时刻，预测轨迹能较好地跟踪理想运动轨迹，有效抑制定位误差的增大。

图2  AUV不可观测路径运动轨迹

Fig.2  AUV unobservable trajectories

图3  AUV不可观测路径运动误差

Fig.3  Estimation errors under unobservable trajectory

图4  AUV可观测路径运动轨迹

Fig.4  AUV observable trajectory

图5  AUV可观测路径运动误差e

Fig.5  Estimation errors under observable trajectory

5  结论

(1) AUV水下定位系统设计时观测量的选取不唯一，更简便的观测量组合往往能有效地降低对AUV设计成本、体积及系统运算性能的要求，具有很重要的现实意义。本文以基于单信标测距的AUV水下定位系统为研究对象，通过分析不同辅助观测量下系统的观测性，得出当辅助测量值组合中含有偏航角时水下定位系统均可观测，在其他情况下定位系统不可观测。

(2) AUV运动过程中控制输入的变化可能会导致系统观测性发生变化。

(3) 基于EKF算法对AUV在不可观测路径及可观测路径运动时系统的定位性能进行仿真验证。当AUV在不可观测路径运动时，系统状态预测发散，不能跟踪理想运动轨迹；但是AUV在可观测路径运动时，能较好的跟踪理想轨迹，有效抑制定位误差的   增长。

(4) 在后续研究中，将考虑存在已知洋流或者未知洋流下定位系统的观测性；另一方面，基于移动声信标的定位系统观测性也是一个值得关注的研究方向。

参考文献：

[1] Forrest A L, Laval B, Doble M J, et al. AUV measurements of under ice thermal structure[C]//Proceedings of MTS/IEEE OCEANS 2008. Quebec City: IEEE Press, 2008: 1-10.

[2] Jun B H, Park J Y, Lee F Y, et al. Development of the AUV ‘ISiMI’ and a free running test in an ocean engineering basin[J]. Ocean Engineering, 2009, 36(1): 2-14.

[3] PETRICH J, WOOLSEY C A, STILWELL D J. Planar flow model identification for improved navigation of small AUVs[J]. Ocean Engineering, 2009, 36 (1): 119-131.

[4] Lee P M, Jun B H, Kim K, et al. Simulation of an inertial acoustic navigation system with range aiding for an autonomous underwater vehicle[J]. IEEE Journal of Oceanic Engineering, 2007, 32 (2): 327-345.

[5] Casey T, Guimond B, Hu J. Underwater vehicle positioning based on time of arrival measurements from a single beacon[C]//Proceedings of MTS/IEEE OCEANS 2007. Vancouver, BC: IEEE Press, 2007: 1-8.

[6] Olson E, Leonard J J, TELLER S. Robust range-only beacon localization[J]. IEEE Journal of Oceanic Engineering, 2006, 31(4): 949-958.

[7] Gao R, Chitre M. Cooperative Positioning using range-only measurements between two AUVs[C]//Proceedings of MTS/IEEE OCEANS 2010. Sydney: IEEE Press, 2010: 1-6, 24-27.

[8] Martinelli A, Siegwart R. Observability analysis for mobile robot localization[C]//2005 IEEE/RSJ International Conference on Intelligent Robots and Systems. Alberta: IEEE Press, 2005: 1471-1476.

[9] Isidori A. Nonlinear control systems[M]. 3rd ed. Berlin: Springer-Verlag, 1995: 37-40.

[10] Song T L. Observability of target tracking with range-only measurements[J]. IEEE Journal of Oceanic Engineering, 1999, 24(3): 383-387.

[11] Roumeliotis S I, Bekey G A. Distributed multirobot localization[J]. IEEE Transactions on Robotics and Automation, 2002, 18(5): 781-795.

[12] Gadre A S. Observability analysis in navigation systems with an underwater vehicle application[D]. Virginia: Virginia Polytechnic Institute and State University, 2007: 39-42, 53-56.

[13] Zhou X S, Roumeliotis S I. Robot-to-robot relative pose estimation from range measurements[J]. IEEE Transactions on Robotics, 2008, 24 (6): 1379-1393.

[14] Dapadopoulos G, Fallon M F, Leonard J J, et al. Cooperative localization of marine vehicles using nonlinear state estimation[C]//2010 IEEE/RSJ International Conference on Intelligent Robots and Systems. Taipei: IEEE Press, 2010: 4874-4879.

[15] Hermann R, Krener A. Nonlinear controllability and observability[J]. IEEE Transactions on Automatic Control, 1977, 22(5): 728-740.

(编辑 邓履翔)

收稿日期：2011-07-04；修回日期：2011-09-19

基金项目：国家自然科学基金资助项目(60875071)；高等学校博士学科点专项科研基金资助项目(200806990008)；西北工业大学基础研究基金资助项目(JC-2010-0225)

通信作者：房新鹏(1987-)，男，山东莱芜人，博士研究生，从事多AUV协同导航、制导与定位研究；电话：15091096202；E-mail：nwpuxinpeng@163.com