LimoPro小車建圖導航

引言

前景提要：我們在上文介紹了使用LIMOcobot實現一個能夠執行復雜任務的復合機器人系統的應用場景的項目，從以下三個方面：概念設計、系統架構以及關鍵組件。

本文主要深入項目內核的主要部分，同樣也主要分為三個部分：機械臂的視覺抓取，LIMOPro在ROS中的功能，建圖導航避障等，以及兩個系統的集成。

設備準備

myCobot280M5stack

myCobotAdaptiveGripper

myCobotCameraFlange2.0

LIMOPRO

機械臂視覺抓取

這是機械臂安裝AdaptiveGripper，和cameraflange2.0之后的樣子。

我們是用的cameraflange2.0是一款2D的相機，他并不能夠依靠他相機本身來獲取到一個物體的三維（長寬高），但我們可以使用標記物來獲得到目標物體的都長寬高。常見的有ArUco，STag，AR碼，AprilTags。今天我們用STag算法來做視覺識別。

STag標記系統

STag是一個為了高穩定性和精確的三位定位而設計的標記系統。它特別適用于環境中有遮擋和光照變化的情況。

下面有一個視頻展示了STag標記碼，ARToolKit+，ArUco，RUNE-Tag碼在同一個環境下的識別效果。

https://www.youtube.com/watch?v=vnHI3GzLVrY

可以從視頻中看出來STag對環境變化的強大適應性和在復雜場景下的高可靠性，使其成為在要求高精度跟蹤和定位的應用中的首選。還有一篇論文專門講解STag穩定的基準標記系統，感興趣的可以自己點擊鏈接去了解一下。

https://arxiv.org/abs/1707.06292

STag系統可以是適配與ROS，有ROS軟件包，用的是c++編寫的，也能夠支持python進行使用。

C++/ROS:GitHub - bbenligiray/stag: STag: A Stable Fiducial Marker System

Python：GitHub - ManfredStoiber/stag-python: Python Package for STag - A Stable, Occlusion-Resistant Fiducial Marker System

用python簡單寫一個例子

importcv2
importstag
importnumpyasnp

#加載相機參數
camera_params=np.load("camera_params.npz")
mtx,dist=camera_params["mtx"],camera_params["dist"]

#初始化STag檢測器
stag_detector=stag.detectMarkers(mtx,dist)

#初始化視頻捕獲
cap=cv2.VideoCapture(0)
whilecap.isOpened():
ret,frame=cap.read()
    if notret:
        break
    
    #應用相機校正（可選）
frame_undistorted=cv2.undistort(frame,mtx,dist, None,mtx)
    #檢測STag標記
    (corners,ids,rejected_corners) =stag.detectMarkers(frame_undistorted, 21)
    #繪制檢測到的標記及其ID
stag.drawDetectedMarkers(frame_undistorted,corners,ids)
    #繪制被拒絕的候選區域，顏色設為紅色
stag.drawDetectedMarkers(frame_undistorted,rejected_corners,border_color=(255, 0, 0))

    #顯示結果
cv2.imshow("STagDetection",frame_undistorted)

    ifcv2.waitKey(1) & 0xFF == ord('q'):
        break

cap.release()
cv2.destroyAllWindows()

標記了STag碼之后可以獲得標記碼四個角的參數，

(array([[[257., 368.],
        [185., 226.],
        [345., 162.],
        [420., 302.]]],dtype=float32),)

給定坐標過后，可以用opencv的‘cv2.solvePnP'函數來計算標記相對于相機的旋轉和偏移量。這個函數需要標記的3D坐標（在物理世界中的位置）和相應的2D圖像坐標（即檢測到的角點），以及相機的內參和畸變系數。solvePnP會返回旋轉向量（rvec）和平移向量（tvec），它們描述了從標記坐標系到相機坐標系的轉換。這樣，就可以根據這些參數計算出標記的位置和朝向。

以下是偽代碼方便理解。

defestimate_pose(corners):
#DosomecalculationswithPnP,camerarotationandoffset
returnrvec，tvec
    
defconvert_pose_to_arm_coordinates(rvec,tvec):
#將旋轉向量和平移向量轉換為機械臂坐標系統中的x,y,z,rx,ry,rz
returnx,y,z,rx,ry,rz
    
