引言

在之前我們已經(jīng)介紹了水果采摘和分揀機器人的應(yīng)用場景，今天我們來介紹復合機器人水果采摘運輸?shù)膱鼍啊?/p>

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

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

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

Compound Robot: Automation of Fruit Sorting and Delivery

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

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

這是一張場景的結(jié)構(gòu)圖，介紹圖中的組成：

● myAGV+R1 Arm ：復合機器人，由myAGV搭載mechArm 270-Pi機械臂


            ● Obstacle：障礙物，阻擋myAGV前行的物體。


            ● Tree：仿真模擬果樹，樹上有果實


            ● R2 Arm:  mechArm 270 Pi，六軸機械臂


            ● Loading Area: 復合機器人到達該位置，等待ARM將果實的裝載


            ● Unloading Area：復合機器人到達該位置，進行卸載果實


            ● Collection Area：收集區(qū)，將負載的果實卸載到收集區(qū)與內(nèi)

復合機器人從初始位置開始運轉(zhuǎn)，它進行自主導航，規(guī)避障礙前往“Loading Area”，抵達位置后，R2 Arm抓取果樹上的果實放置在復合機器人上，復合機器人前往下一個目標“Unloading Area”抵達位置后，R1Arm將裝載的果實放置到“Collection Area”。

產(chǎn)品介紹

Robotic Arm - mechArm 270 Pi

這是一款小六軸機械臂，以Raspberry-Pi為核心控制，ESP32為輔助控制，結(jié)構(gòu)是中心對稱結(jié)構(gòu)（仿工業(yè)結(jié)構(gòu)）。mechArm 270-Pi本體重量1kg， 負載250g，工作半徑270mm，設(shè)計緊湊便攜，小巧但功能強大，操作簡單，能與人協(xié)同、安全工作。

myAGV- Raspberry Pi 4B

myAGV是一個以樹莓派4B為控制主板的移動底盤機器人，它采用了競賽級別的麥克納姆車輪和帶有金屬框架的全包裹設(shè)計，內(nèi)置了SLAM算法來實現(xiàn)雷達建圖和自動導航，動態(tài)避障等功能。Raspberry Pi 4 Model B采用了更快的處理器、更多的內(nèi)存和更快的網(wǎng)絡(luò)連接，并且可以通過多個USB 3.0和USB 2.0端口、千兆以太網(wǎng)和雙頻802.11ac無線網(wǎng)絡(luò)連接，實現(xiàn)更好的外部設(shè)備連接和網(wǎng)絡(luò)性能。樹莓派社區(qū)也是目前全球名列前茅的硬件開發(fā)社區(qū)，里面有許多有趣的案例，常見的解決方法供開發(fā)者交流。

深度攝像頭

隨著使用場景的多樣性，普通的2D攝像頭無法滿足我們使用的需求。在場景中我們使用到的是深度攝像頭。深度攝像頭是一種能夠獲取場景深度信息的相機。它不僅能夠捕捉場景的顏色和亮度信息，還能夠感知物體之間的距離和深度信息。深度攝像頭通常使用紅外線或其他光源來進行測量，以獲取物體和場景的深度信息。

它可以獲取很多信息，例如深度的畫面，彩色的畫面，紅外線的畫面，點云畫面。

有了深度相機，我們就可以精準的獲取到果樹上果實的位置，以及顏色信息。

自適應(yīng)夾爪-機械臂末端執(zhí)行器

自適應(yīng)夾爪是一種用來抓取、握取或夾持物體的末端執(zhí)行器，它由兩個可移動的爪子組成，可以通過機械臂的控制系統(tǒng)來控制其開合程度和開合速度。

功能的實現(xiàn)

開始前的準備

編譯環(huán)境：

# 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

我們主要分類兩個部分進行功能分析，第一部分是整個項目的架構(gòu)，邏輯分析，第二部分是myAGV的功能分析，第三部分是機械臂的控制，其中第二第三部分又分為以下幾個功能點。

