创新实践，复合机器人采摘运输教育沙盘案例研究

引言

在之前我们已经介绍了水果采摘和分拣机器人的应用场景，今天我们来介绍复合机器人水果采摘运输的场景。

作为最热门的技术领域，机器人技术正在彻底改变各行各业，推动全球创新。为了满足这一快速发展领域对专业技术人才日益增长的需求，Elephant Robotics公司为高等院校开发了一个开创性的机器人教育解决方案。该创新解决方案将模拟自动水果采摘机与水果分拣和配送自动化的复合机器人结合起来，为学生提供了在最热门和最有发展趋势的技术领域的全面学习体验。

让让我们带着探索的心情，一起来看看！带着几个问题：

● 复合机器人应用场景是什么？
● 这个场景有什么用，能够让我们学到什么？
● 这个场景有什么特别的地方？

Compound Robot: Automation of Fruit Sorting and Delivery

图中所展示的是复合机器人套装应用的场景，由以下几个部分组成。

mechArm 270 Pi	*2
myAGV	*1
3D Camera/Depth Camera	*1
Simulated Fruit Tree	*1
3D Structural Parts	some
Screen	*2
Keyboard＆mouse	*2
Router	*1

你可能会想知道这个套装是如何怎么运行起来的，有那么多个模块组成，它们分别充当什么成分，它的工作原理又是什么呢。接下来跟随着我们的脚步来探索这未知的事物。

这是一张场景的结构图，介绍图中的组成：

● myAGV+R1 Arm ：复合机器人，由myAGV搭载mechArm 270-Pi机械臂


            ● Obstacle：障碍物，阻挡myAGV前行的物体。


            ● Tree：仿真模拟果树，树上有果实


            ● R2 Arm:  mechArm 270 Pi，六轴机械臂


            ● Loading Area: 复合机器人到达该位置，等待ARM将果实的装载


            ● Unloading Area：复合机器人到达该位置，进行卸载果实


            ● Collection Area：收集区，将负载的果实卸载到收集区与内

复合机器人从初始位置开始运转，它进行自主导航，规避障碍前往“Loading Area”，抵达位置后，R2 Arm抓取果树上的果实放置在复合机器人上，复合机器人前往下一个目标“Unloading Area”抵达位置后，R1Arm将装载的果实放置到“Collection Area”。

产品介绍

Robotic Arm - mechArm 270 Pi

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

myAGV- Raspberry Pi 4B

myAGV是一个以树莓派4B为控制主板的移动底盘机器人，它采用了竞赛级别的麦克纳姆车轮和带有金属框架的全包裹设计，内置了SLAM算法来实现雷达建图和自动导航，动态避障等功能。Raspberry Pi 4 Model B采用了更快的处理器、更多的内存和更快的网络连接，并且可以通过多个USB 3.0和USB 2.0端口、千兆以太网和双频802.11ac无线网络连接，实现更好的外部设备连接和网络性能。树莓派社区也是目前全球名列前茅的硬件开发社区，里面有许多有趣的案例，常见的解决方法供开发者交流。

深度摄像头

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

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

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

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

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

功能的实现

开始前的准备

编译环境：

# for myAGV to realize some functions
ROS1 Melodic(Gmapping,AMCL,DWA)
# for robotic arm(mechArm 270) to realize some functions
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

我们主要分类两个部分进行功能分析，第一部分是整个项目的架构，逻辑分析，第二部分是myAGV的功能分析，第三部分是机械臂的控制，其中第二第三部分又分为以下几个功能点。

myAGV：

● 建图导航
● 静态动态避障

mechArm：

● 机器视觉识别算法
● 机械臂的控制和机械臂路劲规划

我们先从项目的架构开始分析。

项目的结构

我们用一张图来描述整个项目的流程，可以结合上面提到的结构图来进行分析。

首先myAGV前往“loading area” 等待机械臂从果树上抓取果实放到myAGV上，myAGV再通过运输果实到“unloading area“将myAGV上负载得果实运输到”collection area“，通过在myAGV上得机械臂来实现。前面得描述为一个循环，当完成卸载水果后，myAGV会重新回到”loading area“进行装载果实，装载完成后再去卸载水果，进行循环。

