，,2，，,2，,2

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

0 引言

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

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

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

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

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

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

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

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

2 機(jī)械系統(tǒng)

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

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

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

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

3 控制系統(tǒng)

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

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

3.1 UWB定位系統(tǒng)

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

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

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

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

圖4 UWB定位程序流程

3.2 運(yùn)動(dòng)控制系統(tǒng)

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

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

圖5 運(yùn)動(dòng)控制系統(tǒng)

4 混合路徑跟蹤算法

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

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

4.1 自主移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型

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

圖7 移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型

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

(1)

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

(2)

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

(3)

4.2 混合路徑跟蹤控制算法

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

在大偏差狀態(tài)下，即eθ≥eθth或ed≥edth時(shí)，采用PID控制器將誤差快速衰減至小偏差狀態(tài)。控制器將偏航距ed和偏航角eθ進(jìn)行加權(quán)融合，作為控制器輸入，通過(guò)式(4)計(jì)算控制量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)所示。因?yàn)?/p>

(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 實(shí)驗(yàn)與結(jié)果

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

圖8 實(shí)驗(yàn)場(chǎng)景

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

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

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

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

如圖9、圖10和圖11所示，實(shí)線表示測(cè)量的原始坐標(biāo)數(shù)據(jù)，虛線表示KF濾波之后的坐標(biāo)數(shù)據(jù)。3個(gè)測(cè)試點(diǎn)的坐標(biāo)定位值，經(jīng)濾波之后較原始數(shù)據(jù)更加平滑；由表1可知，KF濾波之后，定位結(jié)果的標(biāo)準(zhǔn)差(STD)更小。綜上，經(jīng)濾波之后取得了小于1 cm的重復(fù)性定位精度。表2分別列出了10個(gè)測(cè)試位置的定位誤差。由表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é)果

測(cè)試點(diǎn)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個(gè)測(cè)試點(diǎn)的定位誤差

5.2 路徑跟蹤實(shí)驗(yàn)結(jié)果與分析

首先對(duì)提出的混合路徑跟蹤控制算法進(jìn)行仿真實(shí)驗(yà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)運(yùn)行4 s時(shí)，系統(tǒng)偏差已趨近于零。

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

然后，通過(guò)迎賓機(jī)器人進(jìn)行直線路徑跟蹤試驗(yà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。試驗(yàn)結(jié)果如圖13、圖14、圖15和圖16所示。

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

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

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

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

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

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

6 結(jié)束語(yǔ)

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