面向深海集矿机定位的数据融合

王随平，桂卫华，邓晨

(中南大学 信息科学与工程学院，湖南 长沙，410083)

摘要：面向集矿机行走过程，针对提高集矿机实时位置的准确性、行走稳定性、可靠性和定位控制开展深入研究。从深海集矿机定位系统的功能需求出发，设计集矿机定位系统硬件结构。以集矿机行走控制软件结构为基础，将定位作为子任务模块嵌入整个深海集矿机行走控制软件结构中，并进一步设计定位控制软件结构。考虑单一传感器采集信息的不完善和不确定性对深海集矿机的定位精度产生的影响，研究数据融合理论，讨论自适应卡尔曼滤波器，应用基于信息序列的自适应估计算法解决自适应卡尔曼滤波器的系统状态和测量噪声统计特性的协方差阵的自适应。采用卡尔曼滤波器对直接测量的位置信息与间接推测方法得到的位置信息进行融合，得到深海集矿机位置的最优估计。基于MATLAB平台开展仿真研究，并以集矿机陆地试验模型车为对象开展试验研究，结果验证了面向深海集矿机定位的数据融合算法的有效性和可行性。

关键词：深海集矿机；定位；数据融合；卡尔曼滤波

中图分类号：TP391.9          文献标志码：A         文章编号：1672-7207(2011)S2-0337-04

Date fusion of orientation of deep-seabed mining vehicle

WANG Sui-ping, GUI Wei-hua, DENG Chen

(School of Information Science and Engineering, Central South University, Changsha 410083, China)

Abstract: Aiming at improving the properties of positioning validity, tread stability and credibility, the orientation control was studied. On the basis of the requirement on functions from orientation system, the hardware structure and the software structure were designed as a task module of tread control. The positional precision was influenced by faultiness and uncertainty of the information gathered by single sensor. In order to deduce the influence a self-adaptive Kalman filter on the basis of data fusion theory was studied. The conjectural location data and the measured location data were fused using Kalman filter. The result of fusion is the excellent location data. At the end, the experiment was done using the small-scale model vehicle and the simulation was done using the platform MATLAB. The results show that the algorithm is valid and feasible.

Key words: deep-seabed mining vehicle; orientation; data fusion; Kalman filter

深海集矿机是深海底集矿作业的载体，为深海底采矿系统中技术最复杂、最关键的部分。其技术难点在于如何保证采矿车在6 km深海未知环境中正常、 可靠地行使，并保证以较高的效率收集多金属结核。其运动控制基本要求是：控制集矿机的位置和姿态实现按预定轨迹行走作业，并在行走过程中实现有效避障。在深海集矿机采集作业过程中，当直行采矿时，要求定位精度高、集矿路线控制准确；当绕障、转弯时，亦需要定位参数，以便使集矿车能准确回归到预定的采集路径上，安全高效地行走集矿^[1]。因此，集矿机定位控制是整个深海集矿机行走控制过程中非常重要的环节。

数据融合在军事方面的发展和应用中所表现出来的优势已被世人所共识，与此同时，它在民用领域的应用如机器人方面的巨大优势也引起了人们的广泛关注。目前，多传感器的数据融合在机器人领域中的应用主要集中在移动机器人和遥操作机器人^[2]，这是因为这些机器人工作于动态变化、不确定与非结构化的环境中，这些高度不确定的环境要求机器人具有高度的自治能力和对环境的感知能力，采用多传感器数据融合技术可以使机器人具有感知外界环境及其变化的能力。我国在多传感器数据融合技术方面的研究工作起步较晚，但已有一批高等院校和研究所展开了该领域的研究^[3]，比如，四川大学研制的多航管雷达数据融合系统。

本文作者提出深海集矿机数据融合采用卡尔曼滤波算法，应用基于新息序列的自适应估计算法来实现面向深海集矿机定位的数据融合，对定位系统的位置估计进行仿真研究。仿真结果证明：对于集矿机位置的估计，采用数据融合技术能取得良好的效果。

1  深海集矿机定位系统

