基于UWB的零轉(zhuǎn)徑跟隨機器人控制系統(tǒng)設(shè)計

黃夢雨，秦建軍，高 磊，魯增輝

(1.北京建筑大學 機電與車輛工程學院，北京 100044；2.城市軌道車輛服役性能保障北京重點實驗室，北京 100044；3.北京地平線信息技術(shù)有限公司，北京 100190)

隨著科學技術(shù)的迅速發(fā)展，服務(wù)類機器人已經(jīng)成為當今世界機器人技術(shù)研究的熱點，并作為技術(shù)強國未來優(yōu)先發(fā)展的戰(zhàn)略高技術(shù)。目前市場中服務(wù)機器人種類繁多，但其平臺技術(shù)多為某一專用技術(shù)，如掃地機器人、迎賓機器人、安保機器人、巡檢機器人、陪伴機器人等，僅可滿足人們對于服務(wù)機器人的某一期望[1-2]。同時，根據(jù)機器人的服務(wù)功能，可體現(xiàn)出人機協(xié)同的自動化程度。近年來，綜合考慮智能技術(shù)發(fā)展水平以及市場多方面需求，蘊育出一類跟隨型服務(wù)機器人，如Budgee機器人、Gita私人貨物運輸機器人、Rover機器人行李箱、MoBot機器人購物車等，其服務(wù)能力強、應(yīng)用廣泛，具有廣闊的發(fā)展前景[3-4]。Koide K等在文獻[5]中提出運用激光測距(LRF)進行人員跟蹤，運用識別框架和聯(lián)合特征方法進行識別；文獻[6]采用了激光測距掃描儀+Kinect的方式，通過預(yù)測-觀測-K均值聚類-似然函數(shù)調(diào)整-估計-選擇步驟進行跟蹤；CHIU等[7]提出使用無線電和超聲波的速度差進行距離檢測，以確定用戶與機器人之間的距離。

本文提出一款基于超寬帶(ultra wide band，UWB)的跟隨服務(wù)機器人，其更加貼近生活需要，可作為陪伴者，實時快速、精確跟隨使用者完成日常出行任務(wù)，并能夠運載使用者隨身攜帶的物品，減輕負重。機器人采用零轉(zhuǎn)彎半徑運載底盤的機械結(jié)構(gòu)[8]，具有靈活通過狹隘空間的機動性，適應(yīng)多種場所。其控制系統(tǒng)采用STM32F103VET6型號的微控制器，具有對機器人信息采集處理及運動控制的功能。定位系統(tǒng)利用超寬帶定位技術(shù)完成對跟隨目標位置的定位。超寬帶與主流定位技術(shù)相比，具有精度高、抗干擾能力強、傳輸速率快等優(yōu)勢[9-12]。通過利用基于到達時間差定位(time difference of arrival，TDOA)的Taylor級數(shù)展開的定位算法[13-14]，估算目標位置，調(diào)整誤差，解算出最優(yōu)定位目標，并由控制系統(tǒng)根據(jù)跟隨要求的設(shè)定完成對移動目標的精準跟隨。

1 機器人總體設(shè)計

設(shè)計的跟隨服務(wù)機器人意旨在滿足人們?nèi)粘Ｉ钏璧姆?wù)要求，其集多種硬件設(shè)備和控制電路于一體，并預(yù)置其他功能拓展接口。因此，跟隨服務(wù)機器人的功能實現(xiàn)依靠穩(wěn)定的行走機構(gòu)、精確的定位系統(tǒng)、簡單可靠的控制系統(tǒng)。根據(jù)對跟隨服務(wù)機器人功能的設(shè)定，在其結(jié)構(gòu)上采取了功能互換多層模塊化的結(jié)構(gòu)，將零轉(zhuǎn)彎半徑基礎(chǔ)運載底盤作為基體，負責整機的載重以及移動功能的實現(xiàn)。通過結(jié)合UWB測距模塊，實現(xiàn)機器人智能跟隨的功能。

1.1 機械結(jié)構(gòu)設(shè)計

