一种具备避障能力的机械臂地面小物体抓取方法和系统与流程

1.本发明涉及机器人控制领域和机器视觉领域，尤其涉及一种具备避障能力的机械臂地面小物体抓取方法和系统。


背景技术：

2.在实际应用中，例如清洁机器人、物流仓储等场景下，需要机械臂对地面小物体进行准确的抓取，并且在遇到障碍物时能够自主避障，以便进行后续的处理和运输等任务。然而，在复杂的环境中，机械臂的避障和力反馈控制等问题往往受到多种因素的影响，这就需要通过新的方法来解决。
3.地面小物体抓取研究面临着一些挑战。首先，由于地面小物体的尺寸和形状多样，导致机械臂的抓取难度较大，同时在遇到障碍物时，机械臂需要自主避障以避免碰撞。其次，机械臂的力反馈控制需要对运动的障碍物进行分析和处理，以确保抓取过程的稳定性和安全性。


技术实现要素：

4.本发明要克服现有技术的上述缺点，一种机械臂自主避障并抓取目标的方法。
5.为了解决上述问题，本发明提出了一种地面小物体机械臂避障、力反馈控制抓取方法。该方法利用深度学习和计算机视觉等技术，对地面小物体进行识别和分析，并通过不同的传感器来获取环境信息，以实现机械臂的避障和力反馈控制。此外，本发明还采用了力控制器来控制机械臂的抓取过程，确保抓取的稳定性和安全性。
6.一种具备避障能力的机械臂地面小物体抓取方法，其特征在于，包括：
7.s1:在机器人系统上布置传感器，采集点云进行环境建模；
8.s2:在点云上进行噪点滤除；
9.s3:在点云上生成障碍遮挡部分的假想体积；
10.s4:在点云上滤除目标周围障碍物；
11.s5:生成障碍物体素单元；
12.s6:机械臂避障的力反馈控制；
13.s7:物体检测及抓取末端姿态计算；
14.s8:机械臂规划路径、执行抓取。
15.进一步，步骤s1具体为：
16.所述的机器人系统包括两个前置深度相机，即左目相机和右目相机，一个机械臂手眼相机，一个机械臂；左目相机和右目相机对称分布，固定在机器人上，机械臂通过底座固定在机器人上，位置相对左目和右目相机固定，手眼相机固定于机械臂最末端手掌关节，跟随机械臂末端运动。
17.进一步，步骤s2具体为：
18.s21.采用两个前置深度相机输出的点云作为避障感知的信息来源；
19.s22.采用先均匀降采样，将点云间隔稀疏到4cm左右；再以10cm为搜索半径，对半径内点云数少于6个点的点云进行离群点滤除。
20.进一步，步骤s3具体为：
21.首先捕捉到地面以上5cm所有点云，通过离群点过滤，获得障碍物边缘的点云，再将边缘往后方平移生成假想体积，最终生成的障碍物体素单元。
22.进一步，步骤s4具体为：
23.过滤掉抓取物体本身和周围区间的地面，包括：以石头中心为中心点，生成前后长度30cm、左右长度30cm、上下长度8cm的矩形，移除矩形框内的碰撞体素单元。
24.进一步，步骤s5具体为：
25.将点云数据转换成体素表示，包括以下几个步骤：
26.·
确定体素大小：首先需要确定体素的大小，即每个体素的边长。边长的选择取决于应用需求和点云数据的分辨率。
27.·
创建体素网格：根据确定的体素大小，在三维空间中创建一个体素网格。
28.体素网格是由若干个体素单元组成的三维数组。
29.·
将点云数据映射到体素网格：遍历点云中的每个点，根据点的位置将其映射到对应的体素单元中。可以采用最近邻法或插值法来确定点在体素中的位置。
30.进一步，步骤s6具体为：
31.通过更高频率的机械臂关节力反馈，在机械臂碰撞到障碍物时，及时终止当前轨迹，回到前一位置，并重新规划机械臂路径；
32.检测碰撞的方法是，监测一段时间正常采样过程中各个关节的力反馈，以此区间为基准，再正负方向扩展2n/m的弯矩，作为正常的机械臂关节力反馈区间，超出该区间，即终止机械臂运动。
33.进一步，步骤s7具体为：
34.通过前置深度相机，用卷积神经网络yolox识别、定位目标，获得目标的位置信息；根据目标的位置信息计算机械臂末端位姿，采用kinova gen2两指机械臂，在采集地面小物体时，末端姿态计算需要考虑以下几点因素：
35.·
双指与地面平行，防止抓取时一指先触地；
36.·
在最远处臂完全展开接近直线，相对于垂直地面抓取范围更大；
37.·
远离时爪外倾如图14，靠近时爪内倾；
38.·
远离时爪正朝臂基座，伸的远；
39.·
靠近时双指与机身平行，避免与机身碰撞的前提下增加靠近时抓取范围；
40.因此，可以将末端姿态与抓取目标的位置相关联，假设目标位于机械臂基座坐标系位置为x、y、z，基座坐标系为x超前，y朝侧，z朝上。
41.采用python语言，调用scipy库计算末端姿态：
42.from scipy.spatial.transform import rotation as r
43.首先计算末端正朝下的姿态，作为调整基准姿态，此时末端坐标系z轴朝下，x轴朝侧，y轴朝前。
44.r1＝r.from_euler('zyx',[-3.1415/2,3.1415,0])
[0045]
以末端正朝下作为基准坐标系，根据目标物位置，按照以上原则调整末端姿态。
[0046]
首先为了保证末端远离时爪正朝臂基座，靠近时双指与机身平行，末端绕z轴旋转角为：
[0047]
θ＝atan(y/x)*(x-0.2))*-3
[0048]
为了保证末端远离机身时爪外倾，靠近时爪内倾，末端绕x轴旋转角为：
[0049]
φ＝(sqrt(x**2+y**2)-0.4)*-3
[0050]
为了保证末端始终与地面平行，末端绕y轴旋转角为0度。
[0051]
因此末端姿态描述如下：
[0052]
r2＝r.from_euler('zyx',[(θ,0,φ])
[0053]
最终计算相对基座坐标系下的末端姿态。
[0054]
r＝r1*r2
[0055]
进一步，步骤s8具体为：
[0056]
采用moveit作为机械臂规划器，通过设置目标位姿，确定机械臂抓取时的目标位姿；使用moveit提供的规划器，将目标位姿作为输入，生成机械臂的路径。可以选择不同的规划算法和参数来满足具体的需求；最后，将生成的路径发送给机械臂控制器，执行抓取动作。使用moveit提供的接口或编写自定义的控制逻辑来实现路径执行。
[0057]
本发明的第二个方面涉及一种具备避障能力的机械臂地面小物体抓取系统，包括：
[0058]
环境建模模块，用于布置传感器，采集点云进行环境建模；
[0059]
噪点滤除模块，用于在点云上进行噪点滤除；
[0060]
障碍遮挡部分的假想体积生产模块，用于在点云上生成障碍遮挡部分的假想体积；
[0061]
目标周围障碍物滤除模块，用于在点云上滤除目标周围障碍物；
[0062]
障碍物体素单元生产模块，用于生成障碍物体素单元；
[0063]
力反馈控制模块，用于机械臂避障的力反馈控制；
[0064]
物体检测及抓取末端姿态计算模块，用于物体检测及抓取末端姿态计算；
[0065]
机械臂路径规划、抓取执行模块，用于机械臂规划路径、执行抓取。
[0066]
本发明的第三个方面涉及一种具备避障能力的机械臂地面小物体抓取装置，包括存储器和一个或多个处理器，所述存储器中存储有可执行代码，所述一个或多个处理器执行所述可执行代码时，用于实现本发明的一种具备避障能力的机械臂地面小物体抓取方法。
[0067]
本发明的第四个方面涉及一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时，实现本发明的一种具备避障能力的机械臂地面小物体抓取方法。
[0068]
本发明的工作原理是：
[0069]
通过深度相机感知环境中的抓取目标和障碍物，通过算法根据目标位置和障碍物位置，规划机械臂轨迹，控制机械臂抓取物体，并且在机械臂运动过程中，根据力反馈传感器感知的关节扭矩信息，及时终止当前动作，实现机械臂被动避障。通过算法根据目标位置，生成末端抓取位姿，根据抓取位姿规划机械臂轨迹，实现避障抓取。
[0070]
本发明的创新点是：
[0071]
针对深度相机生成的点云，进行了高效率的噪声滤除，通过自主开发算法生成相
机无法观察到的障碍物遮挡部分体积、以及滤除目标周围影响规划的障碍，实现更加稳定的视觉避障规划方案，同时结合机械臂力反馈传感器，实现了机械臂碰撞到障碍后的自主力感知和规避。通过两种避障方案结合，实现了高稳定性的机械臂避障方案。通过自主算法，根据抓取目标位置，生成机械臂末端抓取位姿，实现了更大的抓取范围，相对深度学习计算机械臂末端位姿，具有更高的稳定性和计算效率。
[0072]
本发明的优点是：
[0073]
通过视觉主动避障和力反馈被动避障相结合，实现在机械臂抓取物体过程中的高稳定性避障能力。通过算法实现较大的机械臂抓取范围。
附图说明
[0074]
图1是本发明所使用的机器人的结构示意图。
[0075]
图2是本发明的机器人坐标是示意图。
[0076]
图3是本发明的场景布置示意图。
[0077]
图4是本发明的机器人点云采集示意图。
[0078]
图5是本发明的点云过滤示意图。
[0079]
图6是是本发明的机器人点云采集示意图。
[0080]
图7是本发明的机器人捕捉到地面以上5cm点云的示意图。
[0081]
图8是本发明通过离群点过滤，获得障碍物边缘的点云的示意图。
[0082]
图9是本发明将边缘往后方平移生成假想体积，最终生成的障碍物体素单元的示意图。
[0083]
图10是本发明移除矩形框内的碰撞体素单元的效果图。
[0084]
图11是本发明的点云转体素单元的效果图。
[0085]
图12是本发明的目标定位和抓取姿态示意图。
[0086]
图13是本发明的机器人的机械臂近处抓取形态图，黄色为假想障碍。
[0087]
图14是本发明点的机器人的机械臂远处抓取形态图，黄色为假想障碍。
[0088]
图15是本发明的机器人的机械臂抓取姿态图。
[0089]
图16是本发明的方法流程图。
[0090]
图17是本发明的系统结构图。
具体实施方式
[0091]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0092]
需要说明的是，在不冲突的情况下，下述的实施例及实施方式中的特征可以相互组合。
[0093]
实施例1
[0094]
本发明通过机器人的相机和机械臂，实现地面小物体抓取。以六足机器人为说明对象，但不限于此类型机器人。当机器人前方出现目标物时，本实施例提供一种机械臂自主
避障并抓取目标的方法。具体步骤如下：
[0095]
1.在机器人系统上布置传感器，采集点云进行环境建模；
[0096]
本发明涉及到的机器人系统包括两个前置深度相机(左目相机和右目相机)，一个机械臂手眼相机，一个机械臂。相机和机械臂的位置关系如图1所示，坐标系关系如图2所示。首先搭建了一个场景作为场景说明，机器人所在场景如图3所示；通过两个前置相机采集点云，采集的点云如图4所示。
[0097]
2.在点云上进行噪点滤除；
[0098]
六足机器人抓取区间为正前方0.5m范围内，由于雷达短距离内误差大的特性，前置补盲雷达在该区域内不能提供有效信息，因此采用两个前置深度相机输出的点云作为避障感知的信息来源。深度相机提供的点云会出现噪点情况，如图4所示，而避障过程中容易把噪点当成障碍，因此需要对噪点进行滤除。
[0099]
深度相机噪声的特点是离散出现或小片出现，因此本文采用先均匀降采样，将点云间隔稀疏到4cm左右；再以10cm为搜索半径，对半径内点云数少于6个点的点云进行离群点滤除，过滤离群点后的点云效果如图5所示。该方法效率高，过滤效果较好。
[0100]
3.在点云上生成障碍遮挡部分的假想体积；
[0101]
由于前置相机的高度在离地面30cm左右，对于一定高度的障碍，只能采集到正前方向的点云，如图6所示，无法采集障碍物顶部的点云，在避障时，可能导致机械臂碰撞到障碍顶部，因此，需要对障碍物无法观察到的部分生成假想体积。出于安全考虑，本文采用了比较保守的方法，假设地面以上5cm的所有区间检测到的障碍，都在障碍物后方有30cm的厚度。因此需要生成这部分假想点云，可以直接将所有点云往后平移几层，但是由于碰撞检测中，机械臂需要避开所有障碍体积，障碍体素单元越多，计算量越大，这种方法会导致生成大量无效碰撞体素单元。本文采用的方法是首先捕捉到地面以上5cm所有点云，如图7所示，通过离群点过滤，获得障碍物边缘的点云，如图8所示，再将边缘往后方平移生成假想体积，最终生成的障碍物体素单元如图9所示。这种方式大量的减少了计算量。生成的假想障碍如图8所示。
[0102]
4.在点云上滤除目标周围障碍物；
[0103]
由于生成障碍体素单元的点云包括抓取物体本身和地面，为了让机械臂能够接近地面抓取物体，生成障碍物时，需要过滤掉抓取物体本身和周围一定区间的地面。本文采用的方法是，以石头中心为中心点，生成前后长度30cm、左右长度30cm、上下长度8cm的矩形，移除矩形框内的碰撞体素单元，效果如图10所示。
[0104]
5.生成障碍物体素单元；
[0105]
体素是三维空间中的离散单元，类似于像素在二维图像中的作用。通过将点云数据转换成体素表示，可以方便进行进一步的处理和分析。
[0106]
点云转体素单元的过程包括以下几个步骤：
[0107]
·
确定体素大小：首先需要确定体素的大小，即每个体素的边长。边长的选择取决于应用需求和点云数据的分辨率。
[0108]
·
创建体素网格：根据确定的体素大小，在三维空间中创建一个体素网格。
[0109]
体素网格是由若干个体素单元组成的三维数组。
[0110]
·
将点云数据映射到体素网格：遍历点云中的每个点，根据点的位置将其映射到
对应的体素单元中。可以采用最近邻法或插值法来确定点在体素中的位置。
[0111]
通过点云转体素单元，可以将点云数据转换为规则的体素表示，方便进行体素级别的操作和分析，如体素化重建、体素分类等。同时，由于体素表示具有较低的维度，可以节省存储空间和计算资源。点云转体素单元的效果如图11所示。
[0112]
6.机械臂避障的力反馈控制；
[0113]
通过视觉检测避障，机械臂抓取具备了一部分避障能力，但是视觉检测的算力成本高，因此本文采用的方法是在同一个位置只生成一次视觉观测障碍，而当视觉检测频率低时，存在运动物体阻挡导致避让不及时的可能。因此，进一步通过更高频率的机械臂关节力反馈，在机械臂碰撞到障碍物时，及时终止当前轨迹，回到前一位置，并重新规划机械臂路径。
[0114]
机械臂各个关节反馈力的控制区间需要满足正常采用下，关节力在控制区间内，而遇到障碍时，在不损害机械臂本身的前提下，尽早检测出碰撞。本文采用的方法是，监测一段时间正常采样过程中各个关节的力反馈，以此区间为基准，再正负方向扩展2n/m的弯矩，作为正常的机械臂关节力反馈区间，超出该区间，即终止机械臂运动。
[0115]
7.物体检测及抓取末端姿态计算；
[0116]
通过前置深度相机，用卷积神经网络yolox识别、定位目标，获得目标的位置信息。进一步根据目标的位置信息计算机械臂末端位姿，本文采用kinova gen2两指机械臂，在采集地面小物体时，末端姿态计算需要考虑以下几点因素：
[0117]
·
双指与地面平行，防止抓取时一指先触地
[0118]
·
在最远处臂完全展开接近直线，相对于垂直地面抓取范围更大，如图15
[0119]
·
远离时爪外倾如图14，靠近时爪内倾如图13
[0120]
·
远离时爪正朝臂基座，伸的远，如图14
[0121]
·
靠近时双指与机身平行，避免与机身碰撞的前提下增加靠近时抓取范围，
[0122]
如图15
[0123]
因此，可以将末端姿态与抓取目标的位置相关联，假设目标位于机械臂基座坐标系(图2的tf_2)下位置为x、y、z，基座坐标系为x超前，y朝侧，z朝上，如图15所示。
[0124]
本文采用python语言，调用scipy库计算末端姿态：
[0125]
from scipy.spatial.transform import rotation as r
[0126]
首先计算末端正朝下的姿态，作为调整基准姿态，如图15黄色姿态，此时末端坐标系z轴朝下，x轴朝侧，y轴朝前。
[0127]
r1＝r.from_euler('zyx',[-3.1415/2,3.1415,0])
[0128]
以末端正朝下作为基准坐标系，根据目标物位置，按照以上原则调整末端姿态。
[0129]
首先为了保证末端远离时爪正朝臂基座，靠近时双指与机身平行，末端绕z轴旋转角为：
[0130]
θ＝atan(y/x)*(x-0.2))*-3
[0131]
为了保证末端远离机身时爪外倾，靠近时爪内倾，末端绕x轴旋转角为：
[0132]
φ＝(sqrt(x**2+y**2)-0.4)*-3
[0133]
为了保证末端始终与地面平行，末端绕y轴旋转角为0度。
[0134]
因此末端姿态描述如下：
[0135]
r2＝r.from_euler('zyx',[(θ,0,φ])
[0136]
最终计算相对基座坐标系下的末端姿态。
[0137]
r＝r1*r2
[0138]
8.机械臂规划路径、执行抓取。
[0139]
本发明采用moveit作为机械臂规划器，通过设置目标位姿，确定机械臂抓取时的目标位姿。进一步使用moveit提供的规划器，将目标位姿作为输入，生成机械臂的路径。可以选择不同的规划算法和参数来满足具体的需求。最后，将生成的路径发送给机械臂控制器，执行抓取动作。可以使用moveit提供的接口或编写自定义的控制逻辑来实现路径执行。
[0140]
实施例2
[0141]
本实施例涉及一种具备避障能力的机械臂地面小物体抓取系统，用于实现实施例1的一种具备避障能力的机械臂地面小物体抓取方法，包括：
[0142]
环境建模模块，用于布置传感器，采集点云进行环境建模；
[0143]
噪点滤除模块，用于在点云上进行噪点滤除；
[0144]
障碍遮挡部分的假想体积生产模块，用于在点云上生成障碍遮挡部分的假想体积；
[0145]
目标周围障碍物滤除模块，用于在点云上滤除目标周围障碍物；
[0146]
障碍物体素单元生产模块，用于生成障碍物体素单元；
[0147]
力反馈控制模块，用于机械臂避障的力反馈控制；
[0148]
物体检测及抓取末端姿态计算模块，用于物体检测及抓取末端姿态计算；
[0149]
机械臂路径规划、抓取执行模块，用于机械臂规划路径、执行抓取。
[0150]
实施例3
[0151]
本实施例涉及一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时，实现本发明的一种具备避障能力的机械臂地面小物体抓取方法。
[0152]
实施例4
[0153]
本实施例涉及一种具备避障能力的机械臂地面小物体抓取装置，包括存储器和一个或多个处理器，所述存储器中存储有可执行代码，所述一个或多个处理器执行所述可执行代码时，用于实现实施例1的一种具备避障能力的机械臂地面小物体抓取方法。
[0154]
在硬件层面，该一种具备避障能力的机械臂地面小物体抓取装置包括处理器、内部总线、网络接口、内存以及非易失性存储器，当然还可能包括其他业务所需要的硬件。处理器从非易失性存储器中读取对应的计算机程序到内存中然后运行。当然，除了软件实现方式之外，本发明并不排除其他实现方式，比如逻辑器件抑或软硬件结合的方式等等，也就是说以下处理流程的执行主体并不限定于各个逻辑单元，也可以是硬件或逻辑器件。
[0155]
对于一个技术的改进可以很明显地区分是硬件上的改进(例如，对二极管、晶体管、开关等电路结构的改进)还是软件上的改进(对于方法流程的改进)。然而，随着技术的发展，当今的很多方法流程的改进已经可以视为硬件电路结构的直接改进。设计人员几乎都通过将改进的方法流程编程到硬件电路中来得到相应的硬件电路结构。因此，不能说一个方法流程的改进就不能用硬件实体模块来实现。例如，可编程逻辑器件(programmable logic device,pld)(例如现场可编程门阵列(field programmable gate array，fpga))就是这样一种集成电路，其逻辑功能由用户对器件编程来确定。由设计人员自行编程来把一
个数字系统“集成”在一片pld上，而不需要请芯片制造厂商来设计和制作专用的集成电路芯片。而且，如今，取代手工地制作集成电路芯片，这种编程也多半改用“逻辑编译器(logic compiler)”软件来实现，它与程序开发撰写时所用的软件编译器相类似，而要编译之前的原始代码也得用特定的编程语言来撰写，此称之为硬件描述语言(hardware description language，hdl)，而hdl也并非仅有一种，而是有许多种，如abel(advanced boolean expression language)、ahdl(altera hardware description language)、confluence、cupl(cornell university programming language)、hdcal、jhdl(java hardware description language)、lava、lola、myhdl、palasm、rhdl(ruby hardware description language)等，目前最普遍使用的是vhdl(very-high-speed integrated circuit hardware description language)与verilog。本领域技术人员也应该清楚，只需要将方法流程用上述几种硬件描述语言稍作逻辑编程并编程到集成电路中，就可以很容易得到实现该逻辑方法流程的硬件电路。
[0156]
控制器可以按任何适当的方式实现，例如，控制器可以采取例如微处理器或处理器以及存储可由该(微)处理器执行的计算机可读程序代码(例如软件或固件)的计算机可读介质、逻辑门、开关、专用集成电路(application specific integrated circuit，asic)、可编程逻辑控制器和嵌入微控制器的形式，控制器的例子包括但不限于以下微控制器：arc 625d、atmel at91sam、microchip pic18f26k20以及silicone labs c8051f320，存储器控制器还可以被实现为存储器的控制逻辑的一部分。本领域技术人员也知道，除了以纯计算机可读程序代码方式实现控制器以外，完全可以通过将方法步骤进行逻辑编程来使得控制器以逻辑门、开关、专用集成电路、可编程逻辑控制器和嵌入微控制器等的形式来实现相同功能。因此这种控制器可以被认为是一种硬件部件，而对其内包括的用于实现各种功能的装置也可以视为硬件部件内的结构。或者甚至，可以将用于实现各种功能的装置视为既可以是实现方法的软件模块又可以是硬件部件内的结构。
[0157]
上述实施例阐明的系统、装置、模块或单元，具体可以由计算机芯片或实体实现，或者由具有某种功能的产品来实现。一种典型的实现设备为计算机。具体的，计算机例如可以为个人计算机、膝上型计算机、蜂窝电话、相机电话、智能电话、个人数字助理、媒体播放器、导航设备、电子邮件设备、游戏控制台、平板计算机、可穿戴设备或者这些设备中的任何设备的组合。
[0158]
为了描述的方便，描述以上装置时以功能分为各种单元分别描述。当然，在实施本发明时可以把各单元的功能在同一个或多个软件和/或硬件中实现。
[0159]
本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。
[0160]
本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产
生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
[0161]
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
[0162]
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
[0163]
在一个典型的配置中，计算设备包括一个或多个处理器(cpu)、输入/输出接口、网络接口和内存。
[0164]
内存可能包括计算机可读介质中的非永久性存储器，随机存取存储器(ram)和/或非易失性内存等形式，如只读存储器(rom)或闪存(flash ram)。内存是计算机可读介质的示例。
[0165]
计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(pram)、静态随机存取存储器(sram)、动态随机存取存储器(dram)、其他类型的随机存取存储器(ram)、只读存储器(rom)、电可擦除可编程只读存储器(eeprom)、快闪记忆体或其他内存技术、只读光盘只读存储器(cd-rom)、数字多功能光盘(dvd)或其他光学存储、磁盒式磁带，磁带磁磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括暂存电脑可读媒体(transitory media)，如调制的数据信号和载波。
[0166]
还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个
……”
限定的要素，并不排除在包括所述要素的过程、方法、商品或者设备中还存在另外的相同要素。
[0167]
本领域技术人员应明白，本发明的实施例可提供为方法、系统或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。
[0168]
本发明可以在由计算机执行的计算机可执行指令的一般上下文中描述，例如程序模块。一般地，程序模块包括执行特定任务或实现特定抽象数据类型的例程、程序、对象、组件、数据结构等等。也可以在分布式计算环境中实践本发明，在这些分布式计算环境中，由通过通信网络而被连接的远程处理设备来执行任务。在分布式计算环境中，程序模块可以位于包括存储设备在内的本地和远程计算机存储介质中。
[0169]
本发明中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分
互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于系统实施例而言，由于其基本相似于方法实施例，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。
[0170]
以上所述仅为本发明的实施例而已，并不用于限制本发明。对于本领域技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原理之内所作的任何修改、等同替换、改进等，均应包含在本发明的权利要求范围之内。

技术特征：
1.一种具备避障能力的机械臂地面小物体抓取方法，其特征在于，包括：s1:在机器人系统上布置传感器，采集点云进行环境建模；s2:在点云上进行噪点滤除；s3:在点云上生成障碍遮挡部分的假想体积；s4:在点云上滤除目标周围障碍物；s5:生成障碍物体素单元；s6:机械臂避障的力反馈控制；s7:物体检测及抓取末端姿态计算；s8:机械臂规划路径、执行抓取。2.根据权利要求1所述的一种具备避障能力的机械臂地面小物体抓取方法，其特征在于，步骤s1具体为：所述的机器人系统包括两个前置深度相机，即左目相机和右目相机，一个机械臂手眼相机，一个机械臂；左目相机和右目相机对称分布，固定在机器人上，机械臂通过底座固定在机器人上，位置相对左目和右目相机固定，手眼相机固定于机械臂最末端手掌关节，跟随机械臂末端运动。3.根据权利要求1所述的一种具备避障能力的机械臂地面小物体抓取方法，其特征在于，步骤s2具体为：s21.采用两个前置深度相机输出的点云作为避障感知的信息来源；s22.采用先均匀降采样，将点云间隔稀疏到4cm左右；再以10cm为搜索半径，对半径内点云数少于6个点的点云进行离群点滤除。4.根据权利要求1所述的一种具备避障能力的机械臂地面小物体抓取方法，其特征在于，步骤s3具体为：首先捕捉到地面以上5cm所有点云，通过离群点过滤，获得障碍物边缘的点云，再将边缘往后方平移生成假想体积，最终生成的障碍物体素单元。5.根据权利要求1所述的一种具备避障能力的机械臂地面小物体抓取方法，其特征在于，步骤s4具体为：过滤掉抓取物体本身和周围区间的地面，包括：以石头中心为中心点，生成前后长度30cm、左右长度30cm、上下长度8cm的矩形，移除矩形框内的碰撞体素单元。6.根据权利要求1所述的一种具备避障能力的机械臂地面小物体抓取方法，其特征在于，步骤s5具体为：将点云数据转换成体素表示，包括以下几个步骤：
·
确定体素大小：首先需要确定体素的大小，即每个体素的边长。边长的选择取决于应用需求和点云数据的分辨率。
·
创建体素网格：根据确定的体素大小，在三维空间中创建一个体素网格。体素网格是由若干个体素单元组成的三维数组。
·
将点云数据映射到体素网格：遍历点云中的每个点，根据点的位置将其映射到对应的体素单元中。可以采用最近邻法或插值法来确定点在体素中的位置。7.根据权利要求1所述的一种具备避障能力的机械臂地面小物体抓取方法，其特征在于，步骤s6具体为：
通过更高频率的机械臂关节力反馈，在机械臂碰撞到障碍物时，及时终止当前轨迹，回到前一位置，并重新规划机械臂路径；检测碰撞的方法是，监测一段时间正常采样过程中各个关节的力反馈，以此区间为基准，再正负方向扩展2n/m的弯矩，作为正常的机械臂关节力反馈区间，超出该区间，即终止机械臂运动。8.根据权利要求1所述的一种具备避障能力的机械臂地面小物体抓取方法，其特征在于，步骤s7具体为：通过前置深度相机，用卷积神经网络yolox识别、定位目标，获得目标的位置信息；根据目标的位置信息计算机械臂末端位姿，采用kinova gen2两指机械臂，在采集地面小物体时，末端姿态计算需要考虑以下几点因素：
·
双指与地面平行，防止抓取时一指先触地；
·
在最远处臂完全展开接近直线，相对于垂直地面抓取范围更大；
·
远离时爪外倾如图14，靠近时爪内倾；
·
远离时爪正朝臂基座，伸的远；
·
靠近时双指与机身平行，避免与机身碰撞的前提下增加靠近时抓取范围；将末端姿态与抓取目标的位置相关联，假设目标位于机械臂基座坐标系位置为x、y、z，基座坐标系为x超前，y朝侧，z朝上。采用python语言，调用scipy库计算末端姿态：from scipy.spatial.transform import rotation as r首先计算末端正朝下的姿态，作为调整基准姿态，此时末端坐标系z轴朝下，x轴朝侧，y轴朝前。r1＝r.from_euler('zyx',[-3.1415/2,3.1415,0])以末端正朝下作为基准坐标系，根据目标物位置，按照以上原则调整末端姿态。首先为了保证末端远离时爪正朝臂基座，靠近时双指与机身平行，末端绕z轴旋转角为：θ＝atan(y/x)*(x-0.2))*-3为了保证末端远离机身时爪外倾，靠近时爪内倾，末端绕x轴旋转角为：φ＝(sqrt(x**2+y**2)-0.4)*-3为了保证末端始终与地面平行，末端绕y轴旋转角为0度。因此末端姿态描述如下：r2＝r.from_euler('zyx',[(θ,0,φ])最终计算相对基座坐标系下的末端姿态：r＝r1*r2。9.根据权利要求1所述的一种具备避障能力的机械臂地面小物体抓取方法，其特征在于，s8具体为：采用moveit作为机械臂规划器，通过设置目标位姿，确定机械臂抓取时的目标位姿；使用moveit提供的规划器，将目标位姿作为输入，生成机械臂的路径。可以选择不同的规划算法和参数来满足具体的需求；最后，将生成的路径发送给机械臂控制器，执行抓取动作。使用moveit提供的接口或编写自定义的控制逻辑来实现路径执行。
10.一种具备避障能力的机械臂地面小物体抓取系统，其特征在于，包括：环境建模模块，用于布置传感器，采集点云进行环境建模；噪点滤除模块，用于在点云上进行噪点滤除；障碍遮挡部分的假想体积生成模块，用于在点云上生成障碍遮挡部分的假想体积；目标周围障碍物滤除模块，用于在点云上滤除目标周围障碍物；障碍物体素单元生成模块，用于生成障碍物体素单元；力反馈控制模块，用于机械臂避障的力反馈控制；物体检测及抓取末端姿态计算模块，用于物体检测及抓取末端姿态计算；机械臂路径规划、抓取执行模块，用于机械臂规划路径、执行抓取。11.一种具备避障能力的机械臂地面小物体抓取装置，其特征在于，包括存储器和一个或多个处理器，所述存储器中存储有可执行代码，所述一个或多个处理器执行所述可执行代码时，用于实现权利要求1-9中任一项所述的一种具备避障能力的机械臂地面小物体抓取方法。12.一种计算机可读存储介质，其特征在于，其上存储有程序，该程序被处理器执行时，实现权利要求1-9中任一项所述的一种具备避障能力的机械臂地面小物体抓取方法。

技术总结
一种具备避障能力的机械臂地面小物体抓取方法和系统，其方法包括：S1:布置传感器，采集点云进行环境建模；S2:在点云上进行噪点滤除；S3:在点云上生成障碍遮挡部分的假想体积；S4:在点云上滤除目标周围障碍物；S5:机械臂避障的力反馈控制；S6:物体检测及抓取末端姿态计算；S7：规划路径执行抓取。本发明用在机械臂抓取硬质地面小尺寸物体时，结合传感器技术、环境建模、避障算法和抓取策略，使机械臂能够在复杂环境中自主地避开障碍物，并准确地抓取地面上的小物体。地面上的小物体。地面上的小物体。


技术研发人员：喻源 李月华 俞志成 李强 朱世强
受保护的技术使用者：之江实验室
技术研发日：2023.08.04
技术公布日：2023/9/20