移動機(jī)器人自主返航控制系統(tǒng)設(shè)計與實(shí)驗

劉開創(chuàng)，施家棟，王建中，李 彬，曹赫

( 北京理工大學(xué) 爆炸科學(xué)與技術(shù)國家重點(diǎn)實(shí)驗室，北京 100081 )

?

移動機(jī)器人自主返航控制系統(tǒng)設(shè)計與實(shí)驗

( 北京理工大學(xué) 爆炸科學(xué)與技術(shù)國家重點(diǎn)實(shí)驗室，北京 100081 )

針對遙控移動機(jī)器人在通信信號中斷后，無法對其進(jìn)行控制的問題，設(shè)計了移動機(jī)器人自主返航控制系統(tǒng)，實(shí)現(xiàn)信號中斷后移動機(jī)器人按原路徑自主返航直至恢復(fù)控制信號或返回初始位置；提出了基于控制意圖融合里程計和光纖陀螺儀的定位算法，對移動機(jī)器人運(yùn)動學(xué)模型的結(jié)構(gòu)參數(shù)進(jìn)行了校正，大幅度提高了移動機(jī)器人定位精度；基于嵌入式Linux平臺設(shè)計了返航控制軟件系統(tǒng)，在信號正常的情況下，啟動遙控模式，遙控機(jī)器人移動并記錄路徑坐標(biāo)點(diǎn)，信號中斷則開啟返航模式，采用線性控制率和PID控制率實(shí)現(xiàn)對移動機(jī)器人路徑跟蹤控制；基于實(shí)際應(yīng)用環(huán)境開展自主返航實(shí)驗驗證，實(shí)驗結(jié)果表明該控制系統(tǒng)能夠在通信信號中斷后以較高的精度控制移動機(jī)器人實(shí)現(xiàn)自主返航；該系統(tǒng)能夠解決移動機(jī)器人在應(yīng)用中的實(shí)際問題，且代碼可移植性高、通用性強(qiáng)。

移動機(jī)器人;自主返航; 融合定位; 路徑跟蹤

0 引言

移動機(jī)器人具有移動功能，廣泛地應(yīng)用于工業(yè)、農(nóng)業(yè)、服務(wù)和國防等行業(yè)。目前無人駕駛汽車和無人戰(zhàn)車已經(jīng)基本能夠?qū)崿F(xiàn)城市化道路和野外環(huán)境中A點(diǎn)到B點(diǎn)的自主機(jī)動，并開始朝著更高智能的方向發(fā)展。然而，小型移動機(jī)器人由于其體積和重量的限制，無法攜帶足夠的高精度傳感器和高性能的處理器，尤其是在無GPS信號的復(fù)雜非結(jié)構(gòu)化未知動態(tài)環(huán)境中，難以實(shí)現(xiàn)長時間、高精度的自主導(dǎo)航。因此，目前小型移動機(jī)器人基本上都采用遙控方式進(jìn)行控制，其智能水平低下，應(yīng)用有限。由于需要反饋視頻、音頻、數(shù)據(jù)等信息，一旦通信信號中斷，移動機(jī)器人將處于失控狀態(tài)。如果在完成任務(wù)后或通信信號中斷時，移動機(jī)器人能夠自主返回指定地點(diǎn)，將極大提高機(jī)器人的生存能力和操作效率。

文獻(xiàn)[1]介紹了移動機(jī)器人基于GPS和航跡推算(DR)組合定位的方法，根據(jù)航跡推算短時間定位精度高、長時間誤差發(fā)散，使用差分GPS無遮擋定位精度高、信號受遮擋定位精度差的特點(diǎn)，提出了使用改進(jìn)比例無跡卡爾曼濾波組合定位技術(shù)，通過實(shí)驗驗證了該組合定位技術(shù)能夠提高移動機(jī)器人的定位精度。文獻(xiàn)[2]是以GPS定位為核心的導(dǎo)航系統(tǒng)，同時融合了方位傳感器和光電碼盤的數(shù)據(jù)信息，提高了移動機(jī)器人的定位精度，初步完成移動機(jī)器人導(dǎo)航任務(wù)。文獻(xiàn)[3]針對移動機(jī)器人室內(nèi)未知環(huán)境下導(dǎo)航問題，使用基于貝葉斯規(guī)則的柵格地圖表達(dá)環(huán)境信息，并改進(jìn)建模精度；使用粒子濾波實(shí)現(xiàn)機(jī)器人的自定位；通過仿真和實(shí)驗證明方法的可行性。