如圖1所示，機器人的整體結(jié)構(gòu)由車體和零轉(zhuǎn)彎半徑運載底盤兩部分組成。其中，車體可分為功能拓展層和電子控制層。功能拓展層用以實現(xiàn)機器人的載物功能，或進行其擴展功能的切換。電子控制層中存放機器人的STM32核心控制電路板及電機驅(qū)動器、鋰電池等硬件設(shè)備，各硬件之間使用電路連接，通過撰寫控制程序，最終實現(xiàn)機器人系統(tǒng)的控制設(shè)計。動力傳動層與移動輸出層構(gòu)成了機器人的零轉(zhuǎn)彎半徑的運載底盤。傳統(tǒng)的輪式服務(wù)機器人的運載底盤通常采用差速驅(qū)動底盤、全向輪運動底盤、多輪行走底盤等，其用來控制機器人移動的電機個數(shù)，一般為2～4個。從控制角度而言，若考慮機器人的機動性，實現(xiàn)機器人的全向移動，控制則十分復(fù)雜[15]。例如，四輪均為麥克納姆輪的全向移動平臺，若實現(xiàn)其直線行駛，則需精確地按照相同的速率驅(qū)動4個輪子轉(zhuǎn)動。零轉(zhuǎn)彎半徑運載底盤的輪系結(jié)構(gòu)圖如圖2所示。

圖1 跟隨服務(wù)機器人機械結(jié)構(gòu)

圖2 輪系結(jié)構(gòu)

圖3 零轉(zhuǎn)彎半徑運載底盤

零轉(zhuǎn)彎半徑運載底盤如圖3所示。創(chuàng)新的底盤設(shè)計與傳統(tǒng)的輪式移動機器人的傳動形式不同，其將實現(xiàn)轉(zhuǎn)向功能和移動功能的機械結(jié)構(gòu)進行了集成，由2個單獨的電機進行驅(qū)動，通過直行同步帶和轉(zhuǎn)向同步帶分別將動力傳遞至換向錐齒輪和轉(zhuǎn)向同步帶輪，達到行進與轉(zhuǎn)向的目的，但對于各自的運動互不干擾。轉(zhuǎn)向電機選用減速比為1∶13.7的步進電機，直行電機選用180 W的直流無刷電機。轉(zhuǎn)彎時，只需由轉(zhuǎn)向電機控制輪系進行原地轉(zhuǎn)向，而機器人本體保持不變，再由直行電機控制機器人移動，由此實現(xiàn)無轉(zhuǎn)彎半徑的全向移動。各層之間通過支撐桿連接，分散整機的受力載荷，保證承載性能。機器人的整體機械結(jié)構(gòu)設(shè)計很好地滿足了移動機器人的穩(wěn)定性、機動性以及可控性3個特征，使機器人具有高承載能力、控制簡單以及在狹小的空間內(nèi)靈活移動的優(yōu)勢。

直行運動的機械傳動原理如圖4所示。轉(zhuǎn)向運動的機械傳動原理如圖5所示。

圖4 直行運動的機械傳動原理 圖5 轉(zhuǎn)向運動的機械傳動原理

1.2 控制系統(tǒng)設(shè)計

跟隨服務(wù)機器人的控制系統(tǒng)分為：核心控制系統(tǒng)、UWB定位系統(tǒng)、超聲波避障系統(tǒng)、環(huán)境感知系統(tǒng)、運動控制系統(tǒng)、平板控制系統(tǒng)、顯示系統(tǒng)。跟隨服務(wù)機器人控制系統(tǒng)框圖如圖6所示。

圖6 跟隨服務(wù)機器人控制系統(tǒng)框圖

機器人設(shè)置了3種工作模式：自主跟隨模式、手動控制模式、待機模式。自主跟隨模式下，利用超寬帶定位技術(shù)，實現(xiàn)對目標位置的跟隨。手動控制模式下，利用藍牙通信技術(shù)，通過手持平板電腦對機器人實現(xiàn)運動控制。

