一种基于ROS的自主移动抓取系统与方法[发明专利]

(12)发明专利申请

(10)申请公布号 CN 111055281 A(43)申请公布日 2020.04.24

(21)申请号 201911320327.6(22)申请日 2019.12.19

(71)申请人 杭州电子科技大学

地址 310018 浙江省杭州市下沙高教园区2

号大街(72)发明人 杨宇翔　孙卫军　高明煜　董哲康　

林辉品　曾毓　(74)专利代理机构 杭州君度专利代理事务所

(特殊普通合伙) 33240

代理人 杨舟涛(51)Int.Cl.

B25J 9/16(2006.01)B25J 5/00(2006.01)

权利要求书3页 说明书7页

()发明名称

一种基于ROS的自主移动抓取系统与方法(57)摘要

本发明涉及一种基于ROS的自主移动抓取系统与方法。本发明可以分为图像采集模块、视觉算法处理模块、位姿检测模块以及抓取控制模块。本方法实现了移动机器人在陌生环境下的环境感知，视觉导航以及实时的物体位姿估计并抓取的功能。将移动机器人与机器视觉相结合，使得机器人可以完成更智能的任务，提供良好的服务，减轻了不必要的负担。本发明具有效率高、检测精度高、可实时检测、良好的适应能力等优点，可以在很大程度上释放日常工作上的人力投入，具有很强的市场潜力。大大方便服务人们工作和生活，有助于促进生活水平的提高。CN 111055281 ACN 111055281 A

权　利　要　求　书

1/3页

1.一种基于ROS的自主移动抓取系统，其特征在于：该系统分为四部分：图像采集模块、视觉导航模块、位姿检测模块以及抓取控制模块；图像采集模块由RGB-D相机、NVIDIA Jetson TX2和转动云台模块构成，RGB-D相机和NVIDIA Jetson TX2负责采集彩色图像和深度图像，转动云台控制模块由电机和蓝牙控制板组成，在执行视觉导航任务时，控制云台控制相机转到水平方向，在执行位姿检测任务时，控制云台控制相机转到竖直方向；视觉导航模块由移动小车、红外传感器和NVIDIA GeForce GTX 1080Ti服务器构成；RGB-D相机采集场景彩色图像和深度图像并传输到服务器，服务器执行SLAM算法进行建图并基于ROS使用move_base库控制移动小车进行导航，红外传感器用于导航过程中的避障；位姿检测模块由NVIDIA GeForce GTX 1080Ti服务器完成，RGB-D相机采集目标彩色图像和深度图像并传输到服务器，服务器执行像素投票神经网络PVnet回归关键点的坐标，并基于PnP算法对物体的6D姿态进行计算；抓取控制模块包括5自由度的机械臂及其控制器，控制器接收目标抓取位姿，使用ROS的Moveit运动规划算法库规划机械臂运动路径，执行抓取任务；其中的ROS为机器人操作系统Robot Operating System；6D姿态包括3D位置和3D方向。

2.根据权利要求1所述的一种基于ROS的自主移动抓取系统的抓取方法，其特征在于：步骤一：将小车放入陌生环境，图像采集模块采集到的彩色图像和深度图像；步骤二：图像采集模块采集到的彩色图像和深度图像通过ROS传输到视觉导航模块并执行SLAM算法，构建地图；利用二维码图标标记目标位置区域，通过检测二维码图标的位姿确定目标区域，移动小车根据ROS的move_base运动导航库规划的路径自主移动到目标区域附近；

其中地图构建基于RGBD-SLAM-V2框架，前端视觉里程计从每一帧彩色图像中提取特征，借助ORB特征描述子，使用RANSAC+ICP算法计算两帧图像之间的位姿关系，之后进行基于g2o优化库的后端以及回环检测优化，优化后的位姿结合彩色图和深度图，生成稠密地图；然后通过二维码图标标记找到目标区域位姿

移动小车接收到

使用

ROS的move_base运动导航库导航到目标区域；

