本发明属于机器视觉领域，具体涉及种基于ros的自主移动抓取系统与方法。
背景技术：
随着日趋严重的老龄化问题、迫使社会转移更多的人力，物力到老年人的生活照顾上，服务机器人能够很大程度上释放日常工作的人力投入，但是目前来说大多数服务机器人都是功能单一、对话型机器人，而对于具备拟人化，自主环境感知、视觉指导任务的移动机器人还尚处于研究阶段，不够成熟，难以完成复杂集成的任务。除此之外，移动机器人在灾害救援、军事侦查等场所应用越来越广。对于高危场所，移动机器人的使用很大程度上可以减轻人员的伤亡，但是在如何将视觉传感技术与自动控制、运动规划、视觉导航等方法融合，完美实现自主抓取是移动机器人领域的关键技术。因此研究出一种可以自主感知环境、实时检测、自主移动并执行相关任务的自主移动抓取系统，减轻人工的劳动，提高高危作业的安全性，具有很重要的理论价值和实际意义。
技术实现要素：
鉴于上述背景技术的不足，本发明的目的在于提供一种基于ros的自主移动抓取系统，该系统通过图像采集、视觉算法处理、视觉导航和视觉抓取完成了对环境的感知和物体抓取功能。很好的集成了视觉传感技术、视觉导航、运动规划和自动控制等技术。节省了不必要的人员作业。解放了不必要的人力投入。具有很好的工程意义。
一种基于ros的自主移动抓取系统，该系统分为四部分：图像采集模块、视觉导航模块、位姿检测模块以及抓取控制模块；图像采集模块由rgb-d相机、nvidiajetsontx2和转动云台模块构成，rgb-d相机和nvidiajetsontx2负责采集彩色图像和深度图像，转动云台控制模块由电机和蓝牙控制板组成，在执行视觉导航任务时，控制云台控制相机转到水平方向，在执行位姿检测任务时，控制云台控制相机转到竖直方向；视觉导航模块由移动小车、红外传感器和nvidiageforcegtx1080ti服务器构成；rgb-d相机采集场景彩色图像和深度图像并传输到服务器，服务器执行slam算法进行建图并基于ros使用move_base库控制移动小车进行导航，红外传感器用于导航过程中的避障；位姿检测模块由nvidiageforcegtx1080ti服务器完成，rgb-d相机采集目标彩色图像和深度图像并传输到服务器，服务器执行像素投票神经网络pvnet回归关键点的坐标，并基于pnp算法对物体的6d姿态进行计算；抓取控制模块包括5自由度的机械臂及其控制器，控制器接收目标抓取位姿，使用ros的moveit运动规划算法库规划机械臂运动路径，执行抓取任务；其中的ros为机器人操作系统robotoperatingsystem；6d姿态包括3d位置和3d方向。
一种基于ros的自主移动抓取系统的抓取方法，该方法具体如下：
步骤一：将小车放入陌生环境，图像采集模块采集到的彩色图像和深度图像；
步骤二：图像采集模块采集到的彩色图像和深度图像通过ros传输到视觉导航模块并执行slam算法，构建地图；利用二维码图标标记目标位置区域，通过检测二维码图标的位姿确定目标区域，移动小车根据ros的move_base运动导航库规划的路径自主移动到目标区域附近；
其中地图构建基于rgbd-slam-v2框架，前端视觉里程计从每一帧彩色图像中提取特征，借助orb特征描述子，使用ransac+icp算法计算两帧图像之间的位姿关系，之后进行基于g2o优化库的后端以及回环检测优化，优化后的位姿结合彩色图和深度图，生成稠密地图；然后通过二维码图标标记找到目标区域位姿
步骤三：到达指定区域之后，通过消息队列遥测传输协议发送指令给蓝牙控制板控制摄像头转向，接下来将图像采集模块采集到的彩色图像进行预处理，将预处理后的彩色图像通过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位姿，具体为：
构建改进型像素级投票神经网络pvnet，具体为：
pvnet的输入f(x,y,z)是大小为h×w的彩色图，其使用预训练的resnet-18作为基础网络，当特征图大小为h/8×w/8时，使用带有三种不同空洞率的atrousspatialpyramidpooling模块进行提取特征，在卷积层和上采样层中重复使用残差连接，然后进行上采样，直到输出的尺寸达到h×w；在最后一层特征图使用1×1的卷积层输出像素级预测方向向量和语义分割结果。具体aspp模块为就是将不同空洞率的卷积核去处理特征图，然后将特征图连接到一起扩大通道数，最后通过一个1×1卷积层得到同样大小的输出特征图。
(a)将输入图片f(x,y,z)送入像素级投票神经网络pvnet回归得到语义分割结果和像素级预测方向向量，具体方向向量的定义为：
对于一个像素p，网络输出与其相关联的对象标签和代表从像素p到物体2d关键点
(b)投票定位关键点坐标
给定语义标签和单位向量，使用基于ransac的投票方案生成关键点假设，首先使用语义标签找到属于目标对象的像素p，然后，随机选择两个像素，并将这两个像素点对应的预测单位方向向量vk(p)的交点作为关键点
其中θ是一个阈值，p∈o表示像素点p属于物体o，选择分数最高的假设作为该关键点的2d坐标预测值
其中将估计位姿
根据得到的2d关键点坐标
根据手眼标定得到的手眼矩阵
根据
然后取最小的θi对应的轴作为最终变换轴即i＝min(θi)
(b)假设估计的物体姿态坐标系cobject和机械臂基坐标系cbase之间的变换关系为旋转矩阵为r，位移为t
根据旋转矩阵得到对应各个轴的欧拉角为：
由(a)知旋转轴i，则根据式(6)得到对应旋转角θi，将机械臂基坐标系旋转到z轴竖直向下，然后将z轴旋转θi，并根据
步骤四：机械臂控制器接收到目标抓取位姿之后，通过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|其中一个大小在阈值之外，反馈没有物体存在。
本发明的有益效果：本发明方法采用图像采集、视觉算法处理、视觉导航与视觉抓取等方式实现了对未知环境的感知和物体的移动抓取功能，集成了多个传感器的信息融合、完成了分布式的设计，具有执行能力强、功能模块分工明确，可快速识别环境中的目标并作出反应，可以代替人工完成特定任务。一定程度上解放人工的劳动力。本发明具有自主感知、检测精度高、实时检测目标位姿等优点，具有可观的理论价值和实践价值。
具体实施方式：
一种基于ros的自主移动抓取系统，该系统分为四部分：图像采集模块、视觉导航模块、位姿检测模块以及抓取控制模块；图像采集模块由rgb-d相机、nvidiajetsontx2和转动云台模块构成，rgb-d相机和nvidiajetsontx2负责采集彩色图像和深度图像，转动云台控制模块由电机和蓝牙控制板组成，在执行视觉导航任务时，控制云台控制相机转到水平方向，在执行位姿检测任务时，控制云台控制相机转到竖直方向；视觉导航模块由移动小车、红外传感器和nvidiageforcegtx1080ti服务器构成；rgb-d相机采集场景彩色图像和深度图像并传输到服务器，服务器执行slam算法进行建图并基于ros使用move_base库控制移动小车进行导航，红外传感器用于导航过程中的避障；位姿检测模块由nvidiageforcegtx1080ti服务器完成，rgb-d相机采集目标彩色图像和深度图像并传输到服务器，服务器执行像素投票神经网络pvnet回归关键点的坐标，并基于pnp算法对物体的6d姿态进行计算；抓取控制模块包括5自由度的机械臂及其控制器，控制器接收目标抓取位姿，使用ros的moveit运动规划算法库规划机械臂运动路径，执行抓取任务；其中的ros为机器人操作系统robotoperatingsystem；6d姿态包括3d位置和3d方向。
一种基于ros的自主移动抓取系统的抓取方法，该方法具体如下：
步骤一：将小车放入陌生环境，图像采集模块采集到的彩色图像和深度图像；
步骤二：图像采集模块采集到的彩色图像和深度图像通过ros传输到视觉导航模块并执行slam算法，构建地图；利用二维码图标标记目标位置区域，通过检测二维码图标的位姿确定目标区域，移动小车根据ros的move_base运动导航库规划的路径自主移动到目标区域附近；
其中地图构建基于rgbd-slam-v2框架，前端视觉里程计从每一帧彩色图像中提取特征，借助orb特征描述子，使用ransac+icp算法计算两帧图像之间的位姿关系，之后进行基于g2o优化库的后端以及回环检测优化，优化后的位姿结合彩色图和深度图，生成稠密地图；然后通过二维码图标标记找到目标区域位姿
步骤三：到达指定区域之后，通过消息队列遥测传输协议发送指令给蓝牙控制板控制摄像头转向，接下来将图像采集模块采集到的彩色图像进行预处理，将预处理后的彩色图像通过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位姿，具体为：
构建像素级投票神经网络pvnet，具体为：
pvnet的输入f(x,y,z)是大小为h×w的彩色图，其使用预训练的resnet-18作为基础网络，当特征图大小为h/8×w/8时，使用带有三种不同空洞率的atrousspatialpyramidpooling模块进行提取特征，在卷积层和上采样层中重复使用残差连接，然后进行上采样，直到输出的尺寸达到h×w；在最后一层特征图使用1×1的卷积层输出像素级预测方向向量和语义分割结果。具体aspp模块为就是将不同空洞率的卷积核去处理特征图，然后将特征图连接到一起扩大通道数，最后通过一个1×1卷积层得到同样大小的输出特征图。
(a)将输入图片f(x,y,z)送入像素级投票神经网络pvnet回归得到语义分割结果和像素级预测方向向量，具体方向向量的定义为：
对于一个像素p，网络输出与其相关联的对象标签和代表从像素p到物体2d关键点
(b)投票定位关键点坐标
给定语义标签和单位向量，使用基于ransac的投票方案生成关键点假设，首先使用语义标签找到属于目标对象的像素p，然后，随机选择两个像素，并将这两个像素点对应的预测单位方向向量vk(p)的交点作为关键点
其中θ是一个阈值，p∈o表示像素点p属于物体o，选择分数最高的假设作为该关键点的2d坐标预测值
其中将估计位姿
根据得到的2d关键点坐标
根据手眼标定得到的手眼矩阵
根据
然后取最小的θi对应的轴作为最终变换轴即i＝min(θi)
(b)假设估计的物体姿态坐标系cobject和机械臂基坐标系cbase之间的变换关系为旋转矩阵为r，位移为t
根据旋转矩阵得到对应各个轴的欧拉角为：
由(a)知旋转轴i，则根据式(6)得到对应旋转角θi，将机械臂基坐标系旋转到z轴竖直向下，然后将z轴旋转θi，并根据
步骤四：机械臂控制器接收到目标抓取位姿之后，通过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|其中一个大小在阈值之外，反馈没有物体存在。