在跟隨服務(wù)機器人的控制系統(tǒng)設(shè)計中，其主控MCU采用了32位Cortex-M3內(nèi)核ARM，型號為STM32F103VET6。其主頻最高可達72 MHz，擁有多達7個Timer，2個I2C和SPI，3個USART等豐富的外設(shè)配置[16-17]，滿足控制系統(tǒng)設(shè)計的要求。機身頂部環(huán)形分布3個DWM1000超寬帶定位基站模塊，由無線方式讀取定位標簽的信息，通過SPI與STM32進行通信，經(jīng)STM32處理得到的跟隨目標信息傳至運動控制單元，來完成電機的行走和轉(zhuǎn)向控制，由此實現(xiàn)機器人核心的自主跟隨功能。機身下方環(huán)形分布6個一體式超聲波測距模塊，在機器人行進過程中，通過檢測周圍障礙物與機器人之間的距離，實現(xiàn)避障功能。機身內(nèi)部置有PM2.5傳感器、溫度傳感器和濕度傳感器，可對機器人所處環(huán)境進行感知，并通過機身顯示屏顯示出檢測數(shù)據(jù)。跟隨服務(wù)機器人的控制流程如圖7所示。

圖7 跟隨服務(wù)機器人的控制流程

由于機器人的基礎(chǔ)形狀為圓形，因此在行進過程中，避障功能可通過開啟位于前方的3個超聲波模塊完成避障功能。機器人避障狀態(tài)判別如圖8所示。

圖8 機器人避障狀態(tài)判別

2 超寬帶定位系統(tǒng)

2.1 超寬帶定位技術(shù)及原理

對于傳統(tǒng)的無線定位系統(tǒng)常采用WiFi、超聲波、ZigBee及藍牙等定位技術(shù)。其中，WiFi定位技術(shù)精度較低；ZigBee及藍牙定位技術(shù)容易受到其他信號干擾；超聲波定位技術(shù)會受到環(huán)境溫度的變化引起誤差，從而影響定位精度。因此傳統(tǒng)定位系統(tǒng)的定位精度及穩(wěn)定性無法滿足室內(nèi)定位的相關(guān)要求。UWB是一種新型的無線通信技術(shù)，根據(jù)美國聯(lián)邦通信委員會的規(guī)范，UWB的工作頻帶為3.1～10.6 GHz，UWB信號的發(fā)生可發(fā)射時間極短的窄脈沖通過微分或混頻等上變頻方式調(diào)制到UWB工作頻段實現(xiàn)[18]，因此在無線定位領(lǐng)域內(nèi)具有較高的定位精度和穩(wěn)定的性能。此外，UWB技術(shù)具有消耗電能小、抗干擾性能強以及傳輸效率高的特點，在工廠倉儲貨物監(jiān)管、礦井人員定位、智能行李箱等場所得到廣泛應(yīng)用[19]。

無線定位技術(shù)是利用采集位置移動的測量數(shù)據(jù)進行模型建立分析的定位算法來實現(xiàn)定位。目前，最常用的定位技術(shù)主要有：信號到達角度測量(angle of arrival，AOA)、到達時間定位(time of arrival，TOA)和到達時間差定位(time difference of arrival，TDOA)等[20-22]。

信號到達角度測量法是通過測量信標未知點和基站之間的角度來計算目標的位置。

到達時間定位法是通過計算信號在空氣中的傳播時間，得出待測標簽和基站之間的距離，從而實現(xiàn)定位。如圖9所示，由于電磁波的傳播速度為已知量，因此，以各基站坐標BSi(xi,yi)(i=1,2,3)為圓心，以待測標簽Tag(x,y)到這3個基站之間的距離di(i=1,2,3)為半徑畫圓，待測標簽位置點就是這3個圓相交的交點坐標。

圖9 TOA定位原理