步骤三：到达指定区域之后，通过消息队列遥测传输协议发送指令给蓝牙控制板控制摄像头转向，接下来将图像采集模块采集到的彩色图像进行预处理，将预处理后的彩色图像通过ROS发送到位姿检测模块；

对彩色图像进行预处理具体为：

f(x,y,z)表示大小为H×W的一帧图像，x,y表示图像的横像素坐标和纵像素坐标，z表示通道数，彩色图z＝3，用于位姿检测时，对彩色图，使用数据集的平均值和方差标准化处理：

f(x,y,z)＝(f(x,y,z)-mean[z])/std[z] z＝3              (1)其中mean＝[0.485,0.456,0.406] std＝[0.229,0.224,0.225]；步骤四：位姿检测模块通过位姿检测算法计算得到物体的6D位姿，并转换为目标抓取位姿，通过ROS系统将目标抓取位姿发送给抓取控制模块；

其中位姿检测模块通过位姿检测算法计算得到物体的6D位姿，具体为：

(a)将输入图片f(x,y,z)送入像素级投票神经网络PVnet回归得到语义分割结果和像素级预测方向向量，具体方向向量的定义为：

2

CN 111055281 A

权　利　要　求　书

2/3页

对于一个像素p，网络输出与其相关联的对象标签和代表从像素p到物体2D关键点

的单位方向向量；vk(p)定义为：

(b)投票定位关键点坐标给定语义标签和单位向量，使用基于RANSAC的投票方案生成关键点假设，首先使用语义标签找到属于目标对象的像素p，然后，随机选择两个像素，并将这两个像素点对应的预测单位方向向量vk(p)的交点作为关键点

的假设hk,i，重复N次，产生一系列关键

点的预测{hk,i|i＝1,2,3...,N}，对于假设hk,i投票的分数wk,i定义为：

其中θ是一个阈值，p∈O表示像素点p属于物体O，选择分数最高的假设作为该关键点的2D坐标预测值

其中将估计位姿

转换为抓取位姿pgrasp具体为：

和已知物体模型上对应关键点的三维点坐标

根据得到的2D关键点坐标

根据2D-3D的对应关系，使用PnP算法得到相对于相机坐标系的估计位姿

根据手眼标定得到的手眼矩阵

根据位姿

标系

将变换到相对于机械臂基坐标下的位姿

得到相对于机械臂基坐标系Cbase的估计物体坐

采用自上而下的抓取方式，机械臂末端夹持器竖直向下时的，机

械臂末端坐标系Z轴也是竖直向下的，机械臂基坐标系的Z轴是竖直向上的；将物体坐标系Cobject和机械臂基坐标系Cbase通过位移使原点重合；基坐标系Z轴的单位向量为坐标系Cobject各个轴的单位向量为

之间的夹角：

计算得到

物体与

根据进一步得到向量之间的锐角θθθx,y,z：

3

CN 111055281 A

权　利　要　求　书

3/3页

然后取最小的θθi对应的轴作为最终变换轴即i＝min(i)

(b)假设估计的物体姿态坐标系Cobject和机械臂基坐标系Cbase之间的变换关系为旋转矩阵为R，位移为T

根据旋转矩阵得到对应各个轴的欧拉角为：

由(a)知旋转轴i，则根据式(6)得到对应旋转角θ将机械臂基坐标系旋转到Z轴竖直向i，下，然后将Z轴旋转θi，并根据

中的位移T做相应的位移即可得到最终抓取位姿

pgrasp；

步骤四：机械臂控制器接收到目标抓取位姿之后，通过ROS的Moveit运动规划库规划机械臂运动抓取；如果机械臂无法到达目标位姿，根据反馈信息相应的调整小车的位置，再次进行检测抓取，直到抓取完成，或者反馈没有物体存在；

