基于SLAM的機器人運動控制系統(tǒng)設(shè)計

楊元義 ，楊超

摘 要：地圖創(chuàng)建和同步定位技術(shù)（SLAM）是移動機器人開發(fā)的核心技術(shù)，是智能移動機器人實現(xiàn)自主和半自主移動的關(guān)鍵，論文基于SLAM對機器人的運動控制系統(tǒng)進(jìn)行設(shè)計。首先采用具有免費、開源優(yōu)勢且注重易用性、安全便捷性的ROS平臺實現(xiàn)移動機器人的SLAM技術(shù)，利用自適應(yīng)蒙特卡羅定位算法估計移動機器人位姿，通過貝葉斯算法構(gòu)建網(wǎng)絡(luò)地圖。其次是路徑規(guī)劃算法對移動機器人運動路徑進(jìn)行規(guī)劃，使得移動機器人沿著規(guī)劃路徑行走，實現(xiàn)移動機器人的自主移動功能。最后是將設(shè)計的移動機器人應(yīng)用于智能采摘試驗中，其算法最長用時為7 s，平均用時為6.7 s，能夠很好地滿足預(yù)期的效果。本論文的研究對移動機器人應(yīng)用于智能生產(chǎn)實踐中具有一定的參考價值。

關(guān)鍵詞：移動機器人;定位與導(dǎo)航;運動控制;系統(tǒng)設(shè)計

中圖分類號：TP242.3 文獻(xiàn)標(biāo)識碼：A ? ? 文章編號：1001-5922（2021）12-0107-05

Design of Robot Motion Control System based on SLAM

Yang Yuanyi， Yang Chao

（Yantai Vocational College， Yantai 264670， China）

Abstract：Simultaneous localization and mapping （SLAM） is the core technology of mobile robot development and the key to realize autonomous and semi autonomous movement of intelligent mobile robot. This paper designs the robot motion control system based on slam. Firstly， the slam technology of the mobile robot is realized by using the ROS platform which has the advantages of free and open source， pays attention to ease of use， safety and convenience， estimates the pose of the mobile robot by using the Adaptive Monte Carlo positioning algorithm， and constructs the network map by Bayesian algorithm. Secondly， the path planning algorithm plans the motion path of the mobile robot， makes the mobile robot walk along the planned path， and realizes the autonomous movement functions of the mobile robot. Finally， the designed mobile robot is applied to the intelligent picking experiment. The longest time of the algorithm is 7 s and the average time is 6.7 s， which can well meet the expected effect. The research of this paper has a certain reference value for the application of mobile robot in intelligent production practice.

Key words：Mobile Robot; Positioning and Navigation; Motion Control; The System Design

0 引言

機器人技術(shù)的快速發(fā)展使得移動機器人在清潔、維修等領(lǐng)域得到了廣泛的應(yīng)用，對智能移動機器人開發(fā)的關(guān)鍵是機器人自主定位與導(dǎo)航。同步定位與映射（simultaneous localization and mapping，SLAM）是解決智能移動機器人自主定位與導(dǎo)航的核心技術(shù)，即移動機器人在未知的環(huán)境條件下從一個未知的位置移動，同時在移動的過程中結(jié)合位置與地圖進(jìn)行自身定位，且在自身定位的基礎(chǔ)上構(gòu)建增量式地圖。機器人操作系統(tǒng)（Robot Operating System，ROS）是專門為機器人軟件所開發(fā)設(shè)計的電腦操作系統(tǒng)架構(gòu)，其提供了一組工具、庫和驅(qū)動程序，以幫助開發(fā)具有硬件抽象的機器人應(yīng)用程序[1-2]。借助ROS，機器人開發(fā)人員能夠快速、準(zhǔn)確地對機器人運動進(jìn)行模擬真實世界的試驗。