到達時間差定位法是采用待測標簽位置點Tag到不同基站BS之前的距離差來實目標標簽位置點的定位。由于無線信號傳播的時間是待測標簽和基站之間的信號接收與發(fā)射的時間差值，這就要求待測標簽與基站的時鐘實現(xiàn)嚴格的同步。因此，利用TDOA定位法只需在各基站之間實現(xiàn)時鐘同步就可以很容易地實現(xiàn)定位。

2.2 定位算法建模

本文采用基于TDOA的Taylor級數(shù)展開的定位算法估算初始位置，并作Taylor級數(shù)展開，利用最小二乘法進行迭代計算，以此來實現(xiàn)跟隨目標位置的精確定位。根據(jù)TDOA定位法得到的定位方程組具有非線性特點，通常需要先將其轉(zhuǎn)化為線性方程組后進行求解。Taylor級數(shù)展開法是一種基于TDOA定位系統(tǒng)中常用的定位方法，同時也是一種求解非線性方程的遞歸迭代算法。Taylor算法具有求解精度高、收斂速度快的優(yōu)點。

本機器人系統(tǒng)選取了3個基于DWM1000模塊的超寬帶，以環(huán)形的方式均勻分布在機身周圍，3個基站模塊形成等邊三角形Δ1。另有一個手持定位標簽，型號為DWM1000模塊。以等邊三角形的中心O1為坐標原點，將超寬帶定位系統(tǒng)建立二維平面直角坐標系xO1y，如圖10所示。

圖10 定位算法模型

假設(shè)手持定位標簽的估計位置為Tag′(x0,y0)，手持定位標簽的真實位置為Tag(x,y)，待測的定位標簽位置到3個基站的距離分別為R1,R2,R3。已知基站BS1位置為(x1,y1)，基站BS2位置為(x2,y2)，基站BS3位置為(x3,y3)，根據(jù)兩點間的距離公式建立距離方程組

(1)

Ri可根據(jù)從已知基站BSi位置傳回的時間差Δt表示

Ri=c×(ti-Δt)

(2)

其中：c為光速；ti為在第i個已知基站BSi位置上收到定位信息的時間。

方程組(1)可表示為

(3)

由此可知，上述方程組為非線性方程組，利用Taylor算法進行求解。

首先，消掉方程組(3)中的時間差Δt，可得到一組TDOA測量值

(4)

其中Ri1為定位標簽與2個已知基站的距離差。

在Taylor級數(shù)展開的過程中采用初值估計坐標(x0,y0)作為迭代的初值。真實位置與估計位置具有如下關(guān)系：

(5)

其中(Δx,Δy)為坐標偏差。

令距離差函數(shù)為Ri1(x,y)

(6)

對Ri1(x,y)在(x0,y0)處進行Taylor級數(shù)展開，且忽略2階及2階以上分量，則

(7)

即：

(8)

(9)

將上述式子化為矩陣的形式ψ=h-Gδ。

則上式的最小二乘解為

(10)

在下一次遞歸計算中，令

(11)

重復(fù)以上計算過程，直至|Δx|+|Δy|≤ε，ε為事先設(shè)定的門限誤差，則此時的手持定位標簽的位置Tag(x,y)為(x0+Δx,y0+Δy)。由此，實現(xiàn)了機器人自主定位功能。

2.3 算法步驟

1)根據(jù)基站位置，每個基站依次向定位標簽發(fā)起測距；根據(jù)公式，計算出測量的距離值，并記定位標簽與基站的距離分別為R1、R2、R3，列出定位方程組，計算出Taylor級數(shù)展開的初始值。

2)將初始值作為泰勒級數(shù)展開點，并通過最小二乘獲取估計值Δ。

3)計算|Δx|+|Δy|。

4)如果滿足誤差門限要求，則停止計算，輸出目標節(jié)點位置；否則，重新計算式(5)，然后返回步驟3)，直至計算出定位標簽的實際坐標。

2.4 定位仿真

3 自主跟隨控制

自主跟隨系統(tǒng)控制框圖如圖12所示。

圖11 超寬帶定位系統(tǒng)仿真圖

圖12 自主跟隨系統(tǒng)控制框圖

