[发明专利]一种基于三维点云特征的船舶位姿估计方法

权利要求书

1.一种基于三维点云特征的船舶位姿估计方法，其特征在于：步骤如下：

步骤一：获取目标船点云数据，拼接完整点云并计算ISS3D特征点；

步骤二：计算ISS3D特征点的快速点特征直方图(FPFH)特征描述子，建立目标船点云模板库；

步骤三：基于SAC-IA的点云粗配准；

步骤四：基于ICP的点云精细配准及位姿估计；

利用精准坐标变换矩阵与点云模板库中船舶的初始位姿信息，完成对目标船进行六自由度的位姿估计。

2.根据权利要求1所述的一种基于三维点云特征的船舶位姿估计方法，其特征在于：步骤一具体包括：采用点云拼接方法获取船舶的完整点云，从前、后、左、右、上、下6个方向获得船舶点云数据，再将点云拼接得到船舶完整点云数据，对每一种船舶的点云提取ISS3D特征点p_f＝[x,y,z]，具体提取方法如下所示：

步骤1.1：对点云中每个点p_i都设置一个局部坐标系，并设置搜索半径r；

步骤1.2：获取点云每个点p_i在半径r内的所有点，权重计算公式如下所示：

步骤1.3：计算各点p_i的协方差矩阵，如下所示：

步骤1.4：对各点p_i的协方差矩阵进行特征值求解，并从大到小排序，得到

步骤1.5：通过特征值判断是否为关键点，选择阈值ε₁,ε₂，当且时，将这个点判断为角点关键点。

3.根据权利要求2所述的一种基于三维点云特征的船舶位姿估计方法，其特征在于：步骤二具体包括：

步骤2.1：对每一个特征点p_q，寻找与它距离小于r的邻域点p_ki，计算特征点与邻域点之间的三元组(α,φ,θ)；

步骤2.2：对于所有与特征点p_q距离小于r的邻域点p_ki，都按照上一步骤的方法，以这些邻域点p_ki为中心搜索距离小于r的邻域点，计算中心点与邻域点的三元组(α,φ,θ)；

步骤2.3：将特征点p_q的三元组(α,φ,θ)与加权的邻域点p_ki的三元组(α,φ,θ)进行统计，得到最终的快速点特征直方图描述子为：

上式中ω_ki为权重值，其与关键点p_q与邻域点p_ki之间距离有关；

将ISS3D特征点p_f＝[x,y,z]与快速点特征直方图特征描述子V_FPFH合并为一个特征向量E_i＝[x,y,z,V_FPFH]，以此来描述一个特征点，每种类型的船舶点云用这些特征向量的集合进行描述M_j＝[E₁ E₂ … E_n]，并将它们存在点云模板数据库中；最终获得所有类型船舶的点云特征模板库[M₁ M₂ … M_n]。