[发明专利]基于条件数理论在多AUV协同导航系统中可观测性分析方法

权利要求书

1.基于条件数理论在多AUV协同导航系统中可观测性分析方法，其特征在于，包括如下步骤：

步骤1：建立AUV运动学模型；

步骤2：建立水声传感器噪声模型；

步骤3：根据步骤1，建立AUV线性化误差状态方程；

步骤4：根据步骤1与步骤2，获得AUV的状态误差预测方程；

步骤5：建立EKF滤波方程，对步骤3与步骤4的状态进行更新；

步骤6：建立协同导航系统的距离量测方程；

步骤7：根据步骤6，获得非线性系统可观测矩阵；

步骤8：计算量测函数的零阶Lie导数；

步骤9：根据步骤8的结果，计算h₁与h₂的一阶Lie导数；

步骤10：由步骤9，得系统的可观测矩阵Obs1；

步骤11：进行协同导航系统的位置坐标转换；

步骤12：计算位置坐标转换后的协同导航系统的可观测矩阵，并获得协同导航系统的可观测度；

步骤13：根据可观测度规划多AUV协同导航系统航行路径；

AUV的二维离散运动学模型为：

AUV水平位置为X(k)＝[x(k) y(k) ψ(k)]^T；

将上式改写为

X(k+1)＝f(X(k),u(k))

其中，X(k+1)和X(k)分别表示为AUV在k+1和k时刻的运动状态坐标，x(k+1)，y(k+1)，ψ(k+1)分别表示AUV在导航坐标系下k+1时刻的水平位置坐标和偏航角；x(k)，y(k)，ψ(k)分别表示AUV在导航坐标系下k时刻的水平位置坐标和偏航角；v(k)和ω(k)分别为AUV的前向速度和偏航角速度；T为解算周期，f为状态预测函数，u(k)为传感器测量的真实值；

步骤2中建立的噪声模型中满足其中，u_m(k)为传感器测量的理论值，为传感器的量测噪声，并假设传感器的量测输入均存在高斯白噪声；

步骤3中建立AUV线性化误差状态方程为其中，为k+1时刻的状态估计误差，为k+1时刻的状态估计；

步骤4中建立在t_k时刻下AUV状态误差预测方程为：

且Φ(k+1,k)与Γ(k)的表达式如下：

其中，T为解算周期，为k时刻的状态估计误差，为k时刻AUV的偏航角速度误差，Φ(k+1,k)和Γ(k)分别为状态预测函数f关于状态和输入噪声的雅各比矩阵，v_m(k)与分别表示速度量测值与航向估计值；

通过EKF滤波算法，对步骤3和步骤4进行状态更新；并得到状态一步预测方程为：

系统预测协方差阵方程为：

卡尔曼滤波增益方程为：

量测值预测方程为：

Z_k|k-1＝h(X_k|k-1,υ_k-1)

系统状态更新方程为：

上式中，为状态一步预测；为k-1时刻的状态估计；u_k-1与w_k-1分别为k-1时刻的状态输入与传感器噪声；P_k\k-1为估计均方误差；Φ_k,k-1为k-1至k时刻的一步转移阵；P_k-1为估计均方误差；Γ_k-1为噪声驱动阵；Q_k-1为系统噪声方差阵；K_k为滤波增益；H_k为量测阵；R_k为量测噪声方差阵；Z_k|k-1为量测预测；υ_k-1为量测噪声；为状态估计；Z_k为k时刻的量测；

步骤6中，设主AUV1的位置坐标为(x₁,y₁)，主AUV2的位置坐标为(x₂,y₂)，从AUV的位置坐标为(x,y)，并根据AUV之间的位置关系得到主AUV与从AUV之间的距离量测方程为：

其中，r(t)为从AUV与主AUV1和主AUV2的距离阵；r₁(t)为从AUV与主AUV1的距离；r₂(t)为从AUV与主AUV2的距离；x(t)和y(t)表示从AUV当前的水平位置信息；x₁(t)和y₁(t)表示主AUV1当前的水平位置信息；x₂(t)和y₂(t)表示主AUV2当前的水平位置信息；

步骤8中，利用Lie导数求解非线性系统的可观测矩阵，选取的量测函数h(t)为：

其中，h₁(t)为从AUV与主AUV1的距离量测关系；h₂(t)为从AUV与主AUV2的距离量测关系；

步骤9中，根据非线性系统微分几何理论，求解步骤8中h₁与h₂的零阶李导数，得到与

其中，与为零阶李导数和关于状态变量x，y的梯度；

根据步骤9得到的h₁与h₂的零阶Lie导数，求解导数，得到h₁与h₂的一阶李导数与和关于状态变量x，y的梯度：

其中，与分别为h₁(t)和h₂(t)的一阶李导数；与分别为对和的梯度；

由步骤8选取的量测函数，并通过步骤9与步骤10得到的h₁与h₂的零阶李导数与一阶李导数，获得系统的可观测矩阵Obs1；

将主从AUV的运行状态转换到极坐标下，得到步骤12中转换后的相对位置关系：

(x₁,y₁)→(0,0)，(x₂,y₂)→(d,0)，(x,y)→(rcosθ,rsinθ)

其中，d为主AUV间基线距离，r为主AUV1和从AUV间距离矢量r₁(t)在水平面的投影，θ为投影r与主AUV基线正的夹角，量测时选取逆时针方向为正，β为距离矢量r₁(t)和r₂(t)的在水平面的投影夹角；

将步骤11得到的可观测矩阵Obs1，经过步骤12的坐标转换，得到可观测矩阵Obs2：

由上式可得协同导航系统的可观测度为