RTK與Lidar組合優(yōu)化機器人導航控制

李 寧

（廣西機電職業(yè)技術學院，廣西 南寧 530007）

1 引言

近年來，移動機器人技術在航空、深海等領域等各領域得到快速發(fā)展，不僅提高了作業(yè)效率，而且可替換惡劣環(huán)境中的人為作業(yè)，極大提高作業(yè)安全性。由于較強的自主靈活性，機器人替代人工操作需求的不斷擴大，機器人遠程控制和導航越來越具有重要意義[1-2]。但傳統(tǒng)RFID 等導航控制方法常受到距離和空間的限制，不利于大范圍長距離的導航和避障控制[3]。網(wǎng)絡通信功能的配備增強，使得簡潔靈的機器人導航控制的成為重要的研究課題。

文獻[4]采用基于改進蟻群算法的全局靜態(tài)路徑規(guī)劃導航和基于預測和避碰的局部動態(tài)滾動相結(jié)合的機器人導航控制方法，算法有效減少傳統(tǒng)方法的路徑規(guī)劃距離，提高安全性。為了克服強外部干擾和不確定非線性對移動機器人控制性能的影響，干擾觀測器被引入強擾動場合的控制研究中，基于自抗擾控制策略的思想，文獻[5]提出了一種基于線性擴張狀態(tài)觀測的自抗擾控制策略，并證明了其穩(wěn)定性。已有研究表明，通過調(diào)節(jié)觀測器的帶寬可使觀測器對擾動的觀測誤差趨于任意小；文獻[6]采用二次型優(yōu)化足力分配，采用動力學前饋平滑擺動軌跡，實現(xiàn)四足機器人的穩(wěn)步路徑規(guī)劃，有效減少沖擊力和遠程平衡控制。文獻[7]提出了基于圖像拓撲的室內(nèi)全局機器人定位方法，在局部極佳圖像劃分并提取魯棒特征基礎上，建立圖像的隱馬爾可夫拓撲模型，將移動人導航轉(zhuǎn)化為有向圖中的最長路徑求解，算法具有較高的定位精度。文獻[8]引入角度限和距離因子消除額外斥力和不可達問題，采用切線法和搜索法組合解決局部極小問題，提出改進人工勢場和自適應步長的機器人路徑規(guī)劃算法，算法在計算復雜度和路徑光滑方面優(yōu)勢明顯。文獻[9]將激光雷達獲得的障礙物高精度距離信息融入到A*全局規(guī)劃和局部避障結(jié)合算法中，得到機器人的最優(yōu)避障路徑，有效解決較近障礙物造成的機器人徘徊問題，且具有較好規(guī)劃效率。

隨著RTK/INS系統(tǒng)相關技術的發(fā)展，通過其全局導航定位技術改進激光雷達短距離定位性能，在全局路徑規(guī)劃和障礙物精確定位方面應用前景廣闊[10]，有利于提高移動機器人在遠距離和長距離路徑上的路徑規(guī)劃和導航定位，為此在已有研究研究基礎上，結(jié)合RTK/INS全局導航和激光雷達局部障礙物測距優(yōu)勢，提出基于RTK和Lidar組合優(yōu)化的機器人導航控制方法，實驗驗證了算法的有效性和對不同路線的適應性。

2 系統(tǒng)數(shù)學模型

坐標原點設置在機器人移動站的重心位置，機體縱軸為坐標y軸方向，以前進方向坐標x軸正向建立坐標系，機器人的轉(zhuǎn)向、行駛及其他軌跡運動等各種姿態(tài)主要通過獨立控制其兩側(cè)車輪速度實現(xiàn)。如圖1（a）所示為巡檢機器人物理模型及坐標系示意圖，圖中O為WGS－84坐標系經(jīng)轉(zhuǎn)換為二維地理坐標系后的坐標原點，R為當前運動軌跡的曲率半徑，W為機器人寬度、ψ為兩坐標系夾角，由幾何學原理可以推導差動轉(zhuǎn)向運動方程，具體過程見文獻[7]。

系統(tǒng)定位設備包括R60雙天線和定向型接收機[8]，采用載波相位差分方法解算，定位精度±（2.5+1×10－6D）mm。路徑規(guī)劃避障采用安裝在機器人車前測量半徑15m的DE01激光雷達，通過時間差或相位差解算目標距離。RTK/INS定位設備和激光雷達安裝設計示意圖，如圖1（b）所示。

圖1 機器人坐標關系及相關設備安裝Fig.1 Coordinate Relationship and Equipment Installation

機械人主要依靠車輪的不同輪速實現(xiàn)轉(zhuǎn)向，為準確描述移動機器人的位姿，圖1（a）中定義雙坐標系：以機器人幾何中心為原點的移動坐標系Or(Xr，Yr)和世界坐標系OG(XG，YG)，對于OM(XM，YM)，當機械人的輪子不打滑時機器人的局部坐標運動學式為［7］：