defconvert_grab(object_coord_list):
#Dosomecalculationstoconvertthecoordinatesintograspingcoordinatesfortheroboticarm
returngrab_position
    
    
cap=cv2.VideoCapture(0)
whilecap.isOpened():
ret,frame=cap.read()
ifnotret:
break
        
    
maker=(corners,ids,rejected_corners)=stag.detectMarkers(image,21)
    
rvec,tvec=stag.estimate_pose(marker)
    
object_coord_list=convert_pose_to_arm_coordinates(rvec,tvec)
grab_position=convert_grab(object_coord_list)
mycobot.move_to_position(grab_position)
機械臂控制

以上的代碼就是大概的抓取的流程，比較復雜的部分解決了，接下來我們處理機械臂的運動控制,用到的是pymycobot庫

frompymycobotimportMyCobot

#Createaninstanceandlinktheroboticarm
mc=MyCobot('com3',115200)

#Controltheroboticarmwithangle
mc.send_angles(angles_list,speed)

#Controltheroboticarmusingcoordinates
mc.send_coords(coords_list,speed,mode)

#Controlgripper,value-0~100
mc.set_gripper_value(value,speed)
 

因為機械臂的開放接口比較多，我們只需要使用坐標控制，夾爪控制就好了。

LimoPro建圖導航

完成了機械臂抓取部分的功能，接下來我們要實現小車的建圖導航模塊了。

首先我們要見圖，有了地圖之后才能夠在地圖上進行導航，定點巡航等一些的功能，目前有多種建圖的算法，因為我們搭建的場景并不是很大，環境相對于靜態我們選擇使用gmapping算法來實現。

建圖

Gmapping是基于濾波SLAM框架的常用開源SLAM算法。Gmapping有效利用了車輪里程計信息，對激光雷達的頻率要求不高，在構建小場景地圖時，所需的計算量較小且精度較高。這里通過使用ROS封裝了的GMapping功能包來實現limo的建圖。

注：以下的功能都是封裝好的可以直接使用

首先需要啟動雷達，打開一個新終端，在終端中輸入命令：

roslaunchlimo_bringuplimo_start.launchpub_odom_tf:=false

然后啟動gmapping建圖算法，打開另一個新終端，在終端中輸入命令：

roslaunchlimo_bringuplimo_gmapping.launch

成功啟動之后會打開rviz可視化工具，這時候看到的界面如圖

這時候就可以把手柄調為遙控模式，控制limo建圖了。

構建完地圖之后，需要運行以下命令，把地圖保存到指定目錄：

1、切換到需要保存地圖的目錄下，這里把地圖保存到~/agilex_ws/src/limo_ros/limo_bringup/maps/，在終端中輸入命令：

cd~/agilex_ws/src/limo_ros/limo_bringup/maps/

2、切換到/agilex_ws/limo_bringup/maps之后，繼續在終端中輸入命令：

rosrunmap_servermap_saver-fmap1

map1為保存地圖的名稱，保存地圖時應避免地圖的名稱重復

導航

前面我們用了gmapping來進行建圖，我們現在來進行導航。導航的關鍵是機器人定位和路徑規劃兩大部分。針對這兩個核心,ROS提供了以下兩個功能包。

（1）move_base：實現機器人導航中的最優路徑規劃。

（2）amcl：實現二維地圖中的機器人定位。

在上述的兩個功能包的基礎上，ROS提供了一套完整的導航框架，

機器人只需要發布必要的傳感器信息和導航的目標位置,ROS即可完成導航功能。在該框架中,move_base功能包提供導航的主要運行、交互接口。為了保障導航路徑的準確性,機器人還要對自己所處的位置進行精確定位,這部分功能由amcl功能包實現。

在導航的過程中，運用了兩種算法DWA和TEB算法，這兩種算法分別處理全局路徑和局部路徑規劃，來保證小車能夠安全的前進到目的地，避免與障礙物發生碰撞。

（1）首先啟動雷達，在終端中輸入命令：

roslaunchlimo_bringuplimo_start.launchpub_odom_tf:=false 

（2）啟動導航功能，在終端中輸入命令：

roslaunchlimo_bringuplimo_navigation_diff.launch