機器人的移動輸出層由3組輪系構(gòu)成，其在圓形底盤上均勻分布，形成等邊三角形Δ2。機器人總體結(jié)構(gòu)可近似為圓柱體，因此，Δ1的中心O1與Δ2的中心O2位于同一條軸線上，以中點O2為坐標原點，沿平行于輪子的方向作極軸，建立跟隨系統(tǒng)的極坐標系，忽略z軸上的距離差，則坐標原點O2(0,0)與坐標原點O1(0,0)重合。由于零轉(zhuǎn)彎半徑運載底盤機械結(jié)構(gòu)的特征，機器人具有無正方向的特點，導致跟隨系統(tǒng)坐標系與定位系統(tǒng)坐標系存在角度差，即從極軸按順時針旋轉(zhuǎn)達到x軸的角度值，記為Δθ，Δθ∈[0,2π)。

同時，機器人移動的過程中，其主體保持不動，定位系統(tǒng)坐標系隨之在二維平面內(nèi)作平移運動，因此，將定位系統(tǒng)坐標系轉(zhuǎn)化為極坐標系，在此坐標系下進行相關(guān)運算。由定位系統(tǒng)獲取手持定位標簽的位置坐標Tag(x,y)可轉(zhuǎn)化為極坐標系的形式T(ρ,θ)，T(ρ,θ)即為機器人跟隨的目標位置。

已知，直角坐標系與極坐標系之間的轉(zhuǎn)化公式

(12)

T(ρ,θ)可表示為

根據(jù)得到的機器人轉(zhuǎn)向角度θ轉(zhuǎn)，計算步進電機需要轉(zhuǎn)動的角度，由STM32發(fā)送脈沖信號至步進電機驅(qū)動器驅(qū)動電機轉(zhuǎn)動至相應(yīng)角度，并通過控制DIR高低電平實現(xiàn)電機正反方向的旋轉(zhuǎn)。然后，STM32發(fā)送脈沖信號至直流無刷伺服電機驅(qū)動器，驅(qū)動電機轉(zhuǎn)動，使機器人實現(xiàn)直行運動。同時，根據(jù)得到的跟隨距離d，調(diào)整給PU的脈沖信號的占空比來控制轉(zhuǎn)速，實現(xiàn)在跟隨過程中，依據(jù)目標距離的遠近實時調(diào)整機器人的跟隨速度。

自主跟隨模式控制流程如圖13所示。

圖13 自動跟隨模式控制流程

4 實驗測試

在實地應(yīng)用測試前，對機器人的直行、轉(zhuǎn)向以及移動能力分別進行基礎(chǔ)測試，如圖14所示。由測試結(jié)果可知，該機器人的機械結(jié)構(gòu)穩(wěn)定，移動靈活，控制精確可靠，能夠很好地實現(xiàn)零轉(zhuǎn)彎半徑運載底盤的基本特性。

圖14 機器人基礎(chǔ)測試

為驗證機器人整體性能，實地測試分別選取了室內(nèi)外2種場地進行測試，如圖15所示。室內(nèi)干擾因素較大，空間狹小，障礙物多，環(huán)境較為復(fù)雜；室外場地空曠，地面不平整，摩擦較大。在測試過程中，機器人能夠適應(yīng)多種復(fù)雜環(huán)境，緊跟目標，實現(xiàn)平穩(wěn)地移動，滿足預(yù)期效果。

圖15 機器人自主跟隨功能實地測試

5 結(jié)束語

本文設(shè)計了一款基于零轉(zhuǎn)彎半徑運載底盤的跟隨服務(wù)機器人，其選取STM32作為控制核心，采用超寬帶定位技術(shù)實現(xiàn)機器人的跟隨服務(wù)功能。與目前市場上的服務(wù)機器人相比，其具有控制簡單、承載力大、機動性及適應(yīng)性強等優(yōu)點，拓展了跟隨類服務(wù)機器人的產(chǎn)品種類，能夠更好地滿足人們?nèi)粘Ｉ钏琛?/p>