让我们一起来分析一下，在整个过程中需要解决一些什么问题。最直观的问题就是，当myAGV到达一个目标地点的时候，机械臂是如何知道什么时候进行抓取工作呢？这就要涉及到myAGV和机械臂通信的一个问题了。

通信

我们得了解我们的产品是什么，mechArm是一个固定的六轴机械臂，以树莓派为控制主板，myAGV是一个可移动的机器人，也是以树莓派为控制主板。所以目前最佳的通信就是用TCP/IP协议来实现。我们常用的一个方法库叫socket，它运作的原理很简单，建立一个服务器和一个客户端，就好比我们现在用的ins，tw，whatsapp聊天功能一样。可以收到信息，也可以发出信息。

下面是建立服务器的一些代码。

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


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

处理完通信就可以解决了myAGV和机械臂之间的问题。当myAGV到达目的地的时候给机械臂发送一个信息“i am here”，机械臂收到这个信息之后执行果实的采摘。

myAGV

建图导航

现在的汽车有导航，自动驾驶功能，能够运用在现实生活中实现无人驾驶，但是这门技术目前还不是很完善。那么复合机器人中所用的到自动导航和避障功能是用什么来实现的呢？

ROS:是一个开源的机器人操作系统，它提供了一系列的工具关于机器人的操作，也包含了待会我们会提及到的功能包，Gmapping,AMCL,DWA。ROS是一个功能强大的机器人操作系统，它提供了丰富的库和工具，以及灵活的模块化架构，使得机器人应用的开发变得更加高效、灵活和可靠。

在日常生活中，我们使用导航我们的操作一般是：打开地图，输入目的地，选择路线前往目的地。

首先，我们要想实现一块区域必须得有这块区域的一个“地图”，我们把这个称之为“建图”

GPS（全球卫星定位系统）是各个定位系统中所用到的，精准的收集地面上的信息，构建出一个准确的地图。

这里涉及到一个建图的算法，Gmapping。

gmapping是一种用于在机器人上建立环境地图的算法。它是一种基于激光雷达数据的SLAM（Simultaneous Localization and Mapping）算法，可以在机器人运动时实时地构建环境地图，并同时确定机器人的位置。

这里我们演示一下，如何进行gmapping建图。

实际中建图的运动

ROS界面

可以看到地图是myAGV通过雷达获取当前位置的数据，排查周围环境的情况，在图中标出障碍物。根据我们搭建的场景，在系统中形成一个闭环，就完成了建图。

接下来我们用到的是navigation功能，顾名思义就是“导航”。导航的前提条件，就是需要定位（myAGV所处的位置），用到的是ROS提供的ACML算法。

AMCL算法是一种概率机器人定位算法，它基于蒙特卡罗方法（Monte Carlo Method）和贝叶斯滤波（Bayesian Filtering）理论，通过对机器人搭载的传感器数据进行处理，实时估计机器人在环境中的位置，并不断更新机器人位置的概率分布。

AMCL算法通过以下步骤实现机器人的自适应定位：

1. 初始化粒子集合：首先，在机器人初始位置周围生成一组粒子，代表机器人可能的位置。

2. 运动模型更新：根据机器人的运动状态和控制信息，更新粒子集合中每个粒子的位置和状态信息。

3. 测量模型更新：根据机器人搭载的传感器数据，计算每个粒子的权重（即代表机器人在该粒子位置时传感器数据与实际数据的匹配程度），并通过归一化处理，将权重转化为概率分布。

4. 重采样：根据粒子的权重，对粒子集合进行重采样，从而提高定位精度并减少计算复杂度。

5. 机器人定位：根据粒子集合的概率分布，确定机器人在环境中的位置，并更新机器人状态估计信息。

在我们使用导航的时候选择目的地，导航软件会给我们提供多条路给我们选择，然而这里也会提供多条路径进行选择，只不过是系统帮忙做了最优的选择。涉及到了两个概念，全局路径规划和局部路径规划。Navigation提供了一套框架，其中global_planner是全局规划器，而local_planner是局部路径规划器，它们之间的有些消息，例如：全局规划的轨迹，就是在框架内部传递，没有topic可以跟踪。总的说，ROS的导航模块提供了一套机制，通过选择不同的规划器，可以实现机器人的自主导航。

