一种基于3D视觉的机械臂精确放置轨迹规划方法

摘要

本发明提供了一种基于3D视觉的机械臂精确放置轨迹规划方法，包括步骤：S1.对两个视觉系统进行标定；S2.计算位于目标放置点正上方预设高度处的一个待规划点的理论位姿；S3.机械臂带动物体移向待规划点处；S4.联合两个视觉系统获取物体位姿；S5.对比获取的物体位姿和步骤S2中得到的理论位姿，判断两者是否相等，若不相等执行步骤S6，若相等执行步骤S7；S6.规划运动轨迹并利用两个视觉系统获取的物体位姿，对机械臂进行闭环控制从而使物体沿规划的轨迹移至待规划点，并执行步骤S5；S7.机械臂带动物体朝下移至目标放置点；该方法可提高机械臂将物体放置到目标点处的精确度。

权利要求

1.一种基于3D视觉的机械臂精确放置轨迹规划方法，其特征在于，包括步骤：S1.对两个视觉系统进行标定，其中一个视觉系统包括4个布置在机械臂操作平台上方的RGBD相机，另一个视觉系统包括4个布置在机械臂操作平台下方的2D相机；

S2.计算位于目标放置点正上方预设高度处的一个待规划点的理论位姿；

S3.机械臂带动物体移向待规划点处；

S4.联合两个视觉系统获取物体位姿；

S5.对比获取的物体位姿和步骤S2中得到的理论位姿，判断两者是否相等，若不相等执行步骤S6，若相等执行步骤S7；

S6.规划运动轨迹并利用两个视觉系统获取的物体位姿，对机械臂进行闭环控制从而使物体沿规划的轨迹移至待规划点，并执行步骤S5；

S7. 机械臂带动物体朝下移至目标放置点；

步骤S1中，对两个视觉系统进行标定包括：各相机的自标定，以获得相机的内、外参数；

各相机的手眼标定，以获得两个视觉系统与机械臂基坐标系的转换矩阵；

两个视觉系统的联合标定，以获得两个视觉系统之间的转换矩阵；

其中，获得每个视觉系统与机械臂基坐标系的转换矩阵的方法包括步骤：

1）对每个相机进行手眼标定，得到每个相机与机械臂基坐标系之间的变换矩阵；

2）根据得出来的变换矩阵，换算出各个相机之间的位置关系；

3）将换算出来的位置关系，输入到视觉系统中，将四个相机所看到工作场景融合为一个整体的工作空间；

4）从四个相机中选取一个作为主相机，进行手眼标定，得到的转换矩阵为该视觉系统与机械臂基坐标系之间的变换矩阵；

步骤S4中，联合两个视觉系统获取物体位姿包括步骤：S401.通过3D视觉系统获取物体位姿；

S402.通过2D视觉系统获取物体位姿；

S403.判断两个视觉系统获得的物体位姿是否相等，若相等则执行步骤S404，若不相等则重复步骤S401、S402、S403；

S404.输出物体位姿测量结果；

步骤S6中，规划运动轨迹并利用两个视觉系统获取的物体位姿，对机械臂进行闭环控制从而使物体沿规划的轨迹移至待规划点包括步骤：S601.采用五次多项式插补算法规划出从当前位置到待规划点的由多个路径点组成的运动轨迹，并计算出在各路径点处的理论物体位姿；

S602.机械臂带动物体沿规划出的运动轨迹依次移向各路径点；

S603.在每个各路径点处联合两个视觉系统获取实际物体位姿；

S604.当在某个路径点处的实际物体位姿与理论物体位姿不相等时重复步骤S601获得新的运动轨迹，并沿新运动轨迹继续移动，直到到达待规划点。

2.根据权利要求1所述的基于3D视觉的机械臂精确放置轨迹规划方法，其特征在于，步骤S401包括：

S4011. 使用三维建模软件对物体进行三维模型的构建，然后利用软件经三维模型转化为点云数据保存为目标点云B；

S4012. 通过RGBD相机获取机械臂和物体的整体点云A；

S4013. 对整体点云A进行滤波处理，得到滤波后的整体点云A’；

S4014.利用平面分割算法把物体的点云从滤波后的整体点云A’中分割出来，得到物体点云A”;

S4015.对物体点云A”进行点云粗匹配获取匹配成功点云E’；