myAGV：

● 建圖導航
● 靜態(tài)動態(tài)避障

mechArm：

● 機器視覺識別算法
● 機械臂的控制和機械臂路勁規(guī)劃

我們先從項目的架構(gòu)開始分析。

項目的結(jié)構(gòu)

我們用一張圖來描述整個項目的流程，可以結(jié)合上面提到的結(jié)構(gòu)圖來進行分析。

首先myAGV前往“l(fā)oading area” 等待機械臂從果樹上抓取果實放到myAGV上，myAGV再通過運輸果實到“unloading area“將myAGV上負載得果實運輸?shù)健眂ollection area“，通過在myAGV上得機械臂來實現(xiàn)。前面得描述為一個循環(huán)，當完成卸載水果后，myAGV會重新回到”loading area“進行裝載果實，裝載完成后再去卸載水果，進行循環(huán)。

讓我們一起來分析一下，在整個過程中需要解決一些什么問題。最直觀的問題就是，當myAGV到達一個目標地點的時候，機械臂是如何知道什么時候進行抓取工作呢？這就要涉及到myAGV和機械臂通信的一個問題了。

通信

我們得了解我們的產(chǎn)品是什么，mechArm是一個固定的六軸機械臂，以樹莓派為控制主板，myAGV是一個可移動的機器人，也是以樹莓派為控制主板。所以目前最佳的通信就是用TCP/IP協(xié)議來實現(xiàn)。我們常用的一個方法庫叫socket，它運作的原理很簡單，建立一個服務(wù)器和一個客戶端，就好比我們現(xiàn)在用的ins，tw，whatsapp聊天功能一樣。可以收到信息，也可以發(fā)出信息。

下面是建立服務(wù)器的一些代碼。

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到達目的地的時候給機械臂發(fā)送一個信息“i am here”，機械臂收到這個信息之后執(zhí)行果實的采摘。

myAGV

建圖導航

現(xiàn)在的汽車有導航，自動駕駛功能，能夠運用在現(xiàn)實生活中實現(xiàn)無人駕駛，但是這門技術(shù)目前還不是很完善。那么復合機器人中所用的到自動導航和避障功能是用什么來實現(xiàn)的呢？

ROS:是一個開源的機器人操作系統(tǒng)，它提供了一系列的工具關(guān)于機器人的操作，也包含了待會我們會提及到的功能包，Gmapping,AMCL,DWA。ROS是一個功能強大的機器人操作系統(tǒng)，它提供了豐富的庫和工具，以及靈活的模塊化架構(gòu)，使得機器人應(yīng)用的開發(fā)變得更加高效、靈活和可靠。

在日常生活中，我們使用導航我們的操作一般是：打開地圖，輸入目的地，選擇路線前往目的地。

首先，我們要想實現(xiàn)一塊區(qū)域必須得有這塊區(qū)域的一個“地圖”，我們把這個稱之為“建圖”

GPS（全球衛(wèi)星定位系統(tǒng)）是各個定位系統(tǒng)中所用到的，精準的收集地面上的信息，構(gòu)建出一個準確的地圖。

這里涉及到一個建圖的算法，Gmapping。

gmapping是一種用于在機器人上建立環(huán)境地圖的算法。它是一種基于激光雷達數(shù)據(jù)的SLAM（Simultaneous Localization and Mapping）算法，可以在機器人運動時實時地構(gòu)建環(huán)境地圖，并同時確定機器人的位置。

這里我們演示一下，如何進行g(shù)mapping建圖。

實際中建圖的運動

ROS界面

可以看到地圖是myAGV通過雷達獲取當前位置的數(shù)據(jù)，排查周圍環(huán)境的情況，在圖中標出障礙物。根據(jù)我們搭建的場景，在系統(tǒng)中形成一個閉環(huán)，就完成了建圖。

接下來我們用到的是navigation功能，顧名思義就是“導航”。導航的前提條件，就是需要定位（myAGV所處的位置），用到的是ROS提供的ACML算法。

