[发明专利]一种基于散斑的六自由度位姿估计系统及方法

权利要求书

1.一种基于散斑的六自由度位姿估计方法，其特征在于，包括以下步骤：

步骤S1：采集红外发射器投射的参考图像，将散斑图像投射到平面上，由摄像机按5x5矩阵每次观察到整幅图像的1/9大小的规定捕捉，得到25张捕捉的部分与前一部分或几部分有重叠的序列图；

步骤S2：恢复完整的红外发射器参考图像并为其构建LUT；

所述步骤S2中的具体过程如下：

步骤S21：对采集到的红外发射器局部的参考图像进行滤波去噪、对比度归一化预处理，使用连通域算法检测出局部的参考图像中散斑的中心点，得到中心点的坐标；

步骤S22：对所述检测出的中心点采用Delaunay三角剖分，将相邻的Delaunay三角形组成的四边形定义为一组quad，将一组quad的尾点作为定义quad的不变量特征，定义为一组kite，即{k₁,k₂,k₃,k₄,k₅}，其中k_1...4为quad的四个点，k₅为一系列尾点；

步骤S23：采用单应性映射将所有的quad映射到单位矩形坐标{(0,0),(0,1),(1,1),(1,0)}下，得到一个单应性矩阵H，采用所述单应性矩阵H映射所有的尾点，得到所述所有的尾点在单位矩形坐标下的尾点k′₅；

步骤S24：将所有单位矩形坐标下的点集映射后的每一组kite采用哈希算法建立LUT；

步骤S25：对拍摄的25张序列图使用投票法和区域增长算法进行匹配，找到quad之间的匹配点从而找到点集之间的匹配，利用匹配点计算得到每个姿态j下的单应性矩阵H_j，结合匹配的点集采用光束平差法进行优化，采用Levenberg-Marquardt求解目标函数，获得相机的内参、相机相对目标平面的位姿，使用所述单应性矩阵H_j和匹配的点集，采用全景拼接算法得到完整的红外发射器参考图像并为其建立LUT；

步骤S3：所述摄像机实时拍摄目标物体，采用所述步骤S21、步骤S22和步骤S23处理摄相机采集画面，采用所述步骤S25中的匹配算法获得匹配关系，采用RANSAC算法和Nister的五点位姿估计算法计算出本征矩阵E，从中提取出旋转矩阵R和平移向量T，得到位姿估计参数结果。

2.根据权利要求1所述的基于散斑的六自由度位姿估计方法，其特征在于，所述步骤S2中还包括对于新加入的图像，执行步骤S21、步骤S22和步骤S23，对于映射到单位矩形坐标下的k′₅，在步骤S24中建立好的LUT中查找，通过投票法选择匹配度最高的两个quad的步骤。

3.根据权利要求1或2所述的基于散斑的六自由度位姿估计方法，其特征在于，所述步骤S22中的所述尾点为距离四个点最近的邻点，所述邻点为一条边或两条边与四个顶点之一相连的点。

4.根据权利要求1或2所述的基于散斑的六自由度位姿估计方法，其特征在于，所述步骤S23中还包括在映射quad和尾点之前，加入σ＝0.25的高斯噪声模拟环境噪声和突变的步骤。

5.根据权利要求1所述的基于散斑的六自由度位姿估计方法，其特征在于，所述LUT为一个二维的阵列，其中第一维为quad的索引，第二维为quad相关的直方图，直方图为所述quad对应的所有尾点k′₅所在的单位矩形坐标下的直方图的总和。