CN114310872A - 一种基于dgg点云分割网络的机械臂自动打菜方法 - Google Patents

Abstract

本发明涉及一种基于DGG点云分割网络的机械臂自动打菜方法。实现自动打菜首先需要对打菜点位姿进行估计，根据不同的菜品种类和菜品量选择不同的打菜动作，再对六轴机械臂的路径和轨迹进行规划，最后实现菜品的挖取。目前餐厅内主要通过人工实现打菜工作，具有效率低、劳动力成本高等缺点。本发明通过深度相机采集菜品表面的点云数据，提出一种基于图卷积的点云分割网络DGG实现对打菜点的预测，根据具体的打菜动作计算得到打菜点的姿态信息，再通过三次B样条插值和Squad插值算法对机械臂末端的轨迹进行规划，最后控制机械臂完成打菜任务。

Description

一种基于DGG点云分割网络的机械臂自动打菜方法

技术领域

本发明属于机器视觉领域，具体涉及一种基于DGG点云分割网络的机械臂自动打菜方法。

背景技术

针对在餐厅、食堂内使用的自动打菜机器人，其使用的打菜算法一般通过分析菜品表面深度信息特征进行计算，然而在多种多样的菜品面前，传统的算法难以得到理想的结果，缺乏一定的灵活性和通用性，而且在噪声面前也缺少一定的稳定性；随着深度学习在3D检测方面的应用不断深入，对3D数据的检测和分割准确率有着比较满意的结果，且能应对各种复杂多变的数据，因此采用深度学习的方法对菜品数据进行处理能够大幅度增加打菜的成功率。

发明内容

鉴于上述的问题，本发明通过深度相机实时采集菜品表面的点云数据，同时提出一种基于图卷积的点云分割网络DGG，对菜品点云数据进行处理并预测得到打菜点的位置信息，再根据打菜点之间菜品量的分布情况选择对应的打菜动作，根据3D空间旋转原理计算得到各个打菜点的姿态信息，再通过三次B样条插值和Squad插值算法对机械臂末端轨迹进行规划，并通过逆运动学得到关节空间内的运动信息，最后控制机械臂完成打菜动作。

本发明提供一种基于DGG点云分割网络的机械臂自动打菜方法，方法具体为：

步骤一：搭建DGG点云分割网络；首先构建局部点云的特征提取模块，定义一个有向图G＝(v,ε)来表征局部点云的几何结构，其中v,ε分别表示顶点和边；从输入的点云中随机选取一个点作为图G的中心，记为p_i，通过K近邻算法从R^3×3空间得到k个最近邻点{p_i1,p_i2,...,p_ik}，把局部点云的位置特征定义为

P_i,m＝{p_i1,m-p_i,m,p_i2,m-p_i,m,...,p_ik,m-p_i}

其中p_ik,m代表邻域里第k个点的第m维特征；

把局部点云的密集程度定义为

其中k代表邻域点数，F_m(j)代表点p_i的第m维特征，并把ρ_i,m作为位置特征P_i,m的权重函数；同时把局部点云的形状特征定义为

其中

代表两个向量之间的内积计算，

为图G内的边；把P_i,m和S_i,m分别经过两层的MLP结构实现进一步的特征提取，再通过最大池化层解决点云无序性问题，最后进行特征聚合后作为局部点云的整体特征。

接着把局部点云的整体特征输入到具有残差连接的三层MLP网络中，接着提升特征维度至1024；为了能够同时训练多种菜品数据，添加一维的菜品种类向量，经过最大池化函数后通过普通的三层MLP，最后一层的卷积输出通道数为分割标签的总数。DGG网络中所有卷积操作的步长为1，激活函数使用Leaky Relu，同时为避免训练过程中出现过拟合，在输出层前添加Dropout，其失活率设置为0.5。

步骤二：利用深度相机采集各种菜品表面的点云数据，其中相机固定在机械臂末端，采取“眼在手上”的标定方式；利用Semantic-Segmentation-Editor工具对数据进行标记，共分为三个标签：入勺区域、出勺区域和无关区域；分为训练集和测试集输入到步骤一中构建的DGG点云分割模型中，设置训练参数：迭代次数300，批次大小32，初始学习率0.001，使用SGD优化器对网络模型权重参数进行调整；保存损失值最低时的网络权重参数。

步骤三：将步骤一搭建的分割模型移植到ROS开发平台上，并加载步骤二中保存的网络权重参数，对ROS中实时采集的菜品点云数据进行目标点的预测，同时对网络的预测结果进行筛选；对预测结果区域点数较少或存在区域分离的情况进行剔除。通过ROS消息机制把预测结果发布出去。