将得到的抓取位姿pgrasp通过ROS以话题的形式发送给抓取控制模块的机械臂控制器；机械臂收到抓取姿态pgrasp后，根据机械臂末端极限位移减去目标位移Δx,Δy,Δz，判断pgrasp在位移上是否超过了机械臂的运动空间；如果Δx,Δy,Δz都为正，机械臂通过ROS的Moveit运动规划库规划机械臂运动，在运动到目标位置前，设置了抓取中间位置pgrasp_mid，pgrasp_mid是由pgrasp向上平移一定的距离得到，抓取之后的放置位置pplace同样经过这个中间位置pgrasp然后到达设定的放置位置pplace；如果Δx,Δy,Δz中有负数，且|Δx|,|Δy|,|Δz|大小在阈值之内，可以进行相应的方向移动使得Δx,Δy,Δz都为正，再次进行试抓，完成最终视觉抓取任务；若|Δx|,|Δy|,|Δz|其中一个大小在阈值之外，反馈没有物体存在。

3.根据权利要求2所述的一种基于ROS的自主移动抓取系统的抓取方法，其特征在于：所述的改进型像素级投票神经网络PVnet为：

PVnet的输入f(x,y,z)是大小为H×W的彩色图，其使用预训练的Resnet-18作为基础网络，当特征图大小为H/8×W/8时，使用带有三种不同空洞率的Atrousspatial pyramid pooling模块进行提取特征，在卷积层和上采样层中重复使用残差连接，然后进行上采样，直到输出的尺寸达到H×W；在最后一层特征图使用1×1的卷积层输出像素级预测方向向量和语义分割结果；具体ASPP模块为就是将不同空洞率的卷积核去处理特征图，然后将特征图连接到一起扩大通道数，最后通过一个1×1卷积层得到同样大小的输出特征图。

4

CN 111055281 A

说　明　书

一种基于ROS的自主移动抓取系统与方法

1/7页

技术领域

[0001]本发明属于机器视觉领域，具体涉及种基于ROS的自主移动抓取系统与方法。背景技术

[0002]随着日趋严重的老龄化问题、迫使社会转移更多的人力，物力到老年人的生活照顾上，服务机器人能够很大程度上释放日常工作的人力投入，但是目前来说大多数服务机器人都是功能单一、对话型机器人，而对于具备拟人化，自主环境感知、视觉指导任务的移动机器人还尚处于研究阶段，不够成熟，难以完成复杂集成的任务。除此之外，移动机器人在灾害救援、军事侦查等场所应用越来越广。对于高危场所，移动机器人的使用很大程度上可以减轻人员的伤亡，但是在如何将视觉传感技术与自动控制、运动规划、视觉导航等方法融合，完美实现自主抓取是移动机器人领域的关键技术。因此研究出一种可以自主感知环境、实时检测、自主移动并执行相关任务的自主移动抓取系统，减轻人工的劳动，提高高危作业的安全性，具有很重要的理论价值和实际意义。发明内容

[0003]鉴于上述背景技术的不足，本发明的目的在于提供一种基于ROS的自主移动抓取系统，该系统通过图像采集、视觉算法处理、视觉导航和视觉抓取完成了对环境的感知和物体抓取功能。很好的集成了视觉传感技术、视觉导航、运动规划和自动控制等技术。节省了不必要的人员作业。了不必要的人力投入。具有很好的工程意义。[0004]一种基于ROS的自主移动抓取系统，该系统分为四部分：图像采集模块、视觉导航模块、位姿检测模块以及抓取控制模块；图像采集模块由RGB-D相机、NVIDIA Jetson TX2和转动云台模块构成，RGB-D相机和NVIDIA Jetson TX2负责采集彩色图像和深度图像，转动云台控制模块由电机和蓝牙控制板组成，在执行视觉导航任务时，控制云台控制相机转到水平方向，在执行位姿检测任务时，控制云台控制相机转到竖直方向；视觉导航模块由移动小车、红外传感器和NVIDIA GeForce GTX 1080Ti服务器构成；RGB-D相机采集场景彩色图像和深度图像并传输到服务器，服务器执行SLAM算法进行建图并基于ROS使用move_base库控制移动小车进行导航，红外传感器用于导航过程中的避障；位姿检测模块由NVIDIA GeForce GTX 1080Ti服务器完成，RGB-D相机采集目标彩色图像和深度图像并传输到服务器，服务器执行像素投票神经网络PVnet回归关键点的坐标，并基于PnP算法对物体的6D姿态进行计算；抓取控制模块包括5自由度的机械臂及其控制器，控制器接收目标抓取位姿，使用ROS的Moveit运动规划算法库规划机械臂运动路径，执行抓取任务；其中的ROS为机器人操作系统Robot Operating System；6D姿态包括3D位置和3D方向。[0005]一种基于ROS的自主移动抓取系统的抓取方法，该方法具体如下：[0006]步骤一：将小车放入陌生环境，图像采集模块采集到的彩色图像和深度图像；[0007]步骤二：图像采集模块采集到的彩色图像和深度图像通过ROS传输到视觉导航模块并执行SLAM算法，构建地图；利用二维码图标标记目标位置区域，通过检测二维码图标的

