UWB 測距技術在跟隨服務機器人中的應用

吳松芮 趙立昊 張朝旭 于 聰 王春梅

（臨沂大學自動化與電氣工程學院，山東 臨沂 276005）

0 引言

傳統的老年人跟蹤式服務機器人適用范圍極其有限，大部分健康保障機器人只有在家中或房間中時才能發揮最大作用，為解決傳統老年人健康保障機器人適用環境過于苛刻以及當老年人獨自出門無人保護的安全性的問題， 提高機器人的戶外作業能力，采用更精準、更穩定、作用范圍更廣的跟蹤定位技術很有必要。

本文測試使用的老年人跟蹤式服務機器人主要面向老年人群體，外部環境復雜多變，因此對機器人的實用性和泛用程度有著比較高的要求。 在戶外作業能力方面，機器人的跟蹤定位技術的精度以及作用距離是非常重要的指標。 對于一個合格的跟蹤式服務機器人來說，跟蹤定位系統的穩定、高精度以及廣闊的作用范圍是其必須應該具備的能力，為此，筆者將原有的UWB 定位模塊依次替換成多種跟蹤定位系統，并分別與UWB 定位技術進行比較分析。

1 設備整體設計

1.1 功能設計

設備的移動和避障功能： 利用軟件編程控制STM32 控制器輸出實時變化的PWM 波用以進行電機驅動和控制電機轉速，并利用安置于設備前端的超聲波傳感器進行對前方障礙物的識別并計算距離，然后將數據返回至微控制器， 控制器遵循編程寫入的PID算法進行對數據的合理處理并將處理后的數據輸出來達到控制電機的效果，進而對設備整體作出姿態調控，并加以UWB 定位模塊進行輔助定位，最終實現設備的移動和避障功能。

在此篇文章中， 主要對其基于UWB 的跟隨功能進行探討。

1.2 硬件設計

設備采用STM32F103ZET6 作為控制器， 并采用雙主控設計，對所有模塊進行分區控制，以減輕主控芯片的負擔，從而提高了程序的運行效率以及設備在工作情況下的精準度，前端設備由UWB 定位模塊、三相直流霍爾電機、大功率霍爾無刷電機驅動板、無線傳輸設備（GPRS 模塊）、攝像頭、六軸加速度傳感器（MPU6050）以及被跟隨者攜帶的子設備組成，整體硬件設計結構框圖如圖1 所示。

圖1 系統結構框圖

1.3 位置式PID 控制算法

PID（Proportion Integral Differential）控制是控制系統中比較成熟， 而且也是應用最廣泛的一種控制器，設計中采用位置式PID 算法，其目的是將跟隨車與服務對象（人）的距離控制在一個固定數值范圍，設置一個0.1ms 的周期時間， 在這一段時間內算出PID 變化。 其中P 值為當前數值減假設定值， 得到了偏差（error），將偏差（error） 乘以積分系數（Ki）就得到了 P值，每個周期都將error*Ki 累加到變量errorsum 中，通過對變量 errorsum 限幅得到了 I，D 值為：D=Kd*（當前數值-上次數值），因此，以T 為采樣周期，作為采樣序號，則離散采樣時間對應連續時間，用矩形法數值一階后向差分可做以下近似變換：

式中，k 為采樣序號，k=0，1，2，…；

u（k）為第k 次采樣時刻的計算機輸出值；

e（k）為第 k 次采樣時刻偏差值；

e（k-1）為第 k-1 次采樣時刻偏差值；

Ki為積分系數，Ki=Kp·T/Ti；

Kd為微分系數，Kd=Kp·Td/T。

將PID 運算后對應的PWM 占空比輸出給電機就使每一刻的改變都映射電機的轉速上，完成對速度的控制。

2 UWB 跟隨功能的實現

2.1 設計思路

利用軟件編程控制STM32 控制器輸出實時變化的PWM 波用以進行電機驅動和控制電機轉速， 并利用安置于設備前端的超聲波傳感器進行對前方障礙物的識別并計算距離， 然后將數據返回至微控制器，控制器遵循編程寫入的PID 算法進行對數據的合理處理并將處理后的數據輸出來達到控制電機的效果，進而對設備整體作出姿態調控， 并加以UWB 定位模塊進行輔助定位，最終實現設備的移動功能。

2.2 UWB 測距功能

UWB 標簽與基站都是由SWM1000 （超寬帶無線收發模塊）、ST 公司的STM32F051 微處理器和高靈敏度UWB 天線組成，通過STM32F051 微處理器對SWM1000芯片進行控制，實現單對單或者單對多的測距，并能夠保持十厘米之內的誤差，最后STM32F051 單片機將測得的結果通過串口輸出。

