[发明专利]一种基于基线几何结构约束的水下航行器定位方法

摘要

本发明提供了一种基于基线几何结构约束的水下航行器定位方法，计算得到各个信标间的3D量测距离，计算两两信标间的距离，构造信标与水下航行器当前位置间的2D计算距离的等式约束条件与不等式约束条件，求得预测定位信息，建立水面信标与水下航行器间的相对距离量测方程，并通过求解约束优化问题得到水下航行器位置、航向及俯仰估计值。本发明实现了水下惯性/航位推算定位方法与水声定位方法相结合，确保了水下航行器长时间有效精确定位；使得声信标间的距离信息在定位过程中得到了有效利用；借助最大后验估计准则与扩展Kalman滤波，使得约束条件有效融入定位求解过程中，实现了水下航行器定位精度的提高。

主权项

1.一种基于基线几何结构约束的水下航行器定位方法，其特征在于包括下述步骤：步骤1，三个具有自定位功能的水声信标以固定时间间隔广播自身位置(x_i,y_i)，i＝1,2,3，水下航行器利用接受到的水声信号延迟时间Δt_i与区域水下平均声速的乘积，来计算得到各个信标间的3D量测距离利用所携带深度计精确测量水下航行器深度值h，将3D量测距离l_i转换为平面2D量测距离步骤2，已知信标位置，计算两两信标间的距离L_ij，即定位基线长度，j＝1,2,3且i≠j；利用定位基线间的几何结构关系，构造信标与水下航行器当前位置(x,y)间的2D计算距离的等式约束条件S(r_i²)＝0与不等式约束条件h(r_i)≥J(r_i)；步骤3，在第k‑1次定位结果基础上，水下航行器利用惯性传感器所测前向速度v_k‑1、航向角速度w_k‑1以及俯仰角速度υ_k‑1，设定各传感器定位采样周期为δ_t，从而求得第k次预测定位信息包括位置、航向角以及俯仰角，即有其对应于状态估计与控制输入u_k‑1＝[v_k‑1 w_k‑1 υ_k‑1]^T的雅可比矩阵和分别为：利用所接收到的三个水面声信标位置(x_i,k,y_i,k)以及测量得到的相对距离在距离量测误差为ε_k的基础上，建立水面信标与水下航行器间的相对距离量测方程其对应于预测信息的雅可比矩阵选择最大后验估计方法，实现等式约束条件与不等式约束条件h(r_i,k)≥J(r_i,)与相对距离相融合，并通过求解以下约束优化问题得到水下航行器位置、航向及俯仰估计值，即有s.t.Σ_k＝Diag(P_k/k‑1,R_k)h(r_i,k)≥J(r_i,k)i＝1,2,3其中，R_k为距离量测误差协方差，而状态估计协方差的预测值P_k/k‑1则借助于扩展Kalman滤波进行递推求解，即