本文根據(jù)移動機(jī)器人的應(yīng)用需求，設(shè)計了一套基于嵌入式Linux平臺的自主返航控制系統(tǒng)，實(shí)現(xiàn)了遙控過程中的定位和路徑記錄；通信信號中斷后自主啟動返航模式，利用機(jī)器人的定位和路徑跟蹤返回初始位置。

1 自主返航控制系統(tǒng)設(shè)計

1.1 系統(tǒng)工作原理

操作人員通過遙控方式控制移動機(jī)器人從起始位置出發(fā)，到達(dá)目標(biāo)區(qū)域并執(zhí)行任務(wù)，在此過程中移動機(jī)器人控制系統(tǒng)處于遙控模式，實(shí)時地采集編碼器和陀螺儀數(shù)據(jù)，基于運(yùn)動學(xué)模型進(jìn)行定位，并每隔一定距離記錄路徑的相對坐標(biāo)點(diǎn)P0、P1、P2…Pn。

當(dāng)移動機(jī)器人行駛或執(zhí)行任務(wù)過程中，通信信號中斷，控制系統(tǒng)通過判斷有無通信數(shù)據(jù)自主啟動返航模式，移動機(jī)器人同樣通過編碼器和陀螺儀的數(shù)據(jù)進(jìn)行實(shí)時定位，并跟蹤記錄的路徑點(diǎn)坐標(biāo)Pn、Pn-1、Pn-2…P0，沿原路徑返回直至恢復(fù)通信信號或回到初始位置。

1.2 系統(tǒng)硬件設(shè)計

移動機(jī)器人采用遙控和自主相結(jié)合的控制方式，其控制系統(tǒng)結(jié)構(gòu)如圖1所示。

圖1 控制系統(tǒng)結(jié)構(gòu)框圖

自主返航控制系統(tǒng)主要由控制單元、陀螺儀、電機(jī)驅(qū)動器等組成，如圖2所示?？刂茊卧钦麄€硬件系統(tǒng)的核心，將接收的控制指令轉(zhuǎn)換為驅(qū)動器對應(yīng)指令形式，發(fā)送給兩側(cè)電機(jī)驅(qū)動器，驅(qū)動電機(jī)轉(zhuǎn)動；遙控信號正常時，控制單元接收里程計和光纖陀螺儀數(shù)據(jù)，同時根據(jù)定位算法，并記錄路徑坐標(biāo)點(diǎn)；控制信號丟失，開啟自主返航模式，控制單元接收傳感器數(shù)據(jù)得出機(jī)器人當(dāng)前位姿，并根據(jù)路徑跟蹤控制算法計算出其所需速度并發(fā)送，使機(jī)器人趨向于目的坐標(biāo)點(diǎn)。

圖2 控制單元和光纖陀螺儀

電機(jī)驅(qū)動器控制電機(jī)轉(zhuǎn)動的同時，連接電機(jī)光電編碼器，電機(jī)工作時，光電編碼器感知電機(jī)轉(zhuǎn)速信息，通過電機(jī)驅(qū)動器發(fā)送到控制單元上，電機(jī)驅(qū)動器同時接收控制單元發(fā)送出來的控制指令，控制電機(jī)轉(zhuǎn)動；光纖陀螺儀感知移動機(jī)器人轉(zhuǎn)彎角速度，并且實(shí)時發(fā)送到控制單元。

控制單元選用以PowerPC架構(gòu)為主芯片的嵌入式Linux控制板，該控制板具有運(yùn)算速度塊、存儲空間大、接口豐富、實(shí)時性好的優(yōu)點(diǎn)，使用Linux C對控制單元實(shí)現(xiàn)移動機(jī)器人自主返航控制系統(tǒng)軟件開發(fā)。使用里程計計算航向角偏差較大，所以引入光纖陀螺儀來計算移動機(jī)器人的航向角。本文中光纖陀螺儀選用Fizoptika VG095M光纖陀螺，該陀螺儀輸出模擬信號，所以使用數(shù)據(jù)采集板將模擬信號轉(zhuǎn)換成數(shù)字信號，并通過RS232接口和控制單元相連接。由于體積限制，本文中選用Elmo Whistle袖珍型數(shù)字伺服驅(qū)動器作為直流電機(jī)的驅(qū)動器，驅(qū)動器和控制單元之間通過CAN總線連接。

