LimoPro小車建圖導(dǎo)航

引言

前景提要：我們?cè)谏衔慕榻B了使用LIMOcobot實(shí)現(xiàn)一個(gè)能夠執(zhí)行復(fù)雜任務(wù)的復(fù)合機(jī)器人系統(tǒng)的應(yīng)用場(chǎng)景的項(xiàng)目，從以下三個(gè)方面：概念設(shè)計(jì)、系統(tǒng)架構(gòu)以及關(guān)鍵組件。

本文主要深入項(xiàng)目?jī)?nèi)核的主要部分，同樣也主要分為三個(gè)部分：機(jī)械臂的視覺抓取，LIMOPro在ROS中的功能，建圖導(dǎo)航避障等，以及兩個(gè)系統(tǒng)的集成。

設(shè)備準(zhǔn)備

myCobot280M5stack

myCobotAdaptiveGripper

myCobotCameraFlange2.0

LIMOPRO

機(jī)械臂視覺抓取

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

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

STag標(biāo)記系統(tǒng)

STag是一個(gè)為了高穩(wěn)定性和精確的三位定位而設(shè)計(jì)的標(biāo)記系統(tǒng)。它特別適用于環(huán)境中有遮擋和光照變化的情況。

下面有一個(gè)視頻展示了STag標(biāo)記碼，ARToolKit+，ArUco，RUNE-Tag碼在同一個(gè)環(huán)境下的識(shí)別效果。

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

可以從視頻中看出來STag對(duì)環(huán)境變化的強(qiáng)大適應(yīng)性和在復(fù)雜場(chǎng)景下的高可靠性，使其成為在要求高精度跟蹤和定位的應(yīng)用中的首選。還有一篇論文專門講解STag穩(wěn)定的基準(zhǔn)標(biāo)記系統(tǒng)，感興趣的可以自己點(diǎn)擊鏈接去了解一下。

https://arxiv.org/abs/1707.06292

STag系統(tǒng)可以是適配與ROS，有ROS軟件包，用的是c++編寫的，也能夠支持python進(jìn)行使用。

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簡(jiǎn)單寫一個(gè)例子

importcv2
importstag
importnumpyasnp

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

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

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

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

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

cap.release()
cv2.destroyAllWindows()

標(biāo)記了STag碼之后可以獲得標(biāo)記碼四個(gè)角的參數(shù)，

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

給定坐標(biāo)過后，可以用opencv的‘cv2.solvePnP'函數(shù)來計(jì)算標(biāo)記相對(duì)于相機(jī)的旋轉(zhuǎn)和偏移量。這個(gè)函數(shù)需要標(biāo)記的3D坐標(biāo)（在物理世界中的位置）和相應(yīng)的2D圖像坐標(biāo)（即檢測(cè)到的角點(diǎn)），以及相機(jī)的內(nèi)參和畸變系數(shù)。solvePnP會(huì)返回旋轉(zhuǎn)向量（rvec）和平移向量（tvec），它們描述了從標(biāo)記坐標(biāo)系到相機(jī)坐標(biāo)系的轉(zhuǎn)換。這樣，就可以根據(jù)這些參數(shù)計(jì)算出標(biāo)記的位置和朝向。

以下是偽代碼方便理解。

defestimate_pose(corners):
#DosomecalculationswithPnP,camerarotationandoffset
returnrvec，tvec
    
defconvert_pose_to_arm_coordinates(rvec,tvec):
#將旋轉(zhuǎn)向量和平移向量轉(zhuǎn)換為機(jī)械臂坐標(biāo)系統(tǒng)中的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)
機(jī)械臂控制

以上的代碼就是大概的抓取的流程，比較復(fù)雜的部分解決了，接下來我們處理機(jī)械臂的運(yùn)動(dòng)控制,用到的是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)
 

因?yàn)闄C(jī)械臂的開放接口比較多，我們只需要使用坐標(biāo)控制，夾爪控制就好了。

LimoPro建圖導(dǎo)航

完成了機(jī)械臂抓取部分的功能，接下來我們要實(shí)現(xiàn)小車的建圖導(dǎo)航模塊了。

