，,2，，,2，,2

(1.蘇州大學(xué)機電工程學(xué)院，江蘇 蘇州 215021；2.蘇州納米科技協(xié)同創(chuàng)新中心，江蘇 蘇州 215021)

0 引言

機器人的研究、開發(fā)和應(yīng)用是彰顯一個國家高新技術(shù)能力的重要指標(biāo)[1]，“機器人革命”有望成為“第三次工業(yè)革命”的一個切入點和增長點。根據(jù)“十三五”規(guī)劃，到2020年我國服務(wù)機器人產(chǎn)業(yè)年銷售額將超過300億元，并重點推進(jìn)服務(wù)機器人在消防、醫(yī)療、公共服務(wù)等領(lǐng)域的發(fā)展[2]，因此越來越多的專家、學(xué)者開始研究室內(nèi)移動機器人相關(guān)技術(shù)。迄今為止，國內(nèi)外學(xué)者已對類人型迎賓機器人、火災(zāi)機器人[3-5]、護(hù)理機器人等[6]室內(nèi)移動機器人做了大量研究，并取得了一定的成果。

室內(nèi)定位與路徑規(guī)劃是室內(nèi)移動機器人研究的重點方向。目前，用于室內(nèi)定位的方法主要包括：通過無線信號的RSS(received signal strength)信息進(jìn)行位置指紋匹配，然后利用曲線擬合和位置搜索算法實現(xiàn)室內(nèi)定位[7-8]；利用機器人視覺，基于神經(jīng)遞質(zhì)調(diào)節(jié)發(fā)育算法等，對機器人進(jìn)行強化學(xué)習(xí)，最終實現(xiàn)視覺定位[9]；基于激光雷達(dá)對移動機器人進(jìn)行全局定位[10-11]等。但目前各種方法大都存在定位精度低、無法滿足動態(tài)環(huán)境等缺點。UWB具有定位精度高、受非視距影響小等優(yōu)點。因此，設(shè)計了一種基于UWB的室內(nèi)定位系統(tǒng)，研究并設(shè)計了基于UWB定位的迎賓機器人系統(tǒng)，基于該系統(tǒng)研究機器人的路徑跟蹤算法，實現(xiàn)了機器人的室內(nèi)精確定位和路徑跟蹤。

1 系統(tǒng)總體結(jié)構(gòu)

本文自主研制的移動機器人系統(tǒng)如圖1所示，主要包括圓形自主移動底盤及人形外殼、UWB定位系統(tǒng)、電子羅盤、運動控制系統(tǒng)、無刷直流電機及驅(qū)動器、433M無線通信單元和供電電源等。

圖1 迎賓機器人系統(tǒng)組成

自主移動機器人主體部分包括移動底盤和外殼，移動底盤采用兩輪差速驅(qū)動方式，安裝于底盤后側(cè)，通過控制兩驅(qū)動輪的轉(zhuǎn)速，實現(xiàn)機器人的直行和轉(zhuǎn)彎運動。前側(cè)安裝2個萬向輪，可以根據(jù)地面情況自動調(diào)節(jié)高度，以保證機器人在不平坦路面上的平穩(wěn)性。

UWB定位系統(tǒng)由安裝在室內(nèi)固定位置的4個定位基站，以及安裝于機器人正上方(與兩驅(qū)動輪中心在同一垂直軸線上)的定位標(biāo)簽組成，實現(xiàn)機器人的實時定位功能。

運動控制系統(tǒng)主要完成自主移動機器人的直線行走、轉(zhuǎn)彎等運動控制，主要由運動控制板、電子羅盤、無刷直流電機和驅(qū)動器等組成。其中，全姿態(tài)電子羅盤水平安裝于移動底盤上部的中心位置，為機器人提供航向角信息。

2 機械系統(tǒng)

自主研制的移動機器人機械系統(tǒng)如圖2所示，主要包括移動底板、蹺蹺板式萬向輪機構(gòu)、無刷電機、驅(qū)動輪和機器人外殼等。

圖2 機器人機械系統(tǒng)主要部件

2個萬向輪安裝于移動底板的前側(cè)位置，具有獨特的蹺蹺板式結(jié)構(gòu)。當(dāng)?shù)孛鎯A斜或凹凸不平時，兩萬向輪根據(jù)地面的高度自動調(diào)節(jié)自身高度，保證機器人的平穩(wěn)性。兩驅(qū)動輪固定在底板上，通過聯(lián)軸器與無刷電機的轉(zhuǎn)動軸連接。鉛蓄電池組安裝于底板中心位置，確保移動底盤的重心在中心位置，并為整個系統(tǒng)提供能源。

