一种基于双目视觉的并联机构末端运动位姿检测方法与流程

本发明涉及基于机器视觉的位姿检测系统，尤其涉及基于双目视觉、针对并联机构末端运动位姿的检测方法。

背景技术：

在并联机构控制中，末端运动位姿是反映机构运动状态的重要参数，精确测得机构的末端运动位姿可有效避免通过运动学模型解算所带来的误差，这将利于实现并联机构的高性能控制。相对于其它检测手段，机器视觉具有非接触、适用性强、高性价比等优点，尤其适用于具有运动多自由度、运动轨迹复杂、难以直接检测的并联机构末端运动位姿检测。视觉位姿检测系统通常可分为单目、双目以及多目检测系统。由于双目视觉算法的鲁棒性和精确性优于单目算法，而其算法的实时性相对于多目视觉又具有较大的优势，因此，双目视觉得到了较为广泛的应用。对于并联机构的末端运动位姿检测问题，采用基于双目视觉检测方法的难点在于如何将视觉系统采集到的并联机构图像经过图像处理后快速而精确地获取该机构的末端位姿信息。在双目视觉位姿检测过程中，立体匹配过程最为重要，匹配的速度与精度直接决定整个位姿检测系统的速度与精度，但由于双目视觉算法复杂，光照变化、背景反光、噪声干扰等外界因素会引起双目视觉系统的图像匹配难以精确实现，导致视觉检测系统的速度和精度受到影响。

文献《一种新型的并联机器人位姿立体视觉检测系统》(吴迪飞，丁永生等.计算机工程与应用，2007，43(33):190-192)建立了一种基于尺度不变特征变换(SIFT)的双目视觉检测系统对并联机器人进行位姿检测，并用仿真实验证明了该检测算法的鲁棒性和可行性，但由于该检测系统匹配过程所采用的SIFT算法会产生大量维数较大的特征向量，增大了算法的复杂度，导致位姿检测的实时性受到影响；文献《Harris-SIFT算法及其在双目立体视觉中的应用》(赵钦君，赵东标等.电子科技大学学报，2010，39(4):546-550)结合Harris显著性和SIFT描述子，提出一种Harris-SIFT算法运用于双目视觉匹配中，并实验验证了方法的有效性，该方法有效提高了匹配算法的实时性，但由于Harris检测出的角点所含信息少于SIFT检测特征点所含信息，Harris-SIFT算法存在错误匹配和误差匹配的问题。

针对Harris-SIFT算法存在的误匹配问题，可考虑采用提纯算法对匹配结果进行提纯。常规提纯算法通常根据一组包含异常点的数据集，计算出匹配点对的目标模型，再利用剩余点来检验模型，从而得到有效数据样本，因此，提纯算法的关键在于快速找到正确的目标模型。然而，目标模型的获取通常存在一些问题：比如选取随机样本时，存在两个候选点距离过近而被认为是一个点，从而导致求取的目标模型不准确的问题；而且每次挑选随机样本集，都要寻找其对应候选模型的支撑集，这将导致花费过多时间寻找存在较多误差观测数据集的对应支撑点集上。

技术实现要素：

鉴于上述双目视觉检测存在的问题，本发明提出一种基于双目视觉的并联机构末端运动位姿检测方法，该方法提出了一种新的匹配算法，算法通过快速Harris-SIFT算法实现图像特征点匹配，并提出一种新的提纯算法对匹配结果进行提纯，从而解决传统双目视觉算法复杂，空间匹配难以精确实现，所导致的检测实时性差、检测精度不高的问题。

本发明采用的技术方案是采用如下步骤：

一种基于双目视觉的并联机构末端运动位姿检测方法，包括如下步骤：

步骤1)利用双目相机采集并联机构的原始图像，并对采集到的机构图像进行基于小波变换的图像去噪预处理；

步骤2)采用基于Harris算子的角点提取法对机构图像特征进行提取；

步骤3)采用Harris-SIFT算法对并联机构图像进行特征匹配；该匹配算法首先通过Harris算子提取图像特征点，再利用SIFT特征描述子对Harris算子提取出的特征点进行匹配；

步骤4)针对Harris-SIFT算法存在错误匹配和误差匹配的问题，采用新的提纯算法对匹配结果做进一步提纯处理：通过分块取点和提前取点验算临时模型改进提纯算法，其中，分块取点：采取图像分块且每个块中随机取一个匹配点的方式；提前取点验算临时模型：随机选取9个匹配对，用8个匹配求出临时模型参数，而第9个匹配对用来验证临时模型的正确性，以此快速确定图像模型；