王玄玄研發(fā)了包含雙電機驅(qū)動底盤系統(tǒng)、2自由度擺臂的移動機器人，并通過實物試驗驗證了所研制DSP控制的移動機器人能夠有效地滿足運動控制的要求。張東濤基于互聯(lián)網(wǎng)技術(shù)對智能割草機器人運動控制進(jìn)行研究，采用物料網(wǎng)技術(shù)實現(xiàn)了對割草機器人的定位功能，所研發(fā)的割草機器人能夠?qū)φ系K物進(jìn)行自主避障且所規(guī)劃路徑最優(yōu)。黃強設(shè)計了一種應(yīng)用于架空導(dǎo)向巡檢機器人的運動控制系統(tǒng)，并通過在控制器設(shè)計中引入S型加減速控制算法減少機器人在運動過程中可能出現(xiàn)的沖擊，通過實物平臺驗證了所設(shè)計的運動控制系統(tǒng)具有比較高的運動和定位精度，能夠有效滿足巡檢機器人的工作需求。張聲成對清雪機器人自主定位與導(dǎo)航系統(tǒng)進(jìn)行設(shè)計開發(fā)，將SLAM應(yīng)用于機器人在未知環(huán)境中的定位和建圖，并采用激光雷達(dá)掃描匹配建立柵格地圖，實現(xiàn)清雪機器人全局路徑規(guī)劃與局部動態(tài)避障。馮波針對機器人SLAM系統(tǒng)在實際場景和低紋理場景中提取有效特征點數(shù)量比較少的問題，提出了基于點線特征與IMU信息融合的雙目慣導(dǎo)SLAM系統(tǒng)，將提出的機器人SLAM算法應(yīng)用于實物實驗中驗證了該系統(tǒng)的平均跟蹤幀率為25 Hz，定位精度可達(dá)0.072 m。SLAM結(jié)合機器人所處的未知環(huán)境信息，使得移動機器人能夠智能地、準(zhǔn)確地和當(dāng)前的環(huán)境地圖進(jìn)行匹配，是移動機器人設(shè)計和開發(fā)的核心技術(shù)，直接關(guān)系到移動機器人的運動過程控制。同時ROS作為機器人的開源的元操作系統(tǒng)，對其代碼進(jìn)行適當(dāng)?shù)男薷木涂梢灾谱饕豢钤跈C器人上運行的軟件，所花費的時間、財力、物力成本比較少。

基于此，本文在ROS平臺下基于SLAM設(shè)計了機器人運動控制系統(tǒng)，提出了基于SLAM的智能移動機器人導(dǎo)航方法。首先采用自適應(yīng)蒙特卡羅定位算法估計移動機器人位姿，由貝葉斯算法構(gòu)建網(wǎng)格地圖。其次采用路徑規(guī)劃算法對移動機器人移動路徑進(jìn)行規(guī)劃，使得移動機器人沿著規(guī)劃路徑行走，實現(xiàn)移動機器人自主移動功能。最后是將設(shè)計的機器人運動控制系統(tǒng)應(yīng)用于智能采摘中，驗證運動控制系統(tǒng)的可行性、有效性、魯棒性。

1 移動機器人系統(tǒng)概述

ROS作為機器人開發(fā)的元操作系統(tǒng)，在機器人的設(shè)計開發(fā)中具有十分廣泛的應(yīng)用。結(jié)合實際項目需求，在ROS操作平臺下進(jìn)行SLAM開發(fā)，確保移動機器人能夠工作在低能見度的室內(nèi)場景和煙霧能見度低的室外場景[3-4]。圖1為安裝有移動行走單元模塊的移動機器人系統(tǒng)裝備。根據(jù)無人平臺的應(yīng)用需求，設(shè)計一套基于SLAM的同步定位與映射的移動機器人自回歸控制系統(tǒng)。通過建立當(dāng)前局部地圖與獲得的全局地圖之間的對應(yīng)關(guān)系，減少由軌跡估計誤差引起的機器人定位不確定性。如果通訊信號丟失，機器人將自動進(jìn)入恢復(fù)模式，系統(tǒng)將跟蹤先前通過路線計劃記錄的路線點，實時完成導(dǎo)航，修復(fù)起始位置來達(dá)到解決該問題的目的。

