Proceedings of 2023 National Symposium on System Science and Engineering 國立臺灣海洋大學

National Taiwan Ocean University, Keelung, 10~12 June, 2023

ROS 架構下應用 SLAM 與電腦視覺之

搭載六軸機械手臂電梯導航機器人
李文鈴、鍾宛君、莊家峰
國立中興大學電機工程學系

摘要 — 本研究以 SLAM 建構地圖，使輪型機器人 計的軸承架來按電梯按鍵。本論文機器人則裝上多用途

可自行導航至指定教室。電腦視覺部分則是透過 六軸機器手臂，除了可按電梯，在教室導航中亦可依需
YOLOv5 深度學習，辨識出機器人所在樓層與電梯按 求進行其它夾取任務。
鈕位置，再由逆向運動學與各軸修正完成六軸機械手臂 上述文獻多探討理論研究並只注重在機器人單一功
控制，實現按電梯按鈕的功能，進而在不同樓層間移 能的實現。而本論文重在實際面向的整合操作。使用
動。以 Tkinter 實現圖形化界面，供使用者在筆電輸入 Pioneer 3-DX 輪型機器人搭配 Rplidar A2 光達，進行
指定教室和確認是否抵達目標，再給予機器人指令，達 SLAM 建 圖 與 導 航 ， 並 加 入 自 定 義 動 態 避 障 。 配 合
成簡易人機介面之設計。另外行進中會發出提示音提醒 YOLOv5 深度學習辨識出按鈕位置，再透過逆向運動學
使用者。最後於 ROS 架構下整合成可重複執行的智慧 完成機械手臂的操控，實現按電梯按鈕的功能，達成機
型電梯導航機器人系統。 器人進出電梯與不同樓層間的移動。最後設計圖形化介
面，供使用者輸入目標點的樓層與教室。
關鍵詞：SLAM、影像辨識、智慧型機器人、機械手
本論文組織如下。第二節為系統軟硬體介紹。第三
臂、機器人導航。
節為研究方法介紹。第四節為實驗結果。第五節為論文
的結論。
一、 簡介
現今疫情肆虐，保護自身身體健康，戴口罩、噴酒 二、 系統軟硬體
精是不可忽略的例行公事，其中人與人的接觸被認為是
2.1 硬體配置
最普遍的傳染途徑。此外，隨著科技日新月異，機器人
機器人的硬體配置如圖一。最上層放置筆電，負責
能完成的事情越來越廣泛，想讓機器取代人力的想法也
接收各個感測器數據控制機器人移動。機械手臂放置於
日漸成熟。基於上述兩項社會趨勢，若能擁有一台電梯
筆電前方，負責完成按電梯按鈕的動作。考量到機械手
導航機器人，便可以達成不同樓層間的無接觸文書工作
臂功能的高度需求，光達放在第二層，進行 360 度掃描
傳遞，減少感染疾病之風險，也減少了人力需求。
檢測，增加導航過程中的準確性。
本論文中，擁有導航功能的服務型機器人和配合影
圖二為圖一紅框內的配置圖。由寶元數控
像辨識的機械手臂操控，是很重要的兩部分。導航部
(https://www.lnc.com.tw/) 所 製 造 的 控 制 器 置 於 筆 電 下
分，文獻[1]利用 ROS 的 SLAM 搭配 A*演算法，實現
方，此控制器擁有 6 個接頭可使用 PWM 訊號做軸控
了機器人的導航功能。文獻[2]提出結合 SLAM 和紅外
制，並提供 EtherCAT 接頭可做數位通訊使用。在本論
線訊號參考點的方式，可簡化自主機器人導航系統，並
文中主要用於操控機械手臂運動。在控制器左側為 12
具有返回其起始位置或參考位置的功能。文獻[3]提出一
伏電池，做為控制器的電力來源。
種在未知環境下的快速導航方式，以縮短導航路徑並能
通過未知的狹小通道。機器手臂操控部分，文獻[4]基於
6-DOF S-RRR 配置的機械手很難由逆向運動學(inverse
kinematics)分析問題，因此提出了具有 RRR-S 配置的新
6-DOF 機械手，依照原始機械手與新機械手的關節關
係，得到原始架構的解決方案。文獻[5]以具有深度資訊
的相機，結合目標定位(target localization)和邊緣檢測演
算法(edge detection algorithms)，即可準確測量物體與機
械手臂之間距離。完成物體識別後，可抓住並放置指定
位置。
在電梯機器人的研究中，文獻[6]搭配地圖建構與
行人偵測的功能，讓機器人可以進出電梯。但是機器人
未具有按電梯樓層與上下樓等功能。文獻[7]利用特定設
圖一： 機器人硬體配置
Proceedings of 2023 National Symposium on System Science and Engineering 國立臺灣海洋大學
National Taiwan Ocean University, Keelung, 10~12 June, 2023

2.2.3 影像偵測與辨識：YOLOv5
YOLOv5 是 一 種 單 階 段 目 標 檢 測 算 法 [10] ， 使 用
Pytorch 框架，能夠方便地訓練自己的數據集。YOLOv5
主 要 提 供 四 種 模 型 : YOLOv5s 、 YOLOv5m 、
YOLOv5l 、 YOLOv5x 。 網 絡 架 構 分 為 輸 入 端 、
backbone、neck、prediction。
本論文使用 YOLOv5 進行電梯按鈕的辨識，運用自
行訓練好的模型判斷出各按鈕代表之意義，例如它是
上、下樓或是哪一層樓的按鈕，再藉由偵測電梯按鈕的
圖二： 圖一紅框內硬體配置
亮暗來判斷是否按到電梯按鈕，以及當按鈕由亮轉暗代
2.2 軟體系統 表抵達該樓層。
2.2.1 同步定位與地圖構建 （Simultaneous Localization 圖五為實際辨識情況，在上下樓按鈕皆沒有亮起
And Mapping，SLAM） 時，兩個按鈕的辨識結果都為 off，當下樓按鈕亮起，
SLAM 讓機器人可以在未知環境下，透過感測器數 則會辨識出 down_on。
據增量式的建立地圖，並運用重複觀測到的數據來定位
自身位置，達到同時定位與地圖構建的目的。本論文使
用的建構地圖方法為 karto SLAM [8]。karto SLAM 使用
圖優化(graph optimization)的方式進行建構，如圖三，
由感測器測得資訊預估機器人位置，再經由閉循環的掃
描匹配，使感測器數據與建構出的地圖相同。並會做非
線性最佳化，使建構出的地圖更為準確。

圖五： 實際辨識上下樓電梯按鈕

圖三： karto 地圖優化[8] 2.2.4 圖形化介面(Graphical User Interface，GUI)

圖形化介面運用圖形方式顯示電腦操作介面，讓使
2.2.2 ROS 內建導航環境
用者可以擁有更好電腦操作體驗。本論文使用 Tkinter
ROS Navigation Stack 是一組可以讓機器人或自動化
作為開發工具來建立圖形化介面。Tkinter（也叫 Tk 接
載具可以在空間中穩定移動的相關程式[9]，透過搜集里
口）是 Tk 圖形用戶界面（GUI）工具包標準的 Python
程計、感測器等數據，控制機器人行走。圖四為整體架
接口[11]，也是一個輕量級的跨平臺圖形用戶界面開發
構圖。導航部分通過 move_base 功能包來實現，透過在
工具。圖六為自行設計之介面。
已知地圖下給定目標點，控制機器人運動，並在過程中
不斷回傳座標訊息。
功能包內的全局路徑規劃(global_planner)使用 A*搜
尋演算法(A star search algorithm)，來尋找地圖中的最短
路徑。透過將地圖劃分為小方格，搜索起點周圍的點，
並不斷從中選擇一個新的點作為當前點再繼續搜索，直
到找到終點。
而局部路徑規劃(local_planner)使用 Dynamic Window
approaches 算法，透過在速度空間取樣多組速度，並模
擬其運動軌跡得出行駛路線，再經由評價函數，選出可
避開障礙物的最優路徑。
圖六： 輸入目標之圖形化界面

2.3 架構示意圖
圖七為機器人的架構示意圖。透過筆電鏡頭取得當
下影像資訊及辨識結果，將資訊提供給筆電做機械手臂
的角度換算，再把角度資訊傳遞給控制器，完成機械手
臂之操控。使用者在圖形化介面輸入想去的目標資訊，
進行後續導航工作。由光達掃描周遭環境，把即時測距
資訊傳回筆電，進行導航和避障功能的運算、切換。
圖四： Navigation Stack
Proceedings of 2023 National Symposium on System Science and Engineering 國立臺灣海洋大學
National Taiwan Ocean University, Keelung, 10~12 June, 2023

圖七： 機器人的架構示意圖

三、 研究方法
3.1 流程圖
3.1.1 整體流程圖
如圖八所示，在圖形化介面上輸入欲到達目標點，
機器人進入搭乘電梯流程。到達指定樓層後，在導航起
始點切換成平面導航模式，導航至指定教室。抵達教室
後使用者需在圖型化介面上確認，讓機器人得以返回搭
乘電梯起始點，並在搭乘電梯起始點等待使用者下一次
的輸入。

圖九： 搭乘電梯流程圖

3.1.3 平面導航流程圖
機器人將自行規劃最佳路徑到達指定教室。途中若
遇障礙物，會切換成避障模式以避開障礙物。在離開障
礙物後，會自行切回導航模式繼續前往目標教室。到達
目標點後，等待使用者確認已抵達，則返回搭乘電梯起
始點，如圖十。

3.2 地圖建構與導航
本論文使用 karto SLAM 建構地圖，為使地圖更符合
實際場景，使用 KolourPaint 繪圖軟體進行地圖調整，
避免因雷達放置高度，導致有些障礙物掃描不到而被誤
圖八： 整體流程圖 判為可行走區域，後續導航皆使用優化過的地圖，以利
導航順利執行。SLAM 導航使用全局路徑規劃器(global
3.1.2 搭乘電梯流程圖 planner)進行路徑規劃，它會自行避開地圖上的已知障
如圖九所示，得到目標樓層後，使用影像辨識確認 礙物，規劃出一條最適路徑。若行走過程中出現臨時障
自身所在樓層，判斷需上或下樓，走到電梯按鈕前透過 礙物，則會切換為自定義的避障模式。避障模式主要分
影像辨識得到按鈕位置，藉由逆向運動學控制手臂運 成三種，以前方避障為例：正前方避障模式、遠離前方
動，進而按到按鈕。若影像辨識判斷按鈕為暗則會重新 牆壁避障模式、遠離前方死角模式，如圖十一。
運算按鈕位置，並再次按按鈕，直到電梯按鈕為亮，等
待並用雷達偵測兩邊電梯門哪一邊開啟。進電梯後如前
面所述，完成按樓層按鈕的步驟，等按鈕燈號熄滅，便
走出電梯到達導航起始點，並切換至導航模式。
Proceedings of 2023 National Symposium on System Science and Engineering 國立臺灣海洋大學
National Taiwan Ocean University, Keelung, 10~12 June, 2023

如圖十三，在白點處機器人將轉成頭朝電梯的方
向，用雷達測距測得哪一邊的電梯門開啟。接下來走到
電梯門前空心圓圈處，確保機器人能安全進入電梯，然
後走到實心藍點，開始電梯內的位置校正。在此處需轉
成筆電鏡頭面對電梯樓層按鈕的方向，以利按按鈕工作
進行。

圖十三： 偵測哪台電梯開門

如圖十四，確認電梯按鈕的燈熄滅之後，機器人轉
成頭朝電梯門的方向，走出電梯到達綠點，在此處從搭
乘電梯的程式切換成平面導航。當平面導航結束後，會
回到白點，等待下一次使用者輸入目標點。

圖十： 平面導航流程圖

圖十四： 出電梯到樓層導航起始位置
3.4 影像辨識
當導航抵達電梯按鈕前，使用影像辨識訓練模型，
辨識出電梯按鈕亮暗、位置等資訊，再傳至筆電以供機
械手臂操控及機器人移動。按電梯按鈕屬於較精細的動
作，機器人需停在準確位置才可執行，因此我們讓機器
人在上下樓電梯按鈕前做位置和角度的校正：目標點範
圍為±0.1 公尺，並會做機器人與牆壁之間距離校正在
0.3 公尺至 0.4 公尺間，以利正確辨識出按鈕座標給機械
圖十一：導航配合動態避障 手臂運作。進入電梯後，也讓機器人在樓層電梯按鈕前
3.3 行走流程 做校正，方法同上。圖十五為電梯內辨識樓層按鈕。
如圖十二，在白點處用 YOLOv5 辨識機器人所在樓
層，因各樓的樓層標示位置不統一，所以在此處順時針
轉 90°，確定在各樓層都能辨識到，再以走直角的方式
朝電梯按鈕方向前進，到達紅點，也就是電梯上下樓按
鈕前的位置，會讓筆電鏡頭朝向牆壁，以利辨識出按鈕
位置，確認按完後，再以走斜直線方式回到白點位置。

圖十五：辨識樓層電梯按鈕

3.5 機械手臂控制
由影像辨識結果判斷電梯按鈕為暗，在符合此條件
下得到電梯按鈕座標，藉由逆向運動學換算出機械手臂
各軸所需轉動角度，先只考慮目標點的 X 和 Y 軸，且
圖十二： 偵測樓層完再按上下樓按鈕 為了達到按按鈕時有最佳效率，因此手臂最前端（L3）
Proceedings of 2023 National Symposium on System Science and Engineering 國立臺灣海洋大學
National Taiwan Ocean University, Keelung, 10~12 June, 2023

需與地面平行，也就是須滿足 θ1+θ2+θ3=90°的條 3.走到直角處校正（rviz） 4. 前進到上下樓梯按鈕前（rviz）

件。再經由網路線從筆電傳遞資訊給控制器，進而控制
機械手臂。圖十六中的 X 和 Y 為 yolov5 得到之目標座
標轉換成機械手臂座標系統內的座標，再由圖十七所式
逆向運動學換算出轉動角度。

5. 按上下樓按鈕 6. 走 斜 直 線 回 搭 乘 電 梯 起 始 點
（rviz）

圖十六： 機械手臂座標系統 7. 偵測哪一邊電梯門開 8. 走到電梯前校正（rviz）

9. 走進電梯（rviz） 10. 校正並按樓層按鈕

圖十七： 機械手臂逆向運動學

四、 實驗結果
4.1 實作流程
我們以在 1 樓輸入指令到 6 樓教室 604 為例，導航
誤差皆在 0.4 公尺以內。 圖十八顯示導航過程。包含進
出電梯、在同層樓的移動與在所件地圖的機器人導航軌
跡圖。
11. 出電梯 12.前進到導航起始點（rviz）

1. 在搭乘電梯起始點輸入指定樓 2. 偵測所在樓層(目前在 1 樓)
層、教室
Proceedings of 2023 National Symposium on System Science and Engineering
National Taiwan Ocean University, Keelung, 10~12 June, 2023
13.到達 6 樓導航起始點
15.到達 604
17.回到搭乘電梯起始點（rviz）
圖十八： 導航實作
4.2 影像辨識準確率
分為上下樓電梯按鈕與樓層電梯按鈕(圖十九)兩處來
做辨識，在有進行機器人位置校正情況下，實驗 56
次，成功 51 次，成功率約為 0.911。除了能辨識各個按
鈕代表之樓層，也能偵測電梯按鈕的亮暗。
圖十九： 辨識樓層電梯按鈕實作
4.3 機械手臂控制準確率
圖二十顯示手臂按電梯上樓、下樓實驗過程。在影
像辨識正確的情形下，實驗 43 次，成功 35 次，手臂控
制成功率約為 0.814。
五、 結論
本論文提出以輪型機器人搭配機械手臂與影像辨
識，完成在不同樓層間的垂直移動，再配合平面樓層導
國立臺灣海洋大學
14.前往 604（rviz） 位置校正及影像辨識 按上下樓按鈕
16.等待使用者確認
圖二十： 機械手臂控制實驗
航，實現在整棟已知地圖的建築物內移動。而近年因疫
情肆虐，無接觸服務越顯重要，若能在現有架構上，增
加置物空間、讓機械手臂有抓握功能，便能達成機器人
自行搬運貨物再運到指定地的功能。在準確性方面，希
望可以對機械手臂的修正進行改善與增強，使得按電梯
按鈕成功率得以提高。
18.等待下次使用者輸入目的地 參考文獻
[1] 謝元碩、李盈臻、莊家峰，”ROS 架構下應用 SLAM 與
影 像 辨 識 之 遠 端 聲 控 導 航 機 器 人 ,”Proc. Nat. Symp.
Systems Science and Engineering, pp. 1-5, Taichung, Taiwan,
May 2022.
[2] I. M. Murwantara, B. Hardjono, H. Tjahyadi, and A. S. Putra,
“A consolidation of SLAM and signal reference point for
autonomous robot navigation,” in Proc. IEEE 17th Int. Conf.
Smart Communities: Improving Quality of Life Using ICT,
IoT and AI (HONET), 2020, pp. 108-112.
[3] C. F. Juang, C. Y. Chou, and C. T. Lin, “Navigation of a
fuzzy-controlled wheeled robot through the combination of
expert knowledge and data-driven multiobjective evolutionary
learning,” IEEE Trans. Cybernetics, vol. 52, no. 8, pp. 7388-
7401, Aug. 2022.
[4] L. Jiang, X. Huo, Y. Liu and H. Liu, “An analytical inverse
kinematic solution with the reverse coordinates for 6-DOF
manipulators,” in Proc. IEEE Int. Conf. Mechatronics and
Automation, Takamatsu, Japan, 2013, pp. 1552-1558.
[5] C. -W. Chen, A. -C. Tsai, Y. -H. Zhang and J. -F. Wang, “3D
object detection combined with inverse kinematics to achieve
robotic arm grasping,” in Proc. 10th Int. Conf. Orange
Technology (ICOT), Shanghai, China, 2022, pp. 1-4.
[6] J. Zhao, Y. Chen, and Y. Lou, “A human-aware robotic
system for mobile robot navigating in multi-floor building
with elevator,” in Proc. WRC Symp. Advanced Robotics and
Automation, 2019, pp. 178-183.
[7] S. Jiang, W. Yao, M. S. Wong, M. Hang, Z. Hong, E. J. Kim,
S. H. Joo, and T. Y. Kuc, “Automatic elevator button
localization using a combined detecting and tracking
framework for multi-story navigation,” IEEE Access, vol. 8,
pp. 1118-1134, 2020.
[8] K. Konolige, G. Grisetti, R. K¨ummerle, B. Limketkai, R.
Vincent, Efficient Sparse Pose Adjustment for 2D Mapping,
In Proc. of Int. Conf. on Intelligent Robots and Systems
(IROS), Oct. 2010.
[9] ROS Navigation Stack, http://wiki.ros.org/navigation.
[10] Yolo v5, https://ultralytics.com/yolov5.
[11] Tkinter, https://docs.python.org/3/library/tkinter.html.