基于UWB 的實(shí)驗(yàn)室跟隨機(jī)器人系統(tǒng)設(shè)計(jì)

鐘世杰，李隆，李響，宋強(qiáng)

（成都工業(yè)學(xué)院，四川成都，610031）

0 引言

隨著科技的快速發(fā)展，人們的生活水平逐漸提升，如何解放生產(chǎn)力，為人類帶來更多便利，已成為未來發(fā)展的主題。尤其是進(jìn)入21 世紀(jì)后，科技發(fā)展更加迅速，人工智能、無人駕駛、量子計(jì)算、5G 通信等技術(shù)都是具有代表性的創(chuàng)新技術(shù)。在“以國內(nèi)大循環(huán)為主，國內(nèi)國際雙循環(huán)相互促進(jìn)的新發(fā)展格局”的雙循環(huán)指導(dǎo)下，未來，我國科技發(fā)展將更加迅速。

近代以來，我國機(jī)器人行業(yè)發(fā)展迅速，但是起步較晚，在機(jī)器人環(huán)境地圖構(gòu)建、路徑規(guī)劃以及障礙物閃避等技術(shù)方面還存在不少空白，適合用于工業(yè)現(xiàn)場的機(jī)器人更是十分稀少，智能型機(jī)器人代替人工已是大勢所趨。如走進(jìn)萬千家庭的智能掃地機(jī)器人、工業(yè)中應(yīng)用廣泛的搬運(yùn)車和國內(nèi)某電商平臺的無人快遞車等。智能型機(jī)器人用途廣泛，可應(yīng)用于各個行業(yè)領(lǐng)域，如：物流倉儲，醫(yī)療護(hù)理，工業(yè)制造等，還可以加快進(jìn)行工業(yè)自動化，數(shù)字化的發(fā)展，提高我國國際影響力和競爭力。

當(dāng)下，針對實(shí)驗(yàn)室、倉庫等室內(nèi)應(yīng)用場景，跟隨機(jī)器人的市場缺口巨大，是一片等待開發(fā)的藍(lán)海，本文利用了激光雷達(dá)和UWB 技術(shù)，設(shè)計(jì)了一種小型化、可跟隨、可避障、低延遲的跟隨機(jī)器人，可以在實(shí)驗(yàn)室、倉庫等室內(nèi)環(huán)境下，實(shí)現(xiàn)地圖構(gòu)建、自主定位、跟隨人體前行和同時(shí)躲避前方障礙物等功能。

1 系統(tǒng)總體設(shè)計(jì)方案

基于UWB 的實(shí)驗(yàn)室跟隨機(jī)器人系統(tǒng)整體可分為硬件設(shè)計(jì)和軟件設(shè)計(jì)兩大部分。主要實(shí)現(xiàn)的功能有：地圖建模、自動避障和人體跟隨。機(jī)器人系統(tǒng)由樹莓派4B 主控模塊、UWB 模塊、電機(jī)驅(qū)動模塊、驅(qū)動控制模塊、激光雷達(dá)模塊和串口顯示屏組成，系統(tǒng)總體框圖如圖1 所示。其中樹莓派4B 負(fù)責(zé)邏輯主控，地圖構(gòu)建，網(wǎng)絡(luò)連接，同時(shí)與外圍傳感器進(jìn)行通信。STM32 單片機(jī)作為電機(jī)的驅(qū)動控制，通過控制電機(jī)驅(qū)動模塊來對電機(jī)進(jìn)行控制。UWB 模塊通過發(fā)射超寬帶窄波與基站進(jìn)行定位，確定當(dāng)前的位姿。激光雷達(dá)模塊采用ToF 算法，識別道路前方的障礙物，將障礙物與自身的距離返回給樹莓派4B 進(jìn)行地圖構(gòu)建，從而實(shí)現(xiàn)避障和跟隨功能。

圖1 系統(tǒng)總體框圖

2 系統(tǒng)硬件設(shè)計(jì)

■2.1 主控模塊