移動機器人的前進(jìn)和轉(zhuǎn)向運動，通過兩驅(qū)動輪的差速控制實現(xiàn)。當(dāng)兩驅(qū)動輪的轉(zhuǎn)速相同且同向時，機器人沿直線向前行駛；當(dāng)兩驅(qū)動輪的轉(zhuǎn)速不同但同向時，機器人向低速輪一側(cè)轉(zhuǎn)彎；當(dāng)兩驅(qū)動輪轉(zhuǎn)速相同但反向時，機器人實現(xiàn)原地自轉(zhuǎn)運動。

3 控制系統(tǒng)

機器人控制系統(tǒng)硬件結(jié)構(gòu)如圖3所示，主要包括UWB定位系統(tǒng)和運動控制系統(tǒng)。

圖3 機器人控制系統(tǒng)硬件結(jié)構(gòu)

3.1 UWB定位系統(tǒng)

室內(nèi)復(fù)雜環(huán)境下，由于障礙物的存在以及多徑效應(yīng)等因素的影響，使得室內(nèi)定位變得相對困難。基于UWB的定位系統(tǒng)因獨特的通信原理，使其具有定位精度高、功耗低、抗干擾性能強和多徑分辨能力強等優(yōu)點。

本文研制了基于UWB的定位系統(tǒng)，主要包括4個定位基站和1個定位標(biāo)簽。UWB通信模塊采用DecaWave公司的DWM1000模塊，該模塊基于業(yè)內(nèi)精度最高的室內(nèi)定位芯片DW1000。4個定位基站安裝在室內(nèi)的固定位置，定位標(biāo)簽安裝在機器人頂部，為機器人提供位置信息。

UWB定位系統(tǒng)采用三邊定位原理求解待定位標(biāo)簽的坐標(biāo)。在實際應(yīng)用環(huán)境中，由于外部環(huán)境以及系統(tǒng)測量誤差等因素的影響，以基站為中心、標(biāo)簽與基站的距離為半徑的圓不一定相交于同一點位置(標(biāo)簽位置)。因此在實際計算標(biāo)簽的坐標(biāo)時，若相鄰基站之間有相交點，則先求得相鄰基站之間靠近其他基站的交點坐標(biāo)；若相鄰基站之間沒有相交點，則求得兩基站坐標(biāo)的中心點坐標(biāo)。然后以求得的4個坐標(biāo)的均值作為待定位標(biāo)簽的測量坐標(biāo)值。為進(jìn)一步消除測量的隨機誤差，采用卡爾曼濾波算法對測量坐標(biāo)值進(jìn)行濾波處理。

UWB定位系統(tǒng)的程序流程如圖4所示。首先測得k時刻標(biāo)簽與4個基站的距離R1，R2，R3，R4，求解相鄰基站之間的交點或中心點坐標(biāo)P1，P2，P3，P4，以4個坐標(biāo)點的均值為當(dāng)前時刻實際測得的標(biāo)簽坐標(biāo)P′，經(jīng)過卡爾曼濾波之后作為k時刻最終的標(biāo)簽坐標(biāo)P。

圖4 UWB定位程序流程

3.2 運動控制系統(tǒng)

運動控制系統(tǒng)以STM32F103ZET6微控制器為控制核心，其與UWB定位系統(tǒng)、電子羅盤以及433M無線通信模塊通過RS232接口連接。無刷直流電機和驅(qū)動器均采用SPG公司的XF系列電機。運動控制器接收定位系統(tǒng)的位置信息以及電子羅盤的航向角信息，通過路徑跟蹤算法對當(dāng)前位姿進(jìn)行校正。

運動控制系統(tǒng)如圖5所示，主要通過控制兩驅(qū)動電機的速度實現(xiàn)機器人的直線行駛和原地轉(zhuǎn)彎功能。在沿直線行駛時，主控制板通過定位標(biāo)簽獲取的位置信息以及預(yù)設(shè)直線路徑，計算機器人當(dāng)前位置到路徑的最短距離，并與電子羅盤的航向角信息一起作為路徑跟蹤算法的反饋輸入，其輸出作用于左右電機，通過同向差速控制實現(xiàn)機器人的直線路徑跟蹤。在轉(zhuǎn)彎運動時，僅將期望航向角作為參考輸入，通過控制左右電機反向同速轉(zhuǎn)動，完成期望的轉(zhuǎn)彎運動。

圖5 運動控制系統(tǒng)

4 混合路徑跟蹤算法