步骤四：机械臂订阅步骤三中发布的话题得到预测结果，结合菜品表面深度信息计算得到末端执行器挖取的最大深度，得到机械臂末端执行器在笛卡尔空间下的姿态信息；具体计算过程如下：

a)设机械臂末端执行器为市场上通用的打菜勺，规定勺口中心为末端坐标系的原点，垂直勺面向上为Z轴方向，指向勺柄方向作为Y轴，X轴方向可根据右手定则确定；

b)将网络的预测区域的中心点作为入勺点(x_s,y_s,z_s)和出勺点(x_e,y_e,z_e)，如果入勺点或出勺点距离菜盆边界的距离小于打菜勺半径r_spoon，则执行步骤三，再次对点云进行预测；入勺点作为打菜勺的起点，出勺点作为打菜勺的前进方向，前进距离通过不同种类的菜品密度来决定；设满勺的菜品质量为G_dish，根据菜品密度ρ_dish得到菜品的体积

结合点云深度信息，以入勺点为起点、出勺点为前进方向，使用凸包近似算法计算并分割出体积为2V_dish的点云区域作为打菜区域，并把打菜区域的结尾坐标作为新的出勺点

在2D平面上选取(x_s,y_s,z_s)和

的中间位置作为过渡点(x_i,y_i)，其深度值z_i的计算公式如下：

其中n₁是打菜区域内介于入勺点和过渡点之间的点数，n₂是介于过渡点和出勺点之间的点数；如果计算得到深度值超过菜盆的深度，则返回步骤三，利用DGG网络重新进行预测。

c)得到打菜勺路点的空间位置后，还需要计算每个点的姿态；根据a)中的坐标系设置，z轴的正方向代表勺子的前进方向；对于菜品量的多少，设计两种不同的打菜动作，第一种是边旋转边前进，第二种是在前进的过程，既存在推也存在旋转的动作，其区别在于路点的姿态变化；第一种，设置入勺点的z轴指向过渡点，而过渡点z轴指向出勺点，出勺点的z轴垂直向上；第二种的入勺点z轴与世界坐标系平行并指向出勺点方向，过渡点的姿态与入勺点相同，代表从入勺点到过渡点之间是推的动作，出勺点的z轴同样垂直向上；所有的姿态都是基于相机基坐标系旋转而来，如第一种情况中入勺点的姿态，欧拉角为：

其中θ_z用于调整勺柄与大地之间的角度，一般控制在35°至50°之间；其他点的姿态计算方法类似。

由于相机固定在机械臂的末端，其坐标系会随着机械臂的移动而改变，所有将所有路点的位姿信息转换到世界坐标系下；假设在相机坐标系中某路点的空间位置为(x₀,y₀,z₀)，欧拉角为(θ_x,θ_y,θ_z)；相机在世界坐标系中位置为(x_c,y_c,z_c)，旋转矩阵为

首先把欧拉角转换为旋转矩阵形式

公式为

进而得到世界坐标系下的位姿信息为

步骤五:根据步骤四中得到基于世界坐标系的位姿信息，结合三次B样条插值和Squad插值算法得到机械臂末端执行器的轨迹信息；具体计算过程如下：

使用三次B样条对机械臂末端空间位置进行插值，其定义公式如下：

S(u)＝∑P_iN_i,k(u)

其中P_i是样条曲线的控制点，此处把入勺点、过渡点、出勺点作为控制点，N_i,k是样条曲线的基函数，k是曲线的次数，此处设置k＝3；样条基函数方程可通过递推公式进行求解：

使用Squad算法对机械臂末端四元素信息进行插值，首先需要把步骤四中计算得到旋转矩阵转换为四元素，转换公式为：

其中T为旋转矩阵，q为转换得到的四元素；假设入勺点、过渡点和出勺点的四元素为q_s,q_i,q_e，持续时间为t，Squad插值公式如下：

最后以时间节点为参考，把空间位置和四元素结合在一起得到机械臂末端完整的轨迹信息；通过逆运动学转换到关节空间下的运动信息，并发送给机械臂控制模块完成打菜任务。

本发明的有益效果：本发明利用传感器采集菜品表面的深度数据，通过DGG点云分割网络对打菜点进行识别，再根据菜品量多少选择不同的打菜动作并估计出打菜点的位姿信息，最后利用轨迹规划算法得到机械臂的运行轨迹。本方法能够准确估计出合适的打菜点，快速规划出对应的打菜轨迹，完成打菜任务。

