[发明专利]基于自适应无迹卡尔曼滤波的超短基线定位方法

权利要求书

1.一种基于自适应无迹卡尔曼滤波的超短基线定位方法，其特征在于，包括以下步骤：

步骤1：利用超短基线进行水下定位，具体为：通过超短基线基阵与水下机器人搭载的应答器进行声通信得到水下机器人的位置信息；超短基线定位方法为：

水下机器人深度可以通过自身携带的深度计测得，三个基阵单元的坐标分别(x₁,y₁,z₁)、(x₂,y₂,z₂)、(x₃,y₃,z₃)，基阵单元与水下机器人的距离分别为s₁、s₂、s₃，深度计测得水下机器人所处深度为h；

其中，式中：c为声波在水中的传播速度，d_i为传播时间，n_i为声波噪声，在此视为高斯白噪声，

记S＝[s₁,s₂,s₃]，可得：

其中，z₁,z₂,z₃均可由深度计直接测量，则h_i＝h-z_i可视为已知量，即求解量为目标的xy平面坐标(x,y)；

步骤2：通过光电混合缆将母船位置信息和超短基线定位到的水下机器人位置信息发送给水下机器人；

步骤3：水下机器人接收到母船发送的位置信息；

步骤4：水下机器人将位置信息融合自身携带的加速度计、惯导、深度计三个传感器，根据自适应无迹卡尔曼滤波算法进行噪声的削弱，具体如下：

通过对加速度计积分求得水下机器人速度，对速度进行分解可获得速度在地图坐标系的X轴Y轴速度分量，结合水下机器人运动模型，可对水下机器人运动速度进行推算；

其中，XY是地图坐标系，xy是水下机器人运动坐标系，选取的惯导测得航向角与地图坐标系间的角度差为θ，速度分量的求取为：

其中，V_X是水下机器人在地图坐标系的X轴速度分量，V_Y是水下机器人在地图坐标系的Y轴速度分量；V是水下机器人通过对加速度计求积分得到的速度；假设水下机器人在水下做匀速运动，则系统方程可以表示为：

其中，(x(k),y(k))为水下机器人k时刻的坐标，T为单位采样时间，w_θ(k)为航向角在转向中受到的外部扰动，a_X(k)为目标运动过程中地图坐标系中的X轴加速度，a_Y(k)为目标运动过程中地图坐标系中的Y轴加速度，为k时刻的航向角速度，加速度由水下机器人本身的加速度u(k)和外界扰动w(k)构成；

a(k)＝u(k)+w(k)；

对于不同时刻k，由具有系统高斯白噪声W(k)的随机变量和具有观测高斯白噪声V(k)的观测变量构成的非线性系统可以由下式描述，

式中，f是非线性状态方程函数；g是非线性观测方程函数，为状态向量，分别为状态变量观测变量随机向量在不同时刻k的映射；

根据无迹卡尔曼滤波算法进行信号滤波后，求得卡尔曼增益矩阵：

最后计算系统状态更新和协方差更新：

其中与是无迹卡尔曼算法所取Sigma点的对应权值；

上述UKF基于系统高斯白噪声W(k)与观测高斯白噪声V(k)已知的基础上；在实际应用中，W(k)与V(k)的噪声特性是未知的；Sage-Husa自适应滤波算法在已知系统噪声特性或者观测噪声特性其中之一的情况下，可动态更新另一噪声的统计特性；已知系统高斯白噪声W(k)，可用下式来动态更新观测高斯白噪声V(k)的统计特性：

其中：b为遗忘因子，取0＜b＜1，H_k为系统的量测矩阵；k时刻的自适应因子d_k由遗忘因子b和时刻k计算得到，并由1向1-b收敛；并且，自适应滤波算法的自适应程度由自适应因子d_k或者遗忘因子的选取决定；

在更新观测噪声前，通过下式更新观测噪声协方差矩阵：

2.根据权利要求1所述的基于自适应无迹卡尔曼滤波的超短基线定位方法，其特征在于，水下机器人深度信息由水下机器人本身深度计提供。