UWB 測(cè)距技術(shù)在跟隨服務(wù)機(jī)器人中的應(yīng)用

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

（臨沂大學(xué)自動(dòng)化與電氣工程學(xué)院，山東 臨沂 276005）

0 引言

傳統(tǒng)的老年人跟蹤式服務(wù)機(jī)器人適用范圍極其有限，大部分健康保障機(jī)器人只有在家中或房間中時(shí)才能發(fā)揮最大作用，為解決傳統(tǒng)老年人健康保障機(jī)器人適用環(huán)境過于苛刻以及當(dāng)老年人獨(dú)自出門無人保護(hù)的安全性的問題， 提高機(jī)器人的戶外作業(yè)能力，采用更精準(zhǔn)、更穩(wěn)定、作用范圍更廣的跟蹤定位技術(shù)很有必要。

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

1 設(shè)備整體設(shè)計(jì)

1.1 功能設(shè)計(jì)

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

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

1.2 硬件設(shè)計(jì)

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

圖1 系統(tǒng)結(jié)構(gòu)框圖

1.3 位置式PID 控制算法

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

式中，k 為采樣序號(hào)，k=0，1，2，…；

u（k）為第k 次采樣時(shí)刻的計(jì)算機(jī)輸出值；

e（k）為第 k 次采樣時(shí)刻偏差值；

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

Ki為積分系數(shù)，Ki=Kp·T/Ti；

Kd為微分系數(shù)，Kd=Kp·Td/T。

將PID 運(yùn)算后對(duì)應(yīng)的PWM 占空比輸出給電機(jī)就使每一刻的改變都映射電機(jī)的轉(zhuǎn)速上，完成對(duì)速度的控制。

2 UWB 跟隨功能的實(shí)現(xiàn)

2.1 設(shè)計(jì)思路

利用軟件編程控制STM32 控制器輸出實(shí)時(shí)變化的PWM 波用以進(jìn)行電機(jī)驅(qū)動(dòng)和控制電機(jī)轉(zhuǎn)速， 并利用安置于設(shè)備前端的超聲波傳感器進(jìn)行對(duì)前方障礙物的識(shí)別并計(jì)算距離， 然后將數(shù)據(jù)返回至微控制器，控制器遵循編程寫入的PID 算法進(jìn)行對(duì)數(shù)據(jù)的合理處理并將處理后的數(shù)據(jù)輸出來達(dá)到控制電機(jī)的效果，進(jìn)而對(duì)設(shè)備整體作出姿態(tài)調(diào)控， 并加以UWB 定位模塊進(jìn)行輔助定位，最終實(shí)現(xiàn)設(shè)備的移動(dòng)功能。

2.2 UWB 測(cè)距功能

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

單個(gè)UWB 標(biāo)簽或基站上電后處于檢測(cè)狀態(tài)，當(dāng)?shù)诙€(gè)UWB 標(biāo)簽或基站上電后，兩個(gè)UWB 測(cè)距模塊會(huì)進(jìn)行數(shù)據(jù)交流， 通過單邊雙向測(cè)距 （Single-sided Two-way Ranging）求得距離。

單邊雙向測(cè)距（Single-sided Two-way Ranging）是對(duì)單個(gè)往返消息時(shí)間上的簡(jiǎn)單測(cè)量，設(shè)備A 主動(dòng)發(fā)送數(shù)據(jù)到設(shè)備B，設(shè)備B 返回?cái)?shù)據(jù)響應(yīng)設(shè)備A，原理示意圖如圖2 所示。

圖2 測(cè)距原理示意圖

測(cè)距流程：設(shè)備A 向設(shè)備B 發(fā)送數(shù)據(jù)并將此刻時(shí)間記錄， 設(shè)備B 接收到數(shù)據(jù)后記錄接收數(shù)據(jù)的時(shí)間，經(jīng)過短暫延時(shí)后， 設(shè)備B 將發(fā)送數(shù)據(jù)并記錄該時(shí)刻，設(shè)備A 接收數(shù)據(jù)并記錄時(shí)間，至此可以得到兩個(gè)時(shí)間差Tround和Treply，將數(shù)據(jù)進(jìn)行如下運(yùn)算：

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