樹莓派4B 是由“Raspberry Pi 基金會”于2019 年上市的第4 代卡片式電腦。它是一款基于ARM 架構(gòu)的微型電腦主板，以SD/MicroSD 卡為內(nèi)存硬盤，擁有一個64 位的四核處理器，主頻1.5GHz，內(nèi)存容量從1GB 到8GB 不等，支持雙頻Wi-Fi、藍(lán)牙5.0、Gigabit 以太網(wǎng)、2 個USB 3.0和2 個USB 2.0 接口、2 個HDMI 接口、MicroSD 卡槽等。同時(shí)具有運(yùn)算速度快、接口豐富、體積小等優(yōu)點(diǎn)，通過它連接各種傳感器、模塊和外設(shè)，可以很好地滿足本系統(tǒng)設(shè)計(jì)的需求。對于本跟隨機(jī)器人系統(tǒng)而言，需要時(shí)刻采集周圍環(huán)境變量，對當(dāng)前地圖進(jìn)行建模，分析定位，并驅(qū)動電機(jī)前進(jìn)。由于整個系統(tǒng)工作量較大，如使用普通的單片機(jī)模塊進(jìn)行運(yùn)算，整個系統(tǒng)超算力運(yùn)行，將會導(dǎo)致行動遲緩等問題，為了獲得更好的算力和功能擴(kuò)展能力，選擇性能強(qiáng)大的樹莓派4B 作為主控模塊是很好的選擇。

■2.2 UWB 模塊

UWB(超寬帶無線載波通信)技術(shù)是一種具有高精度定位、高時(shí)間分辨率、高能效、高穩(wěn)定性特點(diǎn)的技術(shù)。其定位原理為：UWB 定位利用發(fā)射方向控制的超短脈沖信號來測量目標(biāo)與基站之間的時(shí)延，從而計(jì)算出目標(biāo)的位置。

在本系統(tǒng)設(shè)計(jì)當(dāng)中，關(guān)鍵一步是利用UWB 模塊獲取當(dāng)前的位置和姿態(tài)[1]，即分別獲取標(biāo)簽與3 個基站之間的x,y,z坐標(biāo)。其中UWB 模塊既可以做基站，又可以做標(biāo)簽。一般情況下，3 個UWB 基站和1 個UWB 標(biāo)簽就可以組合實(shí)現(xiàn)三維空間的定位[2]，但是卻無法避免z 軸上有兩個取值的問題。因此在設(shè)計(jì)算法時(shí)，需要將兩個z 軸的值進(jìn)行分析，舍去錯誤值。

具體地說，UWB 定位需要至少三個基站分別與目標(biāo)進(jìn)行通信，每個基站發(fā)送一個包含時(shí)間戳的超短脈沖信號。目標(biāo)接收到這些信號后，計(jì)算出每個信號的到達(dá)時(shí)間，并將其與基站發(fā)送信號的時(shí)間戳相減，得到時(shí)延數(shù)據(jù)。由于時(shí)延與距離成正比，因此可以通過時(shí)延數(shù)據(jù)計(jì)算出目標(biāo)與每個基站之間的距離。一旦確定了目標(biāo)與三個或以上的基站之間的距離，就可以使用三角定位法計(jì)算出目標(biāo)的位置。UWB 技術(shù)的應(yīng)用場景大致可以分為三個方面[3]，分別是通信、雷達(dá)和定位，常應(yīng)用于智能家庭、無線網(wǎng)絡(luò)、戰(zhàn)術(shù)組網(wǎng)電臺、探地雷達(dá)、車輛避撞雷達(dá)以及軍用民用等需要精確定位的系統(tǒng)中。在本系統(tǒng)設(shè)計(jì)中，經(jīng)過資料查閱和實(shí)驗(yàn)比對，選擇了可以帶來更好定位效果的UWB 技術(shù)[4]，其特點(diǎn)如表1 所示。

表1 常見的室內(nèi)定位技術(shù)特點(diǎn)

圖2 UWB 信標(biāo)

■2.3 驅(qū)動模塊

2.3.1 驅(qū)動模塊控制系統(tǒng)簡介