1.3 系統(tǒng)軟件設(shè)計

移動機(jī)器人控制系統(tǒng)在通信信號正常的情況下，需要接收遙控終端發(fā)送過來的控制信號，控制左右兩側(cè)電機(jī)，驅(qū)動移動機(jī)器人運(yùn)動，同時，控制單元采集陀螺儀和編碼器信號，基于控制意圖實(shí)現(xiàn)融合定位，并每隔一定距離記錄路徑的相對坐標(biāo)點(diǎn)。處理移動機(jī)器人當(dāng)前的位姿信息并記錄路徑坐標(biāo)點(diǎn)。當(dāng)系統(tǒng)判斷在一定時間范圍內(nèi)無通信數(shù)據(jù)，將自動切換到自主返航模式，系統(tǒng)將實(shí)時計算當(dāng)前的位姿信息，跟蹤記錄的路徑，沿原路徑返回直至恢復(fù)通信或回到初始位置。

系統(tǒng)軟件流程如圖3所示，系統(tǒng)啟動后，對接口進(jìn)行初始化設(shè)置，接收傳里程計和光纖陀螺儀的信息，隨后判斷遠(yuǎn)程遙控信號是否正常，如果正常，則開始路徑記錄模式，將傳感器的數(shù)據(jù)代入運(yùn)動學(xué)模型，計算出機(jī)器人當(dāng)前的位姿， 記錄移動機(jī)器人所走過的路徑坐標(biāo)點(diǎn)，反之，開啟路徑跟蹤模式，控制移動機(jī)器人返航。之后，程序需要掛起一段時間，以匹配控制信號，防止程序執(zhí)行出錯。

圖3 系統(tǒng)軟件流程圖

2 基于控制意圖的融合定位方法

2.1 移動機(jī)器人定位方法

移動機(jī)器人常用的定位方法有相對定位、絕對定位和組合定位。相對定位一般采用航跡推算，通過測量機(jī)器人相對于初始位置的距離和角度來確定機(jī)器人的當(dāng)前位姿，該方法常用本體傳感器，如里程計、慣性傳感器。絕對定位，主要采用導(dǎo)航信標(biāo)、主動或被動標(biāo)識、地圖匹配或全球定位系統(tǒng)進(jìn)行定位，該方法通常采用外部傳感器，如激光雷達(dá)、GPS。組合定位，基于航跡推測與絕對信息較正相結(jié)合的方法[4-5]。

本文采用相對定位方法，使用里程計和光纖陀螺儀作為定位傳感器。里程計具有價格低廉、采樣速率高、在短距離內(nèi)的定位精度高的優(yōu)勢，但是依靠里程計進(jìn)行航跡推算定位，長距離運(yùn)行后累積誤差較大，特別是轉(zhuǎn)向角度。因此，引入單軸光纖陀螺儀對轉(zhuǎn)向角度誤差進(jìn)行校正。光纖陀螺儀，具有結(jié)構(gòu)緊湊、精確度高、抗干擾能力強(qiáng)、啟動時間短、壽命長等優(yōu)勢。

移動機(jī)器人定位原理如圖4所示，可以看出在移動機(jī)器人在工作時，融合里程計和光纖陀螺儀得到的移動機(jī)器人速度，代入運(yùn)動學(xué)模型，計算出移動機(jī)器人相對于初始位置的坐標(biāo)。

圖4 移動機(jī)器人定位原理框圖

2.2 差分運(yùn)動學(xué)模型

移動機(jī)器人運(yùn)動學(xué)模型來描述機(jī)器人平臺的物理學(xué)運(yùn)動學(xué)規(guī)律，是對移動機(jī)器人運(yùn)動軌跡計算和運(yùn)動控制的主要依據(jù)。本文采用的是航跡推算的定位方法，計算移動機(jī)器人相對于初始位置的位姿。移動機(jī)器人采用驅(qū)動方式為差分驅(qū)動，如圖5所示。

圖5 差分移動機(jī)器人運(yùn)動示意圖