單個UWB 標簽或基站上電后處于檢測狀態，當第二個UWB 標簽或基站上電后，兩個UWB 測距模塊會進行數據交流， 通過單邊雙向測距 （Single-sided Two-way Ranging）求得距離。

單邊雙向測距（Single-sided Two-way Ranging）是對單個往返消息時間上的簡單測量，設備A 主動發送數據到設備B，設備B 返回數據響應設備A，原理示意圖如圖2 所示。

圖2 測距原理示意圖

測距流程：設備A 向設備B 發送數據并將此刻時間記錄， 設備B 接收到數據后記錄接收數據的時間，經過短暫延時后， 設備B 將發送數據并記錄該時刻，設備A 接收數據并記錄時間，至此可以得到兩個時間差Tround和Treply，將數據進行如下運算：

再將公式（5）計算得到的Tprop乘以光速就得到基站與標簽的距離信息。

UWB 工作時并不是固定數量的協同工作， 而是可以同時使用多個標簽或基站應用多種不同算法，實現測距功能， 并且可以搭配上位機軟件將UWB 模塊獲得的數據解析從而繪制出平面圖形與標簽運動的軌跡。

2.3 數據處理

當UWB 標簽完成測距并且STM32F051 單片機將數據通過串口發送至主控芯片中后，STM32F103ZET6單片機會依據寫入的程序將所收到的距離信息作為反饋量并與作為輸入量的設定距離求得差值， 應用閉環位置式PID 控制算法計算出結果并通過調節比例、積分和微分系數進而調節設備的響應速度及其設備在自動跟隨時的穩定性， 設備運行時在每次計算之后都會將結果轉化為調節PWM 波占空比的ARR 寄存器的值。 PID 計算方法詳見上文。

2.4 電機與電機驅動

當STM32F103ZET6 微控制器通過位置式PID 控制算法計算出數據后， 單片機用以輸出PWM 波的GPIO 口會產生相應波形，將此GPIO 口用飛線方式連接至霍爾三相直流電機驅動板的信號接口，將電機三相電源線連接至電機驅動板的三相直流輸出端口并將相應控制端口進行置高或置低操作，最終實現單片機對三相直流電機的控制功能。

在大功率電機驅動板工作時， 因為電流較大，會產生大量熱量，所以應當在電機驅動板的下部安裝鋁制散熱片。

2.5 跟隨功能最終實現

在通過UWB 測距獲取數據后進行數據處理并將計算結果輸出至STM32F103ZET6 的CRR 寄存器中，通過改變此寄存器的值會調節PWM 波的占空比，輸出的PWM 波會通過硅膠線輸出至霍爾三相直流電機驅動板的信號端口，并起到驅動三相直流電機的功能。

原理流程圖如圖3 所示。

圖3 跟隨功能流程圖

3 UWB 定位技術與其他常用定位技術的比較優勢

3.1 精度優勢

目前， 市場上的大多數UWB 定位模塊精度都能控制在±3～5 cm 左右， 而其他的測距模塊如超聲波模塊，它的測量范圍只能保持在15 度以內。 而對于一種服務機器人來說，定位精度是一個機器人跟隨功能的核心重點。 通過調查，筆者發現老年人的步伐大約為60 cm。 而老年人的運動有速度慢、 徑向位移小等特點。根據老年人的這些運動特點，選用UWB 定位模塊無疑要比超聲波定位要好得多。

3.2 抗干擾性優勢

根據老年人的運動特點，其實對機器人跟隨系統的感應距離要求并不高。 最近的紅外測距80 cm 的感應距離都可以勉強算合格。 除此之外，超聲波感應距離能達到2 m， 而UWB 模塊更是能達到幾十米到幾百米。 那為什么還要選擇UWB 定位模塊呢？

設備的抗干擾性非常重要。 而紅外測距不能測量玻璃和黑色物體，所以它肯定是無法單獨勝任的。 如果說雜亂的房間超聲波定位還可以勉強應對，那么戶外作業就是一個難以跨過的檻。 戶外作業對超聲波的最大挑戰不在于環境有多么復雜難辨，而是來自其他應用超聲波定位的設備的干擾。 而UWB 技術依靠無線電定位，既能避免相同設備之間的互相干擾，用戶通過佩戴信標和設備進行雙向信息交流也能避免“跟錯人”等問題。

4 結語

本文以老年人服務機器人為實驗基礎，針對該類機器人的應用場景，通過比較分析幾種定位系統的優缺點， 最終論證了UWB 定位技術在該環境下的適應性最高。

關于服務機器人，筆者認為應該選用合適即性價比最高的技術，使機器人在與使用者需求匹配的情況下盡可能降低成本。 這樣才能使科技的福利走進千家萬戶，從而造福這些有需求的人群。 服務人民，便利生活，這是作為研究者經年不移的初心。