5

CN 111055281 A

说　明　书

2/7页

位姿确定目标区域，移动小车根据ROS的move_base运动导航库规划的路径自主移动到目标区域附近；

[0008]其中地图构建基于RGBD-SLAM-V2框架，前端视觉里程计从每一帧彩色图像中提取特征，借助ORB特征描述子，使用RANSAC+ICP算法计算两帧图像之间的位姿关系，之后进行基于g2o优化库的后端以及回环检测优化，优化后的位姿结合彩色图和深度图，生成稠密地图；然后通过二维码图标标记找到目标区域位姿

移动小车接收到

使用

ROS的move_base运动导航库导航到目标区域；[0009]步骤三：到达指定区域之后，通过消息队列遥测传输协议发送指令给蓝牙控制板控制摄像头转向，接下来将图像采集模块采集到的彩色图像进行预处理，将预处理后的彩色图像通过ROS发送到位姿检测模块；[0010]对彩色图像进行预处理具体为：

[0011]f(x,y,z)表示大小为H×W的一帧图像，x,y表示图像的横像素坐标和纵像素坐标，z表示通道数，彩色图z＝3，用于位姿检测时，对彩色图，使用数据集的平均值和方差标准化处理：

[0012]f(x,y,z)＝(f(x,y,z)-mean[z])/std[z]z＝3               (1)[0013]其中mean＝[0.485,0.456,0.406]std＝[0.229,0.224,0.225]；[0014]步骤四：位姿检测模块通过位姿检测算法计算得到物体的6D位姿，并转换为目标抓取位姿，通过ROS系统将目标抓取位姿发送给抓取控制模块；

[0015]其中位姿检测模块通过位姿检测算法计算得到物体的6D位姿，具体为：[0016]构建改进型像素级投票神经网络PVnet，具体为：[0017]PVnet的输入f(x,y,z)是大小为H×W的彩色图，其使用预训练的Resnet-18作为基础网络，当特征图大小为H/8×W/8时，使用带有三种不同空洞率的Atrous spatial pyramid pooling模块进行提取特征，在卷积层和上采样层中重复使用残差连接，然后进行上采样，直到输出的尺寸达到H×W；在最后一层特征图使用1×1的卷积层输出像素级预测方向向量和语义分割结果。具体ASPP模块为就是将不同空洞率的卷积核去处理特征图，然后将特征图连接到一起扩大通道数，最后通过一个1×1卷积层得到同样大小的输出特征图。[0018](a)将输入图片f(x,y,z)送入像素级投票神经网络PVnet回归得到语义分割结果和像素级预测方向向量，具体方向向量的定义为：[0019]对于一个像素p，网络输出与其相关联的对象标签和代表从像素p到物体2D关键点

的单位方向向量；vk(p)定义为：

[0020]

(b)投票定位关键点坐标[0022]给定语义标签和单位向量，使用基于RANSAC的投票方案生成关键点假设，首先使用语义标签找到属于目标对象的像素p，然后，随机选择两个像素，并将这两个像素点对应

[0021]

的预测单位方向向量vk(p)的交点作为关键点

6

的假设hk,i，重复N次，产生一系列

CN 111055281 A

