本发明涉及一种串型番茄采摘装置及多关节机械臂各关节角度确定方法，属于番茄采摘。
背景技术：
1、
2、近年来，串型番茄因其出色的耐存储性和长货架期流行于国内外市场，而且其商品性优良，深受消费者的欢迎，栽培面积逐年扩大，产量也在逐年增长。但是串型番茄成熟时需要耗费大量的劳动力才能顺利完成采摘工作，大大增加了串型番茄的生产成本，降低了串型番茄产业的国际市场竞争力。
3、目前现有的串型番茄自动采摘装置，大多是在车载式底盘上搭载一个升降平台，在升降平台上设置图像采集装置和采摘执行器，这种自动采摘装置采摘执行器自由度不够、作业空间很小，对车载平台的移动精度要求高，从而会导致采摘准确度不高、采摘效率不高。为此有人提出采用多关节机械臂的方式来对番茄进行采摘，但是目前现有的多关节机械臂方式都是先计算出最后一个关节的控制角度，再依次迭代计算出各个关节的控制角度，从而会导致控制算法复杂、控制时间长的问题。
技术实现思路
1、本发明的目的是提供一种串型番茄采摘装置及多关节机械臂各关节角度确定方法，用于解决现有技术中先计算出最后一个关节的控制角度，再依次迭代计算出各个关节的控制角度导致控制算法复杂、控制时间长的问题。
2、为实现上述目的，本发明提供的技术方案是：
3、本发明提供了一种串型番茄采摘装置，包括多关节的机械臂和机械臂末端执行部件，机械臂由控制模块控制，控制模块用于根据接收到的串型番茄杆茎目标位置信息对采摘机械臂进行运动学逆解，计算出各关节角度，并根据计算的角度控制各关节电机转到相应角度；在运动学逆解计算时，根据目标位置信息计算出机械臂在x、y轴组成的平面内的投影长度len以及在z轴上的投影长度high；利用投影长度len、投影长度high和机械臂末端执行部件的俯仰角的取值范围确定出机械臂各关节角度，z轴垂直于地面，x、y轴组成的平面垂直于z轴。
4、进一步地，在确定机械臂各关节角度时，令机械臂末端执行部件的俯仰角在其取值范围内变化，计算出多组关节角度，选取其中的一组作为最终的关节角度。
5、进一步地，所述机械臂为6关节的机械臂，所述机械臂的第五关节和第六关节是末端执行部件，所述机械臂的前四个关节角度计算公式为：
6、
7、
8、其中，θ1为第一关节转动角度，θ2为第二关节转动角度，θ3为第三关节转动角度，θ4为第四关节转动角度，α为采摘机械臂末端执行部件的俯仰角，j1、j2、j3和j4为中间变量，x、y为目标位置x、y平面坐标。
9、进一步地，所述机械臂末端执行部件包括夹持执行器和切割执行器，所述夹持执行器和所述切割执行器分别用于对目标串型番茄的杆茎进行夹持和切割。
10、进一步地，所述夹持执行器和所述切割执行器所在平面垂直于串型番茄杆茎。
11、进一步地，该采摘装置还包括图像采集模块和上位机，所述图像采集模块用于采集串型番茄图像信息，所述上位机用于根据图像采集模块采集的串型番茄图像信息识别出串型番茄杆茎目标位置信息，并将目标位置信息发送给控制模块。
12、进一步地，所述机械臂设置在机座上，所述机座采用的是车载式底盘。
13、有益效果：本发明通过图像采集模块拍摄串型番茄图像，进而识别出串型番茄杆茎目标位置信息即采摘目标点，并将串型番茄杆茎目标位置信息发送给控制模块。控制模块根据接收到的串型番茄杆茎目标位置信息进行采摘机械臂的运动学逆解过程，计算出各关节角度，并根据计算的角度控制各关节电机转到相应角度，以此完成对串型番茄的采摘操作。本发明通过采用6自由度机械臂的方式提高了串型番茄自动采摘装置的灵活性，保证了进行采摘操作所需的作业空间，降低了对车载平台的精度要求，有效地解决了现有技术中先计算出最后一个关节的控制角度，再依次迭代计算出各个关节的控制角度导致控制算法复杂、控制速度慢的问题。
14、本发明还提供了一种多关节机械臂各关节角度确定方法，该方法包括以下步骤：根据机械臂末端执行部件操作的目标位置信息对机械臂进行运动学逆解，计算出各关节角度；在运动学逆解计算时，根据目标位置信息计算出机械臂在x、y轴组成的平面内的投影长度len以及在z轴上的投影长度high；利用投影长度len、投影长度high和机械臂末端执行部件的俯仰角的取值范围确定出机械臂各关节角度，所述z轴垂直于地面，x、y轴组成的平面垂直于z轴。
15、进一步地，在确定机械臂各关节角度时，令机械臂末端执行部件的俯仰角在其取值范围内变化，计算出多组关节角度，选取其中的一组作为最终的关节角度。
16、进一步地，所述机械臂为6关节的机械臂，所述机械臂的第五关节和第六关节是末端执行部件，所述机械臂的前四个关节角度计算公式为：
17、
18、
19、其中，θ1为第一关节转动角度，θ2为第二关节转动角度，θ3为第三关节转动角度，λ4为第四关节转动角度，α为采摘机械臂末端执行部件的俯仰角，j1、j2、j3和j4为中间变量，x、y为目标位置x、y平面坐标。
20、有益效果：本发明根据机械臂末端执行部件操作的目标位置信息对机械臂进行运动学逆解，计算出各关节角度，以此确定多关节机械臂各关节角度，有效地解决了现有技术中先计算出最后一个关节的控制角度，再依次迭代计算出各个关节的控制角度导致控制算法复杂、控制时间长的问题。
1.一种串型番茄采摘装置，包括多关节的机械臂和机械臂末端执行部件，其特征在于，所述机械臂由控制模块控制，所述控制模块用于根据接收到的串型番茄杆茎目标位置信息对采摘机械臂进行运动学逆解，计算出各关节角度，并根据计算的角度控制各关节电机转到相应角度；在运动学逆解计算时，根据目标位置信息计算出机械臂在x、y轴组成的平面内的投影长度len以及在z轴上的投影长度high；利用投影长度len、投影长度high和机械臂末端执行部件的俯仰角的取值范围确定出机械臂各关节角度，所述z轴垂直于地面，x、y轴组成的平面垂直于z轴。
2.根据权利要求1所述的串型番茄采摘装置，其特征在于，在确定机械臂各关节角度时，令机械臂末端执行部件的俯仰角在其取值范围内变化，计算出多组关节角度，选取其中的一组作为最终的关节角度。
3.根据权利要求1所述的串型番茄采摘装置，其特征在于，所述机械臂为6关节的机械臂，所述机械臂的第五关节和第六关节是末端执行部件，所述机械臂的前四个关节角度计算公式为：
4.根据权利要求1所述的串型番茄采摘装置，其特征在于，所述机械臂末端执行部件包括夹持执行器和切割执行器，所述夹持执行器和所述切割执行器分别用于对目标串型番茄的杆茎进行夹持和切割。
5.根据权利要求4所述的串型番茄采摘装置，其特征在于，所述夹持执行器和所述切割执行器所在平面垂直于串型番茄杆茎。
6.根据权利要求1所述的串型番茄采摘装置，其特征在于，该采摘装置还包括图像采集模块和上位机，所述图像采集模块用于采集串型番茄图像信息，所述上位机用于根据图像采集模块采集的串型番茄图像信息识别出串型番茄杆茎目标位置信息，并将目标位置信息发送给控制模块。
7.根据权利要求1所述的串型番茄采摘装置，其特征在于，所述机械臂设置在机座上，所述机座采用的是车载式底盘。
8.一种多关节机械臂各关节角度确定方法，其特征在于，该方法包括以下步骤：根据机械臂末端执行部件操作的目标位置信息对机械臂进行运动学逆解，计算出各关节角度；在运动学逆解计算时，根据目标位置信息计算出机械臂在x、y轴组成的平面内的投影长度len以及在z轴上的投影长度high；利用投影长度len、投影长度high和机械臂末端执行部件的俯仰角的取值范围确定出机械臂各关节角度，所述z轴垂直于地面，x、y轴组成的平面垂直于z轴。
9.根据权利要求8所述的多关节机械臂各关节角度确定方法，其特征在于，在确定机械臂各关节角度时，令机械臂末端执行部件的俯仰角在其取值范围内变化，计算出多组关节角度，选取其中的一组作为最终的关节角度。
10.根据权利要求8所述的多关节机械臂各关节角度确定方法，其特征在于，所述机械臂为6关节的机械臂，所述机械臂的第五关节和第六关节是末端执行部件，所述机械臂的前四个关节角度计算公式为：
