一种虾只定向摆盘装置及其控制方法

本发明涉及机械控制，尤其涉及一种虾只定向摆盘装置及其控制方法。

背景技术：

1、摆盘是虾只自动化机械剥壳的重要环节，要求虾只以背朝上、尾朝外的姿态摆放在盛虾盘上，供夹持机构喂入剥壳机。由于技术要求高，目前摆盘主要采用人工方式，存在劳动强度大、生产成本高、作业效率低等问题。因此，实现虾只自动定向摆盘具有重要意义。传统的摆盘方法通过轮廓线或骨架线提取实现了虾只姿态辨识，但辨识方法常含有多余分支，虾只姿态参数辨识误差较大，同时根据虾只体型特征和摩擦特性实现了虾只定向，但虾只品种多样，参数差异较大，基于虾只物性参数的定向方法可靠性较低，且所使用的算法应用场景仅针对不同抓取对象的相互位置进行路径规划，未考虑机械手与放置位置之间的路径规划问题，同时存在运动不连续和关节力矩突变等问题。

2、现有的虾只定向摆盘装置及其控制方法虾只定向摆盘系统的速率和成功率低，且不能满足虾只定向摆盘要求；为此，我们提出一种虾只定向摆盘装置及其控制方法。

技术实现思路

1、本发明的目的是为了解决现有技术中存在的缺陷，而提出的一种虾只定向摆盘装置及其控制方法。

2、为了实现上述目的，本发明采用了如下技术方案：

3、一种虾只定向摆盘控制方法，该控制方法具体步骤如下：

4、(1)采集虾只原始图像并辨识虾只姿态；

5、(2)分析虾只吸拾定位点以及摆放定位角参数信息；

6、(3)对机械手进行逆解分析并记录分析曲线；

7、(4)根据摆盘作业要求规划机械手路径以及轨迹；

8、(5)验证虾只定向摆盘装置摆盘速率和摆盘成功率。

9、作为本发明的进一步方案，步骤(1)所述虾只姿态辨识具体步骤如下：

10、步骤一：采用otsu方法对原始图像进行阈值分割以获取对应二值化图像，再对二值化图像进行轮廓提取和凹凸点数量计算，并根据虾只背部光滑凹凸点数量比腹部少的特点辨识虾只腹背；

11、步骤二：通过图像扫描方式获取虾只长度直线，并作虾只长度中垂线，再根据虾只长度中垂线将虾只图像分成两个连通域，并分别计算连通区域面积，之后根据虾只首部相对尾部面积大的特点辨识虾只首尾。

12、作为本发明的进一步方案，步骤(2)所述虾只参数信息分析具体步骤如下：

13、步骤1：通过遍历二值化图像所有连通区域，计算连通区域内部像素的重心坐标，并将计算出的重心点坐标作为吸拾定位点；

14、步骤2：通过相机标定获取相机内参，将虾只吸拾定位点的像素坐标转换为相机坐标系下的坐标，再采用手眼标定方法，获取相机坐标系到机械手坐标系之间的转换关系，根据相机标定以及手眼标定结果，获得虾只吸拾定位点在机械手坐标系下的准确位置；

15、步骤3：依据zhang-suen算法逐行、逐列扫描虾只图像，获取虾只的骨架线和线条结构，再根据线性拟合方法，获取虾只首尾姿态方向，并通过与虾只摆盘目标方向比较计算，获得虾只摆放定位角。

16、作为本发明的进一步方案，步骤2所述相机坐标系到机械手坐标系之间的转换关系的具体计算公式如下：

17、

18、式中：cp为吸拾定位点在相机坐标系下的坐标；rp为吸拾定位点在机械手坐标系下的坐标；为相机坐标系到机械手坐标系的变换矩阵。

19、作为本发明的进一步方案，步骤(3)所述逆解分析具体步骤如下：

20、步骤ⅰ：建立动、静两组坐标系，且静坐标系o-xyz的原点o位于静平台的几何中心，z轴垂直于静平台和动平台，之后机械手通过三个分支链连接动、静两个平台，主动臂在电机驱动下进行反复摆动，并通过从动臂和转动副使动平台做平移运动；

21、步骤ⅱ：根据目标点的位置，求解各关节转角的逆解公式，并根据机械手的逆解公式，通过输入位置和机械手结构参数计算获得机械手从初始点到目标点的关节转角，确定机械手动平台圆形路径参数方程，并通过matlab软件进行机械手逆解仿真。

22、作为本发明的进一步方案，步骤ⅱ所述关节转角逆解公式具体如下：

23、

24、其中：

25、

26、

27、

28、式中：r为静平台半径；la代表主动臂；lb为从动臂；o′为动平台的几何中心；r代表；ki、mi、ni为机械手结构参数与输入坐标之间的关系变量，i为机械手主动臂关节编号。

29、作为本发明的进一步方案，步骤(4)所述机械手路径以及轨迹规划具体步骤如下：

30、步骤①：将虾只吸拾位置和盛虾盘位置作为约束条件，并约束条件和最短路径建立对应数学模型，再依据摆盘作业路径规划要求，计算蚁群算法中蚂蚁由节点i访问节点j的概率，并对单次抓取过程的概率选取进行改进；

31、步骤②：根据蚁群算法中经过的总路径越短，信息素权值越大的原理，选择适应度排序靠前的多组蚂蚁进行信息素更新，之后通过matlab软件进行算法仿真，将机械手初始位置坐标、虾只吸拾点位置坐标以及盛虾盘位置坐标输入到路径规划算法中，并寻找最优抓取路径；

