机器人学科建设沙盘套件！智慧农业3D分拣套装详解

作为最热门的技术领域，机器人技术正在彻底改变产业，并推动全球的创新。为了满足这个快速发展的领域对技术人才日益增长的需求，开发了一个开创性的机器人教育解决方案。这个创新的解决方案将自动化水果采摘机的模拟与水果分拣和运送的自动化复合机器人结合起来，为学生提供了一个在最受欢迎和最有趋势的技术领域中的全面学习经验。

在本文中我们将详细为你介绍水果采摘和分拣机器人场景。我们将会从套装的介绍和使用的场景介绍，到套装功能机械臂的实现。

智慧农业套装

图中所展示的就是我们的智慧农业套装，它是由以下的内容组成。

mechArm 270 M5Stack	*2
传送带	*1
3D摄像头/深度摄像头	*2
仿真模拟果树	*1
结构件	若干

你肯定很好奇这个套装是怎么运转的，接下来我将为你介绍这个套装的运作流程。

首先你可以看到我们有两台机械臂，他们分别执行不同的功能，离果树最近的那一台机械臂是采摘机器人下面简称R1；中间那台机械臂是分拣机器人下面简称是R2。从他们的名字上就可以知道它们分别做的是什么工作，R1负责将果树上的果实采摘下来放置在传送带上，R2则是将传送带上不合格的水果分拣出来。

流程：

采摘：R1通过深度摄像头对果树上果实的识别然后对果子进行定位，将果实的坐标发送给R1去采摘。

运输：通过R1的采摘，果实被放置在传送带上。传送带经过运转将果实运输到R2可识别的范围内进行果实好坏的判断。

分拣：R2上方的摄像头将视线范围内的果实进行识别算法的判断，判断为好果实的话，果实将随着传送带传输到果实收集区；判断为坏果实的话，将坏果实的坐标信息传递给R2，R2将坏果实目标进行抓取出来，放置在特定区域。

持续循环上面的流程：采摘->运输->分拣->采摘->运输

产品介绍

下面将简要介绍套装中的产品。

Robotic Arm - mechArm 270 M5Stack

这是一款小六轴机械臂，以M5Stack-Basic为核心控制，ESP32为辅助控制，结构是中心对称结构（仿工业结构）。mechArm 270-M5本体重量1kg， 负载250g，工作半径270mm，设计紧凑便携，小巧但功能强大，操作简单，能与人协同、安全工作。

传送带

传送带是一种用于运输物品的机械设备，通常由一个带状物体和一个或多个滚动轴组成。它们可以运输各种物品，例如包裹、箱子、食品、矿石、建筑材料等等。传送带的工作原理是将物品放置在运动的带子上，然后将其移动到目标位置。传送带通常由电机、传动系统、带子和支撑结构组成。电机提供动力，传动系统将动力传递给带子，使其移动。

目前市面上可以根据用户的需求定制各种传送带，例如传送带的长宽高，履带的材质等。

深度摄像头

随着使用场景的多样性，普通的2D摄像头无法满足我们使用的需求。在场景中我们使用到的是深度摄像头。深度摄像头是一种能够获取场景深度信息的相机。它不仅能够捕捉场景的颜色和亮度信息，还能够感知物体之间的距离和深度信息。深度摄像头通常使用红外线或其他光源来进行测量，以获取物体和场景的深度信息。

它可以获取很多信息，例如深度的画面，彩色的画面，红外线的画面，点云画面。

有了深度相机，我们就可以精准的获取到果树上果实的位置，以及颜色信息。

自适应夹爪-机械臂末端执行器

自适应夹爪是一种用来抓取、握取或夹持物体的末端执行器，它由两个可移动的爪子组成，可以通过机械臂的控制系统来控制其开合程度和开合速度。

项目功能的实现

我们先要准备好编译的环境，该场景是用Python语言来进行编写的。在使用和学习的时候得安装好环境。

编译环境：

numpy==1.24.3
opencv-contrib-python==4.6.0.66
openni==2.3.0
pymycobot==3.1.2
PyQt5==5.15.9
PyQt5-Qt5==5.15.2
PyQt5-sip==12.12.1
pyserial==3.5

项目的功能点我们主要分为三部分：

● 机器视觉识别算法，深度识别算法


            ● 机械臂的控制，路径的规划


            ● 多台机器之间通信和逻辑的处理