在δt的時間內(nèi)，移動機(jī)器人由AB運(yùn)動到A′B′，在AB處移動機(jī)器人位姿為(x0,y0,θ0)，則A′B′處移動機(jī)器人的位姿(x,y,θ)，結(jié)果為：

(1)

在式(1)中，v、ω為移動機(jī)器人直線速度和角速度，如下所示：

(2)

采用里程計進(jìn)行航跡推算，由于兩側(cè)輪徑不相等、軸距不精確和平均輪徑不精確，這些因素會造成定位過程中的誤差。文獻(xiàn)[6]提出了UMBmark實(shí)驗，通過該實(shí)驗?zāi)軌蛐Ｕ苿訖C(jī)器人系統(tǒng)參數(shù)，文獻(xiàn)[7]在UMBmark實(shí)驗的基礎(chǔ)上，提出了校正和補(bǔ)償系統(tǒng)誤差的方法，引入了系統(tǒng)誤差來源的3個方面：平均輪半徑不確定性Es、有效軸距不確定性Eb、左右輪直徑不相等Ed，經(jīng)過實(shí)驗校正之后，移動機(jī)器人的結(jié)構(gòu)參數(shù)如下：

(3)

其中：rL、rR是校正后左右輪半徑，ra是左右兩側(cè)輪實(shí)際平均半徑，rn為公稱半徑，ba為校正后的軸距，bn為公稱軸距。

2.3 基于里程計和光纖陀螺儀混合定位算法

里程計和光纖陀螺儀的數(shù)據(jù)代入運(yùn)動學(xué)模型之前，需要對兩者的數(shù)據(jù)進(jìn)行數(shù)據(jù)融合。根據(jù)轉(zhuǎn)彎半徑的大小，選擇里程計或陀螺儀的數(shù)據(jù)代入運(yùn)動學(xué)模型進(jìn)行航向角的計算，如圖6所示。

根據(jù)當(dāng)前的控制指令求出移動機(jī)器人期望轉(zhuǎn)彎半徑，計算公式如下：

(4)

式中，vE表示期望移動機(jī)器人的直線速度， ωE表示期望移動機(jī)器人的角速度，RE表示期望轉(zhuǎn)彎半徑。

圖6 里程計與陀螺儀數(shù)據(jù)融合流程圖

根據(jù)控制指令計算出移動機(jī)器人轉(zhuǎn)彎半徑，當(dāng)該轉(zhuǎn)彎半徑大于設(shè)定的半徑閾值Rth，使用里程計的數(shù)據(jù)計算得到角度值，反之，使用光纖陀螺儀采集的角速率進(jìn)行轉(zhuǎn)向角度計算。

3 路徑跟蹤控制方法

路徑跟蹤控制的目的是使機(jī)器人可以穩(wěn)定、高效的跟蹤幾何路徑，達(dá)到指定地點(diǎn)[8]。本文根據(jù)控制率計算出合適的直線速度、角速度，使移動機(jī)器人按照記錄的路徑坐標(biāo)點(diǎn)穩(wěn)定、高效到達(dá)目標(biāo)位置，并且能夠穩(wěn)定在該位置點(diǎn)。

路徑跟蹤控制流程如圖7所示，控制信號的中斷后，移動機(jī)器人啟動自主返航模式，控制單元接收傳感器的數(shù)據(jù)，代入運(yùn)動學(xué)模型，計算出機(jī)器人當(dāng)前位姿。在路徑記錄過程中，記錄的最后一個路徑坐標(biāo)點(diǎn)為路徑跟蹤的第一個點(diǎn)。機(jī)器人同時不斷接收傳感器數(shù)據(jù)，計算機(jī)器人當(dāng)前位置，判斷目標(biāo)點(diǎn)是否到達(dá)，并繼續(xù)追蹤下一個點(diǎn)，直至恢復(fù)通信或返回到初始位置。

圖7 路徑跟蹤控制流程圖