S4016.对匹配成功点云E’进行精匹配获得最终变换矩阵;S4017.根据所述最终变换矩阵和目标点云B，计算出此时3D视觉获取的物体位姿，并根据3D视觉系统与机械臂基坐标系的转换矩阵，计算出物体在机械臂基坐标系下的位姿。

3.根据权利要求2所述的基于3D视觉的机械臂精确放置轨迹规划方法，其特征在于，所述对物体点云A”进行点云粗匹配获取匹配成功点云E’包括步骤：

1）对物体点云A”进行下采样得到待匹配点云C；

2）对待匹配点云C和目标点云B进行关键特征的提取，得到特征点云C’和B’；

3）根据FPFH算法对两特征点云进行特征表述，进而计算两特征点云各点间的欧氏距离设置阈值d,保留相似点对E，然后利用RANSAC算法选择点对E中的几个点对计算出刚性变换矩阵；

4）重复进行第3）步预设次数，将点对E中成功率高的点云保留下来作为匹配成功点云E’。

4.根据权利要求3所述的基于3D视觉的机械臂精确放置轨迹规划方法，其特征在于，所述对匹配成功点云E’进行精匹配获得最终变换矩阵包括步骤：

1）把粗匹配后得到的点云E’作为新的待匹配点云，利用最小二乘法对应E’中每个点在特征点云B’中找出距离最短的点，保存为点云F；

2）利用ICP算法将点云E’和点云F进行匹配，得到刚性变换矩阵R和T；

3）根据求得的刚性变换矩阵R和T对点云E’进行进行更新,然后计算更新后的点云E’与点云F的均方差d1；

4）重复执行步骤1）、2）、3），直到均方差d1在预设阈值范围内或迭代次数达到预设执行次数，停止迭代，把最后得到刚性变换矩阵R和T作为最终的变换矩阵R’和T’。

5.根据权利要求1所述的基于3D视觉的机械臂精确放置轨迹规划方法，其特征在于，步骤S402包括：

S4021. 在物体底部贴可识别的二维码；

S4022. 利用ar_track_alvar软件包提供的算法识别物体在空间中的位姿，得出位姿值；

S4023. 利用步骤S4022获取的位姿值根据2D视觉系统与机械臂基坐标系的转换矩阵计算出物体在机械臂基坐标系下的位姿。

6.根据权利要求1所述的基于3D视觉的机械臂精确放置轨迹规划方法，其特征在于，步骤S403中，当两个视觉系统获得的物体位姿之间的偏差值在预设阈值范围内，则判断为两者相等；所述预设阈值为1‑3mm。

说明书

一种基于3D视觉的机械臂精确放置轨迹规划方法

技术领域

[0001] 本发明涉及工业机械臂技术领域，尤其涉及一种基于3D视觉的机械臂精确放置轨迹规划方法。

背景技术

[0002] 随着工业的发展，机械臂的应用越来越普遍，机械臂的抓取和放置运动的应用场景也越来越广泛。

[0003] 目前机械臂对物体的抓取和放置方法有以预设路线在两点之间进行抓取与放置、通过深度学习来实现快速抓取与放置等；

[0004] 对于以预设路线在两点之间进行抓取与放置的方法，由于长期往复运动，机械臂运动误差和夹爪本身误差会积累，导致放置位置不准确；

[0005] 对于通过深度学习来实现快速抓取与放置，需要进行大量的学习训练，工作量非常大，而且达到的效果常常不够理想，机械臂的放置精度依然不够高。

[0006] 因此，在加工精度要求高的场景下，机械臂的精准放置往往不能满足加工需求，无法把物体准确放置到目标点处。

发明内容

[0007] 鉴于上述现有技术的不足之处，本发明的目的在于提供一种基于3D视觉的机械臂精确放置轨迹规划方法，旨在解决了目前机械臂无法准确将物体放置到目标点处的问题。

[0008] 为了达到上述目的，本发明采取了以下技术方案：

[0009] 一种基于3D视觉的机械臂精确放置轨迹规划方法，包括步骤：

[0010] S1.对两个视觉系统进行标定，其中一个视觉系统包括4个布置在机械臂操作平台上方的RGBD相机，另一个视觉系统包括4个布置在机械臂操作平台下方的2D相机；

[0011] S2.计算位于目标放置点正上方预设高度处的一个待规划点的理论位姿；