首先我們要見圖，有了地圖之后才能夠在地圖上進(jìn)行導(dǎo)航，定點(diǎn)巡航等一些的功能，目前有多種建圖的算法，因?yàn)槲覀兇罱ǖ膱?chǎng)景并不是很大，環(huán)境相對(duì)于靜態(tài)我們選擇使用gmapping算法來實(shí)現(xiàn)。

建圖

Gmapping是基于濾波SLAM框架的常用開源SLAM算法。Gmapping有效利用了車輪里程計(jì)信息，對(duì)激光雷達(dá)的頻率要求不高，在構(gòu)建小場(chǎng)景地圖時(shí)，所需的計(jì)算量較小且精度較高。這里通過使用ROS封裝了的GMapping功能包來實(shí)現(xiàn)limo的建圖。

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

首先需要啟動(dòng)雷達(dá)，打開一個(gè)新終端，在終端中輸入命令：

roslaunchlimo_bringuplimo_start.launchpub_odom_tf:=false

然后啟動(dòng)gmapping建圖算法，打開另一個(gè)新終端，在終端中輸入命令：

roslaunchlimo_bringuplimo_gmapping.launch

成功啟動(dòng)之后會(huì)打開rviz可視化工具，這時(shí)候看到的界面如圖

這時(shí)候就可以把手柄調(diào)為遙控模式，控制limo建圖了。

構(gòu)建完地圖之后，需要運(yùn)行以下命令，把地圖保存到指定目錄：

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

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

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

rosrunmap_servermap_saver-fmap1

map1為保存地圖的名稱，保存地圖時(shí)應(yīng)避免地圖的名稱重復(fù)

導(dǎo)航

前面我們用了gmapping來進(jìn)行建圖，我們現(xiàn)在來進(jìn)行導(dǎo)航。導(dǎo)航的關(guān)鍵是機(jī)器人定位和路徑規(guī)劃兩大部分。針對(duì)這兩個(gè)核心,ROS提供了以下兩個(gè)功能包。

（1）move_base：實(shí)現(xiàn)機(jī)器人導(dǎo)航中的最優(yōu)路徑規(guī)劃。

（2）amcl：實(shí)現(xiàn)二維地圖中的機(jī)器人定位。

在上述的兩個(gè)功能包的基礎(chǔ)上，ROS提供了一套完整的導(dǎo)航框架，

機(jī)器人只需要發(fā)布必要的傳感器信息和導(dǎo)航的目標(biāo)位置,ROS即可完成導(dǎo)航功能。在該框架中,move_base功能包提供導(dǎo)航的主要運(yùn)行、交互接口。為了保障導(dǎo)航路徑的準(zhǔn)確性,機(jī)器人還要對(duì)自己所處的位置進(jìn)行精確定位,這部分功能由amcl功能包實(shí)現(xiàn)。

在導(dǎo)航的過程中，運(yùn)用了兩種算法DWA和TEB算法，這兩種算法分別處理全局路徑和局部路徑規(guī)劃，來保證小車能夠安全的前進(jìn)到目的地，避免與障礙物發(fā)生碰撞。

（1）首先啟動(dòng)雷達(dá)，在終端中輸入命令：

roslaunchlimo_bringuplimo_start.launchpub_odom_tf:=false 

（2）啟動(dòng)導(dǎo)航功能，在終端中輸入命令：

roslaunchlimo_bringuplimo_navigation_diff.launch

啟動(dòng)成功之后會(huì)打開rviz界面，如圖

?編輯我們需要把剛才建的地圖給導(dǎo)進(jìn)去，請(qǐng)打開limo_navigation_diff.launch文件修改參數(shù),文件所在目錄為：~/agilex_ws/src/limo_ros/limo_bringup/launch。把map02修改為需要更換的地圖名稱。

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

校正完成后

通過2DNavGoal設(shè)置導(dǎo)航目標(biāo)點(diǎn)

路徑巡檢

如果要在一條路上來回運(yùn)動(dòng)的話，我們要啟用路徑巡檢功能，后續(xù)會(huì)使用上這個(gè)功能。

（1）首先啟動(dòng)雷達(dá)，開啟一個(gè)新的終端，在終端中輸入命令：

roslaunchlimo_bringuplimo_start.launchpub_odom_tf:=false