啟動成功之后會打開rviz界面，如圖

?編輯我們需要把剛才建的地圖給導進去，請打開limo_navigation_diff.launch文件修改參數,文件所在目錄為：~/agilex_ws/src/limo_ros/limo_bringup/launch。把map02修改為需要更換的地圖名稱。

開啟導航之后，會發現激光掃描出來的形狀和地圖沒有重合，需要我們手動校正，在rviz中顯示的地圖上矯正底盤在場景中實際的位置，通過rviz中的工具，發布一個大概的位置，給limo一個大致的位置，然后通過手柄遙控limo旋轉，讓其自動校正，當激光形狀和地圖中的場景形狀重疊的時候，校正完成。操作步驟如圖：

校正完成后

通過2DNavGoal設置導航目標點

路徑巡檢

如果要在一條路上來回運動的話，我們要啟用路徑巡檢功能，后續會使用上這個功能。

（1）首先啟動雷達，開啟一個新的終端，在終端中輸入命令：

roslaunchlimo_bringuplimo_start.launchpub_odom_tf:=false

（2）啟動導航功能，開啟一個新的終端，在終端中輸入命令：

roslaunchlimo_bringuplimo_navigation_diff.launch 

注：如果是阿克曼運動模式，請運行

roslaunchlimo_bringuplimo_navigation_ackerman.launch

（3）啟動路徑記錄功能，開啟一個新的終端，在終端中輸入命令：

roslaunchagilex_pure_pursuitrecord_path.launch

路徑記錄結束之后終止路徑記錄程序，在終端中輸入命令為：Ctrl+c

（4）啟動路徑巡檢功能，開啟一個新的終端，在終端中輸入命令：

注：把手柄調至指令模式

roslaunchagilex_pure_pursuitpure_pursuit.launch

兩個系統的集成

上面分布完成了myCobot機械臂視覺的抓取，LIMO的建圖導航，路徑巡檢功能，現在我們需要把它們集成在ROS系統上。我們預設的場景是，LIMO進行定點的巡檢，當遇到了標志物的時候停止運動，等待機械臂執行抓取物體，完成之后LIMO移動到下一個點位。

功能節點分布

在ROS（RobotOperatingSystem）中實現一個功能的流程涉及到多個步驟和組件，包括節點（nodes）、話題（topics）、服務（services）、參數服務器（parameterserver）和動作（actions）。根據ROS的功能節點架構，我們確定了節點的分布和它們交互的方式：

1.圖像識別節點（ImageRecognitionNode）

職責：持續接收來自攝像頭的圖像流，使用圖像識別算法（如OpenCV或深度學習模型）來檢測特定的標記物。

輸入：來自攝像頭的圖像流。

輸出：當檢測到標記物時，發布一個消息到一個特定的話題（如/marker_detected）。

2.控制節點（ControlNode）

職責：管理機器人的移動，包括啟動、停止和繼續巡檢。

輸入：訂閱/marker_detected話題以監聽圖像識別節點的輸出。也可能訂閱一個專門用于接收手動控制指令的話題（如/control_commands）。

輸出：向機器人底層控制系統（如驅動電機的節點）發送控制命令。

3.任務執行節點（TaskExecutionNode）

職責：執行遇到標記物后的特定任務，這些任務可以是數據采集、狀態報告等。

輸入：監聽來自控制節點的指令，這些指令指示何時開始執行任務。

輸出：任務完成的狀態反饋，可能會發送到控制節點或一個專門的狀態話題（如/task_status）。

4.導航和路徑規劃節點（NavigationandPathPlanningNode）

職責：處理機器人的路徑規劃和導航邏輯，確保機器人可以在環境中安全移動。

輸入：接收來自控制節點的指令，用于啟動、停止或調整導航路徑。

輸出：向機器人底層控制系統發送導航指令，如目標位置、速度和方向。

總結

這個場景算是初步完成了，其實還可以添加許多細節的，比如說在行徑的過程中增添一些移動的障礙物，又或者設定一個紅綠燈之類的物體，更加接近真實的場景。如果你們覺得有什么需要改善的地方，又或者說你想用LIMOcobot來做一些什么，盡管暢所欲言，你的回復和點贊就是我們更新最大的動力！