[0012] S3.机械臂带动物体移向待规划点处；

[0013] S4.联合两个视觉系统获取物体位姿；

[0014] S5.对比获取的物体位姿和步骤S2中得到的理论位姿，判断两者是否相等，若不相等执行步骤S6，若相等执行步骤S7；

[0015] S6.规划运动轨迹并利用两个视觉系统获取的物体位姿，对机械臂进行闭环控制从而使物体沿规划的轨迹移至待规划点，并执行步骤S5；

[0016] S7. 机械臂带动物体朝下移至目标放置点。

[0017] 所述的基于3D视觉的机械臂精确放置轨迹规划方法中，步骤S1中，对两个视觉系统进行标定包括：

[0018] 各相机的自标定，以获得相机的内、外参数；

[0019] 各相机的手眼标定，以获得两个视觉系统与机械臂基坐标系的转换矩阵；

[0020] 两个视觉系统的联合标定，以获得两个视觉系统之间的转换矩阵。

[0021] 所述的基于3D视觉的机械臂精确放置轨迹规划方法中，获得每个视觉系统与机械臂基坐标系的转换矩阵的方法包括步骤：

[0022] 1）对每个相机进行手眼标定，得到每个相机与机械臂基坐标系之间的变换矩阵；

[0023] 2）根据得出来的变换矩阵，换算出各个相机之间的位置关系；

[0024] 3）将换算出来的位置关系，输入到视觉系统中，将四个相机所看到工作场景融合为一个整体的工作空间；

[0025] 4）从四个相机中选取一个作为主相机，进行手眼标定，得到的转换矩阵为该视觉系统与机械臂基坐标系之间的变换矩阵。

[0026] 所述的基于3D视觉的机械臂精确放置轨迹规划方法中，步骤S4中，联合两个视觉系统获取物体位姿包括步骤：

[0027] S401.通过3D视觉系统获取物体位姿；

[0028] S402.通过2D视觉系统获取物体位姿；

[0029] S403.判断两个视觉系统获得的物体位姿是否相等，若相等则执行步骤S404，若不相等则重复步骤S401、S402、S403；

[0030] S404.输出物体位姿测量结果。

[0031] 进一步的，步骤S401包括：

[0032] S4011. 使用三维建模软件对物体进行三维模型的构建，然后利用软件经三维模型转化为点云数据保存为目标点云B；

[0033] S4012. 通过RGBD相机获取机械臂和物体的整体点云A；

[0034] S4013. 对整体点云A进行滤波处理，得到滤波后的整体点云A’；

[0035] S4014.利用平面分割算法把物体的点云从滤波后的整体点云A’中分割出来，得到物体点云A”;

[0036] S4015.对物体点云A”进行点云粗匹配获取匹配成功点云E’；

[0037] S4016.对匹配成功点云E’进行精匹配获得最终变换矩阵;

[0038] S4017.根据所述最终变换矩阵和目标点云B，计算出此时3D视觉获取的物体位姿，并根据3D视觉系统与机械臂基坐标系的转换矩阵，计算出物体在机械臂基坐标系下的位
姿。

[0039] 其中，所述对物体点云A”进行点云粗匹配获取匹配成功点云E’包括步骤：

[0040] 1）对物体点云A”进行下采样得到待匹配点云C；

[0041] 2）对待匹配点云C和目标点云B进行关键特征的提取，得到特征点云C’和B’；

[0042] 3）根据FPFH算法对两特征点云进行特征表述，进而计算两特征点云各点间的欧氏距离设置阈值d,保留相似点对E，然后利用RANSAC算法选择点对E中的几个点对计算出刚性
变换矩阵；

[0043] 4）重复进行第3）步预设次数，将点对E中成功率高的点云保留下来作为匹配成功点云E’。

[0044] 其中，所述对匹配成功点云E’进行精匹配获得最终变换矩阵包括步骤：

[0045] 1）把粗匹配后得到的点云E’作为新的待匹配点云，利用最小二乘法对应E’中每个点在特征点云B’中找出距离最短的点，保存为点云F；

[0046] 2）利用ICP算法将点云E’和点云F进行匹配，得到刚性变换矩阵R和T；

[0047] 3）根据求得的刚性变换矩阵R和T对点云E’进行进行更新,然后计算更新后的点云E’与点云F的均方差d1；