说　明　书

3/7页

关键点的预测{hk,i|i＝1,2,3...,N}，对于假设hk,i投票的分数wk,i定义为：

[0023][0024]

其中θ是一个阈值，p∈O表示像素点p属于物体O，选择分数最高的假设作为该关键

点的2D坐标预测值

[0025][0026]

其中将估计位姿转换为抓取位姿pgrasp具体为：

和已知物体模型上对应关键点的三维点坐

根据得到的2D关键点坐标

标姿

[0027]

根据2D-3D的对应关系，使用PnP算法得到相对于相机坐标系的估计位

根据手眼标定得到的手眼矩阵

根据位姿

将变换到相对于机械臂基坐标下

的位姿物体坐标系

得到相对于机械臂基坐标系Cbase的估计

采用自上而下的抓取方式，机械臂末端夹持器竖直向下

时的，机械臂末端坐标系Z轴也是竖直向下的，机械臂基坐标系的Z轴是竖直向上的；将物体坐标系Cobject和机械臂基坐标系Cbase通过位移使原点重合；基坐标系Z轴的单位向量为

物体坐标系Cobject各个轴的单位向量为

与

[0028]

计算得到

之间的夹角：

[0029]根据进一步得到向量之间的锐角θθθx,y,z：

[0030][0031]

然后取最小的θθi对应的轴作为最终变换轴即i＝min(i)[0032](b)假设估计的物体姿态坐标系Cobject和机械臂基坐标系Cbase之间的变换关系为旋转矩阵为R，位移为T

[0033]

[0034]

根据旋转矩阵得到对应各个轴的欧拉角为：

7

CN 111055281 A

说　明　书

4/7页

[0035]

[0036]

由(a)知旋转轴i，则根据式(6)得到对应旋转角θ将机械臂基坐标系旋转到Z轴i，

中的位移T做相应的位移即可得到最终抓取位

竖直向下，然后将Z轴旋转θ并根据i，

姿pgrasp；

[0037]步骤四：机械臂控制器接收到目标抓取位姿之后，通过ROS的Moveit运动规划库规划机械臂运动抓取；如果机械臂无法到达目标位姿，根据反馈信息相应的调整小车的位置，再次进行检测抓取，直到抓取完成，或者反馈没有物体存在；

[0038]将得到的抓取位姿pgrasp通过ROS以话题的形式发送给抓取控制模块的机械臂控制器；机械臂收到抓取姿态pgrasp后，根据机械臂末端极限位移减去目标位移Δx,Δy,Δz，判断pgrasp在位移上是否超过了机械臂的运动空间；如果Δx,Δy,Δz都为正，机械臂通过ROS的Moveit运动规划库规划机械臂运动，在运动到目标位置前，设置了抓取中间位置pgrasp_mid，pgrasp_mid是由pgrasp向上平移一定的距离得到，抓取之后的放置位置pplace同样经过这个中间位置pgrasp然后到达设定的放置位置pplace；如果Δx,Δy,Δz中有负数，且|Δx|,|Δy|,|Δz|大小在阈值之内，可以进行相应的方向移动使得Δx,Δy,Δz都为正，再次进行试抓，完成最终视觉抓取任务；若|Δx|,|Δy|,|Δz|其中一个大小在阈值之外，反馈没有物体存在。

[0039]本发明的有益效果：本发明方法采用图像采集、视觉算法处理、视觉导航与视觉抓取等方式实现了对未知环境的感知和物体的移动抓取功能，集成了多个传感器的信息融合、完成了分布式的设计，具有执行能力强、功能模块分工明确，可快速识别环境中的目标并作出反应，可以代替人工完成特定任务。一定程度上人工的劳动力。本发明具有自主感知、检测精度高、实时检测目标位姿等优点，具有可观的理论价值和实践价值。具体实施方式：