步骤5)根据双目视觉原理，通过前期图像处理提取到的末端特征点计算出该末端特征点的三维坐标，并计算出并联机构末端姿态角，最终获得该并联机构末端位姿信息。

进一步，所述步骤2)中，Harris算子的角点提取法可直接调用OpenCV库函数实现，在Harris特征点检测中，像素点的二阶矩可表示为:

式中I_x、I_y表示像素点的灰度在x和y方向的梯度；I_xy为二阶混合偏导；像素点的Harris响应函数为：

R＝det M-k(traceM)²

其中det M为矩阵M的行列式，traceM为矩阵M的真迹；k为经验常数，通常取值范围0.04～0.06；设定阈值t，根据上述Harris算子计算响应值R，若R<t，则该点为特征点。

进一步，所述步骤4)中，所述提纯算法通过一组包含异常点的数据集，计算出匹配点对的目标模型，再利用剩余点来检验模型，从而得到有效数据样本；具体步骤如下：

4.1)根据第一幅图中匹配点的坐标边界将图片平均分成b×b块，其中，b>3，随机选取9个互不相同的块且每个块中随机选取一个匹配点，构成9个匹配点对的随机样本集S；

4.2)由随机样本集S随机选取8个匹配点对求出临时候选模型F；

4.3)检测第9个点是否为临时候选模型F的支撑集：如果是，则此临时候选模型F为候选模型F；否则，重新选择9对匹配点，重复4.1)、4.2)进程；

4.4)通过候选模型F和阈值k检测所有匹配点对，得到候选模型D的支撑集对数m；

4.5)若m≥阈值t，则得到目标模型F；否则，重新选择9对匹配点，重复4.1)、4.2)、4.3)、4.4)进程；

4.6)对目标模型F进行优化，最终确定目标模型H；

4.7)每次选择随机样本集，计数器都加1，若重复k次没有找到模型参数，则终止程序。

进一步，所述步骤5)中，根据投影定理求出并联机构末端特征点三维坐标(X,Y,Z)，再通过摄像机旋转矩阵R，求出并联机构末端姿态角；其中并联机构末端位置信息如下：

P_w＝(A^TA)^-1A^TB

其中，(u₁,v₁)、(u₂,v₂)分别表示末端特征点在左右相机投影点的图像坐标；表示左右投影矩阵的第i行，第j列；

同时，并联机构末端姿态角为：

其中，R_ij(i＝1,2,3；j＝1,2,3)表示旋转矩阵R的第i行，第j列，α为绕X轴旋转的俯仰角，单位为：rad，β为绕Y轴旋转的翻滚角，单位为：rad，γ为绕Z轴旋转的航向角，单位为：rad。

进一步，所述步骤5)中，还包括，三维重建时，首先需要得到所建双目视觉模型的摄像机参数，该摄像机参数可通过摄像机标定获得；采用张正友平面标定法对双目视觉系统进行摄像机标定。

进一步，所述张正友平面标定法过程为：首先固定相机，然后对标定板进行拍照，再将标定板图像输入计算机并获取标定板上每个格点的像素坐标，接着把标定板的已知三维坐标载入计算机，通过将标定板的像素坐标和三维坐标代入标定模型，求解出摄像机的内外参数，经过左右摄像机的分别标定。

本发明提出一种基于双目视觉的并联机构末端运动位姿检测方法，通过采用上述技术方案后，其具有以下有益效果：

1、本发明在立体匹配阶段采用Harris-SIFT算法实现立体匹配，通过算法简单的Harris算子提取图像特征点，再利用SIFT特征描述子对图像进行匹配，使得匹配结果兼具实时性和稳定性；

2、本发明针对匹配算法存在的误差匹配和错误匹配问题，提出一种新的提纯算法，即通过分块取点获得更为准确的图像模型参数和提前取点验算临时模型以快速确定图像模型的方式来改进提纯算法，用以提纯Harris-SIFT匹配结果，剔除Harris-SIFT算法存在的误匹配点，在保证匹配算法实时性的同时，提高匹配的正确率，从而提高机构末端三维位姿检测的精度。

附图说明