驅(qū)動模塊控制系統(tǒng)使用STM32F407VET6 單片機(jī)來進(jìn)行控制，驅(qū)動部分則使用FOC（Field Oriented Control，磁場定向控制)三相電機(jī)控制器，由此驅(qū)動2 個200W 的無刷直流輪轂電機(jī)。FOC 三相電機(jī)控制技術(shù)是一種用于控制交流電機(jī)的技術(shù)，現(xiàn)主要用于驅(qū)動無刷直流電機(jī)，其特點(diǎn)包括：可以將電機(jī)磁場方向和電流方向分離控制，從而實(shí)現(xiàn)對電機(jī)的高精度控制；可以控制電機(jī)的功率因數(shù)，最大限度地提高電機(jī)效率，減少系統(tǒng)能耗，使電機(jī)在運(yùn)行時(shí)產(chǎn)生更低的噪音；可以通過閉環(huán)控制降低電機(jī)的溫升，延長電機(jī)壽命，提高電機(jī)系統(tǒng)的可靠性；可以實(shí)時(shí)監(jiān)測電機(jī)狀態(tài)，及時(shí)發(fā)現(xiàn)問題并進(jìn)行調(diào)整，避免電機(jī)損壞和故障發(fā)生。無刷直流電機(jī)是一種新型的電動機(jī)，其可分為具有直流電機(jī)特性的無刷直流電機(jī)和具有交流電機(jī)特性的無刷直流電機(jī)。相較于傳統(tǒng)的有刷直流電動機(jī)，它具有高效率、高功率密度、高可靠性和低噪聲等特點(diǎn)。

在課時(shí)量限制的情況下采用傳統(tǒng)的教師講、學(xué)生聽的方式，以理論講授為主，很難調(diào)動學(xué)生的學(xué)習(xí)積極性。教學(xué)與科研分離現(xiàn)象較為突出，教學(xué)過程中未能有效地結(jié)合基礎(chǔ)前沿理論與具有時(shí)代性的現(xiàn)實(shí)經(jīng)濟(jì)問題。目前案例教學(xué)僅用簡單的經(jīng)濟(jì)學(xué)例子解釋計(jì)量經(jīng)濟(jì)學(xué)模型，并非真正意義上的案例教學(xué)。多數(shù)計(jì)量經(jīng)濟(jì)學(xué)教材上每章提供的案例綜合性較差、陳舊，未能根據(jù)不同專業(yè)特點(diǎn)進(jìn)行設(shè)置，且對如何選題、如何建立和分析模型以解決實(shí)際經(jīng)濟(jì)問題等討論少之甚少，導(dǎo)致學(xué)生在綜合實(shí)證分析中，從模型設(shè)定、查閱資料、收集整理數(shù)據(jù)、模型估計(jì)和診斷等各個建模環(huán)節(jié)，仍存在大量的技術(shù)問題[2]。

2.3.2 電機(jī)驅(qū)動模塊

電機(jī)驅(qū)動采用SimpleFOCShield v2.0.4。SimpleFOCShield v2.0.4 是一款帶電流環(huán)的FOC 無刷電機(jī)驅(qū)動器，主要用于低功耗的FOC 應(yīng)用，驅(qū)動無刷直流電動機(jī)。該驅(qū)動器功能豐富，支持開/閉環(huán)無刷速度/位置控制以及最大功率120W 輸出，最大電流5A 輸出，工作電壓12～24V。此外，SimpleFOC v2.0.4 還可通過多種硬件平臺進(jìn)行控制，如Arduino、STM32和ESP32 等，并且可以與多種編碼器進(jìn)行通信，如ABI、I2C和SPI 等。

本文選擇在STM32 平臺上控制SimpleFOCShield 驅(qū)動器，方法是：先將電機(jī)的三根相線（U、V 和W)連接到SimpleFOCShield 上的相應(yīng)引腳，然后將相位編碼器的各個引腳（A、B 和I)連接到STM32 的外部中斷引腳，最后將兩個驅(qū)動板的TB_PWR 接口并聯(lián)到12V 電源上，需要注意的是，SMT32、SimpleFOCShield 驅(qū)動器、無刷直流電機(jī)，三者需要共地連接。

■2.4 傳感器模塊

2.4.1 光電編碼器

光電編碼器是一種通過光電轉(zhuǎn)換將輸出軸上的機(jī)械幾何位移量轉(zhuǎn)換成脈沖或數(shù)字量的傳感器。具體地說，光電編碼器的信號輸出可以是集電極開路NPN 輸出、集電極開路PNP 輸出、方波、正弦波和推挽式等多種形式。在本設(shè)計(jì)中，采用1000 線的高分辨率光電編碼器來輸出精準(zhǔn)的里程計(jì)信息，可為后續(xù)的精準(zhǔn)定位提供必要條件。其中1000 線是指編碼器軸承在1倍頻的條件下每旋轉(zhuǎn)360°能提供1000個脈沖，線數(shù)越高，代表編碼器的分辨率越高。高精度編碼器的線數(shù)范圍沒有一個具體的界定，一般來說，1000 線及以上的編碼器都可以被認(rèn)為是高精度編碼器，因?yàn)樗鼈兺ǔＬ峁┝溯^高的測量精度和較低的誤差率。