路徑跟蹤控制的關(guān)鍵是控制率的設(shè)計，本文要求移動機(jī)器人在返航過程中能夠穩(wěn)定、高效。對于移動機(jī)器人的直線速度，|Δθ|較小時，直線速度快，|Δθ|較大時，直線速度慢甚至為零，同時要求遠(yuǎn)離目標(biāo)點(diǎn)時速度要快，接近目標(biāo)點(diǎn)時速度要慢。航向角是控制移動機(jī)器人趨向目標(biāo)點(diǎn)的關(guān)鍵，要求能夠準(zhǔn)確可靠的轉(zhuǎn)過計算出來的角度，所以對機(jī)器人的航向角控制采用PID控制算法。

移動機(jī)器人路徑跟蹤如圖8所示，通過對機(jī)器人的直線速度和角速度控制，從當(dāng)前點(diǎn)M行進(jìn)到目標(biāo)點(diǎn)Maim。

圖8 移動機(jī)器人路徑跟蹤示意圖

移動機(jī)器人直線速度和角速度采用的路徑跟蹤控制的控制率為：

(5)

式中，vaim、ωaim分別表示期望達(dá)到的機(jī)器人直線速度和角速度，vmax表示返航過程中機(jī)器人的最大速度，|Δθ|為目標(biāo)角與航向角之差的絕對值，λ表示航向角偏差絕對值的放大系數(shù)， η是一個變量，和Maim與M之間的距離差Δs為線性關(guān)系，η=k×Δs-a，且k<1，a為常數(shù)[9]。kP、kI、kD為角速度PID控制的比例系數(shù)、積分時間常數(shù)、微分時間常數(shù)，Ts為采樣時間， Δθ(k)表示k時刻航向角與目標(biāo)角的偏差，Δθ(k-1)表示 k-1時刻航向角與目標(biāo)角的偏差。

4 實(shí)驗驗證

4.1 實(shí)驗平臺簡介

本文的實(shí)驗平臺為履帶式移動機(jī)器人，采用差速驅(qū)動的運(yùn)動方式，能夠通過遠(yuǎn)程終端對其進(jìn)行遠(yuǎn)程遙控，電機(jī)為MAXON有刷直流電機(jī)，公稱軸間距為Dn=330 mm，履帶輪的公稱半徑為rn=82.5 mm。

基于圖2所示的控制系統(tǒng)，使用嵌入式Linux C進(jìn)行編程，實(shí)現(xiàn)整個自主返航控制系統(tǒng)的定位和控制算法。

4.2 結(jié)構(gòu)參數(shù)校正與定位實(shí)驗

在進(jìn)行返航實(shí)驗之前，需要對實(shí)驗平臺的結(jié)構(gòu)參數(shù)進(jìn)行校正。在此實(shí)驗平臺上，參照文獻(xiàn)[7]進(jìn)行參數(shù)校正實(shí)驗，可以求出：

代入式(3)，可以求出校正后的左右輪半徑和軸距：

在進(jìn)行定位實(shí)驗之前，還需要設(shè)定合適轉(zhuǎn)彎半徑閾值，經(jīng)過多次實(shí)驗，設(shè)置轉(zhuǎn)彎半徑的閾值Rth=1.5 m。

定位實(shí)驗中，控制移動機(jī)器人行走4×4 m的正方形，獲得實(shí)驗數(shù)據(jù)如圖9所示，圖中粗實(shí)線表示融合里程計和光纖陀螺儀的定位軌跡，細(xì)實(shí)線表示只使用里程計進(jìn)行移動機(jī)器人定位的軌跡，實(shí)驗結(jié)果，融合里程計和光纖陀螺儀的定位方法，可以明顯提高移動機(jī)器人的定位精度。

圖9 兩種定位方法實(shí)驗結(jié)果

4.3 自主返航實(shí)驗

為了驗證本文設(shè)計的自主返航系統(tǒng)的有效性，開展了自主返航實(shí)驗。首先通過遙控方式控制移動機(jī)器人直行4.5米后，原地轉(zhuǎn)彎90度后繼續(xù)直行4.5米，然后中斷遙控信號，移動機(jī)器人自主啟動返航模式，自主返回起始位置，如圖10所示。

圖10 移動機(jī)器人自主返航實(shí)驗結(jié)果

在實(shí)驗之前，對PID參數(shù)進(jìn)行標(biāo)定，PID參數(shù)如下：

kI=10 000，kP=0.8，kD=0.05