我们先从机器视觉识别算法介绍：

机器视觉识别算法

使用深度摄像头之前需要进行相机标定。相机标定的教程（https://docs.opencv.org/4.x/dc/dbb/tutorial_py_calibration.html）

相机标定：

相机标定是指通过对摄像机进行一系列测量和计算，确定摄像机内部参数和外部参数的过程。摄像机内部参数包括焦距、主点位置、像素间距等，而摄像机外部参数则包括摄像机在世界坐标系中的位置和方向等。相机标定的目的是为了使摄像机能够准确地捕捉并记录世界坐标系中物体的位置、大小、形状等信息。

我们的目标物体是果实，它颜色不一，形状也不一定，有红的，橙的，黄的。想要准确的抓取且不伤害到果实，就需要获取果实的各个信息，宽度，厚度等，智能的进行抓取。

我们看一下目标果实，它们目前较大的区别就是颜色的不一样，我们设定红色和橙色的目标将被选中，这里就要用到HSV色域来进行目标的定位。下面的代码是用来检测目标果实。

Code：

class Detector:
    class FetchType(Enum):
        FETCH = False
        FETCH_ALL = True

"""
    Detection and identification class
    """

    HSV_DIST = {
# "redA": (np.array([0, 120, 50]), np.array([3, 255, 255])),
# "redB": (np.array([176, 120, 50]), np.array([179, 255, 255])),
"redA": (np.array([0, 120, 50]), np.array([3, 255, 255])),
"redB": (np.array([118, 120, 50]), np.array([179, 255, 255])),
# "orange": (np.array([10, 120, 120]), np.array([15, 255, 255])),
"orange": (np.array([8, 150, 150]), np.array([20, 255, 255])),
"yellow": (np.array([28, 100, 150]), np.array([35, 255, 255])), # old
# "yellow": (np.array([31, 246, 227]), np.array([35, 255, 255])),   # new
}

    default_hough_params = {
"method": cv2.HOUGH_GRADIENT_ALT,
"dp": 1.5,
"minDist": 20,
"param2": 0.6,
"minRadius": 15,
"maxRadius": 40,
}

    def __init__(self, target):
        self.bucket = TargetBucket()
        self.detect_target = target

    def get_target(self):
        return self.detect_target

    def set_target(self, target):
        if self.detect_target == target:
            return
        self.detect_target = target
        if target == "apple":
            self.bucket = TargetBucket(adj_tolerance=25, expire_time=0.2)
        elif target == "orange":
            self.bucket = TargetBucket()
        elif target == "pear":
            self.bucket = TargetBucket(adj_tolerance=35)

    def detect(self, rgb_data):
        if self.detect_target == "apple":
            self.__detect_apple(rgb_data)
        elif self.detect_target == "orange":
            self.__detect_orange(rgb_data)
        elif self.detect_target == "pear":
            self.__detect_pear(rgb_data)

    def __detect_apple(self, rgb_data):
        maskA = color_detect(rgb_data, *self.HSV_DIST["redA"])
        maskB = color_detect(rgb_data, *self.HSV_DIST["redB"])
        mask = maskA + maskB

        kernelA = cv2.getStructuringElement(cv2.MORPH_RECT, (8, 8))
        kernelB = cv2.getStructuringElement(cv2.MORPH_RECT, (2, 2))
        mask = cv2.erode(mask, kernelA)
        mask = cv2.dilate(mask, kernelA)

        targets = circle_detect(
            mask, {"minDist": 15, "param2": 0.5,
"minRadius": 10, "maxRadius": 50}
)
        self.bucket.add_all(targets)
        self.bucket.update()

    def __detect_orange(self, rgb_data):
        mask = color_detect(rgb_data, *self.HSV_DIST["orange"])
        targets = circle_detect(
            mask, {"minDist": 15, "param2": 0.1,
"minRadius": 7, "maxRadius": 30}
)
        self.bucket.add_all(targets)
        self.bucket.update()

    def __detect_pear(self, rgb_data):
        mask = color_detect(rgb_data, *self.HSV_DIST["yellow"])

        kernelA = cv2.getStructuringElement(cv2.MORPH_RECT, (25, 25))
        kernelB = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))
        mask = cv2.erode(mask, kernelA)
        mask = cv2.dilate(mask, kernelA)
        mask = cv2.erode(mask, kernelB)

        targets = circle_detect(
            mask, {"minDist": 15, "param2": 0.1,
"minRadius": 15, "maxRadius": 70}
)
        self.bucket.add_all(targets)
        self.bucket.update()

    def fetch(self):
        return self.bucket.fetch()

    def fetch_all(self):
        return self.bucket.fetch_all()

    def debug_view(self, bgr_data, view_all=True):
        if view_all:
            targets = self.bucket.fetch_all()
        else:
            targets = self.bucket.fetch()
            if targets is not None:
                targets = [targets]
        if targets is not None:
            for target in targets:
                x, y, radius = target["x"], target["y"], target["radius"]