結(jié)合機器人運動控制系統(tǒng)開發(fā)框架，采用移動機器人進(jìn)行同步定位與測繪。移動機器人的主要感知傳感器是激光掃描傳感器，核心控制器為PCM-3363，控制器CPU為Intel Atom D525 Dual core 1.8 GHz，控制器為1GB DDR3 800 MHz內(nèi)存，并嵌入Ubuntu 12.04系統(tǒng)，在Ubuntu系統(tǒng)上嵌入ROS-fuerte。移動機器人由一個帶旋轉(zhuǎn)編碼器移動輪式底座、一個RGB-D相機和一個帶有Wi-Fi連接的上網(wǎng)本組成，其運動由操作桿遙控，設(shè)備的中央工作站是筆記本電腦。

1.1 數(shù)據(jù)流處理

數(shù)據(jù)由分布式機器人的（單機器人）SLAM系統(tǒng)收集和處理。機器人周圍環(huán)境的信息數(shù)據(jù)被發(fā)送到一個系統(tǒng)控制單元模塊，該模塊將結(jié)果傳遞到中央工作站;將環(huán)境地圖進(jìn)行圖優(yōu)化，則將環(huán)境反饋信息傳送給機器人。本文使用機器人操作系統(tǒng)（ROS）在作為服務(wù)器的中央工作站和分布式機器人客戶端之間的Wi-Fi網(wǎng)絡(luò)上處理異步通信。機器人不需要一直連接到中心工作站，斷開連接后，它們繼續(xù)自己映射;重新連接后，所有丟失的姿態(tài)圖和地圖數(shù)據(jù)都被添加到全局圖中。但是，斷開連接時錄制的圖像不會在以后的時間發(fā)送給場景識別，避免了占用大量帶寬，影響框架的實時性操作[5]。

本文將單個機器人SLAM系統(tǒng)連接到TSLAM需要完成的接口包括兩個步驟：1）在創(chuàng)建新節(jié)點時，必須發(fā)送一個姿態(tài)圖更新消息。它由包含新節(jié)點和當(dāng)前周圍環(huán)境的位姿圖組成。然后導(dǎo)航預(yù)處理模塊將處理消息并將其傳遞到中央工作站。2） 必須實現(xiàn)一個回調(diào)函數(shù)，它接受傳入的圖調(diào)整消息，并相應(yīng)地更新本地圖。

1.2 網(wǎng)絡(luò)延遲處理

中心工作站向移動機器人進(jìn)行信息傳遞的過程中存在時間延遲，這導(dǎo)致在圖形優(yōu)化過程中會出現(xiàn)在原有尚未修正的節(jié)點位置基礎(chǔ)上繼續(xù)創(chuàng)建新的節(jié)點，從而引起所創(chuàng)建的圖形歪斜。即一部分位于校正位置;另一部分位于未校正位置?？紤]網(wǎng)絡(luò)延遲，引入信任因子 θ。設(shè) x1 和 x2 為邊的兩個姿態(tài);1和2為優(yōu)化器輸;x'1和x'2為邊的新姿態(tài)。那么，

其中，信任因子 θ 的計算公式為

引入信任因子 θ 能夠有效對優(yōu)化后的地圖進(jìn)行后處理，同時也可以進(jìn)行一致性檢查?？紤]到地圖優(yōu)化前后的姿態(tài)都是有效的，那么其線性組合也是有效的;同時計算機的數(shù)值計算也存在誤差，因此對長度非常短的邊設(shè)信任因子 θ =1，這樣就可以達(dá)到用重新運行不一致的圖來進(jìn)行優(yōu)化確保獲得全局一致的圖[6]。

2 移動機器人SLAM導(dǎo)航

2.1 里程計測量

