[发明专利]一种基于TMA技术的水下机器人单信标组合导航方法

摘要

本发明涉及一种基于TMA技术的水下机器人单信标组合导航方法，实现水下机器人的水下组合导航定位。本发明包括：利用AUV不同时刻的单信标斜距测距测量值，计算当前时刻的AUV声学定位位置；建立基于TMA技术的单信标组合导航的卡尔曼滤波器，计算组合导航的位置估计。与传统长基线导航和超短基线组合导航比较，本方法具有成本低，设备简单，节省作业时间的优点，降低了水下机器人组合导航系统复杂性，提高了水下机器人组合导航系统的可靠性。本方法移植方便，可以适用于各种潜航器水下导航定位。

主权项

1.一种基于TMA技术的水下机器人单信标组合导航方法，其特征在于，包括以下步骤：利用AUV不同时刻的单信标斜距测距测量值，计算当前时刻的AUV声学定位位置；建立基于TMA技术的单信标组合导航的卡尔曼滤波器，计算组合导航的位置估计，包括以下步骤：第1步，定义k时刻的AUV声学定位Z_k(x'_k,y'_k)，其中x'_k和y'_k分别表示k时刻AUV在x方向和y方向的声学定位位置，建立TMA技术声学定位的量测方程：Z_k＝[x'_k,y'_k]^T＝h(x'_k,y'_k)+V_k其中，V_k是k时刻信标的测量噪声，它是零均值高斯白噪声，方差矩阵为R_k，信标噪声的方差矩阵是设备属性，为已知量；第2步，定义k时刻AUV的预测位置为X_k(x_k,y_k)，其中x_k和y_k分别表示k时刻预测AUV在x方向和y方向的位置，建立组合导航的预测方程：其中，u_k和ψ是k时刻AUV的速度和航向角，它们可以分别通过多普勒和航向角传感器测量得到，是已知量，那么定义U_k＝[u_k,ψ]是系统输入矩阵；ΔT是从k‑1时刻到k时刻的时间间隔，是已知量；定义H_k＝[1,1]^T是测量方程的雅各比矩阵，是常量；W_k是k时刻的系统输入噪声，它是零均值高斯白噪声，定义系统输入噪声的方差矩阵为Q_k，Q_k是航向角传感器和多普勒的设备属性，所以输入噪声的方差矩阵Q_k是已知量；x_k‑1和y_k‑1分别表示k‑1时刻AUV在x方向和y方向的位置，是已知量；P_k,k‑1表示k时刻预测AUV位置的预测方差，未知量；P_k‑1表示k‑1时刻估计AUV位置的估计方差，是已知量；通过组合导航的预测方程，计算k时刻AUV的预测位置为X_k(x_k,y_k)；第3步，计算组合导航的位置估计：定义k时刻AUV估计位置和分别表示k时刻AUV在x方向和y方向的估计位置；