[0040]一种基于ROS的自主移动抓取系统，该系统分为四部分：图像采集模块、视觉导航模块、位姿检测模块以及抓取控制模块；图像采集模块由RGB-D相机、NVIDIA Jetson TX2和转动云台模块构成，RGB-D相机和NVIDIA Jetson TX2负责采集彩色图像和深度图像，转动云台控制模块由电机和蓝牙控制板组成，在执行视觉导航任务时，控制云台控制相机转到水平方向，在执行位姿检测任务时，控制云台控制相机转到竖直方向；视觉导航模块由移动小车、红外传感器和NVIDIA GeForce GTX 1080Ti服务器构成；RGB-D相机采集场景彩色图像和深度图像并传输到服务器，服务器执行SLAM算法进行建图并基于ROS使用move_base库控制移动小车进行导航，红外传感器用于导航过程中的避障；位姿检测模块由NVIDIA GeForce GTX 1080Ti服务器完成，RGB-D相机采集目标彩色图像和深度图像并传输到服务器，服务器执行像素投票神经网络PVnet回归关键点的坐标，并基于PnP算法对物体的6D姿态进行计算；抓取控制模块包括5自由度的机械臂及其控制器，控制器接收目标抓取位姿，使用ROS的Moveit运动规划算法库规划机械臂运动路径，执行抓取任务；其中的ROS为机器人操作系统Robot Operating System；6D姿态包括3D位置和3D方向。

8

CN 111055281 A[0041]

说　明　书

5/7页

一种基于ROS的自主移动抓取系统的抓取方法，该方法具体如下：

[0042]步骤一：将小车放入陌生环境，图像采集模块采集到的彩色图像和深度图像；[0043]步骤二：图像采集模块采集到的彩色图像和深度图像通过ROS传输到视觉导航模块并执行SLAM算法，构建地图；利用二维码图标标记目标位置区域，通过检测二维码图标的位姿确定目标区域，移动小车根据ROS的move_base运动导航库规划的路径自主移动到目标区域附近；

[0044]其中地图构建基于RGBD-SLAM-V2框架，前端视觉里程计从每一帧彩色图像中提取特征，借助ORB特征描述子，使用RANSAC+ICP算法计算两帧图像之间的位姿关系，之后进行基于g2o优化库的后端以及回环检测优化，优化后的位姿结合彩色图和深度图，生成稠密地图；然后通过二维码图标标记找到目标区域位姿

移动小车接收到

使用

ROS的move_base运动导航库导航到目标区域；[0045]步骤三：到达指定区域之后，通过消息队列遥测传输协议发送指令给蓝牙控制板控制摄像头转向，接下来将图像采集模块采集到的彩色图像进行预处理，将预处理后的彩色图像通过ROS发送到位姿检测模块；[0046]对彩色图像进行预处理具体为：

[0047]f(x,y,z)表示大小为H×W的一帧图像，x,y表示图像的横像素坐标和纵像素坐标，z表示通道数，彩色图z＝3，用于位姿检测时，对彩色图，使用数据集的平均值和方差标准化处理：

[0048]f(x,y,z)＝(f(x,y,z)-mean[z])/std[z]z＝3                (1)[0049]其中mean＝[0.485,0.456,0.406]std＝[0.229,0.224,0.225]；[0050]步骤四：位姿检测模块通过位姿检测算法计算得到物体的6D位姿，并转换为目标抓取位姿，通过ROS系统将目标抓取位姿发送给抓取控制模块；

[0051]其中位姿检测模块通过位姿检测算法计算得到物体的6D位姿，具体为：[0052]构建像素级投票神经网络PVnet，具体为：

[0053]PVnet的输入f(x,y,z)是大小为H×W的彩色图，其使用预训练的Resnet-18作为基础网络，当特征图大小为H/8×W/8时，使用带有三种不同空洞率的Atrous spatial pyramid pooling模块进行提取特征，在卷积层和上采样层中重复使用残差连接，然后进行上采样，直到输出的尺寸达到H×W；在最后一层特征图使用1×1的卷积层输出像素级预测方向向量和语义分割结果。具体ASPP模块为就是将不同空洞率的卷积核去处理特征图，然后将特征图连接到一起扩大通道数，最后通过一个1×1卷积层得到同样大小的输出特征图。[00](a)将输入图片f(x,y,z)送入像素级投票神经网络PVnet回归得到语义分割结果和像素级预测方向向量，具体方向向量的定义为：[0055]对于一个像素p，网络输出与其相关联的对象标签和代表从像素p到物体2D关键点