在深海集矿机采集作业过程中，当直行采矿时，要求定位精度高、集矿路线控制准确；绕障、转弯时，亦需要定位参数，以便使集矿车能准确回归到预定的采集路径上，安全高效地行走集矿。因此，集矿机定位控制是整个深海集矿机行走控制过程中非常重要的环节。图1所示为深海集矿机定位系统原理框图。

深海集矿机定位系统由长基线水声定位系统、激光多普勒测速仪、HMR3000罗盘、角速度陀螺和 ARM控制系统组成。其中，HMR3000罗盘、角速率陀螺、激光多普勒测速仪等传感器用来测量深海集矿机的航向、角速率和水平面的速度。长基线水声定位系统由导航处理器和换能器基阵组成，长基线基阵安装在母船上，其功能是可计算出集矿机相对母船的位置坐标。

2  深海集矿机多传感器数据融合算法

2.1  自适应卡尔曼滤波

卡尔曼滤波算法需要解决两方面问题，即建模问题和估计问题。其中，建模问题涉及如何准确描述基于多传感器数据融合的深海集矿机集成定位系统的误差模型；估计问题涉及如何根据可以得到的过程信息和传感器数据得到系统状态量的最佳估计。对卡尔曼滤波器来说，估计算法的最优性与模型噪声以及传感器噪声的统计特性(即Q和R)密切相关。若Q和R取值过小，则会产生估计偏差，估计的最优性遭到破坏；反之，若Q和R值过大，则会影响到滤波算法的稳定性。对于基于多传感器数据融合的深海集矿机集成定位系统，系统的运行噪声与测量噪声的统计特性与诸多因素相关，如系统的运行状态、环境等(如罗盘的漂移及声速波动引起长基线测量精度的改变等)，所以，很难获得系统状态和测量噪声的准确统计信息。为改进估计质量，采用自适应卡尔曼滤波技术^[4]。自适应卡尔曼滤波包含两方面的含义，一是系统模型的自适应，二是系统状态和测量噪声统计特性的协方差阵的自适应(即Q和R的自适应)。第一个问题主要涉及系统设计和辨识方面，这里假定系统的模型参数准确，主要讨论应用基于信息序列的自适应估计算法，实现卡尔曼滤波器噪声统计Q和R的自适应。

图1  深海集矿机定位系统原理框图

Fig.1  Principle frame of orientation system for deep-seabed mining vehicle

将系统的信息V_k定义为系统k时刻的测量值Z_k与卡尔曼滤波器k时刻估计值的差值，即

             (1)

理论上，V_i在不同时刻是互不相关的，也就是说，在k时刻，V_k的值无法通过以前时刻的值预测出来，所以，V_k代表k时刻系统的输出值最新信息。根据极大似然原理，可得系统输出噪声的自适应律为：

           (2)

其中：

              (3)

N为自适应窗口长度；为k时刻卡尔曼滤波器的方差估计；j₀=k-N+1同理，可以得出系统状态噪声自适应律为：

   (4)

其中：ΔX_k=K_kV_k，K_k为k时刻卡尔曼滤波增益。应用上述算法，对深海集矿机的位置进行估计。

2.2  基于多传感器数据融合的深海集矿机位置估计

卡尔曼滤波是以最小均方误差为准则的最佳线性估计或滤波。卡尔曼滤波器不需要全部过去的观测数据，它只是根据前一个估计值和最近一个观测数据来估计信号的当前值，用状态方程和递推的方法进行估计，所得的解是以估计值的形式给出的。但利用卡尔曼滤波器进行滤波时，需要知道系统的状态方程和量测方程。下面针对长基线水声定位系统，求出状态方程矩阵和量测方程矩阵。系统状态方程和量测方程满足约束条件，系统过程噪声W_k和观测噪声V_k满足式假设，X(0)通过长基线水声定位系统测量得到。

选择深海集矿机待估计的位置矢量为 ，φ为深海集矿机航向角。同时，水平速度(u, v)和角速率ω作为不变量出现在状态转移方程中，假定深海集矿机在同一水平面运动，即Z(k+1)= Z(k)，同时定义状态变量如下：

           (5)