# draw outline
                cv2.circle(bgr_data, (x, y), radius, BGR_GREEN, 2)

# draw circle center
                cv2.circle(bgr_data, (x, y), 1, BGR_RED, -1)

左边为彩色流视频，右边为深度视频

我们的第一步就是要将目标果实能够正确的检测出来，以便后续获取目标物体的坐标，深度等其他信息。我们定义需要获取的信息方便后续的存储以及调用。

code：

class VideoCaptureThread(threading.Thread):
    def __init__(self, detector, detect_type = Detector.FetchType.FETCH_ALL.value):
        threading.Thread.__init__(self)
        self.vp = VideoStreamPipe()
        self.detector = detector                  
        self.finished = True                      
        self.camera_coord_list = []               
        self.old_real_coord_list = []             
        self.real_coord_list = []                 
        self.new_color_frame = None              
        self.fruit_type = detector.detect_target  
        self.detect_type = detect_type            
        self.rgb_show = None
        self.depth_show = None

最后我们要获取的是果实的坐标，能够发送给机械臂去执行抓取的坐标，通过深度坐标转化为世界坐标，已经是成功了一大半了，最后只需要将世界坐标跟机械臂的坐标系进行转换就可以获得抓取目标果实的坐标了。

# get world coordinate
 def convert_depth_to_world(self, x, y, z):
        fx = 454.367
        fy = 454.367
        cx = 313.847
        cy = 239.89

        ratio = float(z / 1000)
        world_x = float((x - cx) * ratio) / fx
        world_x = world_x * 1000
        world_y = float((y - cy) * ratio) / fy
        world_y = world_y * 1000
        world_z = float(z)

        return world_x, world_y, world_z

该阶段，我们实现了检测目标物体，并且返回可抓取的坐标，传递给机械臂。接下来我们来处理机械臂的控制以及轨迹的规划。

机械臂的控制和轨迹规划

说到机械臂的控制，一开始大家可能都会觉得很困难，怎么让机械臂按照自己的想法动起来。不用担心，我们mechArm270机械臂有pymycobot，一个相对比较成熟的机械臂控制库，只需要简单的几行代码就能够让机械臂运动起来。

PS：pymycobot 是pyhon的一个库，用于控制机械臂运动的一个库，我们使用的是最新版，pymycobot==3.1.2

#Introduce two commonly used control methods
'''
Send the degrees of all joints to robot arm.
angle_list_degrees: a list of degree value(List[float]), length 6
speed: (int) 0 ~ 100,robotic arm's speed
'''
send_angles([angle_list_degrees],speed)
send_angles([10,20,30,45,60,0],100)
'''
Send the coordinates to robot arm
coords:a list of coordiantes value(List[float]), length 6
speed: (int) 0 ~ 100,robotic arm's speed
'''
send_coords(coords, speed)
send_coords([125,45,78,-120,77,90],50)

以角度/坐标都可以把发送给机械臂去进行执行。

控制机械臂运动

采摘机器人轨迹规划

在实现简单的控制之后，我们就要设计机械臂抓取果实的轨迹规划了。在识别算法当中，我们获得到了果子的世界坐标。处理果子的世界坐标，转化到机械臂坐标系坐标进行目标抓取。

#处理果实世界坐标的方法
deftarget_coords(self):
        coord = self.ma.get_coords()
whilelen(coord)==0:
            coord = self.ma.get_coords()

        target = self.model_track()
        coord[:3]= target.copy()
        self.end_coords = coord[:3]

if DEBUG ==True:
print("coord: ", coord)
print("self.end_coords: ", self.end_coords)

        self.end_coords = coord

return coord

有了目标坐标之后，我们规划机械臂运动的轨迹。在机械臂运动的途中，不能撞到其他结构，打落水果等。

