郭隆臻 徐定明 李子信

摘?要：移動機器人的地圖構建與運動控制是智能移動機器人進行自主導航的基礎。針對工業(yè)裝配生產線的不同工位需要送料的需求，本文以一臺輪式差速移動送料機器人為研究對象，利用激光雷達作為傳感器，搭建一個裝配生產線，設計、實現(xiàn)了基于ROS的移動送料機器人的地圖構建與運動控制策略。利用slam_gmapping算法包，實現(xiàn)了機器人在特定裝配生產線環(huán)境下的同時定位與地圖構建;在主程序代碼中利用狀態(tài)機的機制，實現(xiàn)了機器人到達指定工位的運動控制。實驗結果表明，該算法和運動控制策略可以滿足在所搭建的裝配生產線中進行定位和運行。

關鍵詞： 移動機器人;輪式差速;機器人操作系統(tǒng);地圖構建;運動控制

文章編號： 2095-2163（2021）01-0055-06 中圖分類號：TP242 文獻標志碼：A

【Abstract】The map construction and motion control of mobile robots are the basis for autonomous navigation of intelligent mobile robots. In response to the needs of different stations in the industrial assembly line for feeding， this paper takes a wheeled differential mobile feeding robot as the research object， using lidar as a sensor to build an assembly production line， designing and implementing the ROS-based mobile feeding robot map construction and motion control strategy. The slam_gmapping algorithm package is used to realize the simultaneous positioning and map construction of the robot in a specific assembly line environment; the state machine mechanism is used in the main program code to realize the motion control of the robot to the designated station. The experimental results show that the algorithm and motion control strategy can satisfy the positioning and operation in the assembled assembly line.

【Key words】mobile robot; wheeled differential; robot operating system; map construction; motion control

0 引?言

隨著制造業(yè)和機器人行業(yè)的快速發(fā)展，越來越多的機器人開始在制造業(yè)中發(fā)揮重要作用，將行業(yè)工人從單一和危險的工作環(huán)境中解放出來。特別是對于一些小型裝配生產線，每個工位上都必須有原料供給，這一工作若由移動機器人來完成，將會大大提升生產效率，節(jié)約勞動力成本。而對于移動機器人領域來說，同時定位與建圖（SLAM）和路徑規(guī)劃是目前兩大研究熱點，并且也是兩大研究難點。而移動機器人的地圖構建和運動控制是機器人在陌生環(huán)境中進行路徑規(guī)劃的基礎。

在地圖構建方面，基于激光雷達的SLAM技術起源于上世紀八十年代，Smith等人[1]創(chuàng)造性地提出了基于卡爾曼濾波的SLAM算法，將機器人的位姿與路標組成聯(lián)合狀態(tài)，并且通過激光雷達用最大似然估計將數(shù)據(jù)進行關聯(lián)，從而完成對環(huán)境的一致性估計，這一研究的最大貢獻就是使基于卡爾曼濾波的SLAM算法成為當時應用的主流方法。Gordon等人將Hammersley在20世紀中期提出的粒子濾波算法思想[2]應用于SIS的非線性濾波，使得粒子濾波在許多領域獲得廣泛使用，并得到長足發(fā)展。Murphy[3]使用Rao-Blackwell統(tǒng)計量尋找更優(yōu)的統(tǒng)計估計，并且與粒子濾波相結合提出了RBPF算法。Montemerlo等人將粒子濾波引入了基于遞歸概率的SLAM設計思想中，提出了Fast SLAM算法。Grisetti等人[4]為了解決Fast SALM在大尺度環(huán)境中需要較多粒子來計算機器人后驗概率分布的問題，提出了gmapping方案。該算法將粒子的數(shù)目進行限定，在一定程度上減小了內存消耗，目前在建立二維柵格地圖中應用十分廣泛。

國內關于SLAM的研究雖起步較晚，但其發(fā)展的速度非常快。陳白帆等人[5]對Fast SLAM做出了優(yōu)化，使粒子群更為集中，提升了算法的運算速度。朱磊等人[6]將人工魚群算法應用在對粒子分布預測的過程中，使更新粒子的準確度與機器人的真實狀態(tài)分布更為相符。厲茂海等人[7]在RBPF算法的重采樣過程中使用自適應方法，將進化策略和RBPF算法結合起來，得到了較好的估計結果。Yuan等人[8]提出了一種使用均勻分布的樣本來表示粒子先驗位姿的方法，改進了重采樣過程中的粒子退化問題，并且將所有粒子樣本進行批量處理，極大提高了激光SLAM算法的實時性。