[0048] 4）重复执行步骤1）、2）、3），直到均方差d1在预设阈值范围内或迭代次数达到预设执行次数，停止迭代，把最后得到刚性变换矩阵R和T作为最终的变换矩阵R’和T’。

[0049] 进一步的，步骤S402包括：

[0050] S4021. 在物体底部贴可识别的二维码；

[0051] S4022. 利用ar_track_alvar软件包提供的算法识别物体在空间中的位姿，得出位姿值；

[0052] S4023. 利用步骤S4022获取的位姿值根据2D视觉系统与机械臂基坐标系的转换矩阵计算出物体在机械臂基坐标系下的位姿。

[0053] 所述的基于3D视觉的机械臂精确放置轨迹规划方法中，步骤S403中，当两个视觉系统获得的物体位姿之间的偏差值在预设阈值范围内，则判断为两者相等；所述预设阈值
为1‑3mm。

[0054] 所述的基于3D视觉的机械臂精确放置轨迹规划方法中，步骤S6中，规划运动轨迹并利用两个视觉系统获取的物体位姿，对机械臂进行闭环控制从而使物体沿规划的轨迹移
至待规划点包括步骤：

[0055] S601.采用五次多项式插补算法规划出从当前位置到待规划点的由多个路径点组成的运动轨迹，并计算出在各路径点处的理论物体位姿；

[0056] S602.机械臂带动物体沿规划出的运动轨迹依次移向各路径点；

[0057] S603.在每个各路径点处联合两个视觉系统获取实际物体位姿；

[0058] S604.当在某个路径点处的实际物体位姿与理论物体位姿不相等时重复步骤S601获得新的运动轨迹，并沿新运动轨迹继续移动，直到到达待规划点。

[0059] 有益效果：

[0060] 本发明提供的一种基于3D视觉的机械臂精确放置轨迹规划方法，具有以下优点：

[0061] 1.设置两个视觉系统来获取物体的位姿，并在获取物体的位姿时，只有当两个视觉系统获取的位姿值相等是才会作为有效位姿值，从而提高获取位姿精确度；

[0062] 2. 用两个视觉系统来实时监控机械臂运动，获取的机械臂实时运动状态的数据的精度高；

[0063] 3. 通过规划运动轨迹并利用两个视觉系统获取的物体位姿，对机械臂进行闭环控制从而使物体沿规划的轨迹移至待规划点，可大大提高物体达到待规划点的精确度，从
而提高放置精度。

附图说明

[0064] 图1为本发明提供的基于3D视觉的机械臂精确放置轨迹规划方法的流程图。

[0065] 图2为本发明提供的基于3D视觉的机械臂精确放置轨迹规划方法的原理图。

[0066] 图3为视觉系统标定的流程图。

[0067] 图4为获取物体位姿的流程图。

[0068] 图5为控制物体从待规划点移动至目标放置点的过程的流程图。

[0069] 图6为使物体移动至待规划点的闭环控制过程。

[0070] 图7为视觉系统中的四个相机的一种示例性布置图。

具体实施方式

[0071] 下面详细描述本发明的实施方式，所述实施方式的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参
考附图描述的实施方式是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。

[0072] 在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“长度”、“宽度”、“厚度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”、“顺时
针”、“逆时针”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于
描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特
定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”仅用于
描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。
由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个所述特征。在
本发明的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。

[0073] 下文的公开提供的实施方式或例子用来实现本发明的不同结构。为了简化本发明的公开，下文中对特定例子的部件和设置进行描述。当然，它们仅仅为示例，并且目的不在
于限制本发明。此外，本发明可以在不同例子中重复参考数字和/或参考字母，这种重复是
为了简化和清楚的目的，其本身不指示所讨论各种实施方式和/或设置之间的关系。此外，
本发明提供了的各种特定的工艺和材料的例子，但是本领域普通技术人员可以意识到其他
工艺的应用和/或其他材料的使用。

[0074] 请参阅图1、2，本发明提供的一种基于3D视觉的机械臂精确放置轨迹规划方法，包括步骤：

[0075] S1.对两个视觉系统进行标定，其中一个视觉系统包括4个布置在机械臂操作平台上方的RGBD相机，另一个视觉系统包括4个布置在机械臂操作平台下方的2D相机；