在规划的时候可以考虑设计：

● 初始姿态


            ● 待抓取姿态


            ● 避障姿态

各种的姿态应该根据自己的场景需求来设定。

分拣机器人路径规划

前有采摘机器人，现在来讲解分拣机器人路径的规划。其实这两个机器人的路径规划都是大同小异，分拣目标是传送带上的目标，通过深度摄像头获取传送带上坏果实的坐标将其分拣出来。

采摘机器人和分拣机器人的控制和轨迹规划就到这里了，接下来我们来介绍比较重要的，也是这个套装的重中之重，这两台机械臂之间是如何通信的呢，它们是如何有效让两台机械臂和一台传送带不进入死循环能够运行的如此顺畅。

多台机械臂的通信和逻辑的处理

如果没有一个完整的逻辑，相信这两个机械臂早就打起架来了。我们看整个程序的流程图。

就是在R2发现坏果的时候,R1是机械臂是暂停的状态，等到R2完成分拣工作了之后会给R1传递信息让R1继续进行采摘工作。

Socket通信

既然要通信，那就少不了Socket这个库。Socket库是一个在网络编程中经常使用的标准库，它提供了一组用于网络通信的API，可以方便地实现不同计算机之间的数据传输。为了解决R1 和R2 之间通信的问题，我们的解决方法是简历一个服务器，和一个客户端的效果。

服务器建立的相关代码，提前初始化需要传递的信息

Code：

classTcpServer(threading.Thread):
def__init__(self, server_address)- >None:
        threading.Thread.__init__(self)
        self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.s.bind(server_address)
print("server Binding succeeded!")
        self.s.listen(1)
        self.connected_obj =None

        self.good_fruit_str ="apple"
        self.bad_fruit_str ="orange"
        self.invalid_fruit_str ="none"

        self.target = self.invalid_fruit_str
        self.target_copy = self.target
        self.start_move =False

客户端建立的相关代码。

Code：

classTcpClient(threading.Thread):
def__init__(self, host, port, max_fruit =8, recv_interval =0.1, recv_timeout =30):
        threading.Thread.__init__(self)

        self.good_fruit_str ="apple"
        self.bad_fruit_str ="orange"
        self.invalid_fruit_str ="none"

        self.host = host
        self.port = port

# Initializing the TCP socket object
# specifying the usage of the IPv4 address family
#  designating the use of the TCP stream transmission protocol
        self.client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)

        self.client_socket.connect((self.host, self.port))
        self.current_extracted =0
        self.max_fruit = max_fruit
        self.recv_interval = recv_interval
        self.recv_timeout = recv_timeout

        self.response_copy = self.invalid_fruit_str
        self.action_ready =True

建立好了服务器和客户端，我们就可以让R1和R2像我们平时发信息一样进行通信了。

R1:"我现在在抓果实“

R2:"收到好的"

技术要点

这整个项目下来关键是多台机械臂之间的通信和逻辑的处理。要让两个机械臂之间建立通信有很多种办法，例如常见的物理通信串口通信，以太网口通信，蓝牙通信等。

我们现在使用的就是以太网口通信，使用现有的TCP/IP协议，用python当中的socket库来进行实现。

建房子先建地基这个道理大家都懂吧，建立一个项目就要搭建好它的框架也是同一个道理。此外机械臂控制的原理也是至关重要的，学会如何将目标物体转化为世界坐标在换成机械臂坐标系中的目标。

总结

该水果采摘和分拣机器人的应用场景不仅可以帮助学生更好地理解机械原理和电子控制技术，还可以促进他们对科学技术的兴趣和热爱，并提供一个锻炼实践能力和竞赛思维的机会。

学习机械臂相关知识需要实践操作，而该应用场景提供了一个具有实际意义的机会，让学生通过实际操作来加深对机械臂的理解和认识。此外，该场景还可以让学生学习和掌握机械臂运动控制、视觉识别和物品抓取等技术，帮助他们更好地掌握机械臂的相关知识和技能。

除此之外，该应用场景还可以帮助学生锻炼团队合作、创新思维和竞赛思维等能力，为他们未来的职业发展打下坚实的基础。通过参与机器人竞赛、科技展览等活动，学生可以不断提高自己的竞赛水平和创新能力，从而更好地适应未来的社会发展和科技变革。

审核编辑 黄宇