以上的SLAM算法中，由于gmapping可以實時構建室內地圖，在構建小場景地圖時所需的計算量較小且精度較高，對激光雷達頻率要求低、魯棒性高，不需要太多的粒子，而且也沒有回環(huán)檢測，有效地利用了車輪里程計信息，里程計可以提供機器人的位姿先驗。因此，本文選用gmapping對環(huán)境進行地圖構建，建立的柵格地圖可以滿足該移動送料機器人的運動控制。

1 機器人平臺設計

在硬件方面，考慮到穩(wěn)定性與防撞性，移動機器人平臺的下部分車體為鋼材質，上部分車體為鋁合金材質，前部為北陽雷達，后部為思嵐雷達。采用六輪底盤結構，雙輪差速驅動，4個萬向輪支撐，彈性懸掛;在機械臂的選擇上，采用了遨博i5協(xié)作機器人;電源方面，移動平臺與機械臂采用2個獨立的電池。其硬件參數(shù)配置信息見表1。末端執(zhí)行器包括電磁鐵模塊、智能視覺多模板識別模塊和雙抓手模塊，可以共同完成識別、抓取或吸附等一系列流程。移動操作機器人的實物如圖1所示。

在軟件方面，由于機器人操作系統(tǒng)（ROS）框架具有靈活、高效、集成度高、代碼開源等優(yōu)點，可以簡化機器人平臺下的復雜任務創(chuàng)建和穩(wěn)定行為控制，幫助用戶快速完成機器人應用的建立、編寫和多機整合，所以被應用得越來越廣泛。本文采用ROS框架，利用gazebo（仿真環(huán)境）進行仿真環(huán)境的創(chuàng)建，利用rviz（三維可視化平臺）進行可視化顯示，采用C++語言編寫控制程序。

2 運動學模型的建立

在移動機器人的轉向設計中，差速轉向由于結構緊湊，重量輕，不需要專門的轉向機構，而且可實現(xiàn)較小半徑、甚至零半徑的轉向，所以近年來差速轉向越來越多地運用于各式移動機器人中。本文選用輪式差速轉向機器人，其運動學模型如圖2所示，一共6個輪子，中間2個為驅動輪，前后4個為萬向輪。這種結構簡單，在理論上的旋轉半徑可為零，但由于轉動過程中的速度瞬心位于兩驅動輪軸心連線上，因而即使旋轉半徑為零，旋轉中心也與車體幾何中心不一致[9]。由于移動平臺上需要加裝機械臂和移動電源，需要移動平臺部分有較大的承載力，而此結構具有4個萬向輪，能增大平臺的承載能力和穩(wěn)定性，可以滿足要求。

在圖2中，假設其內部布局均勻對稱，質心與幾何中心重合，在兩驅動輪軸線中心位置。設在t時刻，機器人速度瞬心為點O，其余各物理量見表2。

在圖2中，由運動學知識可得：

由公式（1）～（5），結合圖2，可求得機器人在局部坐標系中的運動學模型為：

機器人在全局坐標系中的運動學模型為：

在實際的差速模型中，可以通過左右驅動輪編碼器的讀數(shù)，實現(xiàn)移動機器人的軌跡推算。差速機器人的運動模型有3種，即：切線模型、割線模型和圓弧模型。其中，ROS中采用的是切線模型，故本文也選取切線模型，假設機器人在原來方向上沿著切線運動到下一點，再轉過一定的角度。已知機器人在t時刻的位姿，通過t+1時刻編碼器的讀數(shù)，能夠計算出移動機器人在t+1時刻的位姿。建立模型如圖3所示，將時間離散化，取采樣間隔為Ts，機器人從點A運動到點B為一個采樣間隔，可得到移動機器人的軌跡推算模型為：

3 環(huán)境地圖建模

環(huán)境地圖建模需要將創(chuàng)建的地圖模型與傳感器所獲得的環(huán)境信息進行整合，對環(huán)境模型的要求是便于機器人控制器進行存儲、處理、更新和應用。在建立地圖的過程中，機器人主要的數(shù)據(jù)來源是里程計信息和激光傳感器的信息。在?ROS 中使用較為廣泛的地圖構建方法是柵格地圖法（Grid mapping），由德國研究者 Grisetti 等人[10]提出并優(yōu)化，是一種基于粒子濾波器的能同時定位與構建地圖的算法。算法的優(yōu)點是建模方便、易于編程、運算速度快并且精度高。因此，本文選用該方法進行環(huán)境地圖的建模。

