[发明专利]一种基于DGG点云分割网络的机械臂自动打菜方法

权利要求书

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,m}

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

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

其中k代表邻域点数，F_m(j)为近邻点p_ij的第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优化器对网络模型权重参数进行调整；保存损失值最低时的网络模型权重参数；

步骤三：将步骤一搭建的DGG点云分割网络移植到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插值公式如下：

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