UWB 工作時(shí)并不是固定數(shù)量的協(xié)同工作， 而是可以同時(shí)使用多個(gè)標(biāo)簽或基站應(yīng)用多種不同算法，實(shí)現(xiàn)測(cè)距功能， 并且可以搭配上位機(jī)軟件將UWB 模塊獲得的數(shù)據(jù)解析從而繪制出平面圖形與標(biāo)簽運(yùn)動(dòng)的軌跡。

2.3 數(shù)據(jù)處理

當(dāng)UWB 標(biāo)簽完成測(cè)距并且STM32F051 單片機(jī)將數(shù)據(jù)通過串口發(fā)送至主控芯片中后，STM32F103ZET6單片機(jī)會(huì)依據(jù)寫入的程序?qū)⑺盏降木嚯x信息作為反饋量并與作為輸入量的設(shè)定距離求得差值， 應(yīng)用閉環(huán)位置式PID 控制算法計(jì)算出結(jié)果并通過調(diào)節(jié)比例、積分和微分系數(shù)進(jìn)而調(diào)節(jié)設(shè)備的響應(yīng)速度及其設(shè)備在自動(dòng)跟隨時(shí)的穩(wěn)定性， 設(shè)備運(yùn)行時(shí)在每次計(jì)算之后都會(huì)將結(jié)果轉(zhuǎn)化為調(diào)節(jié)PWM 波占空比的ARR 寄存器的值。 PID 計(jì)算方法詳見上文。

2.4 電機(jī)與電機(jī)驅(qū)動(dòng)

當(dāng)STM32F103ZET6 微控制器通過位置式PID 控制算法計(jì)算出數(shù)據(jù)后， 單片機(jī)用以輸出PWM 波的GPIO 口會(huì)產(chǎn)生相應(yīng)波形，將此GPIO 口用飛線方式連接至霍爾三相直流電機(jī)驅(qū)動(dòng)板的信號(hào)接口，將電機(jī)三相電源線連接至電機(jī)驅(qū)動(dòng)板的三相直流輸出端口并將相應(yīng)控制端口進(jìn)行置高或置低操作，最終實(shí)現(xiàn)單片機(jī)對(duì)三相直流電機(jī)的控制功能。

在大功率電機(jī)驅(qū)動(dòng)板工作時(shí)， 因?yàn)殡娏鬏^大，會(huì)產(chǎn)生大量熱量，所以應(yīng)當(dāng)在電機(jī)驅(qū)動(dòng)板的下部安裝鋁制散熱片。

2.5 跟隨功能最終實(shí)現(xiàn)

在通過UWB 測(cè)距獲取數(shù)據(jù)后進(jìn)行數(shù)據(jù)處理并將計(jì)算結(jié)果輸出至STM32F103ZET6 的CRR 寄存器中，通過改變此寄存器的值會(huì)調(diào)節(jié)PWM 波的占空比，輸出的PWM 波會(huì)通過硅膠線輸出至霍爾三相直流電機(jī)驅(qū)動(dòng)板的信號(hào)端口，并起到驅(qū)動(dòng)三相直流電機(jī)的功能。

原理流程圖如圖3 所示。

圖3 跟隨功能流程圖

3 UWB 定位技術(shù)與其他常用定位技術(shù)的比較優(yōu)勢(shì)

3.1 精度優(yōu)勢(shì)

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

3.2 抗干擾性優(yōu)勢(shì)

根據(jù)老年人的運(yùn)動(dòng)特點(diǎn)，其實(shí)對(duì)機(jī)器人跟隨系統(tǒng)的感應(yīng)距離要求并不高。 最近的紅外測(cè)距80 cm 的感應(yīng)距離都可以勉強(qiáng)算合格。 除此之外，超聲波感應(yīng)距離能達(dá)到2 m， 而UWB 模塊更是能達(dá)到幾十米到幾百米。 那為什么還要選擇UWB 定位模塊呢？

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

4 結(jié)語

本文以老年人服務(wù)機(jī)器人為實(shí)驗(yàn)基礎(chǔ)，針對(duì)該類機(jī)器人的應(yīng)用場(chǎng)景，通過比較分析幾種定位系統(tǒng)的優(yōu)缺點(diǎn)， 最終論證了UWB 定位技術(shù)在該環(huán)境下的適應(yīng)性最高。

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