[发明专利]一种无人艇组合定位定向方法和系统

权利要求书

1.一种无人艇组合定位定向方法，其特征在于，包括：

步骤1，根据无人艇使用环境和传感器性能特点，确定基于MEMS+差分导航+多天线测姿+激光雷达多传感器信息的组合定位定向系统的配置；

步骤2，根据步骤1的配置结果，建立组合定位定向系统融合策略；

步骤3，根据步骤2建立的组合定位定向系统融合策略，对激光雷达数据进行预处理；

步骤4，根据步骤3输出的激光雷达数据预处理结果，采用相对定位随机采样一致算法，解算得到无人艇与障碍物之间的相对距离；

步骤5，根据步骤4输出的无人艇与障碍物之间的相对距离，通过观测矢量融合算法，解算得到无人艇的定位定向信息，并输出；

根据步骤4输出的无人艇与障碍物之间的相对距离，通过观测矢量融合算法，解算得到无人艇的定位定向信息，并输出，包括：

获取速度量测信息Z＝[V_E,V_N]^T、第一天线的输出信息ZG＝[V_GE,V_GN]^T和第二天线的输出信息ZJ＝[V_JE,V_JN]^T；其中，V_E和V_N分别表示无人艇的东向速度和北向速度，V_GE和V_GN分别表示第一天线所在位置测得的东向速度和北向速度，V_JE和V_JN分别表示第二天线所在位置测得东向速度和北向速度；

取：

其中，δV_GE表示第一天线输出的东向速度误差，δV_GN表示第一天线输出的北向速度误差，δV_JE表示第二天线输出的东向速度误差，δV_JN表示第二天线输出的北向速度误差；

通过观测矢量融合算法，得到速度融合结果Z_融：

Z_融＝(R_G-1+R_J-1)^-1(R_G-1ZG+R_J-1ZJ)

其中，R_G-1表示第一天线速度误差方差阵，R_J-1表示第二天线速度误差方差阵；

在差分GPS有效的条件下，采用差分GPS得到的速度位置信息对通过捷联解算得到的速度位置信息进行替换，即得到无人艇的位置和速度信息：

其中：为无人艇位置的三维坐标值经捷联解算插值后结果，为无人艇速度的三维坐标值值经过捷联解算插值后结果，为无人艇接收机差分位置三维坐标值，为无人艇接收机差分速度三维值。

2.根据权利要求1所述的无人艇组合定位定向方法，其特征在于，根据步骤2建立的组合定位定向系统融合策略，对激光雷达数据进行预处理，包括：

从激光雷达数据提取得到点云的球面坐标；

根据提取得到的点云的球面坐标，对从激光雷达数据提取得到点云进行筛选，将筛选结果作为激光雷达数据预处理结果输出。

3.根据权利要求2所述的无人艇组合定位定向方法，其特征在于，根据提取得到的点云的球面坐标，对从激光雷达数据提取得到点云进行筛选，将筛选结果作为激光雷达数据预处理结果输出，包括：

根据提取得到的点云的球面坐标，对从激光雷达数据提取得到点云进行一次筛选，保留无人艇船采集到的正前方环境的点云，得到点云集合A；其中，正前方指无人艇的前进方向；

根据点云集合A中的各点云的球面坐标，对点云集合A中的点云进行二次筛选，过滤掉与无人艇之间的距离大于设定距离阈值的点云，得到点云集合B；

根据点云集合B中的各点云的球面坐标，对点云集合B中的点云进行三次筛选，过滤掉由无人艇激光雷达周围三根天线构成的点云，得到点云集合C；

根据点云集合C中的各点云的球面坐标，对点云集合C中的点云进行四次筛选，仅保留满足阈值上限和阈值下限的点云，得到点云集合D；

将点云集合D作为激光雷达数据预处理结果输出。