圖3 光電編碼器原理示意圖

2.4.2 ToF 激光雷達(dá)

機(jī)器人需要避開環(huán)境中的障礙物前進(jìn)，就需要時(shí)刻獲取與距離障礙物的位置距離。然而在室內(nèi)環(huán)境下，障礙物多且分布雜亂，傳統(tǒng)的測距方式難以實(shí)現(xiàn)。例如超聲波測距，它是一種常用的非接觸式測距方法，利用了超聲波的回聲來測量與其發(fā)射源之間的距離。然而超聲波易受到環(huán)境的影響，例如溫度、濕度、氣壓等因素都會影響超聲波的傳播速度和回聲反射情況，從而影響測量精度。例如在實(shí)驗(yàn)室場景下，其測距的誤差通常在數(shù)厘米至數(shù)十厘米之間，精度較低。

圖4 激光雷達(dá)測距電路

■2.5 串口LCD 顯示屏

顯示屏是一種將一定的電子文件通過特定的傳輸設(shè)備顯示到屏幕上的輸出工具。在本設(shè)計(jì)中，串口顯示屏用于顯示當(dāng)前跟隨機(jī)器人的系統(tǒng)狀態(tài)，例如當(dāng)前電量、當(dāng)前地圖坐標(biāo)和當(dāng)前位姿等。顯示屏采用串口通信協(xié)議連接LCD 液晶顯示屏。相較于OLED 顯示屏，LCD 顯示屏具有壽命長、成本低等優(yōu)點(diǎn)，如表所示。LCD 顯示屏雖然壽命較長，但是在長時(shí)間使用后依舊會損壞，所以在安裝顯示屏?xí)r，應(yīng)預(yù)留一定的拆卸空間，保障后續(xù)的更換操作能夠順利進(jìn)行。

表2 顯示屏特點(diǎn)

3 系統(tǒng)軟件設(shè)計(jì)

本系統(tǒng)軟件設(shè)計(jì)主要由Linux 環(huán)境下的VS code 軟件進(jìn)行程序編寫。Linux 可以在各種硬件平臺上運(yùn)行，從小型嵌入式設(shè)備到大型服務(wù)器集群都可以使用。這種跨平臺性使得Linux 成為很多領(lǐng)域的首選操作系統(tǒng)。除此以外，Linux 還提供了豐富的開發(fā)工具，如gcc 編譯器、gdb 調(diào)試器、make 構(gòu)建工具等，可以大大地縮短開發(fā)周期?；谝陨显颍鞠到y(tǒng)軟件設(shè)計(jì)選擇在Linux 環(huán)境下進(jìn)行。

本系統(tǒng)軟件設(shè)計(jì)包括：機(jī)器人的初始化程序，激光測距程序，UWB 定位程序，避障跟隨程序，電機(jī)驅(qū)動程序和顯示模塊程序。實(shí)驗(yàn)室跟隨機(jī)器人的軟件層核心任務(wù)是實(shí)現(xiàn)避障跟隨。整個系統(tǒng)上電后，主程序首先對系統(tǒng)時(shí)鐘以及串口等通信接口進(jìn)行初始化，樹莓派計(jì)算機(jī)通過讀取激光雷達(dá)以及UWB 的定位信息，確定當(dāng)前坐標(biāo)和位姿以及障礙物的方向和距離，再通過STM32 驅(qū)動系統(tǒng)來驅(qū)動電機(jī)，實(shí)現(xiàn)機(jī)器人的路徑規(guī)劃、地圖構(gòu)建、自主定位、跟隨人體前行的同時(shí)躲避前方障礙物等功能。

圖5 主程序流程圖

■3.1 UWB 定位算法