附图说明

图1：为本发明的局部点云特征提取模块；

图2：为本发明的DGG点云分割网络结构。

具体实施步骤下面通过具体实施例，并结合附图，对本发明的技术方案作进一步的具体说明。

实施例1：

步骤一：搭建如图二所示的DGG点云分割网络；首先构建如图一所示的局部点云特征提取模块，定义一个有向图G＝(v,ε)来表征局部点云的几何结构，其中v,ε分别表示顶点和边。从输入的点云中随机选取一个点作为图G的中心，记为p_i，通过K近邻算法从R^3×3空间得到k＝40个最近邻点{p_i1,p_i2,...,p_ik}，把局部点云的位置特征定义为

P_i,m＝{p_i1,m-p_i,m,p_i2,m-p_i,m,...,p_ik,m-p_i}

其中p_ik,m代表邻域里第k个点的第m维特征；

把局部点云的密集程度定义为

其中k代表邻域点数，F_m(j)代表点p_i的第m维特征，此处m＝{1,2,3}；并把ρ_i,m作为位置特征P_i,m的权重函数；同时把局部点云的形状特征定义为

其中

代表两个向量之间的内积计算，

为图G内的边；把P_i,m和S_i,m分别经过两层的MLP(64，128)结构实现进一步的特征提取，再通过最大池化层解决点云无序性问题，最后进行特征聚合后作为局部点云的整体特征。

接着把局部点云的特征输入到具有残差连接的三层MLP(128，256，256)网络中，接着提升特征维度至1024；为了能够同时训练多种菜品数据，添加一维的菜品种类向量，经过最大池化函数后通过普通的三层MLP(1024，512，num)，最后一层的卷积输出通道数num为分割标签的总数。DGG网络中所有卷积操作的步长为1，激活函数使用Leaky Relu，同时为避免训练过程中出现过拟合，在输出层前添加Dropout，其失活率设置为0.5。

步骤二：利用深度相机采集各种菜品表面的点云数据，其中相机固定在机械臂末端，采取“眼在手上”的标定方式；利用Semantic-Segmentation-Editor工具对数据进行标记，共分为三个标签：入勺区域、出勺区域和无关区域；按照9:1比例分为训练集和测试集输入到步骤一中构建的DGG点云分割模型中，设置训练参数：迭代次数300，批次大小32，初始学习率0.001，使用SGD优化器对网络模型权重参数进行调整；保存损失值最低时的网络权重参数。

步骤三：将步骤一搭建的分割模型移植到ROS开发平台上，并加载步骤二中保存的网络权重参数，对ROS中实时采集的菜品点云数据进行目标点的预测，同时对网络的预测结果进行筛选；对预测结果区域点数较少或存在区域分离的情况进行剔除。通过ROS消息机制把预测结果发布出去。

步骤四：机械臂订阅步骤三中发布的话题得到预测结果，结合菜品表面深度信息计算得到末端执行器挖取的最大深度，得到机械臂末端执行器在笛卡尔空间下的姿态信息；具体计算过程如下：

a)假定机械臂末端执行器为市场上通用的打菜勺，规定勺口中心为末端坐标系的原点，垂直勺面向上为Z轴方向，指向勺柄方向作为Y轴，X轴方向可根据右手定则确定；

b)将网络的预测区域的中心点作为入勺点(x_s,y_s,z_s)和出勺点(x_e,y_e,z_e)，如果入勺点或出勺点距离菜盆边界的距离小于打菜勺半径r_spoon＝0.05m，则执行步骤三，再次对点云进行预测；入勺点作为打菜勺的起点，出勺点作为打菜勺的前进方向，前进距离通过不同种类的菜品密度来决定；假定满勺的菜品质量为G_dish，根据菜品密度ρ_dish得到菜品的体积

结合点云深度信息，以入勺点为起点、出勺点为前进方向，使用凸包近似算法计算并分割出体积为2V_dish的点云区域作为打菜区域，并把打菜区域的结尾坐标作为新的出勺点

在2D平面上选取(x_s,y_s,z_s)和

的中间位置作为过渡点(x_i,y_i)，其深度值z_i的计算公式如下：

其中n₁是打菜区域内介于入勺点和过渡点之间的点数，n₂是介于过渡点和出勺点之间的点数；如果计算得到深度值超过菜盆的深度，则返回步骤三，利用DGG网络重新进行预测。