雙輪差速移動機器人路徑跟蹤系統(tǒng)是一個單輸入雙輸出的系統(tǒng)。常規(guī)的PID控制器在解決這此類系統(tǒng)控制問題時，往往無法獲得較為理想的穩(wěn)態(tài)控制精度，而當(dāng)系統(tǒng)誤差較小時，線性二次型最優(yōu)控制器能夠獲得較好的控制效果，因此提出了一種融合PID和LQR最優(yōu)控制的混合路徑跟蹤算法。如圖6所示，在較大偏差時，采用PD控制器，快速消除較大的系統(tǒng)誤差；當(dāng)誤差較小時，采用LQR最優(yōu)控制器，以獲得更好的穩(wěn)態(tài)性能。

圖6 控制系統(tǒng)的算法流程

4.1 自主移動機器人運動學(xué)模型

移動機器人的簡化模型如圖7所示，主要由2個后驅(qū)動輪和2個前萬向輪構(gòu)成，并假定兩驅(qū)動輪中心位置為機器人位置。在UWB定位系統(tǒng)的笛卡爾坐標(biāo)系下，l為設(shè)定路徑，d為左右驅(qū)動輪中心距，v為前進(jìn)速度，vL為左輪轉(zhuǎn)速，vR為右輪轉(zhuǎn)速。假設(shè)t時刻機器人偏航距為ed，偏航角為eθ，并規(guī)定左偏為負(fù)，右偏為正。

圖7 移動機器人運動學(xué)模型

機器人左右輪轉(zhuǎn)速為：

(1)

系統(tǒng)狀態(tài)變量為x=[eθed]T，則常系數(shù)狀態(tài)方程為：

(2)

系統(tǒng)的輸出方程為：

(3)

4.2 混合路徑跟蹤控制算法

PID控制策略因其算法簡單、魯棒性好、可靠性高等，被廣泛應(yīng)用于過程控制和運動控制。但對于多輸入多輸出系統(tǒng)，PID控制往往很難獲得令人滿意的性能。基于二次型指標(biāo)函數(shù)的LQR控制策略在小偏差下具有較好的性能，因此本文提出了一種融合PID和LQR最優(yōu)控制的混合路徑跟蹤算法。

在大偏差狀態(tài)下，即eθ≥eθth或ed≥edth時，采用PID控制器將誤差快速衰減至小偏差狀態(tài)?？刂破鲗⑵骄鄀d和偏航角eθ進(jìn)行加權(quán)融合，作為控制器輸入，通過式(4)計算控制量u。

(4)

其中，e=q1ed+q2eθ。

在小偏差狀態(tài)下，即eθ

(5)

選取加權(quán)陣Q=diag(q1,q2)，且q1>0，q2>0；R=1。

系統(tǒng)狀態(tài)空間方程如式(2)和式(3)所示。因為

(6)

所以系統(tǒng)可控；rank(Q)=2，且特征值均大于零，滿足二次型控制器的附加條件。

取哈密頓函數(shù)為：

(7)

則由文獻(xiàn)[12]可知，協(xié)態(tài)方程為：

(8)

控制方程為：

(9)

黎卡提矩陣代數(shù)方程為：

-KA-ATK+KBR-1BTK-Q=0

(10)

解式(10)即可得到K值，再將其代入式(9)即可得最優(yōu)控制為：

u=-R-1BTKx=-Gx

(11)

G為最優(yōu)反饋增益陣。

綜上所述，混合路徑跟蹤控制算法的控制量為：

(12)

5 實驗與結(jié)果

迎賓機器人實驗場地如圖8所示。在長8 m、寬6 m的室內(nèi)環(huán)境下，對自主研制的室內(nèi)移動機器人進(jìn)行了UWB室內(nèi)定位實驗和基于UWB室內(nèi)定位的路徑跟蹤實驗。

圖8 實驗場景

5.1 UWB室內(nèi)定位實驗結(jié)果與分析

對自主設(shè)計的室內(nèi)定位系統(tǒng)和室內(nèi)定位算法進(jìn)行實驗驗證，實驗中設(shè)置4個定位基站，其坐標(biāo)分別為(0，0)、(0，500)、(500，0)、(500，500)，以cm為單位。在位置(317，217.5)、(135.5，360)、(256.5，312.5)處分別進(jìn)行定位實驗，實驗結(jié)果分別如圖9、圖10和圖11所示。

圖9 坐標(biāo)(317，217.5)的定位結(jié)果

圖10 坐標(biāo)(135.5，360)的定位結(jié)果

