《基于三维点云数据的工件识别实验探究》14000字（论文）

第4章点云配准算法4.1点云配准的刚体变换理论在工业现场，工业机器人在完成工件识别后，还需进行工件抓取，因此需要将经过处理的场景点云和工件的模型点云进行配准，获取工件在实际场景中的六自由度位姿形态。两坐标系下的点云配准可以通过固定模型坐标系，将相机坐标系下的点云通过旋转和平移转换到模型坐标系下使两片点云完成重合，这实际上是一个刚体变换的过程。4.1.1旋转变换和平移变换定义：模型点云；场景点云；其中和表示三维空间中点云和中的数据点，点云，点云可用的矩阵来表示，则场景点云可通过平移和旋转与模型点云重合，记平移矩阵为，旋转矩阵为，则两片点云之间的刚体变换可用式（4-1）来表示： （4-1）下面用四阶矩阵形式进行数学表示：记场景点云中一点坐标，平移矩阵，点经平移变换后的坐标为，则平移变换可用式（4-2）表示： （4-2）记点绕x轴旋转，绕y轴旋转，绕z轴旋转变换后的坐标为，则旋转变换可用式（4-3）和式（4-4）表示： （4-3） （4-4）综上所述，刚体变换可用式（4-5）描述： （4-5）其中为旋转矩阵，为平移矩阵，为矩阵。4.1.2刚体变换矩阵的求解在确定点云和后，点云的配准过程实际上是求解两片点云的刚体变换矩阵，即求解旋转矩阵和平移矩阵的过程，目前一般采用奇异值分解法[71-76]或四元数法来求解。奇异值分解法定义模型点云，场景点云首先求取两片点云的质心： （4-6）所要进行最小化的目标函数为： （4-7）其中代表从点云Q中寻找与欧氏距离最近的点，其中使误差函数达到最小的旋转矩阵和位移矩阵即为最优解。令，当时，误差函数达到最小。记： （4-8）此时的误差函数为： （4-9）只需令误差函数的负数项达到最小。设，要让达到最小，则可以计算最优旋转矩阵： （4-10） （4-11）可以计算最优位移矩阵： （4-12）四元数法四元数法是一种应用于三维空间旋转计算的方法，表达式如式（4-13）：

（4-13）其中是实数单位，是虚数单位。在点云配准过程中，一般通过矩阵求取、特征分解等方法求取两片点云作刚体变换的四元数表达式，从而求取旋转矩阵和平移矩阵，这种方法稳定性和计算效率都较高。四元数法的计算步骤如下，定义：模型点云，场景点云(1)求取两片点云的质心： （4-14）(2)将两片点云做相对质心平移： （4-15）得到移动后的点集：，(3)利用移动后的点集构造协方差矩阵：