式中：v—機器人平臺的質(zhì)心速度；ω—機器人擺動角速度；vr、vl—平臺左右車輪質(zhì)心轉(zhuǎn)速；ωr、ωl—平臺左右輪轉(zhuǎn)速。相應的在全局坐標系下的運動學模型為：

式中：X、Y—機器人沿坐標軸位移量。設在無障礙物下的理想最優(yōu)控制路徑及機器人的位姿表示為：

式中：Xd、Yd—以坐標形式表示的機器人期望位移量；ψd—機器人理想擺角。則在全局坐標系下機器人實際行進路徑與期望路徑之間的誤差為：

式中：XGc、YGC、ψGC—全局坐標系下機器的實際位移和擺角。

機器在行進過程中，還要充分考慮局部障礙物，因而需要將全局坐標下的誤差值轉(zhuǎn)換為局部坐標系下，即

對式（5）中各分量求導可得位移的動態(tài)微分方程。機器人的最優(yōu)路徑跟蹤時，實時的計算機身位姿及理想路徑的當前目標點，通過姿態(tài)誤差調(diào)整不斷向當前目標點行進過程，如圖2所示。因而最優(yōu)路徑當前目標點的合理設置并不斷修正位姿誤差是機器人遠程控制和規(guī)劃避障的關鍵。

圖2 機器人遠程導航控制原理Fig.2 Robot Remote Navigation and Control Principle

3 導航地圖創(chuàng)建與避障控制

根據(jù)已有研究，文中移動機器人遠程長距離導航控制采用全局RTK地圖改進PID路徑規(guī)劃控制與局部激光雷達測距人工勢場相結(jié)合的方式，其控制原理為首先通過RTK北斗導航數(shù)據(jù)建立機器人工作場景的大范圍全局粗略地圖，在全局粗地圖中根據(jù)路徑終點設置理想路徑目標點并進行PID控制行進，其次根據(jù)激光雷達短距離探測數(shù)據(jù)構(gòu)建局部避障地圖，在對探測到的障礙物邊緣數(shù)據(jù)擬合處理基礎上，通過改進的人工勢場算法（Artificial Potential Field，APF）進行避障控制，從而形成優(yōu)勢互補的組合導航控制方式，增強系統(tǒng)對環(huán)境的魯棒性。

3.1 RTK全局地圖構(gòu)建及PID控制

全局地圖基于場景中路段與節(jié)點信息的存儲而構(gòu)建，其數(shù)據(jù)來源為RTK/INS北斗導航定位設備。數(shù)據(jù)庫中節(jié)點表中存儲的是直接從RTK 數(shù)據(jù)幀中解算出的對應每個道路節(jié)點的的WGS-84大地坐標系經(jīng)緯度信息，而文中數(shù)字地圖使用平面坐標系構(gòu)建。為此在構(gòu)建地圖時需要將讀取的節(jié)點信息通過高斯轉(zhuǎn)換算法由WGS-84坐標系轉(zhuǎn)換為高斯平面坐標系[6]，然后再通過坐標旋轉(zhuǎn)或平移將高斯平面坐標系轉(zhuǎn)換為機體坐標系。

PID控制是一種被廣泛應用于各類控制問題的穩(wěn)定控制，但傳統(tǒng)單參PID算法僅主要控制俯仰角，易出現(xiàn)和震蕩，導致調(diào)節(jié)時長過大、超調(diào)震顫和高能耗等問題，在實際系統(tǒng)應用時控制效果不理想[5]，文中采用線性自抗擾控制器，設機器人控制系統(tǒng)為非線性時變系統(tǒng)，其系統(tǒng)方程為：

式中：ω(t)—外界擾動；b、u、y—控制系統(tǒng)的增益、輸入和輸出；f－系統(tǒng)動態(tài)，包括引起系統(tǒng)控制狀態(tài)變化的所有外界作用，其為自抗擾控制的核心思想。根據(jù)文獻[13]證明，由f的估計值可將系統(tǒng)標準化為積分形式，則系統(tǒng)的降階擴張觀測器為：

式中：x2—系統(tǒng)的可測狀態(tài)；β1、β2—ESO增益，設置使用增益值，使ESO狀態(tài)量[z1，z2]T逼近系統(tǒng)的狀態(tài)量[x2，f]T，則得到線性自抗擾控制器為：

式中：uPD—PID控制器；kp、kD—PID比例常數(shù)和時間積分常數(shù)。

3.2 激光雷達局部地圖構(gòu)建與APF避障