AMCL算法是一種概率機器人定位算法，它基于蒙特卡羅方法（Monte Carlo Method）和貝葉斯濾波（Bayesian Filtering）理論，通過對機器人搭載的傳感器數(shù)據(jù)進行處理，實時估計機器人在環(huán)境中的位置，并不斷更新機器人位置的概率分布。

AMCL算法通過以下步驟實現(xiàn)機器人的自適應(yīng)定位：

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

2. 運動模型更新：根據(jù)機器人的運動狀態(tài)和控制信息，更新粒子集合中每個粒子的位置和狀態(tài)信息。

3. 測量模型更新：根據(jù)機器人搭載的傳感器數(shù)據(jù)，計算每個粒子的權(quán)重（即代表機器人在該粒子位置時傳感器數(shù)據(jù)與實際數(shù)據(jù)的匹配程度），并通過歸一化處理，將權(quán)重轉(zhuǎn)化為概率分布。

4. 重采樣：根據(jù)粒子的權(quán)重，對粒子集合進行重采樣，從而提高定位精度并減少計算復雜度。

5. 機器人定位：根據(jù)粒子集合的概率分布，確定機器人在環(huán)境中的位置，并更新機器人狀態(tài)估計信息。

在我們使用導航的時候選擇目的地，導航軟件會給我們提供多條路給我們選擇，然而這里也會提供多條路徑進行選擇，只不過是系統(tǒng)幫忙做了最優(yōu)的選擇。涉及到了兩個概念，全局路徑規(guī)劃和局部路徑規(guī)劃。Navigation提供了一套框架，其中g(shù)lobal_planner是全局規(guī)劃器，而local_planner是局部路徑規(guī)劃器，它們之間的有些消息，例如：全局規(guī)劃的軌跡，就是在框架內(nèi)部傳遞，沒有topic可以跟蹤。總的說，ROS的導航模塊提供了一套機制，通過選擇不同的規(guī)劃器，可以實現(xiàn)機器人的自主導航。

Navigation框架中集成了靜態(tài)和動態(tài)的避障。

導航避障功能

從上面的圖中可以看到除了規(guī)劃器，導航模塊還包括cost_map，也就是柵格地圖，并且也包括了靜態(tài)障礙物的信息，也就是說哪些區(qū)域可以通過哪些不可以通過。同時動態(tài)障礙物信息是通過sensor topics來發(fā)布，然后實時更新cost_map來實現(xiàn)動態(tài)避障。除了地圖，導航模塊還需要定位信息，是由amcl模塊來提供定位信息，如果想采用其它的定位模塊替代，只需要發(fā)布相同的topic即可。同時還要提供tf信息，也就是說不同傳感器之間的轉(zhuǎn)換關(guān)系，這在機器人中非常常見。機器人的位姿信息則由odometry來提供，包括機器人的速度、角度等，提供給局部規(guī)劃器來規(guī)劃路徑。

在避障功能中的主力軍是DWA算法，DWA算法旨在使機器人能夠在復雜和動態(tài)的環(huán)境中快速、安全地規(guī)劃和跟蹤路徑。具體來說，DWA算法通過以下幾個步驟來實現(xiàn)：

● 生成候選速度：在機器人當前速度和方向的基礎(chǔ)上，根據(jù)機器人的動力學限制和環(huán)境條件，生成一組速度和方向的候選值。
 ● 評估軌跡：對于每個候選速度，預(yù)測機器人在未來一段時間內(nèi)的軌跡，并評估軌跡的安全性和可達性。
● 選擇最佳速度：根據(jù)軌跡的評估結(jié)果，選擇滿足安全和可達性要求的最佳速度和方向，并將其應(yīng)用于機器人的控制系統(tǒng)中。

機器視覺算法

從前面已經(jīng)提到普通的2D攝像頭已經(jīng)沒辦法滿足我們的需求，于是我們使用3D深度攝像頭。使用深度攝像頭之前需要進行相機標定。相機標定的教程（）