32、步骤③：分别使机械手运动过程中的起点和终点速度、加速度为0，再根据约束条件构建五次多项式轨迹函数，通过五次多项式轨迹函数获取机械手运行任意时刻关节的角度、角速度以及角加速度；

33、步骤④：通过matlab软件进行算法仿真，设定10s内完成从a点运动到b点，以获得机械手3组关节的角度、角速度以及角加速度随时间的变化曲线，并选择“门”型运动轨迹作为机械手运动轨迹，同时使用五次多项式函数对门型路径各个阶段进行插补。

34、作为本发明的进一步方案，步骤①所述数学模型具体表达公式如下：

35、

36、式中：l为作业路径长度，cm；dij为目标点位置i和目标点位置j之间的距离，cm；xij为目标点位置i和目标点位置j之间的条件距离，当目标点之间满足约束条件则获取距离计算作业路径，反之距离为0；

37、步骤①所述蚁群算法节点方位概率具体计算公式如下：

38、

39、式中：τii′(t)为t时刻蚂蚁在路径ii′上残留的信息素浓度；i为吸拾位置；i′为摆放位置；τi′j(t)为t时刻蚂蚁在路径i′j上残留的信息素浓度，j为下一个吸拾位置；ηii'(t)为t时刻摆放位置i′对虾只吸拾位置i的启发信息；ηi'j(t)为下一个吸拾位置j对虾只放置位置i′的启发信息；allowedplacek＝{0,1,...,n-1}为蚂蚁k下一步允许选择的摆放点；allowedk＝{0,1,...,n-1}为蚂蚁k下一步允许选择的吸拾点；a为信息素重要程度因子；β为启发素重要程度因子；

40、步骤①所述信息素更新具体计算公式如下：

41、

42、式中：τij(t)为t时刻蚂蚁在路径ij上残留的信息素浓度；ρ为信息素程度挥发因子ρ∈(0,1)；值越大代表信息素挥发越快；w为选择信息素更新的数量；lr为排名第r位的蚂蚁从i点到j点的路径长度；q为蚂蚁循环一次释放的信息素总量；

43、步骤③所述五次多项式轨迹函数具体计算公式如下：

44、θi(t)＝a0+a1t+a2t2+a3t3+a4t4+a5t5             (7)

45、式中：θi(t)(i＝1,2,3)为运动过程中各关节的角度位移；a0、a1、a2、a3、a4、a5为待定系数；

46、对式(7)进行求导，获取该轨迹下机械手运行速度公式：

47、

48、对式(8)进行求导，获取该轨迹下机械手运行加速度公式：

49、

50、将约束条件代分别入式(7)、(8)以及(9)中，可得：

51、a0＝a1＝a2＝0 (10)

52、t＝tz-t0 (11)

53、h ＝θiz-θi0 (12)

54、

55、

56、

57、式中：t0为机械手运动初始时刻；θi0为初始角度；tz为终止时刻；θiz为终止角度。

58、一种虾只定向摆盘装置，包括传送机构，所述传送机构外部设置有支撑架，所述传送机构上端两侧分别固定连接有光学传感器，所述传送机构右端后侧固定连接有盛虾盘；所述支撑架下端设置有两组三角板；所述支撑架下端通过两组三角板活动连接有三组机械手；所述支撑架底端前侧固定连接有摄像头；所述机械手底端固定连接有纠偏机构。

59、需要进一步说明的是，传送机构将虾只运输至摄像头感知区域后停止运行，之后摄像头获取虾只的图像信息，并发送至上位机进行虾只姿态辨识和参数获取，上位机将结果传送给下位机，由下位机控制机械手进行摆盘操作，同时机械手移动至待纠偏虾只正上方，通过纠偏机构调整虾只姿态，再将虾只放置在盛虾盘上，同时一组光学传感器实时发送信号给另一组光学传感器，当光学传感器持续接受信号时长超过预设阈值，则判定虾只已全部摆放完成，并停止传送机构继续运输。

60、相比于现有技术，本发明的有益效果在于：

61、该虾只定向摆盘控制方法采用otsu方法对原始图像进行阈值分割以获取对应二值化图像，并进行轮廓提取和凹凸点数量计算识别虾只腹背，再生成虾只长度中垂线，并计算连通区域面积大小辨识虾只首尾，计算连通区域内部像素的重心坐标，并将计算出的重心点坐标作为吸拾定位点，再通过相机标定以及手眼标定结果，获得虾只吸拾定位点在机械手坐标系下的准确位置，获取虾只的骨架线和线条结构，再根据线性拟合方法，获取虾只首尾姿态方向，并计算虾只摆放定位角，将虾只吸拾位置和盛虾盘位置作为约束条件，并约束条件和最短路径建立对应数学模型，再依据摆盘作业路径规划要求，并对单次抓取过程的概率选取进行改进，根据蚁群算法中经过的总路径越短，信息素权值越大的原理，选择适应度排序靠前的多组蚂蚁进行信息素更新，之后将机械手初始位置坐标、虾只吸拾点位置坐标以及盛虾盘位置坐标输入到路径规划算法中，并寻找最优抓取路径，根据约束条件构建五次多项式轨迹函数，通过五次多项式轨迹函数获取机械手运行任意时刻关节的角度、角速度以及角加速度，并选择“门”型运动轨迹作为机械手运动轨迹，同时使用五次多项式函数对门型路径各个阶段进行插补，提高虾只定向摆盘系统的速率和成功率，更适宜虾只定向摆盘要求。