以下结合附图和具体实施方式对本发明作进一步详细说明。

图1是本发明所提出的一种基于双目视觉的并联机构末端位姿检测系统流程图。

图2是采用基于小波变换方法的并联机构图像去噪效果图。其中：a为有噪声的图像，b为去噪后的图像。

图3是Harris特征点提取图。

图4是基于Harris-SIFT算法的特征点立体匹配效果图。

图5是对匹配结果提纯后的效果图。

图6是本发明方法的双目视觉成像原理图。

图7是建立在机构平台的定坐标系与动坐标系。

图8是本发明方法实验结果与激光测距仪和电子罗盘测量结果对比得到的机构末端位姿参数的跟踪误差图。

具体实施方式

本发明提供了一种基于双目视觉的并联机构末端位姿检测方法，为解决双目视觉匹配过程实时性差的问题，本发明采用Harris-SIFT算法实现立体匹配；而针对Harris-SIFT算法存在错误匹配和误差匹配的问题，本发明提出一种提纯算法，以剔除Harris-SIFT算法存在的误匹配点，在保证匹配算法实时性的同时，提高匹配的正确率，进而使得位姿检测的实时性和精度都得以提高。具体步骤如下：

1、参见图1，通过双目视觉左右相机采集并联机构原始图像。通过双目相机实时获取并联机构图像信息，相机应调节到适宜的位置以保证在并联机构运动过程中，并联机构末端始终在相机视野范围之内。

2、参见图1和图2，将左右摄像机采集到的原始图像进行基于小波变换的图像去噪预处理操作。通过采用Visual Studio集成开发工具进行检测系统人机界面开发，并使用C/C++语言编程，同时结合OpenCV(开源跨平台计算机视觉库)，实现各模块相关功能的编程。去噪过程采用图像小波阈值去噪(WaveDec())的方法对原始机构图像进行预处理，通过设置阈值(g_HighPassFilterValue)，再对高频小波系数进行阈值处理，最后对图像进行小波重构(WaveRec())，实现图像去噪。