c)得到打菜勺路点的空间位置后，还需要计算每个点的姿态；根据a)中的坐标系设置，z轴的正方向代表勺子的前进方向；对于菜品量的多少，设计两种不同的打菜动作，一种是边旋转边前进，另一种是在前进的过程，既存在推也存在旋转的动作，其区别在于路点的姿态变化；对于第一种，设置入勺点的z轴指向过渡点，而过渡点z轴指向出勺点，出勺点的z轴垂直向上；第二种的入勺点z轴与世界坐标系平行并指向出勺点方向，过渡点的姿态与入勺点相同，代表从入勺点到过渡点之间是推的动作，出勺点的z轴同样垂直向上；所有的姿态都是基于相机基坐标系旋转而来，例如第一种情况中入勺点的姿态，欧拉角为：

其中θ_z用于调整勺柄与大地之间的角度，一般控制在35°至50°之间；其他点的姿态计算方法类似。

由于相机固定在机械臂的末端，其坐标系会随着机械臂的移动而改变，所有将所有路点的位姿信息转换到世界坐标系下；假设在相机坐标系中某路点的空间位置为(x₀,y₀,z₀)，欧拉角为(θ_x,θ_y,θ_z)；相机在世界坐标系中位置为(x_c,y_c,z_c)，旋转矩阵为

首先把欧拉角转换为旋转矩阵形式

公式为

进而得到世界坐标系下的位姿信息为

步骤五:根据步骤四中得到基于世界坐标系的位姿信息，结合三次B样条插值和Squad插值算法得到机械臂末端执行器的轨迹信息；具体计算过程如下：

使用三次B样条对机械臂末端空间位置进行插值，其定义公式如下：

S(u)＝∑P_iN_i,k(u)

其中P_i是样条曲线的控制点，此处把入勺点、过渡点、出勺点作为控制点，N_i,k是样条曲线的基函数，k是曲线的次数，此处设置k＝3；样条基函数方程可通过递推公式进行求解：

使用Squad算法对机械臂末端四元素信息进行插值，首先需要把步骤四中计算得到旋转矩阵转换为四元素，转换公式为：

其中T为旋转矩阵，q为转换得到的四元素；假设入勺点、过渡点和出勺点的四元素为q_s,q_i,q_e，持续时间为t，Squad插值公式如下：

最后以时间节点为参考，把空间位置和四元素结合在一起得到机械臂末端完整的轨迹信息；最后通过逆运动学转换到关节空间下的运动信息，并发送给机械臂控制模块完成打菜任务。

Claims (1)

1.一种基于DGG点云分割网络的机械臂自动打菜方法，其特征在于：该方法包括以下步骤：

步骤一：搭建DGG点云分割网络；构建局部点云的特征提取模块，定义一个有向图G＝(v,ε)来表征局部点云的几何结构，其中v,ε分别表示顶点和边；从输入的点云中随机选取一个点作为图G的中心，记为p_i，通过K近邻算法从R^3×3空间得到k个最近邻点{p_i1,p_i2,...,p_ik}，把局部点云的位置特征定义为：

P_i,m＝{p_i1,m-p_i,m,p_i2,m-p_i,m,...,p_ik,m-p_i}

其中p_ik,m代表邻域里第k个点的第m维特征；

把局部点云的密集程度定义为：

其中k代表邻域点数，F_m(j)代表点p_i的第m维特征，并把ρ_i,m作为位置特征P_i,m的权重函数；

把局部点云的形状特征定义为：

其中

代表两个向量之间的内积计算，

为图G内的边；把P_i,m和S_i,m分别经过两层的MLP结构实现进一步的特征提取，通过最大池化层解决点云无序性问题，进行特征聚合后作为局部点云的整体特征；

把局部点云的整体特征输入到具有残差连接的三层MLP网络中，提升特征维度至1024；为了能够同时训练多种菜品数据，添加一维的菜品种类向量，经过最大池化函数后通过普通的三层MLP，最后一层的卷积输出通道数num为分割标签的总数；DGG网络中所有卷积操作的步长为1，激活函数使用Leaky Relu，同时为避免训练过程中出现过拟合，在输出层前添加Dropout，其失活率设置为0.5；

步骤二：利用深度相机采集菜品表面的点云数据，其中相机固定在机械臂末端；利用Semantic-Segmentation-Editor工具对数据进行标记，分为三个标签：入勺区域、出勺区域和无关区域；分为训练集和测试集输入到步骤一中构建的DGG点云分割模型中，设置训练参数：迭代次数300，批次大小32，初始学习率0.001，使用SGD优化器对网络模型权重参数进行调整；保存损失值最低时的网络权重参数；