相機標定：

相機標定是指通過對攝像機進行一系列測量和計算，確定攝像機內(nèi)部參數(shù)和外部參數(shù)的過程。攝像機內(nèi)部參數(shù)包括焦距、主點位置、像素間距等，而攝像機外部參數(shù)則包括攝像機在世界坐標系中的位置和方向等。相機標定的目的是為了使攝像機能夠準確地捕捉并記錄世界坐標系中物體的位置、大小、形狀等信息。

目標物體是果實，它顏色不一，形狀也不一定，有紅的，橙的，黃的。想要準確的抓取且不傷害到果實，就需要獲取果實的各個信息，寬度，厚度等，智能的進行抓取。

目標果實目前較大的區(qū)別就是顏色的不一樣，所以設(shè)定紅色和橙色的目標將被選中，關(guān)于機器顏色的識別，就要用到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
}

我們的第一步就是要將目標果實能夠正確的檢測出來，以便后續(xù)獲取目標物體的坐標，深度等其他信息。我們定義果實的屬性，坐標等信息方便后續(xù)的存儲以及調(diào)用。

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

最后我們要獲取的是果實的坐標，能夠發(fā)送給機械臂去執(zhí)行抓取的坐標，通過深度坐標轉(zhuǎn)化為世界坐標，已經(jīng)是成功了一大半了，最后只需要將世界坐標跟機械臂的坐標系進行轉(zhuǎn)換就可以獲得抓取目標果實的坐標了。

# 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

該階段，我們實現(xiàn)了檢測目標物體，并且返回可抓取的坐標，傳遞給機械臂。接下來我們來處理機械臂的控制以及軌跡的規(guī)劃。

機械臂的控制和軌跡規(guī)劃

說到機械臂的控制，大家開始可能都會覺得困難，思考怎么讓機械臂按照自己的想法動起來。不過不用擔心，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)

場景中有兩個機械臂，它們分別執(zhí)行不同的職能。其中一個是執(zhí)行果實的采摘，另一個是執(zhí)行果實的裝卸。

在實現(xiàn)機械臂的控制，就需要設(shè)計機械臂抓取果實的軌跡規(guī)劃了。在獲取了果實的坐標之后，對機械臂的路徑有所要求，在機械臂運動的過程中不能撞到其他結(jié)構(gòu)，打落水果等。

下面是規(guī)劃路徑的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

裝卸機械臂的路徑規(guī)劃也是同樣的原則來實現(xiàn)裝卸這個過程。

技術(shù)亮點

從整個項目來看，不論是myAGV的建圖導航，自動避障功能，還是3D攝像頭機器識別的算法，機械臂的路勁規(guī)劃，亦或者是myAGV跟機械臂之間的通信，邏輯上的處理。每一個功能點都有值得學習的地方。

我們在項目中使用到的一些算法并不是最優(yōu)的方法，舉一個例子說明，建圖中用到的gmapping算法。除了gmapping算法還有，F(xiàn)astSLAM算法：FastSLAM算法是一種基于粒子濾波的建圖算法；Topological Mapping算法是一種基于拓撲結(jié)構(gòu)的建圖算法，它通過對環(huán)境進行拓撲分析，將環(huán)境表示為一個圖的結(jié)構(gòu)等等。

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

總結(jié)

隨著科技的不斷發(fā)展，在未來世界里。機器將會是我們?nèi)祟愖畲蟮膸褪郑谖磥淼陌l(fā)展趨勢也是朝著智能化、靈活化、高效化、安全化的方向發(fā)展。移動小車的發(fā)展趨勢主要包括自主導航、智能路徑規(guī)劃、多模態(tài)感知等方面；機械臂的發(fā)展趨勢則主要包括自主學習、自適應(yīng)控制、視覺識別等方面。同時，復合機器人的發(fā)展也需要解決移動與操作的協(xié)同問題，提高機器人系統(tǒng)的整體性能和效益。

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

審核編輯 黃宇