本发明涉及一种基于双臂移动机器人的自动打菜系统。本发明包括：订单处理模块、图像数据采集模块、打菜点预测模块、双机械臂抓取模块以及移动模块。本方法实现了双臂移动机器人自动识别菜品种类并定位、打菜点位姿预测并挖取、双臂协同倒菜、自动出餐等功能。本发明采用深度相机采集图像数据，通过投票特征算法实现打菜点的预测，采用主从模式控制双臂之间的协调操作。本发明具有高灵活性、高精度、操作简单、环境适应性强等优点。大大减小了餐厅、食堂等工作人员的工作量，有助于餐厅工作效率的提高，具有广泛的市场需求。
1.一种基于双臂移动机器人的自动打菜系统的控制方法，其特征在于，包括以下步骤：步骤一：通过移动模块，使用图像采集模块对当前窗口所有菜品进行扫描，利用训练好的Yolov4神经网络对彩色图像进行识别，生成当前窗口的菜单并记录下每个菜品在移动小车基坐标系下的位置信息，同时把菜单同步到客户端上；
步骤二：自动打菜系统在开始处理订单信息时，客户端接受用户反馈；系统通过MQTT通信协议控制移动小车运动至目标菜品附近；同时系统为辅机械臂计算生成轨迹信息并控制落碗装置共同执行餐具的抓取任务；
械臂目标状态下的六个关节值，为了防止辅机械臂在运动过程中与落碗装置发生碰撞，设置若干个过渡状态 利用三次样条插值算法进行计算，对辅机械臂每两
个状态之间用三次方程Si(x)＝ai+bix+cix +dix 进行拟合，同时需要保证运动轨迹连续平滑，即Si(x),S′i(x),S″i(x)连续；对于只有一个过渡点时，需要对两个区间进行三次方程拟合，共有八个未知数a1,b1,c1,d1,a2,b2,c2,d2，以自然边界为约束，其中开始、结束速度均为
0，并结合一阶、二阶导数连续的要求，通过矩阵法进行求解；
其中t1,t2分别为两个状态之间运动的时间，对于有多个过渡点的情况，重复使用矩阵法进行求解；
步骤三：到达目标位置后，对图像模块采集到的点云数据进行预处理，利用统计滤波器剔除点云中的离群点，利用体素滤波器进行下采样处理，把处理后的点云数据通过ROS消息机制发送到打菜点预测模块；
步骤四：打菜点预测模块通过投票特征算法预测得到合适的打菜点，并转换为移动小车基坐标系下的位姿信息，通过MoveIt中的OMPL运动规划算法生成机械臂的打菜轨迹并发送给双机械臂抓取模块；
其中打菜点预测模块通过投票特征算法预测得到打菜点，过程为：
子步骤四一：对点云数据构建KDTree实现点云邻域的快速搜索，设置点云邻域数k，得到每个点的邻域集合Γk(pi)，并计算每个邻域深度特征Fdepth、曲率特征Fcurve、密度特征Fdensity和位置特征Fpos；定义公式其中，式中Xbowl,Ybowl,Dbowl分别是菜盆的长、宽和深度，di为点pi距离盆底的距离；λmin为点pi在邻域内的法向量近似值；xi,yi为点pi在2D平面上的坐标，ε为x,y方向上的权重分配；
子步骤四二，采用主成分分析法对每个点的法向量进行估计，对于每个点pi构建协方差矩阵C3×3：其中 为邻域的质心，协方差矩阵表示了邻域集合Γk(pi)中内点和质心 间的距离特征，再根据 求解到协方差矩阵的特征值λj(λ0≤λ1≤λ2)和特征向量 取最小特征值λmin对应的特征向量 作为pi的法向量，而λmin表征邻域内点在法线方向的变化程度，若λmin＝0，代表点pi邻域是一个平面，λmin值越大代表曲面越尖锐；
子步骤四三，完成对邻域特征的计算后，从中投票选出打菜开始点和打菜结束点；把邻域内的点作为投票人，对每个点的特征进行投票，把打菜开始点和结束点的投票分数定义为：其中 是设定的阈值，在对打菜开始点投票时将ε设置为0.6，对结束点投票时设置为
0.4；最终选择分数最高的点作为打菜开始点和结束点；
把打菜点位姿转换到移动小车基坐标系下的具体过程为：
根据手眼标定得到的手眼矩阵 把 变换到主机械臂末端坐标系下的位姿
根据主机械臂与移动小车基坐标系之间的转换关系，得到相对于小
车基坐标系的姿态 得到打菜开始点和结束点的位姿信息
后，通过ROS中MoveIt运动库的OMPL算法、逆解算法得到关节空间下主机械臂的运动信息，如果规划失败，则重新预测新的打菜点，直到规划成功；
步骤五：主机械臂完成菜品挖取任务和辅机械臂完成餐具抓取任务时，将菜品完整地倒入餐具中；采用主从模式的双臂控制方法，根据双臂末端之间的约束关系，通过双臂协同规划算法计算得到双臂末端的轨迹信息；
设在倒菜任务开始前，主机械臂和辅机械臂的末端在移动小车基坐标系里的空间位置分别为Pur3＝(xur3,yur3,zur3),Pur10＝(xur10,yur10,zur10)，末端姿态为 打菜勺半径为rspoon，最大深度为h，并以圆心为原点建立坐标系Ospoon，碗的半径为rbowl，以碗口中心为原点建立坐标系Obowl；在倒菜过程中，设定主机械臂末端的空间位置和辅机械臂末端的姿态是固定不变的，辅机械臂的末端位置随着主机械臂末端的姿态改变而不断变化；假设主机械臂末端绕坐标系Ospoon的y轴顺时针方向旋转，旋转角度为α，辅机械臂末端沿着坐标系Obowl的y轴正方向移动，移动距离为dy；倒菜开始前，坐标系Ospoon原点在坐标系Obowl中处于(0,rbowl,h+△h)，其中△h是勺子与碗在z轴方向上的安全距离；倒菜结束时，坐标系Ospoon原点在坐标系Obowl中处于(0,0,h+△h)，由此得到双臂末端的约束方程为：其中△dy,△α分别表示为辅机械臂末端当前移动距离和主机械臂末端当前旋转角度；
进而得到整个倒菜过程中双臂末端在笛卡尔空间内的位姿信息，通过逆解算法得到在关节空间内的轨迹信息；
步骤六：辅机械臂将盛有菜品的餐具放置在传送带上，传送带运动至出餐窗口，客户端通知用户订单已完成，前往窗口进行取餐。
2.一种基于双臂移动机器人的自动打菜系统的控制方法的自动打菜系统，其使用如权利要求1所述的一种基于双臂移动机器人的自动打菜系统的控制方法，其特征在于，该系统包括：订单处理模块、图像数据采集模块、打菜点预测模块、双机械臂抓取模块和移动模块；
订单处理模块，用于对用户订单进行数据处理并发送至打菜点预测模块；
图像数据采集模块，包括：Realsense D435深度相机；Realsense D435深度相机采集RGB彩色图像数据和深度图像数据；RGB彩色图像用于Yolov4网络实现菜品的识别和定位，深度图像用于菜品的打菜点预测；深度相机安装于主机械臂末端；
打菜点预测模块，利用投票特征算法进行估计，预测得到合适的打菜点；
双机械臂抓取模块，包括：主机械臂、打菜勺、六轴力矩传感器、辅机械臂和夹爪，主机械臂和打菜勺负责菜品的挖取任务，六轴力矩传感器用于测量打取菜品的质量，辅机械臂和夹爪负责餐具的抓取和搬运任务；双机械臂抓取模块安装在移动模块上；
移动模块，包括：移动小车、超声波传感器、工控机、落碗装置和传送带；超声波传感器用于小车移动过程中的避障，传送带用于输送菜品至出餐口，工控机用于接收打菜系统的指令并控制移动小车、落碗装置、传送带的运动。
一种基于双臂移动机器人的自动打菜系统及其控制方法
技术领域
[0001] 本发明属于机器人技术领域，具体涉及一种基于双臂移动机器人的自动打菜系统。
背景技术
[0002] 在我国众多快餐店、学校、政府机关、工厂等单位的餐厅和食堂的点菜、打菜和支付结算工作都由人工完成，但这会消耗巨大的人力资源和成本，并具有效率低、人工成本高等缺点。随着机器视觉、深度学习、人工智能等技术的发展，智能服务机器人逐渐能够自动完成一些简单场景下的工作，可以将人们从重复性高、枯燥的工作中解放出来，其中传感技术、视觉处理算法、运动规划、双臂协同技术等是研发智能服务机器人系统的关键技术。针对餐厅内的打菜、支付结算工作，研发一种基于智能服务机器人的自动打菜系统对餐厅具有重要的实际意义。
发明内容
[0003] 鉴于上述背景中存在的问题，本发明的目的在于提供一种基于双臂移动机器人的自动打菜系统，该系统通过图像采集、yolov4网络识别定位、投票特征算法预测打菜点和机械臂轨迹规划完成对菜品的识别、定位和抓取功能。集成了神经网络、自动控制、双臂控制和轨迹规划等多种技术。能大幅度减少餐厅内的人工成本，有效提高餐厅的运行效率。
[0004] 一种基于双臂移动机器人的自动打菜系统，该系统包括：订单处理模块、图像数据采集模块、打菜点预测模块、双机械臂抓取模块和移动模块；
[0005] 订单处理模块，用于对用户订单进行数据处理并发送至打菜点预测模块；
[0006] 图像数据采集模块，包括：Realsense D435深度相机；Realsense D435深度相机采集RGB彩色图像数据和深度图像数据；RGB彩色图像用于Yolov4网络实现菜品的识别和定位，深度图像用于菜品的打菜点预测；深度相机安装于主机械臂末端；
[0007] 打菜点预测模块，利用投票特征算法进行估计；
[0008] 双机械臂抓取模块，包括：主机械臂UR10、打菜勺、六轴力矩传感器、辅机械臂UR3和夹爪AG95，主机械臂和打菜勺负责菜品的挖取任务，六轴力矩传感器用于测量打取菜品的质量，辅机械臂和夹爪负责餐具的抓取和搬运任务；双机械臂抓取模块安装在移动模块上；
[0009] 移动模块，包括：移动小车、超声波传感器、工控机、落碗装置和传送带；超声波传感器用于小车移动过程中的避障，传送带用于输送菜品至出餐口，工控机用于接收打菜系统的指令并控制移动小车、落碗装置、传送带的运动。
[0010] 一种基于双臂移动机器人的自动打菜系统的控制方法，包括以下步骤：
[0011] 步骤一：通过移动模块，使用图像采集模块对当前窗口所有菜品进行扫描，利用训练好的Yolov4神经网络对彩色图像进行识别，生成当前窗口的菜单并记录下每个菜品在移动小车基坐标系下的位置信息，同时把菜单同步到客户端上。
[0012] 步骤二：自动打菜系统在开始处理订单信息时，客户端对用户进行接受反馈；系统通过MQTT通信协议控制移动小车运动至目标菜品附近；同时系统为辅机械臂计算生成轨迹信息并控制落碗装置共同执行餐具的抓取任务。
[0013] 计算辅机械臂的轨迹信息过程为：
[0014] 表示辅机械臂起始状态下的六个关节值， 表示辅机械臂目标状态下的六个关节值，为了防止辅机械臂在运动过程中与落碗装置发生碰撞，设置若干个过渡状态 利用三次样条插值算法进行计算，对辅机械臂
2 3
每两个状态之间用三次方程Si(x)＝ai+bix+cix+dix进行拟合，同时需要保证运动轨迹连续平滑，即Si(x),S′i(x),S″i(x)连续；对于只有一个过渡点时，需要对两个区间进行三次方程拟合，共有八个未知数a1,b1,c1,d1,a2,b2,c2,d2，以自然边界为约束，其中开始、结束速度均为0，并结合一阶、二阶导数连续的要求，通过矩阵法进行求解；
[0015]
[0016] 其中t1,t2分别为两个状态之间运动的时间，对于有多个过渡点的情况，重复使用矩阵法进行求解。
[0017] 步骤三：到达目标位置后，对图像模块采集到的点云数据进行预处理，利用统计滤波器剔除点云中的离群点，利用体素滤波器进行下采样处理，把处理后的点云数据通过ROS消息机制发送到打菜点预测模块。
[0018] 步骤四：打菜点预测模块通过投票特征算法预测得到合适的打菜点，并转换为移动小车基坐标系下的位姿信息，通过MoveIt中的OMPL运动规划算法生成机械臂的打菜轨迹并发送给双机械臂抓取模块。
[0019] 其中打菜点预测模块通过投票特征算法预测得到打菜点，过程为：
[0020] 子步骤四一：对点云数据构建KDTree实现点云邻域的快速搜索，设置点云邻域数k，得到每个点的邻域集合Γk(pi)，并计算每个邻域深度特征Fdepth、曲率特征Fcurve、密度特征Fdensity和位置特征Fpos；定义公式
[0021]
[0022] 其中，式中Xbowl,Ybowl,Dbowl分别是菜盆的长宽和深度，di为点pi距离盆底的距离；λmin为点pi在邻域内的法向量近似值；xi,yi为点pi在2D平面上的坐标，ε为x,y方向上的权重分配；k为邻域内的点数，S为邻域在2D平面上的面积。
[0023] 子步骤二，采用主成分分析法对每个点的法向量进行估计，对于每个点pi构建协方差矩阵C3×3：
[0024]
[0025] 其中p为邻域的质心，协方差矩阵表示了邻域Γk(pi)中内点和质心p间的距离特征，再根据 求解到协方差矩阵的特征值λj(λ0≤λ1≤λ2)和特征向量 ，取最小特征值λmin对应的特征向量 作为pi的法向量，而λmin表征邻域内点在法线方向的变化程度，若λmin＝0，代表点pi邻域是一个平面，λmin值越大代表曲面越尖锐。
[0026] 子步骤三，完成对邻域特征的计算后，从中投票选出打菜开始点和打菜结束点；把邻域内的点作为投票人，对每个点的特征进行投票，把打菜开始点和结束点的投票分数定义为:
[0027]
[0028] 其中 是设定的阈值，在对打菜开始点投票时将ε设置为0.6，对结束点投票时设置为0.4；最终选择分数最高的点作为打菜开始点和结束点。
[0029] 把打菜点位姿转换到移动小车基坐标系下的具体过程为：
[0030] 根据手眼标定得到的手眼矩阵 ，把 变换到主机械臂末端坐标系下的位姿 ，根据主机械臂与移动小车基坐标系之间的转换关系，得到相对于小车基坐标系的姿态 ；得到打菜开始点和结束点的位姿信息
后，通过ROS中MoveIt运动库的OMPL算法、逆解算法得到关节空间下主机械臂的运动信息，如果规划失败，则重新预测新的打菜点，直到规划成功。
[0031] 步骤五：主机械臂完成菜品挖取任务和辅机械臂完成餐具抓取任务时，将菜品完整地倒入餐具中；采用主从模式的双臂控制方法，根据双臂末端之间的约束关系，通过双臂协同规划算法计算得到双臂末端的轨迹信息。
[0032] 双臂轨迹规划过程如下：
[0033] 设在倒菜任务开始前，主机械臂和辅机械臂的末端在移动小车基坐标系里的空间位置分别为Pur3＝(xur3,yur3,zur3),Pur10＝(xur10,yur10,zur10)，末端姿态为 ，打菜勺半径为rspoon，最大深度为h，并以圆心为原点建立坐标系Ospoon，碗的半径为rbowl，以碗口中心为原点建立坐标系Obowl；在倒菜过程中，设定主机械臂末端的空间位置和辅机械臂末端的姿态是固定不变的，辅机械臂的末端位置随着主机械臂末端的姿态改变而不断变化；假设主机械臂末端绕坐标系Ospoon的y轴顺时针方向旋转，旋转角度为α，辅机械臂末端沿着坐标系Obowl的y轴正方向移动，移动距离为dy；倒菜开始前，坐标系Ospoon原点在坐标系Obowl中处于(0,rbowl,h+Δh)，其中Δh是勺子与碗在z轴方向上的安全距离；倒菜结束时，坐标系Ospoon原点在坐标系Obowl中处于(0,0,h+Δh)，由此得到双臂末端的约束方程为：
[0034]
[0035] 其中Δdy,Δα分别表示为辅机械臂末端当前移动距离和主机械臂末端当前旋转角度；进而得到整个倒菜过程中双臂末端在笛卡尔空间内的位姿信息，通过逆解算法得到在关节空间内的轨迹信息。
[0036] 步骤六：辅机械臂将盛有菜品的餐具放置在传送带上，传送带运动至出餐窗口，客户端通知用户订单已完成，前往窗口进行取餐。
[0037] 本发明的有益效果：本发明利用多传感器的信息采集能力，通过神经网络技术对菜品进行识别定位，使用投票特征算法对打菜点位姿进行估计，最后通过双臂协调规划完成一系列动作；系统采用模块化设计，分工明确，层次结构清晰，可快速预测得到不同菜品的打菜点位姿；具有实时检测菜品种类、自主选择打菜动作、动态规划双臂轨迹等优点，可以代替餐厅员工快速、稳定地完成打菜任务，具有一定的实践价值。附图说明：
[0038] 图1：为本发明的整体硬件图；
[0039] 图2：为本发明的微信小程序界面图。具体实施方式：
[0040] 实施例1
[0041] 一种基于双臂移动机器人的自动打菜系统，整体硬件图如图一所示，该系统分为五部分：订单处理模块、图像数据采集模块、打菜点预测模块、双机械臂抓取模块、移动模块；本实施例中订单处理模块主要在微信小程序上完成，即微信小程序为本实施例中的客户端，数据后台把用户订单发送至打菜系统进行处理；图像数据采集模块由Realsense D435深度相机组成，主要采集RGB彩色图像和深度图像两种数据；RGB彩色图像用于Yolov4网络实现菜品的识别和定位，深度图像用于菜品的打菜点预测；深度相机安装于UR10末端，采用“眼在手上”的标定方式；打菜点预测模块利用投票投票特征算法进行估计；双机械臂抓取模块由UR10、打菜勺和六轴力矩传感器、UR3和AG95夹爪组成，UR10和打菜勺负责菜品的挖取任务，六轴力矩传感器用于测量打取菜品的质量；UR3和AG95负责餐具的抓取和搬运任务；移动模块由移动小车、超声波传感器、工控机、落碗装置、传送带组成，超声波传感器用于小车移动过程中的避障，传送带用于输送菜品至出餐口，工控机用于接收打菜系统的指令并控制移动小车、落碗装置、传送带的运动。
[0042] 一种基于双臂移动机器人的自动打菜方法，该方法具体如下：
[0043] 步骤一：移动机器人通过图像采集模块对当前窗口所有菜品进行扫描，利用训练好的Yolov4神经网络对彩色图像进行识别，生成当前窗口的菜单并记录下每个菜品在移动小车基坐标系下的位置信息，同时把菜单同步到如图二所示的微信小程序上。
[0044] 步骤二：自动打菜系统在开始处理订单信息时，微信小程序通知用户您的订单正在处理中；系统通过MQTT通信协议控制移动小车运动至目标菜品附近；同时系统为UR3计算生成轨迹信息并控制落碗装置共同执行餐具的抓取任务；
[0045] 计算UR3的轨迹信息过程为：
[0046] 表示UR3起始状态下的六个关节值， 表示UR3目标状态下的六个关节值，为了防止UR3在运动过程中与落碗装置发生碰撞，设置若干个过渡状态 ，接着利用三次样条插值算法进行计算，对UR3每两个状态之间用三
2 3
次方程Si(x)＝ai+bix+cix+dix 进行拟合，同时需要保证运动轨迹连续平滑，即Si(x),S′i(x),S″i(x)连续；对于只有一个过渡点时，需要对两个区间进行三次方程拟合，共有八个未知数a1,b1,c1,d1,a2,b2,c2,d2，以自然边界为约束(开始、结束速度均为0)，并结合一阶、二阶导数连续的要求，通过矩阵法进行求解；
[0047]
[0048] 其中t1,t2分别为两个状态之间运动的时间，对于有多个过渡点的情况，重复使用矩阵法进行求解即可。
[0049] 步骤三：到达目标位置后，对图像模块采集到的点云数据进行预处理，利用统计滤波器剔除点云中的离群点和体素滤波器进行下采样处理，把处理后的点云数据通过ROS消息机制发送到菜品打菜点预测模块；
[0050] 步骤四：打菜点预测模块通过投票特征算法预测得到合适的打菜点，并转换为移动小车基坐标系下的位姿信息，通过MoveIt中的OMPL运动规划算法生成机械臂的打菜轨迹并发送给双臂抓取控制模块；
[0051] 其中打菜点预测模块通过投票特征算法预测得到打菜点，具体为：
[0052] 首先对点云数据构建KDTree实现点云邻域的快速搜索，设置点云邻域数k＝40，得到每个点的邻域集合Γk(pi)，并计算每个邻域深度特征Fdepth、曲率特征Fcurve、密度特征Fdensity和位置特征Fpos；定义公式
[0053]
[0054] 式中Xbowl＝0.31m,Ybowl＝0.46m,Dbowl＝0.068m分别是菜盆的长宽和深度，di为点pi距离盆底的距离；λmin为点pi在邻域内的法向量近似值；xi,yi为点pi在2D平面上的坐标，ε为x,y方向上的权重分配；k为邻域内的点数，S为邻域在2D平面上的面积；
[0055] 其中采用主成分分析法对每个点的法向量进行估计，对于每个点pi构建协方差矩阵C3×3：
[0056]
[0057] 其中 为邻域的质心，协方差矩阵表示了邻域Γk(pi)中内点和质心 间的距离特征，再根据 求解到协方差矩阵的特征值λj(λ0≤λ1≤λ2)和特征向量，取最小特征值λmin对应的特征向量 作为pi的法向量，而λmin表征邻域内点在法线方向的变化程度，若λmin＝0，代表点pi邻域是一个平面，λmin值越大代表曲面越尖锐。
[0058] 完成对邻域特征的计算后，需要从中投票选出打菜开始点和打菜结束点；把邻域内的点作为投票人，对每个点的特征进行投票，把打菜开始点和结束点的投票分数定义为：
[0059]
[0060] 其中 是设定的阈值，在对打菜开始点投票时将ε设置为0.6，对结束点投票时设置为0.4；最终选择分数最高的点作为打菜开始点和结束点。
[0061] 把打菜点位姿转换到移动小车基坐标系下的具体过程为：
[0062] 根据手眼标定得到的手眼矩阵 ，把 变换到UR10末端坐标系下的位姿根据UR10与移动小车基坐标系之间的转换关系，得到相对于小车基坐标系的姿态 ；得到打菜开始点和结束点的位姿信息后，
通过ROS中MoveIt运动库的OMPL算法、逆解算法得到关节空间下UR10的运动信息，如果规划失败，则重新预测新的打菜点，直到规划成功。
[0063] 步骤五：UR10完成菜品挖取任务和UR3完成餐具抓取任务时，进一步将菜品完整地倒入餐具中；采用主从模式的双臂控制方法(UR10作为主臂，UR3为从臂)，根据双臂末端之间的约束关系，通过双臂协同规划算法计算得到双臂末端的轨迹信息。
[0064] 双臂轨迹规划过程具体如下：
[0065] 假设在倒菜任务开始前，UR10和UR3的末端在移动小车基坐标系里的空间位置分别为Pur3＝(xur3,yur3,zur3),Pur10＝(xur10,yur10,zur10)，末端姿态为 ，打菜勺半径为rspoon＝0.055m，最大深度为h＝0.04m，并以圆心为原点建立坐标系Ospoon，碗的半径为rbowl＝0.05m，以碗口中心为原点建立坐标系Obowl；在倒菜过程中，设定UR10末端的空间位置和UR3末端的姿态是固定不变的，UR3的末端位置随着UR10末端的姿态改变而不断变化；假设UR10末端绕坐标系Ospoon的y轴顺时针方向旋转，旋转角度为α＝100°，UR3末端沿着坐标系Obowl的y轴正方向移动，移动距离为dy＝0.05m；倒菜开始前，坐标系Ospoon原点在坐标系Obowl中处于(0,rbowl,h+Δh)，其中Δh＝0.02m是勺子与碗在z轴方向上的安全距离；倒菜结束时，坐标系Ospoon原点在坐标系Obowl中处于(0,0,h+Δh)，由此得到双臂末端的约束方程为：
[0066]
[0067] 其中Δdy,Δα分别表示为UR3末端当前移动距离和UR10末端当前旋转角度；进而得到整个倒菜过程中双臂末端在笛卡尔空间内的位姿信息，通过逆解算法得到在关节空间内的轨迹信息。
[0068] 步骤六：UR3将盛有菜品的餐具放置在传送带上，传送带运动至出餐窗口，微信小程序通知用户订单已完成，前往窗口进行取餐。
