基于UWB的零轉徑跟隨機器人控制系統設計

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

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

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

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

1 機器人總體設計

設計的跟隨服務機器人意旨在滿足人們日常生活所需的服務要求，其集多種硬件設備和控制電路于一體，并預置其他功能拓展接口。因此，跟隨服務機器人的功能實現依靠穩定的行走機構、精確的定位系統、簡單可靠的控制系統。根據對跟隨服務機器人功能的設定，在其結構上采取了功能互換多層模塊化的結構，將零轉彎半徑基礎運載底盤作為基體，負責整機的載重以及移動功能的實現。通過結合UWB測距模塊，實現機器人智能跟隨的功能。

1.1 機械結構設計

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

圖1 跟隨服務機器人機械結構

圖2 輪系結構

圖3 零轉彎半徑運載底盤

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

直行運動的機械傳動原理如圖4所示。轉向運動的機械傳動原理如圖5所示。

圖4 直行運動的機械傳動原理 圖5 轉向運動的機械傳動原理

1.2 控制系統設計

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

圖6 跟隨服務機器人控制系統框圖

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

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

圖7 跟隨服務機器人的控制流程

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

圖8 機器人避障狀態判別

2 超寬帶定位系統

2.1 超寬帶定位技術及原理

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

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

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

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

圖9 TOA定位原理

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

2.2 定位算法建模

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

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

圖10 定位算法模型

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

(1)

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

Ri=c×(ti-Δt)

(2)

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

方程組(1)可表示為

(3)

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

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

(4)

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

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

(5)

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

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

(6)

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

(7)

即：

(8)

(9)

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

則上式的最小二乘解為

(10)

在下一次遞歸計算中，令

(11)

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

2.3 算法步驟

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

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

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

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

2.4 定位仿真

3 自主跟隨控制

自主跟隨系統控制框圖如圖12所示。

圖11 超寬帶定位系統仿真圖

圖12 自主跟隨系統控制框圖

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

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

已知，直角坐標系與極坐標系之間的轉化公式

(12)

T(ρ,θ)可表示為

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

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

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

4 實驗測試

在實地應用測試前，對機器人的直行、轉向以及移動能力分別進行基礎測試，如圖14所示。由測試結果可知，該機器人的機械結構穩定，移動靈活，控制精確可靠，能夠很好地實現零轉彎半徑運載底盤的基本特性。

圖14 機器人基礎測試

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

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

5 結束語

本文設計了一款基于零轉彎半徑運載底盤的跟隨服務機器人，其選取STM32作為控制核心，采用超寬帶定位技術實現機器人的跟隨服務功能。與目前市場上的服務機器人相比，其具有控制簡單、承載力大、機動性及適應性強等優點，拓展了跟隨類服務機器人的產品種類，能夠更好地滿足人們日常生活所需。