[0076] S2.计算位于目标放置点正上方预设高度处的一个待规划点的理论位姿；

[0077] S3.机械臂带动物体移向待规划点处；

[0078] S4.联合两个视觉系统获取物体位姿；

[0079] S5.对比获取的物体位姿和步骤S2中得到的理论位姿，判断两者是否相等，若不相等执行步骤S6，若相等执行步骤S7；

[0080] S6.规划运动轨迹并利用两个视觉系统获取的物体位姿，对机械臂进行闭环控制从而使物体沿规划的轨迹移至待规划点，并执行步骤S5；

[0081] S7. 机械臂带动物体朝下移至目标放置点。

[0082] 以下对各步骤进行详述。

[0083] S1.对两个视觉系统进行标定，其中一个视觉系统包括4个布置在机械臂操作平台上方的RGBD相机，另一个视觉系统包括4个布置在机械臂操作平台下方的2D相机。

[0084] 由于现有技术中通常是使用单个3D视觉系统进行位姿获取的，得到的位姿误差较大，无法满足高精准度放置的要求，此处，将使用两个视觉系统进行位姿获取，以提高位姿
精确度，为实现高精准度放置提供支持。

[0085] 其中，第一个视觉系统为3D视觉系统，包括4个布置在机械臂操作平台上方的RGBD相机；第二个视觉系统为2D视觉系统，包括4个布置在机械臂操作平台下方的2D相机。

[0086] 对于每一个视觉系统，采用4个相机来构建环境相机，其中，4个相机的分布位置可根据实际环境进行布置，但需确保在机械臂的运动范围内的任一位置，至少1个相机可拍摄
到物体。例如，可以采用图7所示的布置方式，4个相机呈矩形分布，但限于此。

[0087] 具体的，见图3，对两个视觉系统进行标定包括：

[0088] 各相机的自标定，以获得相机的内、外参数；

[0089] 各相机的手眼标定，以获得两个视觉系统与机械臂基坐标系的转换矩阵；

[0090] 两个视觉系统的联合标定，以获得两个视觉系统之间的转换矩阵。

[0091] 其中，获得每个视觉系统与机械臂基坐标系的转换矩阵的方法具体包括步骤：

[0092] 1）对每个相机进行手眼标定，得到每个相机与机械臂基坐标系之间的变换矩阵（该过程可按现有技术的手眼标定方法执行）；

[0093] 2）根据得出来的变换矩阵，换算出各个相机之间的位置关系；

[0094] 3）将换算出来的位置关系，输入到视觉系统中，将四个相机所看到工作场景融合为一个整体的工作空间（可以采用现有的手动icp融合方法进行融合，但不限于此）；

[0095] 4）从四个相机中选取一个作为主相机，进行手眼标定，得到的转换矩阵为该视觉系统与机械臂基坐标系之间的变换矩阵。

[0096] 对两个视觉系统均采用上述方法得到各自与机械臂基坐标系之间的变换矩阵。

[0097] 其中，两个视觉系统的联合标定，以获得两个视觉系统之间的转换矩的具体方法为：根据上面得到的两个视觉系统的主相机与机械臂基坐标系之间的变换矩阵，计算出两
个视觉系统之间的转换矩阵。

[0098] S2.计算位于目标放置点正上方预设高度处的一个待规划点的理论位姿。

[0099] 对于机械臂抓取物体后如何精准放置在目标放置点的问题，解决方式有很多中，本发明提供的是一种比较简单，实施方便的方法：先把物体移动至目标放置点正上方预设
高度h处（把该位置称为待规划点），然后向下移动进行放置，由于向下移动的过程位置精度
容易控制，只要能够保证物体能够精准地移动至待规划点，则可精确地把物体放置在目标
放置点。

[0100] 而在待规划点处的物体的理论位姿是可通过计算得到的，此为现有技术，此处不对计算过程进行详述。

[0101] S3.机械臂带动物体移向待规划点处。

[0102] 该步骤的具体过程是根据步骤S2中计算得到的物体理论位姿计算出机械臂的理论位姿，然后控制机械臂动作，使机械臂位姿到达理论位姿。

[0103] S4.联合两个视觉系统获取物体位姿。

[0104] 见图4，联合两个视觉系统获取物体位姿包括步骤：

[0105] S401.通过3D视觉系统获取物体位姿；