里程表誤差的定量測量是移動機器人運動控制的關(guān)鍵，同時解決的難度比較大。由于缺少明確的測量程序來量化里程表誤差，這導(dǎo)致移動平臺的校準(zhǔn)效果并不理想[7-8]。本文開發(fā)一種定量測量系統(tǒng)里程表誤差的方法，其可以在一定程度上改善非系統(tǒng)里程表誤差。該方法是將移動機器人按照預(yù)先編程的邊長為4 m×4 m的正方形路徑和4個現(xiàn)場90°轉(zhuǎn)彎，其中順時針5次，逆時針5次，采用里程計的測程法求取機器人返回位置與實際返回位置并進(jìn)行比較。順時針和逆時針運行后的停止位置聚集在兩個不同的區(qū)域，順時針和逆時針集群內(nèi)的分布是非系統(tǒng)錯誤的結(jié)果。

順時針和逆時針的中心不對稱是由兩者的優(yōu)勢所造成的，定義A類誤差為連續(xù)波和逆時針方向的機器人轉(zhuǎn)動量方向誤差，其來源是有效軸距的不確定性;B類誤差為連續(xù)波和順時針方向的機器人轉(zhuǎn)動量方向誤差，其來源是車輪直徑不相等。里程表精度Emax， syst為

其中，

在順時針和逆時針方向分別進(jìn)行5次測試，找到xc.g.cw和xc.g.ccw，將該方法應(yīng)用于多個差動驅(qū)動平臺中，其系統(tǒng)誤差均減少10～20倍。

2.2 自主建圖

2.2.1 尺度地圖

通常，在尺度地圖中，采用一個坐標(biāo)值來表示位置，這是地圖的基本形式，圖2給出了尺度地圖。

2.2.2 Voronoi 圖

移動機器人導(dǎo)航定位的前提是構(gòu)建環(huán)境地圖，常見的環(huán)境地圖類型有拓?fù)浣Y(jié)構(gòu)地圖、幾何地圖、柵格地圖。設(shè)集合F為平面上上全體點R2集的子集，與F中每一個點相關(guān)的Voronoi區(qū)域為VF （ p ），即

集合F 的Voronoi圖是所有Voronoi區(qū)域的并集，即

設(shè)S={P1，P2…Pn}是平面上n個點的集合，對于每一個Pi，設(shè)R（S ， Pi）是距離Pi比距離Pj 更近的點集，（ j≠i ）那么

其中，表示歐氏距離。

區(qū)域R（S ， Pi）稱之為Pi的Vorinoi圖，其邊界稱之為Voronoi邊，具體如圖3所示。

2.2.3 拓?fù)涞貓D

路線圖其尺度和大小跟實際情況是有差別的，在圖5中位置表示為節(jié)點，鏈接表示為弧。精確到坐標(biāo)在這種圖上并不重要，重要的是節(jié)點之間的連接。圖5中左邊的拓?fù)鋱D和右邊的拓?fù)鋱D是等價的，弧被用來表述節(jié)點間的連接代價或是限制條件，這種圖在軌跡規(guī)劃測驗中很有用。

2.3 自適應(yīng)蒙特卡羅定位

AMCL （Adaptive Monte Carlo Localization，AMCL）算法用于移動機器人的定位。AMCL是一種面向二維移動機器人的概率定位系統(tǒng)。該系統(tǒng)采用自適應(yīng)蒙特卡羅定位方法，利用粒子濾波器跟蹤機器人在已知地圖上的姿態(tài)。

AMCL節(jié)點主要與激光掃描和激光地圖一起工作，但它可以擴展到與其他傳感器數(shù)據(jù)一起工作，如聲納或立體視覺。本項目采用激光掃描，激光是一種用于區(qū)域掃描的激光傳感器設(shè)備。該傳感器的光源為波長為785 nm的紅外激光，激光1級安全。掃描區(qū)域為240°半圓，最大半徑4 000 mm，俯仰角為0.36°，傳感器輸出每一點（683步）測量的距離。在2 000 mm處光束直徑小于20 mm，在4 000 mm處最大發(fā)散度為40 mm。如圖5所示，為精準(zhǔn)的近距離掃描效果，雙脈沖設(shè)置，減少近距離非線性影響，掃描精度得到有效提升。

