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

摘要

本发明提供一种基于散斑的六自由度位姿估计系统及方法，本发明系统包括一个红外发射器，一个摄像头和进行算法处理的计算机，摄像头固定在三脚架上，红外发射器可手持，工作时，摄像机视野与红外发射器投射图像交叉；本发明方法包括：采集红外发射器投射的参考图像，结合连通域算法、哈希算法、投票法、区域增长算法、光束平差法、全景拼接算法恢复完整的红外发射器参考图像并为其构建LUT；采用RANSAC算法和Nister的五点位姿估计算法计算出本征矩阵E，得到位姿估计参数结果。本发明主要利用主动视觉的优点，结合连通域算法、哈希算法、投票法、区域增长算法、光束平差法、全景拼接算法、RANSAC算法和Nister的五点位姿估计算法，得到一个高效鲁棒的位姿估计结果。

主权项

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，得到位姿估计参数结果。