[0106] S402.通过2D视觉系统获取物体位姿；

[0107] S403.判断两个视觉系统获得的物体位姿是否相等，若相等则执行步骤S404，若不相等则重复步骤S401、S402、S403；

[0108] S404.输出物体位姿测量结果。

[0109] 由于在物体不动的情况下，通过3D视觉获取的位姿数据并非恒定不变的，每次获得的位姿数据会在一定的允差范围内随机变化的，因此每次获得的位姿会不同。其中，当两
个视觉系统获得的物体位姿之间的偏差值在预设阈值范围内，则可认为两者相等，具体的
预设阈值可以根据实际精度需要进行设置（如预设阈值为1‑3mm），例如要求精度较高，则预
设阈值较小，如要求精度较低，则预设阈值较大。

[0110] 此处，通过两个视觉系统同时获取物体的位姿，只有当两个位姿相等时才作为有效位姿而被采用，否则重新获取，其误差相对于单独采用3D视觉系统或2D视觉系统获时小
了很多，位姿精确度更高。

[0111] 进一步的，步骤S401通过3D视觉系统获取物体位姿的过程包括：

[0112] S4011. 使用三维建模软件（可以但不限于是solidworks）对物体进行三维模型的构建，然后利用软件经三维模型转化为点云数据保存为目标点云B；

[0113] S4012. 通过RGBD相机获取机械臂和物体的整体点云A；具体的，4个RGBD相机自获取一组点云，再合成为一组点云A；

[0114] S4013. 对整体点云A进行滤波处理，得到滤波后的整体点云A’；

[0115] S4014.利用平面分割算法把物体的点云从滤波后的整体点云A’中分割出来，得到物体点云A”;

[0116] S4015.对物体点云A”进行点云粗匹配获取匹配成功点云E’；

[0117] S4016.对匹配成功点云E’进行精匹配获得最终变换矩阵;

[0118] S4017.根据最终变换矩阵和目标点云B，计算出此时3D视觉获取的物体位姿，并根据3D视觉系统与机械臂基坐标系的转换矩阵（在步骤S1中标定得到），计算出物体在机械臂
基坐标系下的位姿。

[0119] 其中，S4015对物体点云A”进行点云粗匹配获取匹配成功点云E’包括步骤：

[0120] 1）对物体点云A”进行下采样得到待匹配点云C；

[0121] 具体是通过构造一个三维体素栅格，将每个体素内的所有点云用一个重心点来表示，这样大大减少了点云数量，经过下采样后得到点云C；

[0122] 2）对待匹配点云C和目标点云B进行法向关键特征的提取，得到特征点云C’和B’；

[0123] 通过该步骤可提高待配准点云数据在匹配点的搜索速度和准确性，其中进行关键特征的提取的方法为现有技术，此处不对其进行详述；

[0124] 3）根据FPFH算法对两特征点云进行特征表述，利用RANSAC算法选择点对E中的几个点对计算出刚性变换矩阵；

[0125] 4）重复进行第3）步预设次数，将点对E中与B’中匹配分数高的点云保留下来作为匹配成功点云E’。

[0126] 其中，S4016对匹配成功点云E’进行精匹配获得最终变换矩阵包括步骤：

[0127] 1）把粗匹配后得到的点云E’作为新的待匹配点云，利用最小二乘法对应E’中每个点在特征点云B’中找出距离最短的点（即在特征点云B’中找出离E’的点最近的点），保存为
点云F；

[0128] 2）利用ICP算法将点云E’和点云F进行匹配，得到刚性变换矩阵R和T；

[0129] 3）根据求得的刚性变换矩阵R和T对点云E’进行进行更新,然后计算更新后的点云E’与点云F的均方差d1；

[0130] 4）重复执行步骤1）、2）、3），直到均方差d1在预设阈值范围内或迭代次数达到预设执行次数，停止迭代，把最后得到刚性变换矩阵R和T作为最终的变换矩阵R’和T’；

[0131] 其中，预设阈值和或迭代次数可根据实现精度需要进行设置。

[0132] 进一步的，步骤S402通过2D视觉系统获取物体位姿包括：

[0133] S4021. 在物体底部贴可识别的二维码(如ARTag标签)；

[0134] S4022. 利用ar_track_alvar软件包提供的算法识别物体在空间中的位姿，得出位姿值；