局部地圖用于引導機器人避開行進過程中的障礙物，由機器人配裝的激光雷達探測的障礙物距離和角度信息動態(tài)繪制。激光雷達數(shù)據(jù)需要進一步處理以提取障礙物邊緣和輪廓信息以便識別障礙物形狀[11]，為此，首先采用中值濾波去除激噪聲干擾，然后設置距離閾值對數(shù)據(jù)進行聚類，當聚類運算后的某一類別數(shù)據(jù)點數(shù)少于預設的點數(shù)N 時，則認識該類為障礙物的可能性不大，去除這些聚類，由于較遠障礙物掃描到的數(shù)據(jù)點也相應較少，因此，預設點數(shù)N需根據(jù)實際距離情況進行遞減，激光雷達掃描點在聚類前后的分布對比圖，如圖3所示。

圖3 聚類前后對比圖Fig.3 Comparison Chart Before and After Clustering

從圖中可以看出，在圖3（a）所示的未進行聚類前，離散探測點存在較多的干擾，導致最優(yōu)路徑規(guī)劃無法準確實施，而圖5（b）所示為經(jīng)過聚類去除部分數(shù)據(jù)點之后的離散點分布，聚類后路沿、障礙物等數(shù)據(jù)點更加清晰，輪廓更明確，有利于路徑規(guī)劃。

單純的PID 控制存在的障礙物不確定情況而造成碰撞，為此，引入人工勢場（APF）算法，通過引力場和斥力場2種虛擬力聲分析機器人在規(guī)劃路徑中的運動[12]。但傳統(tǒng)APF算法只考慮機目距離，存在計算冗余、目標不可達、局部極小值以及狹窄通道震蕩等問題，為此，采用改進的人工勢場法引導機器人避開障礙物。

斥力勢場與障礙物的距離及障礙物的屬性相關，因此設置斥力系數(shù)ζ來反映不同障礙物的危險等級，

式中：M—障礙物質(zhì)量；e—自然常數(shù)；c—障礙物類型；G—調(diào)整基數(shù)。

目標不可達問題，可以通過引導機器人所受的引力與斥力同時趨零來解決，修改斥力計算式，并將機目相對距離引入計算式，即對于障礙物斥力作用范圍ρob≤ρo內(nèi)

式中：ρo—障礙物斥力的作用距離；ρob—機器人到障礙物的距離，ρg—斥力場調(diào)節(jié)因子。機器人向目標點行進，當X=Xg時，斥力為：

4 實驗與仿真驗證

為驗證文中提出的基于RTK和Lidar組合優(yōu)化的機器人導航控制方法的有效性，實驗搭建一個簡易的四輪機器人，其上根據(jù)圖1（b）所示方法安裝相關探測設備，以5cm步進對探測的RTK數(shù)據(jù)和激光雷達探測數(shù)據(jù)進行柵格化，實驗前對數(shù)據(jù)完成聚類和分段擬合，不同路況及期望路徑下的機器人遠程導航控制實驗結(jié)果，如圖4所示。圖中“△”和“+”為實驗起始點和目標點。

圖4 不同場景下路徑規(guī)劃避障行進軌跡Fig.4 Path Planning Obstacle Avoidance in Different Scenarios

圖4（a）中的實驗場地為空曠場地，路徑中的障礙物隨機設置，而在期望路徑終點，障礙物與目標點很接近，其半徑為3m，斥力作用范圍為7m，影響范圍包含了目標點位置，傳統(tǒng)APF會出現(xiàn)止步不前的目標不可達問題，而從圖中結(jié)果可以看出，機器人準確避開障礙物并順利到達目標點，在障礙物較小的區(qū)域，機器人不受障礙物影響，保持直線行進，實驗行走19.5m。

圖4（b）的實驗場景為一段道路彎道，其障礙物眾多，實驗環(huán)境更加復雜，從實驗軌跡可以看出機器人能夠準確檢測障礙物，且沒有受障礙物斥力影響而產(chǎn)生震蕩，繞過彎道路障，順利進入路段的轉(zhuǎn)折段，最優(yōu)路徑較平滑，行走23.8m。

從整個實驗結(jié)果可以看出，文中基于RTK和Lidar組合優(yōu)化的機器人導航控制系統(tǒng)能夠滿足遠程大范圍長距離的導航需求，且對不同的環(huán)境和障礙具有較好的魯棒性。

5 結(jié)論

針對傳統(tǒng)機器人導航靈活性較差且易受到距離和空間的限制的問題，提出基于RTK定位系統(tǒng)和激光雷達組合優(yōu)化的機器人遠程導航控制方法，介紹了系統(tǒng)的數(shù)學模型，通過RTK系統(tǒng)精確定位數(shù)據(jù)建構(gòu)全局導航地圖和線性自抗擾控制進行全局導航算法，通過激光雷達探測掃描的局部數(shù)據(jù)構(gòu)建局部避障地圖并通過改進的APF算法進行局部局部避障路徑規(guī)劃，從而實現(xiàn)在大范圍長距離的機器人導航控制和路徑規(guī)劃。仿真實驗驗證系統(tǒng)的合理性和算法的可行性。