（2）啟動(dòng)導(dǎo)航功能，開啟一個(gè)新的終端，在終端中輸入命令：

roslaunchlimo_bringuplimo_navigation_diff.launch 

注：如果是阿克曼運(yùn)動(dòng)模式，請(qǐng)運(yùn)行

roslaunchlimo_bringuplimo_navigation_ackerman.launch

（3）啟動(dòng)路徑記錄功能，開啟一個(gè)新的終端，在終端中輸入命令：

roslaunchagilex_pure_pursuitrecord_path.launch

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

（4）啟動(dòng)路徑巡檢功能，開啟一個(gè)新的終端，在終端中輸入命令：

注：把手柄調(diào)至指令模式

roslaunchagilex_pure_pursuitpure_pursuit.launch

兩個(gè)系統(tǒng)的集成

上面分布完成了myCobot機(jī)械臂視覺的抓取，LIMO的建圖導(dǎo)航，路徑巡檢功能，現(xiàn)在我們需要把它們集成在ROS系統(tǒng)上。我們預(yù)設(shè)的場(chǎng)景是，LIMO進(jìn)行定點(diǎn)的巡檢，當(dāng)遇到了標(biāo)志物的時(shí)候停止運(yùn)動(dòng)，等待機(jī)械臂執(zhí)行抓取物體，完成之后LIMO移動(dòng)到下一個(gè)點(diǎn)位。

功能節(jié)點(diǎn)分布

在ROS（RobotOperatingSystem）中實(shí)現(xiàn)一個(gè)功能的流程涉及到多個(gè)步驟和組件，包括節(jié)點(diǎn)（nodes）、話題（topics）、服務(wù)（services）、參數(shù)服務(wù)器（parameterserver）和動(dòng)作（actions）。根據(jù)ROS的功能節(jié)點(diǎn)架構(gòu)，我們確定了節(jié)點(diǎn)的分布和它們交互的方式：

1.圖像識(shí)別節(jié)點(diǎn)（ImageRecognitionNode）

職責(zé)：持續(xù)接收來自攝像頭的圖像流，使用圖像識(shí)別算法（如OpenCV或深度學(xué)習(xí)模型）來檢測(cè)特定的標(biāo)記物。

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

輸出：當(dāng)檢測(cè)到標(biāo)記物時(shí)，發(fā)布一個(gè)消息到一個(gè)特定的話題（如/marker_detected）。

2.控制節(jié)點(diǎn)（ControlNode）

職責(zé)：管理機(jī)器人的移動(dòng)，包括啟動(dòng)、停止和繼續(xù)巡檢。

輸入：訂閱/marker_detected話題以監(jiān)聽圖像識(shí)別節(jié)點(diǎn)的輸出。也可能訂閱一個(gè)專門用于接收手動(dòng)控制指令的話題（如/control_commands）。

輸出：向機(jī)器人底層控制系統(tǒng)（如驅(qū)動(dòng)電機(jī)的節(jié)點(diǎn)）發(fā)送控制命令。

3.任務(wù)執(zhí)行節(jié)點(diǎn)（TaskExecutionNode）

職責(zé)：執(zhí)行遇到標(biāo)記物后的特定任務(wù)，這些任務(wù)可以是數(shù)據(jù)采集、狀態(tài)報(bào)告等。

輸入：監(jiān)聽來自控制節(jié)點(diǎn)的指令，這些指令指示何時(shí)開始執(zhí)行任務(wù)。

輸出：任務(wù)完成的狀態(tài)反饋，可能會(huì)發(fā)送到控制節(jié)點(diǎn)或一個(gè)專門的狀態(tài)話題（如/task_status）。

4.導(dǎo)航和路徑規(guī)劃節(jié)點(diǎn)（NavigationandPathPlanningNode）

職責(zé)：處理機(jī)器人的路徑規(guī)劃和導(dǎo)航邏輯，確保機(jī)器人可以在環(huán)境中安全移動(dòng)。

輸入：接收來自控制節(jié)點(diǎn)的指令，用于啟動(dòng)、停止或調(diào)整導(dǎo)航路徑。

輸出：向機(jī)器人底層控制系統(tǒng)發(fā)送導(dǎo)航指令，如目標(biāo)位置、速度和方向。

總結(jié)

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