[0135] S4023. 利用步骤S4022获取的位姿值根据2D视觉系统与机械臂基坐标系的转换矩阵（在步骤S1中标定得到）计算出物体在机械臂基坐标系下的位姿。

[0136] S5.对比获取的物体位姿和步骤S2中得到的理论位姿，判断两者是否相等，若不相等执行步骤S6，若相等执行步骤S7（如图5所示）。

[0137] 由于步骤S3中，机械臂完成移动后，物体的实际位姿与理论位姿通常是具有一定误差的（即物体停止位置与待规划点之间具有一定偏差），此时需要控制物体进一步移动至
待规划点，直到物体到达待规划点后再朝下移动至目标放置点。

[0138] S6.规划运动轨迹并利用两个视觉系统获取的物体位姿，对机械臂进行闭环控制从而使物体沿规划的轨迹移至待规划点，并执行步骤S5。

[0139] 其中，见图6，规划运动轨迹并利用两个视觉系统获取的物体位姿，对机械臂进行闭环控制从而使物体沿规划的轨迹移至待规划点包括步骤：

[0140] S601.采用五次多项式插补算法规划出从当前位置到待规划点的由多个路径点组成的运动轨迹，并计算出在各路径点处的理论物体位姿；

[0141] S602.机械臂带动物体沿规划出的运动轨迹依次移向各路径点；

[0142] S603.在每个各路径点处联合两个视觉系统获取实际物体位姿；获取实际物体位姿的具体过程与步骤S4相同；

[0143] S604.当在某个路径点处的实际物体位姿与理论物体位姿不相等时重复步骤S601获得新的运动轨迹，并沿新运动轨迹继续移动，直到到达待规划点。

[0144] 在机械臂带动物体到达待规划点的过程中，如果只是一次性进行规划出运动轨迹后，使机械臂直接沿该运动轨迹移动，则在移动过程中机械臂的运动与规划的轨迹会存在
误差，且该误差会积累，最终导致无法精确到达待规划点。

[0145] 本发明在该过程中，规划出运动轨迹后，机械臂使物体沿该轨迹运动并通过视觉系统实时对物体的位姿进行监控，一旦在某个路径点处的位姿偏离理论值时（偏差超过允
差范围即判定为偏离理论值，其中允差范围根据实际精度要求进行设置），立刻重新规划从
当前位置到待规划点的运动轨迹，并使机械臂沿新的运动轨迹执行动作，从而有效避免误
差积累，大大地提高了到达目标点的精确度；此外，由于在某个路径点处的位姿偏离理论值
时，是重新规划运动轨迹，而不是控制物体进行位置调整直到回到该路径点的正确位姿后
再沿原来轨迹继续运动，可避免往复运动位置调整的过程，大大提高运行效率。

[0146] 此外，若完成一次规划运动轨迹并利用两个视觉系统获取的物体位姿，对机械臂进行闭环控制从而使物体沿规划的轨迹移至待规划点的过程后，获取的物体位姿和步骤S2
中得到的理论位姿依然不相等，则重复步骤S6，直到两者相等，如图5所示，可进一步保证达
目标点的精确度。

[0147] S7. 机械臂带动物体朝下移至目标放置点。

[0148] 由于物体通过步骤S6已经精确地到达目标放置点正上方预设高度h处的待规划点，此时只需要带动动物体朝下移动距离h即可实现精确放置。

[0149] 由上可知，该基于3D视觉的机械臂精确放置轨迹规划方法，具有以下优点：

[0150] 1.设置两个视觉系统来获取物体的位姿，并在获取物体的位姿时，只有当两个视觉系统获取的位姿值相等是才会作为有效位姿值，从而提高获取位姿精确度；

[0151] 2. 用两个视觉系统来实时监控机械臂运动，获取的机械臂实时运动状态的数据的精度高；

[0152] 3. 通过规划运动轨迹并利用两个视觉系统获取的物体位姿，对机械臂进行闭环控制从而使物体沿规划的轨迹移至待规划点，可大大提高物体达到待规划点的精确度，从
而提高放置精度。

[0153] 综上所述，虽然本发明已以优选实施例揭露如上，但上述优选实施例并非用以限制本发明，本领域的普通技术人员，在不脱离本发明的精神和范围内，均可作各种更动与润
饰，其方案与本发明实质上相同。