|
卡尔曼滤波算法在自主式水下机器人超短基线定位中的应用0 引言 目前，自主式水下机器人成为人类获取海洋信息的重要平台，具有广阔的发展前景。自主式水下机器人利用自身携带的设备如声能换器、地磁传感器、惯性测量器、地形匹配装置等实现导航。由于海洋环境自身的复杂性，以声波探测为基础的水声定位技术是传递和获取水下信息的有效手段之一，被广泛应用于水下机器人中。 水声定位系统根据接收基阵尺寸可以分成超短基线（ usbl）、短基线（ sbl）和长基线（ lbl） 3 种定位系统。基于超短基线的定位系统利用测量信号的相位差原理进行水下定位，其中，接收基阵的基元间距离通常是小于或等于波长的一半，使得设备更加简单、易于安装，同时具有很强的准确性、适应性和灵活性。文深入研究基于超短基线的水声定位系统的组成及其定位原理，在此基础上，对声波传播过程中存在的能量损失和噪声干扰进行分析，提出基于卡尔曼滤波算法的测量和运动方程，最后进行仿真实验。 1 水声定位系统 1.1 超短基线水声定位系统 在水声定位系统中，根据基线的长短可以将水声定位分为超短基线、短基线和长基线定位。其中，超短基线定位利用基元间接收信号的相位差实现对水下机器人的定位，该技术中基元间距离不大于 1/2 个波长，基阵长度通常在厘米量级或几十厘米量级。在基于超短基线的水下机器人定位系统中，通常将由至少 3 个换能器组成的声基阵安装在水面船体上，并且在安装时对船和声基阵间的坐标关系进行精确测定，换能器之间的距离一般为几厘米。在定位过程中，换能器向装在自主式水下机器人上的应答装置发送声波信号，然后，对换能器的接收信号进行测量，通过计算信号间的相位差来确定应答器位置，进而确定水下机器人的置。通过计算声波船舶时间，再利用声速剖面波束线进行修正，从而确定换能器和水下机器人之间的距离。 将基于超短基线的水下机器人定位系统和船载GPS 系统相结合，能够有效提升水下机器人定位的准确性。将 GPS 接收装置放置在水面船舶上，利用 GPS系统对船舶位置进行实时测定，同时，对测量船舶和水下机器人间的相对位置和方位进行同步测定，从而计算出水下机器人的准确位置。基于超短基线的水下机器人定位系统具有性价比高、易操作、安装简便、测量精度高等优点。 基于超短基线的定位系统的工作原理如图1 所示，其中，接收基阵装于水面船舶的底部，应答器装于水下机器人的上部。
图 1 定位系统工作示意图 1.2 定位原理 基于超短基线的定位系统由接收基阵、应答器和发射换能器构成。接收基阵和发射换能器安装在水面船舶上，应答器安装于水下机器人上。发射换能器向应答器发射声脉冲，当应答器接收到声脉冲后将发送应答声脉冲，当接收基阵接收到应答声脉冲后，对X、 Y 方向上的相位差进行测量，然后根据声波传播的时间求出水下机器人与接收基阵间的距离，进而求得水下机器人在平面坐标系上的位置及其水下深度。 基于相位差的定位原理是：假设在平面中，等间距安装 5 个接收换能器，特性一致，且位置满足垂直正交，每个接收换能器作为一个基元。在坐标原点 O处放置 1 个接收换能器，其他 4 个接收换能器 Xa，Xb， Ya， Yb 放置在距离原点 O 为 d/2 的位置，放置在水下机器人上的应答器为 P， P 到坐标原点 O 距为 S，到其他接收换能器的距离为 SXa， SXb， SYa， SYb。换能器发射脉冲信号，当应答器接收到脉冲信号后发射应答信号，在各个基元分别接收到应答信号后，可以根据 Xa， Xb 信号的相位差求得 OP 和 X 轴间的夹角 θX，根据 Ya， Yb 信号的相位差求得 OP 和 Y 轴间的夹角θY。数学模型为
式中： ∆ϕ 为基元间的信号相位差； f 为信号频率。 2 卡尔曼滤波算法 2.1 算法原理 卡尔曼滤波算法是最小线性方差估计的一种，具有如下特点： 1）由于卡尔曼滤波算法在时域内采用状态空间方法设计滤波器，并且递推，所以该算法适用于对多维随机系统进行估计。 2）卡尔曼滤波算法利用状态方程和动力学方程对待估计量的变化规律进行描述，对于待估计量信息通常采用动力学方程和激励白噪声统计方法来确定。由于动力方程已知，激励白噪声平稳，所以待估计量可以表现出平稳状态，也可以表现出非平稳状态，也就是说卡尔曼滤波算法对非平稳过程同样适用。 3）卡尔曼滤波算法具有离散型和连续型 2 种算法，其中，离散型算法对非平稳过程也适用。 对于离散控制系统，采用随机线性微分方程进行描述，则卡尔曼滤波算法如下： 1）首先利用系统模型对系统的下一状态进行预测，如下式：
2）采集系统状态测量值，根据预测值求得系统状态的最优估算值，如下式：
其中 Kg 为卡尔曼增益。
为了使算法能够自回归运算，还需要对系统 k 状态下对应的 covariance 进行更新，如下式：
2.2 水下机器人定位方程 根据基阵测量得到的混有噪声的数据，利用卡尔曼滤波算法对水下机器人定位进行最优估计，构建水下机器人的运动方程，其向量形式为：
3 仿真实验 本文在 Matlab 平台对基于卡尔曼滤波的水下机器人定位算法进行了仿真实验，在实验过程中，假设测量船舶发生横摇或者纵摇，设置横摇角为 10°，纵摇角也为 10°，实验结果如图2所示。 图 2 中，第 1 个点是测量误差在 [0，1）范围内点的数量，第 n 个点是测量误差在 [n – 1，n）范围内点的数量，从实验结果可知，误差在15m 内的占总数的75%，所以，本文提出的算法在降低误差方面具有明显效果。 4 结语 在超短基水声定位技术被广泛应用于自主式水下机器人定位系统的背景下，如何利用卡尔曼滤波算法提高定位的准确率是本文研究的重点。本文对卡尔曼滤波算法应用于基于超短基的水下机器人定位系统的可行性进行了研究，建立了相应的运动和测量模型，最后，进行了仿真实验，实验结果达到预期。 参考文献： [1] 李晔, 苏玉民, 万磊, 等. 自适应卡尔曼滤波技术在水下机器人运动控制中的应用 [J]. 中国造船, 2006, 47(4): 83–88. [2] 李晔, 常文田, 万磊, 等. 水下机器人自适应卡尔曼滤波技术研究[J]. 智能系统学报, 2006, 1(2): 44–47. |