（4-16）(4)将中元素组合构建四维对称矩阵： （4-17）对称矩阵的特征向量即为四元数的四个分量： （4-18）可以由四元数计算得到旋转矩阵和平移矩阵： （4-19） （4-20）4.2基于SAC-IA算法的点云粗配准在进行精配准之间，需要先对两片点云进行粗配准，初步估计旋转矩阵和平移矩阵。传统的贪婪算法计算效率较低且容易陷入局部最优解。目前主流的粗配准算法包括采样一致性配准算法（SAC-IA）、Super4PCS算法、3D-NDT算法等，本文着重介绍SAC-IA算法。SAC-IA算法是一种基于局部特征描述符的粗配准算法，这种方法基于FPFH描述符，具有较好的描述性和抗干扰性，对初始位姿偏差较大的点云也能够有较好的粗配准效果。需要计算点云的FPFH，采用采样一致性思想，通过大量的点对应关系来寻找误差最小的矩阵，认为这个矩阵就是最佳变换矩阵。SAC-IA算法步骤如下：（1）采样。样本点尽量要求FPFH特征分散，要求样本点的距离大于最小值。（2）将采样点的FPFH特征和模型点云的FPFH特征进行比对，将距离相近的点作为匹配点对存入列表中。（3）利用上述的匹配点对进行初步的刚体变换矩阵计算，通过Huber惩罚函数来描述配准误差，惩罚函数计算公式如式（4-21）： （4-21）其中为给定的阈值，表示第对的变换距离误差。（4）重复（1）-（3）操作，选取误差和最小的变换矩阵，作为粗配准的最佳变换矩阵。由于计算FPFH特征在点云数据量较大的情况下效率较低，一般在进行SAC-IA粗配准之前需要进行点云的降采样，本文在第2章已经对点云进行精简处理。SAC-IA算法流程图如图4.1所示：图4.1SAC-IA算法流程图4.3基于ICP算法的点云精配准通过粗配准后的两片点云已经具备了较好的初始形态，但是通过粗配准计算得到的旋转变换矩阵数据还不够精确，因此需要对待配准点云进行精配准，获得更加精确的物体姿态。迭代最近点算法(IterativeClosestPoint，ICP)是点云精配准领域中一种最常见的算法。ICP算法事实上是一个基于最小二乘法的匹配最优解问题，通过确立一种正确匹配的收敛准则，反复进行点集的确定和最优刚体变换的计算过程，当收敛准则满足则可认为达到解最优。这种算法精确度高，无需对点云集进行分割提取处理，且在初始形态较好的情况下能够达到极高的收敛精度。但是ICP法也具有一定的缺陷，这种方法需要遍历点云集中所有的点都进行欧氏距离的迭代计算，针对阈值较为集中的点需要进行多次迭代，这会造成极大的计算量。其次ICP法所定义的目标函数存在一定的局限性，算法认为欧氏距离最近的点即为对应点，在目标点云初始位置不理想的情况下使用ICP法，容易产生点与点之间的错误对应和局部最优解，造成配准的错误。ICP算法通过寻找距离误差最小的刚体变换矩阵来使两片点云达到理想的匹配效果[77-81]。这种算法的基本思路是在两片点云下寻找欧氏距离最近的点，利用所形成的点集计算待配准点云数据和参考点云数据之间的旋转矩阵和位移矩阵，通过定义误差函数和收敛阈值对点集数据进行迭代计算，当误差函数值小于阈值时停止计算，即可输出最优解和。不妨设场景点云为，模板点云为，求解点云之间变换矩阵的问题事实上就是基于最小二乘法的最优解匹配问题。在没有误差的情况下，从P坐标系转化到Q的公式为： （4-22）所要进行最小化的目标函数为： （4-23）其中代表从点云Q中寻找与欧氏距离最近的点。其中使误差函数达到最小的旋转矩阵和位移矩阵即为最优解。ICP算法步骤如下：（1）记粗配准后的变换点集为，记参考点集为，将和作为算法的原始点集。（2）采样。计算中每一个点在中欧氏距离最小的点作为对应点，建立对应点集。（3）计算变换矩阵。计算关于上述对应点集距离最小的刚体变换旋转矩阵和位移矩阵。（4）计算误差函数。定义匹配点对的均方误差函数：将刚体变换矩阵和代入计算，要求均方误差函数的值达到最小。（5）使用变换矩阵对点集进行更新。公式为： （4-24）（6）迭代计算。规定点云距离阈值和最大迭代次数，迭代计算每一次经过刚体变换后的点云和模型点云之间的距离误差。当均方误差小于距离阈值或迭代次数大于时，循环结束。每一步刚体变换的更新步骤如下：（1）在第次刚体变换后建立点云的对应关系： （4-25）（2）基于（1）中的对应关系，利用均方误差最小来计算新的刚体变换和： （4-26）（3）更新第k步的刚体变换： （4-27）ICP算法流程图如图4.2所示：图4.2ICP算法流程图4.4本章小结（1）本章主要介绍了基于点云配准的刚体变换理论，介绍了计算点云旋转变换矩阵和平移变换矩阵的奇异值分解法和四元数法。（2）介绍了一种基于采样性一致算法的点云粗配准算法，并使用ICP算法进一步对点云进行精配准。第5章实验与分析5.1实验场景为了验证算法的可行性，本文搭建了如图5.1所示的实验平台进行实验。本文选取的研究对象是箱体类机械工件，图中已用红色圆形圈出来，工作台上有两个工件，本文的研究对象是①号工件，因此在实验过程中首先需要将环境中的无效数据滤除，并将工件与工作台、工件与工件分割开来，获得独立的①号工件的点云数据。图5.1实验场景5.2点云的预处理获取的点云数据如图5.2所示，从点云的分布情况来看，目标工件点云在Z轴的最下面，沿着Z轴可以将点云分为几个部分，将不同部分用不同的颜色标出，其中工件部分用蓝色标记。沿着Z轴对点云进行统计，得到一个数组，保存为CloudStatics.csv文件，绘制后如图5.3所示。由于物体的顶面点云比较完整，物体侧面基本没有点云，可以从Z轴的最低点，根据固定的间隔到Z轴的最高点，去统计每个间隔范围内的点云的数量，本文设定点云数量阈值为200个，即第一次超过200个点云数据的节点开始寻找下一个波谷，在低于200个点云数据的位置即是分割节点。通过直通滤波，即可将无效的点云数据滤除，获取位于工作台上的两个点云数据。图5.2场景点云初沿z轴分布图图5.3沿z轴点云数量分布图经过滤波后的点云如图5.4、图5.5所示，两张图表示的是两个不同的视角观测得到的点云数据，其中绿色的表示的是模型点云，红色的是经过过滤后得到的工件的场景点云。图5.4过滤后的模型点云和场景点云图5.5过滤后的模型点云和场景点云在滤除初始点云数据中的无效点后，目前已经获得较为完整的工件点云数据，下一步需要将两个工件进行分割，获得目标工件点云数据。本文采用一种基于欧式距离的分割算法，将距离阈值范围内的点进行聚类，达到分割的效果。本文设置搜索近邻半径为2厘米，聚类所需最少点数目占比为20%，聚类所需最多点数目占比为100%，分割效果如图5.6、图5.7所示，其中图5.6代表目标工件点云，图5.7代表干扰工件点云。图5.6分割得到的目标工件点云数据图5.7分割得到的干扰工件点云数据在上一阶段，通过对场景点云数据的滤波和分割，已经获得了目标工件点云数据，现在需要对工件的模型点云和场景点云进行体素下采样，在保留点云几何特征的同时精简冗余数据，提高后续法线特征计算以及FPFH描述符构建的计算效率。本文选取体素栅格边长为0.05厘米，对工件点云进行下采样，图5.8表示模板点云下采样效果图，图5.9表示目标点云下采样效果图。图5.8模板工件点云下采样效果图图5.9目标工件点云下采样经过点云下采样，模板工件、目标工件点云数据在保留几何特征的同时大幅精简了数据，提高了后续计算的效率。经过下采样后的点云数据量如表5.1所示。表5.1下采样后的点云数据量点云类别点云数据量（个）模板点云750目标点云10025.3点云的几何特征提取和FPFH描述符构建本节首先对模板点云进行法线特征提取。在邻域半径选取过程中，如果邻域范围过大，将无法描述点云表面细节，容易造成法线失真。如果邻域范围过小，容易受噪声影响，计算效率将会降低。本文选取点云球形邻域半径为1厘米，能够有较好的法向量提取效果。如图5.10、图5.11所示，这两张图是不同视角下模板点云的法向量。图5.10模板点云法向量示意图图5.11模板点云法向量示意图同样的，对目标工件点云进行法线特征提取，如图5.12、图5.13所示，这两张图是不同视角下目标工件点云的法向量。图5.12目标工件点云法向量示意图图5.13目标工件点云法向量示意图在提取得到法向量后，设定邻域半径为0.5厘米，利用PCL库计算模型点云和目标弓箭工件点云的FPFH特征描述符，并将其可视化，得到图5.14，其中黑色折线代表的是模型点云的FPFH，红色折线代表的是目标工件点云的FPFH。图5.14模板工件点云和目标工件点云FPFH本节模板点云和目标点云的法向量和FPFH特征点数量如表5.2所示。表5.2模板点云和目标点云的法向量和FPFH特征点数量数据种类数量（个）模板点云法向量数量750模板点云FPFH特征点数量750目标点云法向量数量1002目标点云FPFH特征点数量10025.4点云的位姿估计在提取模板工件点云和目标工件点云的FPFH特征后，如图5.14所示，在同一张直方图中将二者的FPFH特征进行比对，发现二者的FPFH特征相似度很高，因此本节将针对二者的特征点使用SAC-IA算法进行初步配准，随后使用ICP算法进行进一步的精配准。5.4.1SAC-IA算法粗配准在粗配准中，使用PCL库默认的参数，设置样本之间的最小距离为1厘米，每次迭代计算使用的样本数量为2，计算协方差所选择的近邻点个数为20，对模型点云和目标点云进行粗配准。根据式（4-10）、式（4-12），SAC-IA算法进行粗配准得到的齐次坐标变换矩阵如图5.15所示。粗配准效果图如图5.16、图5.17所示。其中绿色的是模板点云，红色的是目标点云，蓝色的是模板点云在粗配准后，根据SAC-IA得到的坐标变换矩阵进行坐标变换后的结果，两张图是不同视角下的同一个结果。图5.15SAC-IA算法计算得到的坐标变换矩阵图5.16SAC-IA粗配准效果图图5.17SAC-IA粗配准效果图5.4.2ICP算法精配准粗配准过程已经求解得到了一个初始的刚体变换矩阵，这能大幅提高ICP算法的计算效率。使用ICP算法进行精配准过程，本章设定参数如下：点云配准后的最大距离为4厘米，最大迭代次数为20次，两次变化矩阵之间的差值为10-10,结束的均方误差条件为0.1。ICP算法进行精配准得到的变换矩阵如图5.18所示。图5.18ICP算法得到的坐标变换矩阵精配准效果图如图5.19所示。其中绿色的代表模板点云，红色的代表目标工件点云，黄色的代表模板点云根据精配准计算得到的坐标变换矩阵进行坐标变换后得到的点云。从图中可以看出，点云之间已经达到较好的配准效果。图5.19ICP精配准效果图5.5本章小结本章基于放置于工作平台上的箱体类零件展开目标识别和位姿估计。5.1节根据点云分布使用滤波技术获取工件点云数据，并使用欧式聚类方法对两团工件点云进行分割，获得目标工件点云数据。5.2节针对点云数据量庞杂这个问题，本章采用了一种基于体素栅格的下采样方法，在保留点云几何特征的同时精简了点云数据。5.3节进行点云的法线特征计算并构建点云的FPFH描述符，提高点对的对应正确率。5.4节对模型点云和目标点云的FPFH特征进行比对，并使用SAC-IA算法进行粗配准，并使用ICP算法进行精配准，获得较好的配准结果。

