[发明专利]一种无人船水质检测作业路径的双雷达精准定位方法

权利要求书

1.一种无人船水质检测作业路径的双雷达精准定位方法，其特征在于，该方法包括如下步骤：

步骤1)设定雷达观测点的几何位置,实时获取双雷达对无人船的距离及径向速度数据；

步骤2)建立无人船位置状态及双雷达信息互享的观测方程；

步骤3)基于集合卡尔曼滤波思想，递推求解双雷达间信息互享的无人船运动轨迹，对无人船在水质检测作业中的路径高精度定位；所述步骤1)依据渔业养殖场的实际物理尺寸及其地理分布特点，采用南北向的矩形对渔业养殖场进行优化匹配拟合，将矩形的东西向的中心点及南北向的中心点设定为双毫米波调频连续波雷达的观测点；所述步骤1)中通过串口协议对雷达输出的距离及径向速度数据进行实时获取采集；所述步骤2)中建立无人船位置状态及双雷达信息互享的观测方程包括如下步骤：

步骤2-1)采用笛卡尔两维坐标系统，定义矩形的东西向为x轴，南北向为y轴，设定无人船位置状态为距离与速度的向量，采样时间间隔为Δt，在n时刻，无人船的位置状态方程为s_n＝Fs_n-1+w_n；式中状态转移矩阵F为：

噪声为w_n＝[w_n，1，w_n，2，w_n，3，w_n，4]，w_n的均值为零，协方差矩阵Q为：

步骤2-2)设定第一部雷达观测得到无人船的东西向的距离及径向速度为x，第二部雷达观测得到无人船的南北向的距离及径向速度为y，设定在n时刻，第i个雷达的观测方程为：

y_i，n＝H_is_n+v_i，n

式中，i取1或2，观测矩阵H为：

噪声为v_i，n＝[v_i，n，1，v_i，n，2，v_i，n，3，v_i，n，4],其均值为零，协方差矩阵为：

其中，表示第i个观测噪声的方差。

2.根据权利要求1所述的方法，其特征在于，所述步骤3)中具体包括如下步骤：

步骤3-1)设定两雷达的联合观测方程为：

y_n＝Hs_n+v_n

式中，观测值观测矩阵为噪声的协方差矩阵为R_1，n、R_2，n分别为第一个及第二个雷达观测的协方差矩阵；步骤3-2)基于序列贝叶斯估计，可以得到转移概率分布p(s_n|s_n-1)、似然概率分布p(y_n|s_n)及后验概率p(s_n|Y_n)分别为

式中Y_n＝{y₁，y₂，…，y_n},是一个均值为m，协方差矩阵为P的高斯分布函数，下标n|n表示利用时刻n的观测数据估计时刻n的状态值，均值m_n|n及P_n|n的迭代计算过程为：

m_n|n-1＝Fm_n-1|n-1

P_n|n-1＝FP_n-1|n-1F^T+Q_n

上式中，T表示矩阵装置操作，F为状态方程中的状态转移矩阵；

步骤3-3)通过上述推导可得到无人船水质检测作业路径的轨迹估计值为