在這個項目中，它采用基于激光的地圖掃描方法，轉(zhuǎn)換信息并生成一個概率姿態(tài)。定位啟動時，AMCL根據(jù)設(shè)置中提供的參數(shù)初始化粒子濾波器。如果不設(shè)置初始位置，AMCL將從坐標(biāo)原點開始。機器人姿態(tài)可以從主題slam_out_pose發(fā)布，如果想查看機器人姿態(tài)的信息，可以使用rostopic show /slam_out_pose命令顯示機器人的位置和移動機器人的四元數(shù)， “slam_out_pose”的信息展示了四元數(shù)計算機器人的航向角。

2.4 路徑規(guī)劃

移動機器人在環(huán)境網(wǎng)格地圖移動的過程中會發(fā)現(xiàn)障礙物，此時導(dǎo)航堆棧會重新計算新的路徑，避免移動機器人在移動的過程中和障礙物發(fā)生碰撞，同時進(jìn)行機器人移動路徑的全局規(guī)劃[9-10]。機器人移動路徑規(guī)劃采用Dijkstra算法，該算法用于尋找移動機器人由起始位置到目標(biāo)位置的最短路徑。移動機器人移動路徑規(guī)劃在ROS平臺下進(jìn)行仿真，設(shè)置路徑規(guī)劃器的角度值。obstacle_range和raytrace_range屬性用于指示傳感器將讀取并在成本圖中引入新信息的最大距離。在模擬仿真的過程中，如果移動機器人檢測到一個距離小于2.5 m的障礙物，它會把這個障礙物放到成本地圖上。raytrace_range用于清理成本地圖，并在機器人移動時更新其中的空閑空間。inflation_radius屬性是設(shè)定的值，用于保持機器人幾何形狀與障礙物之間的最小距離。

3 實驗

與大多數(shù)當(dāng)前的SLAM方法一樣，激光距離掃描儀被用作制圖信息的主要來源。該方法完全依賴掃描匹配來估計機器人的位移[11-12]。因此，不需要建立顯式運動模型。相應(yīng)的協(xié)方差矩陣可以通過采樣或直接從掃描匹配結(jié)果中計算。在現(xiàn)有機器人SLAM系統(tǒng)的基礎(chǔ)上，本文提出了如下兩個實驗進(jìn)行驗證。

3.1 自主建圖和定位

為了驗證SLAM算法的有效性，我們使用跟蹤的移動機器人進(jìn)行室內(nèi)激光掃描定位和測繪，如圖6所示。這幅圖是實際環(huán)境，走廊的類型類似“L”。圖中的黑線為障礙邊緣;白色區(qū)域為無障礙區(qū)域;灰色區(qū)域為未知位置區(qū)域。

實驗測試結(jié)果表明，ROS平臺可以大大縮短機器人的開發(fā)周期，在ROS上輕松實現(xiàn)SLAM，機器人可以實現(xiàn)自主移動。

3.2 導(dǎo)航規(guī)劃實驗

通過建立模糊避障邏輯算法節(jié)點，每個周期獲取環(huán)境中各虛擬傳感器所反饋的距離信息，將運算得出的左右輪速輸出給環(huán)境中的移動機器人模型。在較為寬敞及狹窄的環(huán)境可分別得到如圖7所示的到指定標(biāo)記位置導(dǎo)航。

如圖8，展示了目前廣為流行的幾種SLAM技術(shù)，下面SLAM技術(shù)都能夠成功映射場景。性能下降是由于機器人位置的估計錯誤和激光掃描數(shù)據(jù)上的噪聲存在，對誤差的分析可以給出關(guān)于算法性能的更準(zhǔn)確的信息。

在圖8中，紅色為真實情況，藍(lán)色為最終地圖。寬敞環(huán)境下，激光數(shù)據(jù)能夠?qū)崿F(xiàn)對較遠(yuǎn)距離的環(huán)境分布進(jìn)行檢測，有助于機器人快速定位運動的方向。狹窄環(huán)境下，激光雷達(dá)能夠有效對狹長通道進(jìn)行掃描，但是僅僅依賴于激光雷達(dá)數(shù)據(jù)往往不能夠?qū)ふ业胶线m的行走規(guī)則，必須通過在多個區(qū)域內(nèi)標(biāo)記的位置信息才能夠更好地實現(xiàn)對長窄環(huán)境特征下的導(dǎo)航。