的单位方向向量；vk(p)定义为：

[0056]

9

CN 111055281 A[0057]

说　明　书

6/7页

(b)投票定位关键点坐标

[0058]给定语义标签和单位向量，使用基于RANSAC的投票方案生成关键点假设，首先使用语义标签找到属于目标对象的像素p，然后，随机选择两个像素，并将这两个像素点对应的预测单位方向向量vk(p)的交点作为关键点

的假设hk,i，重复N次，产生一系列

关键点的预测{hk,i|i＝1,2,3...,N}，对于假设hk,i投票的分数wk,i定义为：

[0059]

[0060]

其中θ是一个阈值，p∈O表示像素点p属于物体O，选择分数最高的假设作为该关键

点的2D坐标预测值

[0061][0062]

其中将估计位姿转换为抓取位姿pgrasp具体为：

和已知物体模型上对应关键点的三维点坐

根据得到的2D关键点坐标

标姿

[0063]

根据2D-3D的对应关系，使用PnP算法得到相对于相机坐标系的估计位

根据手眼标定得到的手眼矩阵

根据位姿

将变换到相对于机械臂基坐标下

的位姿物体坐标系

得到相对于机械臂基坐标系Cbase的估计

采用自上而下的抓取方式，机械臂末端夹持器竖直向下

时的，机械臂末端坐标系Z轴也是竖直向下的，机械臂基坐标系的Z轴是竖直向上的；将物体坐标系Cobject和机械臂基坐标系Cbase通过位移使原点重合；基坐标系Z轴的单位向量为

物体坐标系Cobject各个轴的单位向量为

与

之间的夹角：

计算得到

[00]

[0065]根据进一步得到向量之间的锐角θθθx,y,z：

[0066][0067]

然后取最小的θθi对应的轴作为最终变换轴即i＝min(i)[0068](b)假设估计的物体姿态坐标系Cobject和机械臂基坐标系Cbase之间的变换关系为旋转矩阵为R，位移为T

10

CN 111055281 A

说　明　书

7/7页

[0069]

[0070]

根据旋转矩阵得到对应各个轴的欧拉角为：

[0071]

[0072]

由(a)知旋转轴i，则根据式(6)得到对应旋转角θ将机械臂基坐标系旋转到Z轴i，

中的位移T做相应的位移即可得到最终抓取位

竖直向下，然后将Z轴旋转θ并根据i，

姿pgrasp；

[0073]步骤四：机械臂控制器接收到目标抓取位姿之后，通过ROS的Moveit运动规划库规划机械臂运动抓取；如果机械臂无法到达目标位姿，根据反馈信息相应的调整小车的位置，再次进行检测抓取，直到抓取完成，或者反馈没有物体存在；

[0074]将得到的抓取位姿pgrasp通过ROS以话题的形式发送给抓取控制模块的机械臂控制器；机械臂收到抓取姿态pgrasp后，根据机械臂末端极限位移减去目标位移Δx,Δy,Δz，判断pgrasp在位移上是否超过了机械臂的运动空间；如果Δx,Δy,Δz都为正，机械臂通过ROS的Moveit运动规划库规划机械臂运动，在运动到目标位置前，设置了抓取中间位置pgrasp_mid，pgrasp_mid是由pgrasp向上平移一定的距离得到，抓取之后的放置位置pplace同样经过这个中间位置pgrasp然后到达设定的放置位置pplace；如果Δx,Δy,Δz中有负数，且|Δx|,|Δy|,|Δz|大小在阈值之内，可以进行相应的方向移动使得Δx,Δy,Δz都为正，再次进行试抓，完成最终视觉抓取任务；若|Δx|,|Δy|,|Δz|其中一个大小在阈值之外，反馈没有物体存在。

11