步骤三：将步骤一搭建的分割模型移植到ROS开发平台上，并加载步骤二中保存的网络权重参数，对ROS中实时采集的菜品点云数据进行目标点的预测，同时对网络的预测结果进行筛选；对预测结果区域点数较少或存在区域分离的情况进行剔除；通过ROS消息机制把预测结果发布出去；

步骤四：机械臂订阅步骤三中发布的话题得到预测结果，结合菜品表面深度信息计算得到末端执行器挖取的最大深度，得到机械臂末端执行器在笛卡尔空间下的姿态信息；计算过程如下：

a)设机械臂末端执行器为市场上通用的打菜勺，规定勺口中心为末端坐标系的原点，垂直勺面向上为Z轴方向，指向勺柄方向作为Y轴，X轴方向可根据右手定则确定；

b)将网络的预测区域的中心点作为入勺点(x_s,y_s,z_s)和出勺点(x_e,y_e,z_e)，如果入勺点或出勺点距离菜盆边界的距离小于打菜勺半径r_spoon，则执行步骤三，再次对点云进行预测；入勺点作为打菜勺的起点，出勺点作为打菜勺的前进方向，前进距离通过不同种类的菜品密度来决定；设满勺的菜品质量为G_dish，根据菜品密度ρ_dish得到菜品的体积

结合点云深度信息，以入勺点为起点、出勺点为前进方向，使用凸包近似算法计算并分割出体积为2V_dish的点云区域作为打菜区域，并把打菜区域的结尾坐标作为新的出勺点

在2D平面上选取(x_s,y_s,z_s)和

的中间位置作为过渡点(x_i,y_i)，其深度值z_i的计算公式如下：

其中n₁是打菜区域内介于入勺点和过渡点之间的点数，n₂是介于过渡点和出勺点之间的点数；如果计算得到深度值超过菜盆的深度，则返回步骤三，利用DGG网络重新进行预测；

c)得到打菜勺路点的空间位置后，还需要计算每个点的姿态；根据a)中的坐标系设置，z轴的正方向代表勺子的前进方向；对于菜品量的多少，设计两种不同的打菜动作，第一种是边旋转边前进，第二种是在前进的过程，既存在推也存在旋转的动作，其区别在于路点的姿态变化；

第一种，设置入勺点的z轴指向过渡点，而过渡点z轴指向出勺点，出勺点的z轴垂直向上；

第二种的入勺点z轴与世界坐标系平行并指向出勺点方向，过渡点的姿态与入勺点相同，代表从入勺点到过渡点之间是推的动作，出勺点的z轴同样垂直向上；所有的姿态都是基于相机基坐标系旋转而来，其中第一种情况中入勺点的姿态，欧拉角为：

其中θ_z用于调整勺柄与大地之间的角度，控制在35°至50°之间；

由于相机固定在机械臂的末端，其坐标系会随着机械臂的移动而改变，所有将所有路点的位姿信息转换到世界坐标系下；假设在相机坐标系中某路点的空间位置为(x₀,y₀,z₀)，欧拉角为(θ_x,θ_y,θ_z)；相机在世界坐标系中位置为(x_c,y_c,z_c)，旋转矩阵为

首先把欧拉角转换为旋转矩阵形式

公式为：

进而得到世界坐标系下的位姿信息为：

步骤五:根据步骤四中得到基于世界坐标系的位姿信息，结合三次B样条插值和Squad插值算法得到机械臂末端执行器的轨迹信息；计算过程如下：

使用三次B样条对机械臂末端空间位置进行插值，其定义公式如下：

S(u)＝∑P_iN_i,k(u)

其中P_i是样条曲线的控制点，此处把入勺点、过渡点、出勺点作为控制点，N_i,k是样条曲线的基函数，k是曲线的次数，设置k＝3；样条基函数方程可通过递推公式进行求解：

使用Squad算法对机械臂末端四元素信息进行插值，把步骤四中计算得到旋转矩阵转换为四元素，转换公式为：

其中T为旋转矩阵，q为转换得到的四元素；设入勺点、过渡点和出勺点的四元素为q_s,q_i,q_e，持续时间为t，Squad插值公式如下：

最后以时间节点为参考，把空间位置和四元素结合在一起得到机械臂末端完整的轨迹信息；通过逆运动学转换到关节空间下的运动信息，并发送给机械臂控制模块完成打菜任务。

Images