一個10×10 大小的柵格地圖區(qū)域如圖4所示。研究中將該區(qū)域劃分為2種狀態(tài)區(qū)域：自由狀態(tài)區(qū)域和障礙狀態(tài)區(qū)域。其中，白色柵格體現(xiàn)為自由狀態(tài)區(qū)域，機器人可自由行駛;黑色柵格體現(xiàn)為障礙狀態(tài)區(qū)，機器人不可通過。

要確定各柵格的坐標，可以令U={1，2，3，…，100}為柵格的序列集，以地圖的左下角點（0，0）為原點，再由左至右、由下至上，對地圖按序編號，從1～100。序號j的坐標通過下式確定：

其中，mod表示求余運算;int表示取整運算;R表示柵格行數(shù)。

本文選用gmapping功能包，分別在仿真環(huán)境和真實環(huán)境進行地圖建模。gmapping功能包集成了Rao-Blackwellized粒子濾波算法，其核心運行節(jié)點為slam_gmapping，slam_gmapping的總體框架如圖5所示。gmapping功能包訂閱機器人的深度信息、IMU信息和里程計信息，同時完成一些必要參數(shù)的設置，即可創(chuàng)建并輸出基于概率的二維柵格地圖。

3.1 仿真環(huán)境建圖

在ROS中gazebo上所搭建的仿真實驗環(huán)境如圖6所示。每個工位之間保持著一定的距離，需要移動機器人到每個工位進行送料。移動機器人底盤部分的模型在rviz中的顯示如圖7所示。

在構建地圖時，首先啟動gazebo仿真環(huán)境，接著啟動rviz可視化環(huán)境，稍后將啟動鍵盤控制器來移動機器人。然后，通過遙控界面控制移動操作機器人在仿真環(huán)境內運動，環(huán)繞全場進行地圖構建，在可視化環(huán)境中可以看到地圖構建的實時特征。最后， 在地圖構建成功后，通過地圖服務節(jié)點（map server）將地圖進行保存，用于后續(xù)的定位導航，建好的仿真環(huán)境地圖如圖8所示。在地圖構建過程中，ROS 節(jié)點會不斷地校正機器人在環(huán)境中的位置信息，由此確保地圖的準確性，進而通過鍵盤控制機器人的移動?？紤]到仿真環(huán)境與真實環(huán)境是等比例的，所以仿真環(huán)境總體尺寸較大，在有些地方機器人與障礙物之間的距離超過了激光雷達的有效距離，會造成地圖發(fā)生偏移。故在建圖過程中，將指揮機器人環(huán)繞全場完成總體建圖，再進行中間障礙物的掃描，如此可使建立的地圖更加精確。

3.2 真實環(huán)境建圖

利用實驗室內的設備與器材，搭建了一個裝配生產線的場景，其場景與仿真環(huán)境相似，用來進行真實環(huán)境的建圖，如圖9所示。

所不同的是，真實環(huán)境會與工業(yè)裝配線更為貼切，有些工位的底座部分是鏤空的，而本文所用的是2D激光雷達，會造成激光所在平面掃描不到障礙物的情況，如圖10所示，有些工位只顯示一些離散的點。

針對這個問題，先選用Matlab中的imread和imwrite函數(shù)進行格式轉換，將ROS中保存的默認格式.pgm轉換為.png格式，而后使用Photoshop等畫圖軟件進行人工修正，最后用Matlab將圖片轉換成標準黑白格式，替換掉之前的地圖文件。修正后的地圖如圖11所示。

4 運動控制實驗

移動機器人主程序使用自定義ROS消息類型ControlCommand進行控制，sendMotionCommand函數(shù)向控制程序發(fā)送指令，并將last_motion_command_done變量置為false。當控制程序完成sendMotionCommand申明的動作，會將last_motion_command_done變量置為true?？赏ㄟ^定時輪詢該變量控制機器人的狀態(tài)機轉換，實現(xiàn)各種功能。主程序中用到的command包括MOVE_TO_TARGET、MOVE_TO_TARGET_WITH_PATH、TURN_TO_TARGET、TURN_TO_ANGLE和JOKSTICK_CONTROL。前兩個為移動命令，中間兩個為轉動命令，最后一個為切換手柄控制命令。