4 結(jié)語

SLAM是移動機器人自主定位和導(dǎo)航的核心技術(shù)，在ROS平臺下對二維SLAM技術(shù)進(jìn)行開發(fā)，實現(xiàn)對移動機器人的運動控制。分析了基于SLAM的移動機器人運動控制系統(tǒng)，該系統(tǒng)包括主控模塊、GPS定位模塊、INS測量模塊和超聲波測距模塊，這些模塊獲取移動機器人周圍的實時信息提供給移動機器人控制系統(tǒng)，實現(xiàn)其自主移動控制且同步更新機器人所處的當(dāng)前環(huán)境信息。在此基礎(chǔ)上提出了基于SLAM的移動機器人導(dǎo)航方法，采用自適應(yīng)蒙特卡羅定位算法估計移動機器人位姿，由貝葉斯算法構(gòu)建網(wǎng)格地圖，通過路徑規(guī)劃算法對移動機器人的運動路徑進(jìn)行規(guī)劃，使得移動機器人沿著規(guī)劃路徑行走，實現(xiàn)自主移動。將其應(yīng)用于移動機器人實物試驗中，結(jié)果表明設(shè)計算法的最長用時為7 s，平均用時為6.7 s，可以達(dá)到預(yù)期的效果。本論文的研究對更好地將移動機器人應(yīng)用于農(nóng)業(yè)智能生產(chǎn)實踐中具有一定的參考價值。

參考文獻(xiàn)

[1]張聲成，蔣永翔，孫宏昌，等. 清雪機器人自主定位與導(dǎo)航系統(tǒng)設(shè)計與開發(fā)[J]. 機床與液壓，2021，49（21）：73-78.

[2]馮 波，劉桂華，曾維林，等. 雙目點線特征與慣導(dǎo)融合的機器人SLAM算法研究[J]. 小型微型計算機系統(tǒng)，2021，42（6）：1 267-1 275.

[3]文相容，周宇生. 三輪移動機器人的精確運動控制設(shè)計[J]. 重慶大學(xué)學(xué)報，2021，44（5）：124-134.

[4]黃 強，李松濤，高 源，等. 一種架空導(dǎo)線巡檢機器人的運動控制系統(tǒng)[J]. 儀表技術(shù)與傳感器，2020（12）：86-94.

[5]李 榮，欒貽青，王海鵬，等. 全向四驅(qū)變電站巡檢機器人運動控制系統(tǒng)設(shè)計[J]. 機床與液壓，2020，48（8）：112-116.

[6]張東濤，周瑩瑩，吳東林. 基于物聯(lián)網(wǎng)的智能割草機器人運動控制系統(tǒng)研究[J]. 農(nóng)機化研究，2020，42（10）：37-41.

[7]王玄玄，莊煥偉，張 鐵. 基于DSP的移動機器人運動控制系統(tǒng)[J]. 機床與液壓，2017，45（21）：76-80.

[8]王立謙，汪小志，林衛(wèi)國. 農(nóng)業(yè)激光自動采摘定位機器人控制系統(tǒng)設(shè)計-基于PID控制[J]. 農(nóng)機化研究，2015，（9）：211-216.

[9]姜秀玲. 基于PLC的蘋果采摘機械手運動控制系統(tǒng)設(shè)計[J]. 農(nóng)機化研究，2017， 39（11）：203-207.

[10]張 杰. 三自由度蘋果采摘機器人機械結(jié)構(gòu)和控制系統(tǒng)設(shè)計[D]. 南京：南京農(nóng)業(yè)大學(xué)，2017.

[11]方建軍. 采摘機器人開放式控制系統(tǒng)設(shè)計[J]. 農(nóng)業(yè)機械學(xué)報，2005，36（5）：83-83.

[12]張歡歡，田軍委，熊靖武，等. 水下機器人運動控制系統(tǒng)設(shè)計與分析[J]. 計算機系統(tǒng)應(yīng)用，2018，27（12）：83-89.