進(jìn)行了6組自主返航實(shí)驗，實(shí)驗數(shù)據(jù)如圖11所示，并對6個返航終點(diǎn)坐標(biāo)進(jìn)行處理，得到平均中心點(diǎn)坐標(biāo)為(-0.182 9, -0.228 4)。實(shí)驗結(jié)果表明，本文設(shè)計的自主返航控制系統(tǒng)能夠有效地實(shí)現(xiàn)較為精確的返航功能。

5 總結(jié)

針對移動機(jī)器人在控制信號丟失后會出現(xiàn)失控狀態(tài)，設(shè)計了移動機(jī)器人自主返航控制系統(tǒng)。移動機(jī)器人定位采用融合光纖陀螺儀和里程計的方法，該方法能夠?qū)崿F(xiàn)移動機(jī)器人的精確定位?？刂菩盘杹G失后，采用線性控制和PID控制實(shí)現(xiàn)移動機(jī)器人對路徑坐標(biāo)點(diǎn)進(jìn)行跟蹤控制。通過實(shí)驗驗證移動機(jī)器人

自主返航控制系統(tǒng)能夠?qū)崿F(xiàn)短路程的精確返航。

[1] 林 武.基于GPS/DR的移動機(jī)器人組合定位技術(shù)研究[D]. 南京：南京理工大學(xué), 2011.

[2] 孟祥荔.基于GPS定位的移動機(jī)器人導(dǎo)航系統(tǒng)的研究[D]. 天津：天津理工大學(xué), 2007.

[3] 吳月華.室內(nèi)未知環(huán)境下移動機(jī)器人導(dǎo)航關(guān)鍵問題研究[D]. 天津：天津大學(xué), 2007.

[4] 于金霞,王 璐,蔡自興. 未知環(huán)境中移動機(jī)器人自定位技術(shù)[M]. 北京:電子工業(yè)出版社, 2011.

[5] 徐 德,鄒 偉. 室內(nèi)移動式服務(wù)機(jī)器人的感知、定位與控制[M]. 北京：科學(xué)出版社，2008.

[6] Borenstein J, Feng L. UMBmark: A benchmark test for measuring odometry errors in mobile robots[A], SPIE Conference on Mobile Robots[C]. 1995,2591:113-124.

[7] Shi J D, Liu J, Wang J Z. Novel method to calibrate kinematic parameters for mobile robots[J]. Journal of Beijing Institute of Technology, 2015, 24(1), 91-96.

[8] 高 健. 小型履帶式移動機(jī)器人遙自主導(dǎo)航控制技術(shù)研究[D]. 北京:北京理工大學(xué), 2015.

[9] 聶 鑫,王耀南,余洪山. 基于行為控制的移動機(jī)器人導(dǎo)航方法[J].計算機(jī)測量與控制, 2011,19(11): 2699-2701.

Design and Experiment of Autonomous Returning System for Mobile Robots

Liu Kaichuang, Shi Jiadong, Wang Jianzhong, Li Bin, Cao Minghe

(State Key Laboratory of Explosion Science and Technology, Beijing Institute of Technology,Beijing 100081, China)

This paper introduces a control system of autonomous return mode for the mobile robot out of control when the control signal is interrupted. The system can enable the mobile robot to autonomous return follow the original path until the signal is re-connected or returned to the starting position after the interruption of the control signal. A fusion location algorithm based on control intention using odometer and fiber optic gyroscope is proposed. The structural parameters of the positioning algorithm are corrected, and more precise positioning is achieved. The return control system based on the embedded Linux platform is carried out. Remote control mode is activated when the control signal is normal. While mobile robot is moving and records the coordinate points. When the control signal is lost the autonomous return mode will be started, the mobile robot path-following control is achieved by linear control rate and PID control rate. Based on the actual application environment the autonomous return experimental verifications are launched, and the results show that the system is able to control the mobile robot to autonomous return with higher precision after interruption of control signal. This system can solve the practical problems in the application of the mobile robot, and the code is highly portable and general.

mobile robots; autonomous return; fusion location; path-following

2015-09-25；

2015-11-12。

北京理工大學(xué)基礎(chǔ)研究基金(20130242015)。

劉開創(chuàng)(1989-)，河北邯鄲人，碩士研究生，主要從事移動機(jī)器人導(dǎo)航、控制方向的研究。

1671-4598(2016)03-0071-05DOI：10.16526/j.cnki.11-4762/tp

TP

A