则可得系统的状态方程为：

   (6)

其中：T_s为系统的采样时间；为k时刻深海集矿机航向角的最优估计。上述状态方程是非线性方程，根据扩展卡尔曼滤波思想，可将(6)在sv(k)点进行线性化，则系统的状态矩阵表示为：

 (7)

根据长基线水声定位系统的定位基本原理可得 方程：

 (8)

式中：和为已知坐标；t₁₀和t₂₀为已知常量，可以得出量测方程矩阵：

          (9)

3  仿真结果

采用模型车作为载体，由于此模型车是针对陆地实验设计的，不能进行水下实验，因此，必须用其他方式对长基线水声定位系统进行模拟。具体的方法是在实验场地4个角落各安放一个摄像机，当模型车运动时，4台摄像机同时对其进行拍摄，得到关于模型车相对实验场地矩形4个顶点的直线距离，根据几何关系计算出模型车的实时位置信息。由于长基线水声定位系统在测量过程中存在一定的时间延迟和白噪声的影响，而此模拟手段不存在这种影响，因此，还要人为加入时间延迟和白噪声。

图2和3所示分别是坐标X和坐标Y的估计值和真值的比较图。图2和3中的坐标真值是利用安装在试验场地4个角落的摄像机得到的，具体是采用摄像机对运动的试验模型车进行实时拍摄，根据几何关系可以得出任意时刻模型车的位置信息，通过这种方法得到的位置信息误差可以忽略不计，因此，可以认为是坐标的真值。

图2  X坐标估计值和真值

Fig.2  Estimated value and values of coordinate X

图3  Y坐标估计值和真值

Fig.3  Estimated value and values of coordinate Y

由于多普勒信号存在测量误差，位置估计误差呈增大趋势，并随着时间增长，位置估计误差趋于无穷大。采用长基线水声定位系统直接进行位置测量，这种直接测量可以保证测量误差有界，但其测量噪声很大。从图2和3可以看出：卡尔曼滤波算法综合了间接测量(推算)的数据平滑性和直接测量过程误差的有界性，获得了比较好的估计效果。

4  结论

设计了深海集矿机定位系统，由于单一传感器获得的仅是环境特征的局部、片面的信息,它的信息量是非常有限的，因此，采集到的信息往往不完善，存在较大的不确定性，这些都会对深海集矿机的定位精度产生影响。为了解决直接测量方法对于深海集矿机定位精度不良影响的问题，提出利用卡尔曼滤波器对位置数据和数据进行数据融合，最后得到位置数据的最优估计。仿真结果验证了此算法的有效性。

参考文献：

[1] 王随平. 海底采矿集矿车的导航控制[J]. 矿业研究与开发, 1998, 18(3): 30-32.
WANG Sui-ping. Navigation control of nodule collector for sea floor mining[J]. Mining Research and Development, 1998, 18(3): 30-32.

[2] 徐涛, 杨国庆, 陈松灿. 数据融合的概念. 方法及应用[J]. 南京航空航天大学学报, 1995, 27(2): 258-265.
XU Tao, YANG Guo-qing, CHEN Song-can. The concept, methods and applications of data fusion[J]. Transactions of Nanjing University of Aeronautics and Astronautics, 1995, 27(2): 258-265.

[3] 刘同命. 数据融合技术及其应用[M]. 北京: 国防工业出版社, 2001: 55-65.
LIU Tong-ming. The technologies and applications of data fusion[M]. Beijing: National Defense Industry Press, 2001: 55-65.

[4] Mohame A H, Mohamed A H. Adaptive Kalman filter for INS/GPS[J]. Journal of Geodesy, 1999, 73(1): 193-203.

(编辑 陈卫萍)

收稿日期：2011-06-15；修回日期：2011-07-15

基金项目：国际海底区域研究开发“十五”项目(DY105-03-02-06)；国家自然科学基金资助项目(60505018)

通信作者：王随平(1956-)，男，河南焦作人，博士，教授，从事人工智能、深海机器人、现场总线及计算机控制系统等研究；E-mail: wangsp@csu.edu.cn