如圖9、圖10和圖11所示，實線表示測量的原始坐標(biāo)數(shù)據(jù)，虛線表示KF濾波之后的坐標(biāo)數(shù)據(jù)。3個測試點的坐標(biāo)定位值，經(jīng)濾波之后較原始數(shù)據(jù)更加平滑；由表1可知，KF濾波之后，定位結(jié)果的標(biāo)準(zhǔn)差(STD)更小。綜上，經(jīng)濾波之后取得了小于1 cm的重復(fù)性定位精度。表2分別列出了10個測試位置的定位誤差。由表2可知，X軸和Y軸的最大定位誤差分別為-4.4 cm和-4.8 cm，其中誤差均值分別為-0.4 cm和-2.7 cm，具有小于5 cm的定位精度。

圖11 坐標(biāo)(256.5，312.5)的定位結(jié)果

測試點X軸STDY軸STD原始數(shù)據(jù)/cmKF濾波/cm原始數(shù)據(jù)/cmKF濾波/cm11.4810.6601.6380.82221.6151.0851.6440.96131.3070.6401.4940.61641.3730.5231.4520.43051.6700.6431.4000.42561.2520.4321.1610.33171.2940.5281.4590.39181.5560.5011.6970.76991.1550.5391.3180.464101.2570.4461.4050.425

表2 10個測試點的定位誤差

5.2 路徑跟蹤實驗結(jié)果與分析

首先對提出的混合路徑跟蹤控制算法進(jìn)行仿真實驗。MATLAB數(shù)值仿真結(jié)果如圖12所示。其中，設(shè)定前進(jìn)速度v=0.5 m/s，系統(tǒng)初始狀態(tài)為eθ=-0.5 rad，ed=0.5 m，加權(quán)陣Q=diag(0.8,3.5)，R=1.5。從仿真結(jié)果可知，在系統(tǒng)運行4 s時，系統(tǒng)偏差已趨近于零。

圖12 路徑跟蹤控制器仿真結(jié)果

然后，通過迎賓機器人進(jìn)行直線路徑跟蹤試驗。設(shè)定行進(jìn)速度為v=0.5 m/s，系統(tǒng)初始狀態(tài)為eθ=-0.4 rad，ed=1 m，選取性能指標(biāo)的加權(quán)陣Q=diag(0.6,3.0)，R=2.5，伺服周期Ts=0.1 s。試驗結(jié)果如圖13、圖14、圖15和圖16所示。

圖13 混合路徑跟蹤算法的動態(tài)跟蹤誤差響應(yīng)曲線

圖13和圖14分別為混合路徑跟蹤算法和PID算法的動態(tài)響應(yīng)曲線，由圖可知，混合路徑跟蹤算法在7 s左右跟蹤誤差已趨于穩(wěn)定，PID算法在11 s左右趨于穩(wěn)定，因此混合路徑跟蹤算法具有更好的動態(tài)響應(yīng)特性。圖15和圖16分別為穩(wěn)態(tài)響應(yīng)曲線，對比可知，混合跟蹤算法穩(wěn)態(tài)精度要優(yōu)于PID算法。

圖14 PID算法的動態(tài)跟蹤誤差響應(yīng)曲線

圖15 混合路徑跟蹤算法的穩(wěn)態(tài)跟蹤誤差響應(yīng)曲線

圖16 PID算法的穩(wěn)態(tài)跟蹤誤差響應(yīng)曲線

綜上所述，提出的混合路徑跟蹤算法能通過UWB定位系統(tǒng)提供的坐標(biāo)信息，以及電子羅盤提供的航向角信息有效地跟蹤設(shè)定路徑，跟蹤誤差小于5 cm。

6 結(jié)束語

設(shè)計了一種基于UWB室內(nèi)定位系統(tǒng)和一種基于UWB室內(nèi)定位的迎賓機器人系統(tǒng)。將定位標(biāo)簽安裝于迎賓機器人上方位置，通過定位系統(tǒng)輸出的位置信息以及電子羅盤輸出的航向角信息進(jìn)行室內(nèi)直線路徑跟蹤。實驗結(jié)果表明，所設(shè)計的移動機器人軟硬件系統(tǒng)運行穩(wěn)定，室內(nèi)定位系統(tǒng)定位精度小于5 cm，重復(fù)精度小于1 cm，能有效消除外界干擾；基于UWB定位的混合路徑跟蹤算法能夠準(zhǔn)確跟蹤直線路徑，跟蹤誤差小于5 cm，實時性高。本文的室內(nèi)定位技術(shù)研究不僅可用于迎賓機器人，還可應(yīng)用于其他室內(nèi)移動機器人和工業(yè)搬運機器人等。