3、参见图1和图3，采用基于Harris算子的角点提取法对机构图像特性进行提取。为了保证特征提取的快速性和稳定性，考虑到并联机构图像是刚性图像且存在多个角点，而Harris角点检测不仅计算简单、稳定，而且不易受光照、刚体几何形变的影响，因此，采用Harris角点检测算法对机构图像进行特征点提取。Harris特征点提取可直接调用OpenCV库函数((conerHarris())实现，在Harris特征点检测中，像素点的二阶矩可表示为:

式中I_x、I_y表示像素点的灰度在x和y方向的梯度；I_xy为二阶混合偏导。因此，像素点的Harris响应函数为:

R＝det M-k(traceM)² (2)

其中det M为矩阵M的行列式，traceM为矩阵M的真迹；k为经验常数，通常取值范围0.04～0.06。设定阈值t，根据上述Harris算子计算响应值R，若R<t，则该点为特征点。

4、参见图1和图4，采用Harris-SIFT算法实现特征点立体匹配。通过使用Harris特征点取代SIFT中的极值点，并为每个特征点定义主方向，根据特征点生成特征向量描述子，再根据SIFT匹配算法的欧氏距离进行特征点匹配，将Harris角点检测算法与SIFT尺度不变特征变换有机结合，使得到的特征点具有两种算法的共同特征，不仅可提高SIFT算子的实时性，而且能提高特征点的稳定性。基于Harris-SIFT算法的特征匹配需要编程实现，程序编写时采用了如下公式：

其中：公式(3)为特征点x的DoG算子在尺度空间的泰勒展开式，D(x)表示空间尺度函数，△x为特征点x的偏移量；公式(4)为特征点精确位置的偏移量；公式(5)表示非边沿点的判断条件，r为设定的阈值，H为2×2的Hessian矩阵，Tr(H)为H矩阵的真迹，Det(H)为H矩阵的行列式；公式(6)为2×2的Hessian矩阵，D_xx表示特征点的DOG算子沿x方向的二阶偏导数，D_yy表示特征点的DOG算子沿y方向的二阶偏导数，D_xy表示特征点的DOG算子沿xy方向的二阶偏导数；公式(7)表示特征点处高斯梯度的模值m(x,y)和方向θ(x,y)，L(x,y)表示特征点所在的尺度空间值。

5、参见图1和图5，采用新的提纯算法对匹配结果进行提纯，剔除误匹配对。针对Harris-SIFT算法存在错误匹配和误差匹配的问题，本发明采用新的提纯算法对匹配结果做进一步提纯处理，以剔除Harris-SIFT算法存在的误匹配点，在保证匹配算法实时性的同时，提高匹配的正确率，从而提高机构末端三维位姿检测的精度。进行提纯处理时，由于提纯算法的关键在于快速找到正确的目标模型，而目标模型是由包含内点数最多的候选模型优化而得到，为了快速求出候选模型参数，本发明随机选取9个匹配对，其中8个匹配对用来求出临时模型参数，而第9个匹配对用来验证临时模型的正确性，从而快速确定目标模型；同时，本发明采取图像分块且每个块中随机取一个匹配点的方式避免存在两个匹配点距离过近而被认为是一个点，而导致的基本矩阵不准确的问题。获取目标模型的具体步骤如下：

[1]根据第一幅图中匹配点的坐标边界将图片平均分成b×b(b>3)块，随机选取9个互不相同的块且每个块中随机选取一个匹配点，构成9个匹配点对的随机样本集S。

[2]由随机样本集S随机选取8个匹配点对求出临时候选模型F。

[3]检测第9个点是否为临时候选模型F的支撑集：是，则此临时候选模型F为候选模型F；否则，重新选择9对匹配点，重复[1]、[2]进程。

[4]通过候选模型F和阈值k检测所有匹配点对，得到候选模型D的支撑集对数m。

[5]若m≥阈值t，则得到目标模型F；否则，重新选择9对匹配点，重复[1]、[2]、[3]、[4]进程。

[6]对目标模型F进行优化，最终确定目标模型H。

[7]每次选择随机样本集，计数器都加1，若重复k次没有找到模型参数，则终止程序。

6、参见图1和图6，根据双目视觉原理实现三维重建，将前期图像处理提取到的末端匹配点对带入双目视觉模型，最终获得并联机构末端位姿信息。三维重建时，首先需要得到所建双目视觉模型的摄像机参数，该摄像机参数可通过摄像机标定获得，本发明采用张正友标定法对双目视觉系统进行摄像机标定，根据张正友平面标定流程：首先固定相机，然后对标定板进行拍照，再将标定板图像输入计算机并获取标定板上每个格点的像素坐标，接着把标定板的已知三维坐标载入计算机，通过将标定板的像素坐标和三维坐标代入标定模型，求解出摄像机的内外参数，经过左右摄像机的分别标定，再根据式(8)便可获得双目视觉系统的摄像机参数:

式中，R_l、T_l和R_r、T_r分别表示左右摄像机的旋转矩阵和平移矩阵。

摄像机标定完成后便可得到双目视觉投影矩阵。假定并联机构末端特征点P在左右摄像机的像点分别为Pl和Pr，左右摄像机已完成标定且它们的投影矩阵分别为Ml、Mr，则根据投影定理有：

其中s_l，s_r表示比例因子，将上式展开来则有：

式中，(u₁,v₁,1)，(u₂,v₂,1)分别表示投影点P_l和P_r的齐次坐标；(X,Y,Z,1)为末端特征点在世界坐标系下的齐次坐标；表示左右投影矩阵的第i行，第j列。

式(10)和式(11)联立后可得如下方程：

令：则可采用最小二乘法求出并联机构末端位置三维坐标P_W，其表示如下：

P_w＝(A^TA)^-1A^TB (13)

再将旋转矩阵表示为如式(14)所示：

则并联机构末端姿态角为式(15)所示：

式中，α为绕X轴旋转的俯仰角(单位为：rad)，β为绕Y轴旋转的翻滚角(单位为：rad)，γ为绕Z轴旋转的航向角(单位为：rad)。

至此，并联机构的末端位姿参数求解已完成。

实施例

本发明着重提出了一种新的基于双目视觉的并联机构末端运动位姿检测方法，以解决传统双目视觉算法在并联机构末端位姿检测中存在的实时性差、检测精度不高的问题。下面以一种新型3-DOF并联机构末端为检测对象，该检测方法的具体实施方式如下：

1、采集并联机构原始图像。通过维视双目相机采集新型3-DOF并联机构图像，其中，相机型号为MV-1300FM，镜头型号为AFT-0814MP，相机安装并联机构的前方，且距地高度和角度均可灵活调节。此外，本检测系统采用计算机操作系统为Windows7，处理器型号为Intel(R)Core(TM)2Duo，主频为2.66GHz，内存为2GB。

2、对原始图像进行基于小波变换的去噪处理。为了降低噪声对并联机构末端位姿检测所带来的不良影响，采用基于小波变换的去噪方法对原始图像进行预处理，通过去噪处理有效抑制图像噪声，以便下一步特征提取操作，去噪效果如图2所示。

3、采用基于Harris算子的角点提取法对机构图像特性进行提取。考虑到新型3-DOF并联机构具有明显的刚体特征，而Harris特征点检测不仅在刚体几何变形和亮度变化方面有着高复检率，而且算法简单、稳定，因此选取Harris角点检测算法对机构图像进行特征点提取。像素点的Harris响应函数为：

R＝det M-k(traceM)² (16)

其中det M为矩阵M的行列式，traceM为矩阵M的真迹；k为经验常数，通常取值范围0.04～0.06。设定阈值t，根据上述Harris算子计算响应值R，若R<t，则该点为特征点，本实施例取经验常数k＝0.04，并直接通过调用OpenCV库函数((conerHarris())实现新型3-DOF并联机构特征点提取，新型3-DOF并联机构特征点提取图如图3所示。

4、采用Harris-SIFT算法实现特征点立体匹配。本实施例采用Harris-SIFT算法进行新型3-DOF并联机构图像特征点匹配。该匹配算法使用Harris特征点取代SIFT中的极值点，采用拟合三维二次函数的方法实现对特征点的精确定位，并剔除对噪声敏感的低对比度点和难以定位的处于图像边沿的点，从而筛选出稳定的点作为图像的SIFT特征点。基于Harris-SIFT算法的立体匹配分为以下步骤：

(1)对特征点x的DoG算子进行泰勒展开：

其中，△x为特征点x的偏移量，当x为DoG算子的极值点时，D(x)的一阶偏导为0，则特征点精确位置的偏移量△x可求得：

经过多次迭代后得到被测点的精确位置和尺度，将其代入公式(16)可求得该点的DoG值并取其绝对值，设定对比度阈值Tc，从而剔除对比度绝对值小于阈值Tc的低对比度点。

而对于边沿点的剔除，可通过DoG函数极值点的主曲率与阈值比较来实现，通常，边沿的DoG函数极值点比非边沿点的主曲率大，因此，可将主曲率比值大于阈值的边沿点剔除。非边沿点满足关系如式(19)所示：

其中，r为设定的阈值，本实施例取r＝10；H为2×2的Hessian矩阵，H矩阵表示如式(20)所示：

(2)确定特征点方向。设点L(x,y)的梯度模为m(x,y)，方向为θ(x,y)，则该点高斯梯度模和方向表示如式(21)所示：

统计关键点邻域像素的梯度分布，并选取直方图的主峰值作为关键点的方向。至此，每个特征点都具备了3个重要的信息：位置、尺度和方向。

(3)生成特征描述子。生成特征描述子需要将坐标轴旋转为特征点方向，并均匀划分该特征点相邻的16×16像素区域为8个4×4的子区域，接着在每个4×4的子区域上计算8个方向的梯度直方图，这样每个特征点就可产生128个数据以形成128维的SIFT特征向量，将生成的特征向量进行长度归一化，可进一步减少光照变化的影响。

(4)特征描述子生成后，便可根据SIFT匹配算法的欧氏距离进行特征点匹配。具体实现如下：首先，将左图像中的某一特征点p与待匹配的右图像的所有特征点分别进行欧氏距离计算；然后，取出右图像中与p点欧氏距离最近和次近的两个点p1和p2；最后，计算最近欧式距离与次近欧式距离的比值并与一个阈值相比较，若该比值小于阈值，则匹配成功，此时，(p1,p2)为图像序列的一个匹配点对，反之，则匹配失败。本实施例设定阈值为0.5，基于Harris-SIFT算法的新型3-DOF并联机构匹配效果如图4所示。

5、采用本发明提出的提纯算法剔除匹配结果中存在的误匹配点。进行提纯处理时，可根据一组包含异常点的数据集，计算出匹配点对的目标模型，再利用剩余点来检验模型，从而得到有效数据样本。为了快速求出候选模型参数，根据左图中匹配点的坐标边界将图片平均分成16块，随机选取9个互不相同的块且每个块中随机选取一个匹配点，从9个匹配对中随机选择8个匹配对用来求出临时模型参数，而第9个匹配对用来验证临时模型的正确性，从而快速确定目标模型。提纯匹配结果效果如图5所示，目标模型H的选取和Sampson距离表示如下：

(1)两图像间目标模型H的选取：按照以上分块取点的方式随机选择8个匹配点对计算临时候选模型F，并求出模型F所对应匹配点对的Sampson距离d，设定阈值k，将符合d<k条件的点作为内点，并将内点数最多的候选模型F做进一步优化得到目标模型H，最后通过目标模型H重新估计匹配点，剔除误匹配对。投影变换模型表示如式(22)所示：

式中，(x,y,1)和(x’,y’,1)分别表示匹配点对m(x,y)和m(x’,y’)的齐次坐标。

(2)Sampson距离d的表示：对于任一匹配点对m(x,y)和m(x’,y’)来说，其在候选模型F中的Sampson距离d可表示为：

6、求解新型3-DOF并联机构末端运动位姿。根据新型3-DOF并联机构运动特点，建立机构平台的定坐标系与动坐标系如图7所示，其中，{B}＝{O-XYZ}、{T}＝{O'-X'Y'Z'}分别表示定坐标系和动坐标系，则并联机构末端位姿的变化即可表示为动平台中心点O'点的位姿变化。本实施例采用张正友标定法实现双目视觉系统的摄像机标定，并通过前期图像处理和后期坐标变换求出新型3-DOF并联机构末端位置信息如下：

P_w＝(A^TA)^-1A^TB (24)

其中，(u₁,v₁)、(u₂,v₂)分别表示末端特征点在左右相机投影点的图像坐标，表示左右投影矩阵的第i行，第j列。

同时，新型3-DOF并联机构末端姿态角为式(25)所示：

由于本并联机构做三自由度运动，因此可求出其在Y轴、Z轴的平动位移，以及绕Y轴的翻滚角β，则机构位姿参数即为(Y,Z,β)。

本实施例定时采集12张图像，然后求出每个时刻机构三维位姿参数，并将结果与激光测距仪INR-Laser Scanner H和电子罗盘Honeywell-HMR3100测得的机构末端实际位姿参数进行对比，跟踪误差如图8所示。分析图8可见，测量点P1和P7在Y方向的跟踪误差较大，而P4和P10在Z方向的跟踪误差较大，这与机构的对称性和运行轨迹有关，由于机构在上述测量点方向发生改变，使得速度和加速度变化较大，外加机构惯性的影响，导致误差较大，这表明实验测量得到的并联机构末端位姿运动状态与实际运动状态相一致；同时，根据跟踪误差可计算出各个测量点的最大偏差分别为Y方向：0.622mm，Z方向：0.782mm，β旋转角：0.677°，说明检测具有较高精度。实验结果表明，本发明所提出的基于双目视觉的位姿检测方法，能较好地实现并联机构末端运动位姿的实时检测。

综上，本发明的一种基于双目视觉的并联机构末端运动位姿检测方法，首先，将采集到的并联机构图像进行基于小波变换的图像去噪预处理，以消除图像噪声对随后特征提取造成的影响；然后，采用Harris-SIFT算法对并联机构图像进行特征匹配。不同于一般的匹配算法，该匹配算法首先通过Harris算子提取图像特征点，再利用SIFT特征描述子对Harris算子提取出的特征点进行匹配，使得匹配结果兼具实时性和稳定性；接着，为进一步提高匹配正确率，提出一种新的提纯算法对匹配结果进行提纯，该提纯算法通过分块取点和提前取点验算临时模型的方法，解决了提纯算法中目标模型获取费时、模型求解不准确的问题；最后，将匹配提纯后的并联机构末端特征点对带入双目视觉模型，通过坐标变换求出机构末端三维位姿。本发明所提出的基于双目视觉的并联机构末端位姿检测方法，由于在匹配阶段首先通过Harris算子提取图像特征点，再利用SIFT特征描述子对Harris算子提取出的特征点进行匹配，可大幅度降低图像处理时间，另通过所提出的新的提纯算法对匹配结果进行提纯，可进一步提高匹配正确率，进而使得并联机构末端位姿检测的实时性和精度都得以提高。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示意性实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。