第6章总结与展望6.1工作总结本文在综合分析了基于三维点云的目标识别和位姿估计的国内外研究现状的基础上，提出了一种基于点云法线特征和FPFH描述符的点云配准方法，使用SAC-IA算法和ICP算法相结合的方法对点云进行配准，获得了点云的工件类别和位姿信息，获得较好的配准效果。本文的具体研究工作总结如下：（1）总结了基于三维点云的目标识别和位姿估计研究现状及发展趋势，并对点云的预处理、识别和配准等基础理论进行介绍。本文首先基于三维点云的识别和配准研究现状，阐述了研究的背景和意义，并介绍了点云的滤波技术、基于欧式聚类的分割算法、基于体素栅格的下采样方法、点云的邻域搜索理论、点云配准的刚体变换理论等。（2）提出一种基于点云表面法线和FPFH特征的配准方法。本文提出一种基于最小二乘拟合平面的表面法线计算方法，并对关键点使用FPFH作为点云的局部几何特征描述，对模型点云和目标点云进行FPFH特征比对确定对应点集，有效地提高了点云识别的计算效率。(3)使用SAC-IA算法进行粗配准，并使用ICP算法进行精配准。本文通过对两团点云特征点FPFH特征的比对，使用SAC-IA算法计算得到初步的刚体变换矩阵，并使用ICP算法获得最终的刚体变换矩阵，得出最终的位姿结果，实验结果显示配准效果较好。6.2研究展望本文提出一种基于点云表面法线和FPFH特征的点云识别配准方法，使用SAC-IA算法和ICP算法结合的方式对点云进行配准，取得了较好的效果。但是本课题以下方面仍有待进一步研究：（1）增加算法的通用性。本文只对箱体类零件的点云数据进行目标识别和位姿估计，还未对其他类型的工件进行测试，后续可以针对不同种类的工件进行实验，增加算法的通用性。（2）增加关于工件抓取方面的研究。本文只完成了对目标工件的点云识别和位姿估计，得出了两片点云之间的旋转平移矩阵，但是没有涉及到关于工件抓取的研究，后续可以增加关于工件抓取和机器人移动方面的研究。参考文献[1]隋婧,金伟其.双目立体视觉技术的实现及其进展[J].电子技术应用,2004,30(010):4-6.[2]DavidMarr,Vision:AcomputationalInvestigationintothehumanrepresentationandprocessingofvisualinformation,W.H.FreemanandCompany,1982.[3]ChenX,HuangY,ChenS.Modelanalysisandexperimentaltechniqueoncomputingaccuracyofseamspatialpositioninformationbasedonstereovisionforweldingrobot[J].IndustrialRobot,2012,39(4):192–204.[4]FangY,MasakiI,HornB.Depth-BasedTargetSegmentationforIntelligentVehicles:FusionofRadarandBinocularStereo[J].IEEETransactionsonIntelligentTransportationSystems,2002,3(3):196-202.[5]OhJK,LeeCH.Developmentofastereovisionsystemforindustrialrobots[C].InternationalConferenceonControl.2007.[6]任慧娟,金守峰,程云飞,等.面向分拣机器人的多目标视觉识别定位方法[J].机械与电子,2019,037(012):64-68.[7]Salehian,Behzad,Fotouhi,etal.Dynamicprogramming-baseddensestereomatchingimprovementusinganefficientsearchspacereductiontechnique[J].Optik:ZeitschriftfurLicht-undElektronenoptik:=JournalforLight-andElectronoptic,2018,160:1-12.[8]H.Hirschmuller,StereoProcessingbySemiglobalMatchingandMutualInformation[J].inIEEETransactionsonPatternAnalysisandMachineIntelligence,vol.30,no.2,pp.328-341,Feb.2008.[9]龚文.基于动态规划的立体匹配算法研究[D].南昌航空大学,2015.[10]Abdellali,H.,Kato,Z.3Dreconstructionwithdepthpriorusinggraph-cut[J].CentEurJOperRes29,387–402(2021).[11]K.Zhang,Y.Fang,D.Min,etal.Cross-ScaleCostAggregationforStereoMatching[C].2014IEEEConferenceonComputerVisionandPatternRecognition(CVPR),Columbus,America,2014.[12]A.Klaus,M.SormannandK.Karner,Segment-BasedStereoMatchingUsingBeliefPropagationandaSelf-AdaptingDissimilarityMeasure[C],18thInternationalConferenceonPatternRecognition(ICPR'06),2006,pp.15-18,[13]刘振.基于结构光的双目立体成像技术研究[D].中国科学院大学,2013.[14]张洪龙.基于结构光的室内场景精确三维重建技术研究[D].深圳:中国科学院深圳先进技术研究院,2018.[15]MaS,ShenY,QianJ,etal.Binocularstructuredlightstereomatchingapproachfordensefacialdisparitymap[J].AI2011:AdvancesinArtificialIntelligence,2011:550-559.[16]CarrihillB,HummelR.Experimentswiththeintensityratiodepthsensor[J].Cvgip,1985,32(3):337-358.[17]BrunoF,BiancoG,MuzzupappaM,etal.Experimentationofstructuredlightandstereovisionforunderwater3Dreconstruction[J].ISPRSJournalofPhotogrammetryandRemoteSensing,2011,66(4):508-518.[18]BickelmannC,MorrowJM,DuJ,etal.Themolecularoriginandevolutionofdim-lightvisioninmammals[J].Evolution,2015,69(11):2995.[19]KazhdanM,HoppeH.ACMtransactionsongraphics[M].AssociationforComputingMachinery,2005:1-11.[20]曲学军,李海固,樊龙欣，etal.基于组合编码的条纹结构光主动立体视觉匹配[J].计算机测量与控制,2014,22(011):3712-3714.[21]刘子伟，许廷发，王洪庆，深度成像理论与实现[J].红外与激光工程，2016，45(7):242-246.[22]PrusakA,MelnychukO,RothH,etal.PoseestimationandmapbuildingwithaTime-Of-Flightcameraforrobotnavigation[J].InternationalJournalofIntelligentSystemsTechnologiesandApplications,2008,5(3/4):355.[23]JohnS.Massa,GeraldS.Buller,AndrewC.Walker,etal.Time-of-flightopticalrangingsystembasedontime-correlatedsingle-photoncounting[J].AppliedOptics,1998,37(31):7298-304.[24]LindnerM,SchillerI,KolbA,etal.Time-of-Flightsensorcalibrationforaccuraterangesensing[J].ComputerVision&ImageUnderstanding,2010,114(12):1318-1328.[25]S.Chua,X.Wang,N.Guo,etal.Improvingthree-dimensional(3D)rangegatedreconstructionthroughtime-of-flight(TOF)imaginganalysis[J].JournaloftheEuropeanOpticalSocietyRapidPublications,2016(11):101-123.[26]HussmannS,LiepertT.RobotVisionSystembasedona3D-TOFCamera[C].Instrumentation&MeasurementTechnologyConference.IEEE,2007.[27]FalieD,BuzuloiuV.DistanceerrorscorrectionfortheTimeofFlight(ToF)cameras[C]//IEEEInternationalWorkshoponImagingSystems&Techniques.IEEE,2008,pp:123-126.[28]杨晶晶，冯文刚.连续调制TOF图像误差来源及降噪处理[J].合肥工业大学学报(自然科学版),2012,35(04):485-488.[29]SotoodehS,ZurichE,Hierarchicalclusteredoutlierdetectioninlaserscannerpointclouds[C].36thISPRS,2007.[30]CLARENZU,DROSKED,HENNS.Computationalmethodsfornonlinearimageregistration[C].MathematicalModelsforRegistrationandApplicationstoMedicalImagingMathematicsinIndustry,2006:81-101.[31]CHOUDHURYP,TUMBLINJ.Thetrilateralfilterforhighcontrastimagesandmeshes[C].EurographicsSymposiumonRendering,2003:186-196.[32]赵灿,汤春瑞,刘丹丹.基于表面波变换的散乱点云去噪方法[J].组合机床与自动化加工技术,2009(2):31-34.[33]CHENYH,NGCT,WANGYZ.Datareductioninintegratedreverseengineeringandrapidprototyping[J].InternationalJournalofComputerIntegratedManufacturing,1999,12(2):97-103[34]KakadiarisIA,TodericiG,EvangelopoulosG,etal.3D-2DFaceRecognitionwithPoseandIlluminationNormalization[J].ComputerVisionandImageUnderstanding,2016,154:137-151.[35]RabbaniT,HeuvelFAVD,VosselmanG.Segmentationofpointcloudsusingsmoothnessconstraint[C]//InternationalArchivesofPhotogrammetry,RemoteSensingandSpatialInformationSciences.2006.[36]FischlerMA,BollesRC.Randomsampleconsensus:aparadigmformodelfittingwithapplicationstoimageanalysisandautomatedcartography[J].CommunicationoftheACM，1981,24（60）：381-395.[37]李宝，程志全，党岗，等.一种基于RANSAC的点云特征线提取算法[J]．计算机工程与科学，2013，35（02）：147-153.[38]LariZ,HabibA.Anadaptiveapproachforthesegmentationandextractionofplanarandlinear/cylindricalfeaturesfromlaserscanningdata[J].ISPRSJournalofPhotogrammetryandRemoteSensing,2014,93(11):192-212.[39]LigonJ,BeinD,LyP,etal.3Dpointcloudprocessingusingspinimagesforobjectdetection[C]//2018IEEE8thAnnualComputingandCommunicationWorkshopandConference(CCWC).IEEE,2018:731-736.[40]DengX,DaF,ShaoH.Efficient3DfacerecognitionusinglocalcovariancedescriptorandRiemanniankernelsparsecoding[J].Computers&ElectricalEngineering,2017,62:81-91.[41]GuoY,BennamounM,SohelF,LuM,WanJ.3Dobjectrecognitioninclutteredsceneswithlocalsurfacefeatures:asurvey[J].IEEETransactionsonPatternAnalysisandMachineIntelligence,2014,36(11):2270-2287.[42]JohnsonAE,HebertM.Usingspinimagesforefficientobjectrecognitionincluttered3Dscenes[J].IEEETransactiononPatternAnalysisandMachineIntelligence,1999,21(5):433-449.[43]FeldmarJ,AyacheN.Rigid,affineandlo