引言

在之前我們已經介紹了水果采摘和分揀機器人的應用場景，今天我們來介紹復合機器人水果采摘運輸的場景。

作為最熱門的技術領域，機器人技術正在徹底改變各行各業，推動全球創新。為了滿足這一快速發展領域對專業技術人才日益增長的需求，Elephant Robotics公司為高等院校開發了一個開創性的機器人教育解決方案。該創新解決方案將模擬自動水果采摘機與水果分揀和配送自動化的復合機器人結合起來，為學生提供了在最熱門和最有發展趨勢的技術領域的全面學習體驗。

讓讓我們帶著探索的心情，一起來看看！帶著幾個問題：

● 復合機器人應用場景是什么？
● 這個場景有什么用，能夠讓我們學到什么？
● 這個場景有什么特別的地方？

Compound Robot: Automation of Fruit Sorting and Delivery

圖中所展示的是復合機器人套裝應用的場景，由以下幾個部分組成。

你可能會想知道這個套裝是如何怎么運行起來的，有那么多個模塊組成，它們分別充當什么成分，它的工作原理又是什么呢。接下來跟隨著我們的腳步來探索這未知的事物。

這是一張場景的結構圖，介紹圖中的組成：

● 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可以跟蹤?？偟恼f，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算法是一種基于拓撲結構的建圖算法，它通過對環境進行拓撲分析，將環境表示為一個圖的結構等等。

俗話說的好“機器是死的，人是活的”，我們可以根據各種外界因素來選擇方法，來達到效果最優。這個過程當中是需要不斷地去學習，不斷地去探討。

總結

隨著科技的不斷發展，在未來世界里。機器將會是我們人類最大的幫手，在未來的發展趨勢也是朝著智能化、靈活化、高效化、安全化的方向發展。移動小車的發展趨勢主要包括自主導航、智能路徑規劃、多模態感知等方面；機械臂的發展趨勢則主要包括自主學習、自適應控制、視覺識別等方面。同時，復合機器人的發展也需要解決移動與操作的協同問題，提高機器人系統的整體性能和效益。

我們建立該應用場景也是提供給熱愛機器人的創客，學生一個具有實際意義的機會。讓學生通過實際操作來加深對機械臂的理解和認識。此外，該場景還可以讓學生學習和掌握機械臂運動控制、視覺識別和物品抓取等技術，幫助他們更好地掌握機械臂的相關知識和技能。還可以幫助學生鍛煉團隊合作、創新思維和競賽思維等能力，為他們未來的職業發展打下堅實的基礎。

審核編輯 黃宇