在主程序代碼中利用了狀態(tài)機的機制，來實現(xiàn)機器人到達指定工位的運動控制。至關重要的函數(shù)是move_loop（），將會以 100 ms的頻率被調用。該函數(shù)下面有int類型的全局變量move_process，每一個move_process代表機器人的一種狀態(tài)機，通過對move_process賦值，使機器人跳轉狀態(tài)。

基于修正后的在真實環(huán)境下構建的環(huán)境地圖，在地圖中選取了5個工位進行實驗，第一個工位為取料處，后四個工位為送料處。為了提高安全性，設定機器人的最大直線速度為0.7 m/s，最大角速度為0.4 rad/s。與仿真實驗不同的是，增加了機械臂來完成工件的識別與抓取。本次實驗選取膨脹螺栓作為被抓取的工件，墊片用電磁鐵來吸取，其余由電動手來抓取，電動手抓實物視像如圖12所示。為了提高送料的效率，將膨脹螺栓的4類組件抓取后分別放入對應的盒子中，在送料時直接將盒子放到對應工位即可。

設定5個工位分別為A、B1、B2、B3、B4，其對應關系見表3。機器人到達抓取工位后，由移動平臺給機械臂發(fā)送信號，此時，移動平臺原地不動，由機械臂進行目標的識別與抓取。待抓取動作完成后，機械臂給移動平臺發(fā)送信號，機器人按照事先規(guī)劃好的路線到達每個送料處，送料的次序為墊圈、螺母、螺栓、套筒。到達每個送料處后，也是先由移動平臺向機械臂發(fā)送指令，待送料完成后，再由機械臂向移動平臺發(fā)送指令。直到最后一次送料完成后，實驗結束。其送料流程圖如圖13所示。

5 結束語

本研究基于機器人操作系統(tǒng)，以一臺輪式差速移動送料機器人為研究對象，設計并得到了定位導航系統(tǒng)。首先搭建了類似汽車裝配生產線的室內環(huán)境;然后利用ROS系統(tǒng)中的開源slam_gmapping算法包，實現(xiàn)了機器人在該環(huán)境下的同時定位與地圖構建;最后，利用狀態(tài)機的機制，實現(xiàn)了機器人到達指定工位的運動控制。實驗結果驗證了該算法和運動控制策略的可行性和可靠性，能夠進行準確定位和運動控制，便于后續(xù)在系統(tǒng)中加入路徑規(guī)劃算法，實現(xiàn)更加智能化的自動送料，具有良好的工業(yè)應用前景。

參考文獻

[1]SMITH R， SELF M， CHEESEMAN P. Estimating uncertain spatial relationships in robotics[J]. Machine Intelligence & Pattern Recognition， 1988， 5：435-461.

[2]Chen Zhe. Bayesian filtering： From Kalman filters to particle filters， and beyond[R]. Hamilton：McMaster University， 2003.

[3]MURPHY K. Bayesian map learning in dynamic environments[C]//Advances in Neural Information Processing Systems. San Mateo， CA： Morgan Kaufman， 1999，12：1015-1021.

[4]GRISETTI G， STACHNISS C， BURGARD W. Improved techniques for grid mapping with rao-blackwellized particle filters[J]. IEEE Transactions on Robotics， 2007， 23（1）：34-46.

[5]陳白帆， 蔡自興， 袁成. 基于粒子群優(yōu)化的移動機器人SLAM方法[J]. 機器人， 2009， 31（6）： 513-517.

[6]朱磊， 樊繼壯， 趙杰， 等. 未知環(huán)境下的移動機器人SLAM方法[J]. 華中科技大學學報（自然科學版）， 2011， 39（7）： 9-13.

[7]厲茂海， 洪炳熔， 羅榮華. 用改進的Rao-blackwellized粒子濾波器實現(xiàn)移動機器人同時定位和地圖創(chuàng)建[J]. 吉林大學學報（工學版）， 2007， 30（2）： 401-406.

[8]YUAN Rupeng， ZHANG Fuhai， QU Jiadi， et al. An enhanced pose tracking method using progressive scan matching[J]. Industrial Robot-The International Journal of Robotics Research and Application， 2019， 46（2）： 235-246.

[9]楊俊駒， 林睿， 王振華， 等. 輪式移動機器人運動控制系統(tǒng)研究與設計[J]. 現(xiàn)代電子技術， 2016， 39（2）： 22-27.

[10]GRISETTI G， STACHNISS C， BURGARD W. Improved techniques for grid mapping with rao-blackwellized particle filters[J]. IEEE Transactions on Robotics， 2007， 23（1）： 34-46.