UWB 標(biāo)簽的常用定位方法有：單邊雙向測距法(SSTWR)，雙邊雙向測距法（DS-TWR)，雙向飛行時(shí)間法(TOF)，到達(dá)時(shí)間差法(TDOA)和到達(dá)時(shí)間算法(TOA)。本系統(tǒng)設(shè)計(jì)考慮到實(shí)現(xiàn)難度與實(shí)現(xiàn)效果，最終選擇了到達(dá)時(shí)間差法定位。要使用到達(dá)時(shí)間差法，至少需要3 個UWB 基站與1 個UWB 標(biāo)簽。3 個UWB 基站和1 個UWB 標(biāo)簽就可以組合實(shí)現(xiàn)三維空間的定位，但是卻無法避免z 軸上有兩個取值的問題。在當(dāng)前設(shè)計(jì)的應(yīng)用場景中，載具車輛高度較低，約在10～20cm 高，而UWB 標(biāo)簽一般是戴在人的手上，距地面約80～100cm，所以在應(yīng)用場景中可以始終選擇正z 作為真實(shí)z 值[6]。

■3.2 地圖構(gòu)建子程序

機(jī)器人需要在室內(nèi)進(jìn)行移動，就需要進(jìn)行地圖構(gòu)建。SLAM 算法(Simultaneous Localization and Mapping，同時(shí)定位與地圖構(gòu)建)是指在未知環(huán)境中，利用移動機(jī)器人的傳感器信息，同時(shí)進(jìn)行自我定位和環(huán)境地圖構(gòu)建的一類算法。該算法的主要思想是將機(jī)器人在未知環(huán)境中移動的過程中所獲得的傳感器數(shù)據(jù)和運(yùn)動控制信息進(jìn)行融合，同時(shí)實(shí)現(xiàn)自我定位和環(huán)境地圖構(gòu)建的過程。簡單來說，就是機(jī)器人在未知環(huán)境中不斷移動，通過傳感器獲取周圍環(huán)境的信息，如激光雷達(dá)，同時(shí)根據(jù)機(jī)器人的運(yùn)動狀態(tài)更新自己在環(huán)境中的位置，并把所有的數(shù)據(jù)融合起來生成機(jī)器人所在的地圖。其軟件設(shè)計(jì)流程圖如圖6 所示。

圖6 地圖構(gòu)建子程序流程圖

■3.3 避障跟隨子程序

在獲得了自身定位與環(huán)境之間的關(guān)系后，工具車讀取UWB 信息進(jìn)行解碼，確定自身的位置和姿態(tài)，方便調(diào)整角度來驅(qū)動電機(jī)越過障礙物或者跟隨人體前行。其程序流程圖如圖7 所示。

圖7 避障子程序流程圖

4 結(jié)語

本系統(tǒng)以UWB 技術(shù)為基礎(chǔ)，以樹莓派4B 為主控，采用FOC 三相電機(jī)控制器，搭配霍爾傳感器和1000 線光電編碼器，實(shí)現(xiàn)了機(jī)器人的自身定位、環(huán)境地圖構(gòu)建、路徑規(guī)劃以及障礙物閃避等功能。

經(jīng)測試，跟隨機(jī)器人運(yùn)行平穩(wěn)，能夠?qū)崿F(xiàn)避障跟隨等功能，同時(shí)在實(shí)驗(yàn)室、教室等復(fù)雜場景工作下，也有良好的表現(xiàn)。系統(tǒng)整機(jī)測試如圖8 所示。

圖8 UWB 基站與跟隨機(jī)器人實(shí)物測試圖

在本系統(tǒng)中，UWB 技術(shù)用于實(shí)現(xiàn)機(jī)器人的位置和姿態(tài)的確定，樹莓派4B 作為主控負(fù)責(zé)控制機(jī)器人的運(yùn)動，F(xiàn)OC三相電機(jī)控制器用于控制電機(jī)的轉(zhuǎn)速和轉(zhuǎn)向，霍爾傳感器和1000 線光電編碼器用于獲取機(jī)器人的運(yùn)動狀態(tài)。通過這些技術(shù)的組合，本系統(tǒng)能夠?qū)崿F(xiàn)高精度的定位和運(yùn)動控制，同時(shí)能夠構(gòu)建環(huán)境地圖、規(guī)劃路徑并避開障礙物，具有較高的實(shí)用性和可靠性。通過實(shí)際測試，機(jī)器人在實(shí)驗(yàn)室環(huán)境下運(yùn)行良好，能夠自主定位與避障，提高工作效率，減少人力成本，保障人員安全等，是未來智能化，自動化的發(fā)展趨勢，具有一定的實(shí)際應(yīng)用價(jià)值和可觀的市場前景。