Navigation框架中集成了静态和动态的避障。

导航避障功能

从上面的图中可以看到除了规划器，导航模块还包括cost_map，也就是栅格地图，并且也包括了静态障碍物的信息，也就是说哪些区域可以通过哪些不可以通过。同时动态障碍物信息是通过sensor topics来发布，然后实时更新cost_map来实现动态避障。除了地图，导航模块还需要定位信息，是由amcl模块来提供定位信息，如果想采用其它的定位模块替代，只需要发布相同的topic即可。同时还要提供tf信息，也就是说不同传感器之间的转换关系，这在机器人中非常常见。机器人的位姿信息则由odometry来提供，包括机器人的速度、角度等，提供给局部规划器来规划路径。

在避障功能中的主力军是DWA算法，DWA算法旨在使机器人能够在复杂和动态的环境中快速、安全地规划和跟踪路径。具体来说，DWA算法通过以下几个步骤来实现：

● 生成候选速度：在机器人当前速度和方向的基础上，根据机器人的动力学限制和环境条件，生成一组速度和方向的候选值。
 ● 评估轨迹：对于每个候选速度，预测机器人在未来一段时间内的轨迹，并评估轨迹的安全性和可达性。
● 选择最佳速度：根据轨迹的评估结果，选择满足安全和可达性要求的最佳速度和方向，并将其应用于机器人的控制系统中。

机器视觉算法

从前面已经提到普通的2D摄像头已经没办法满足我们的需求，于是我们使用3D深度摄像头。使用深度摄像头之前需要进行相机标定。相机标定的教程（）

相机标定：

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

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

目标果实目前较大的区别就是颜色的不一样，所以设定红色和橙色的目标将被选中，关于机器颜色的识别，就要用到HSV色域来进行目标的检测。下面的部分代码是用来检测目标果实。

Code

classDetector:
classFetchType(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
}

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

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)

场景中有两个机械臂，它们分别执行不同的职能。其中一个是执行果实的采摘，另一个是执行果实的装卸。

在实现机械臂的控制，就需要设计机械臂抓取果实的轨迹规划了。在获取了果实的坐标之后，对机械臂的路径有所要求，在机械臂运动的过程中不能撞到其他结构，打落水果等。

下面是规划路径的Code：

#处理果实世界坐标的方法
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

装卸机械臂的路径规划也是同样的原则来实现装卸这个过程。

技术亮点

从整个项目来看，不论是myAGV的建图导航，自动避障功能，还是3D摄像头机器识别的算法，机械臂的路劲规划，亦或者是myAGV跟机械臂之间的通信，逻辑上的处理。每一个功能点都有值得学习的地方。

我们在项目中使用到的一些算法并不是最优的方法，举一个例子说明，建图中用到的gmapping算法。除了gmapping算法还有，FastSLAM算法：FastSLAM算法是一种基于粒子滤波的建图算法；Topological Mapping算法是一种基于拓扑结构的建图算法，它通过对环境进行拓扑分析，将环境表示为一个图的结构等等。

俗话说的好“机器是死的，人是活的”，我们可以根据各种外界因素来选择方法，来达到效果最优。这个过程当中是需要不断地去学习，不断地去探讨。

总结

随着科技的不断发展，在未来世界里。机器将会是我们人类最大的帮手，在未来的发展趋势也是朝着智能化、灵活化、高效化、安全化的方向发展。移动小车的发展趋势主要包括自主导航、智能路径规划、多模态感知等方面；机械臂的发展趋势则主要包括自主学习、自适应控制、视觉识别等方面。同时，复合机器人的发展也需要解决移动与操作的协同问题，提高机器人系统的整体性能和效益。

我们建立该应用场景也是提供给热爱机器人的创客，学生一个具有实际意义的机会。让学生通过实际操作来加深对机械臂的理解和认识。此外，该场景还可以让学生学习和掌握机械臂运动控制、视觉识别和物品抓取等技术，帮助他们更好地掌握机械臂的相关知识和技能。还可以帮助学生锻炼团队合作、创新思维和竞赛思维等能力，为他们未来的职业发展打下坚实的基础。

审核编辑 黄宇