基于UWB 的實驗室跟隨機器人系統設計

鐘世杰，李隆，李響，宋強

（成都工業學院，四川成都，610031）

0 引言

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

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

當下，針對實驗室、倉庫等室內應用場景，跟隨機器人的市場缺口巨大，是一片等待開發的藍海，本文利用了激光雷達和UWB 技術，設計了一種小型化、可跟隨、可避障、低延遲的跟隨機器人，可以在實驗室、倉庫等室內環境下，實現地圖構建、自主定位、跟隨人體前行和同時躲避前方障礙物等功能。

1 系統總體設計方案

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

圖1 系統總體框圖

2 系統硬件設計

■2.1 主控模塊

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

■2.2 UWB 模塊

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

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

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

表1 常見的室內定位技術特點

圖2 UWB 信標

■2.3 驅動模塊

2.3.1 驅動模塊控制系統簡介

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

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

2.3.2 電機驅動模塊

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

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

■2.4 傳感器模塊

2.4.1 光電編碼器

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

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

2.4.2 ToF 激光雷達

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

圖4 激光雷達測距電路

■2.5 串口LCD 顯示屏

顯示屏是一種將一定的電子文件通過特定的傳輸設備顯示到屏幕上的輸出工具。在本設計中，串口顯示屏用于顯示當前跟隨機器人的系統狀態，例如當前電量、當前地圖坐標和當前位姿等。顯示屏采用串口通信協議連接LCD 液晶顯示屏。相較于OLED 顯示屏，LCD 顯示屏具有壽命長、成本低等優點，如表所示。LCD 顯示屏雖然壽命較長，但是在長時間使用后依舊會損壞，所以在安裝顯示屏時，應預留一定的拆卸空間，保障后續的更換操作能夠順利進行。

表2 顯示屏特點

3 系統軟件設計

本系統軟件設計主要由Linux 環境下的VS code 軟件進行程序編寫。Linux 可以在各種硬件平臺上運行，從小型嵌入式設備到大型服務器集群都可以使用。這種跨平臺性使得Linux 成為很多領域的首選操作系統。除此以外，Linux 還提供了豐富的開發工具，如gcc 編譯器、gdb 調試器、make 構建工具等，可以大大地縮短開發周期。基于以上原因，本系統軟件設計選擇在Linux 環境下進行。

本系統軟件設計包括：機器人的初始化程序，激光測距程序，UWB 定位程序，避障跟隨程序，電機驅動程序和顯示模塊程序。實驗室跟隨機器人的軟件層核心任務是實現避障跟隨。整個系統上電后，主程序首先對系統時鐘以及串口等通信接口進行初始化，樹莓派計算機通過讀取激光雷達以及UWB 的定位信息，確定當前坐標和位姿以及障礙物的方向和距離，再通過STM32 驅動系統來驅動電機，實現機器人的路徑規劃、地圖構建、自主定位、跟隨人體前行的同時躲避前方障礙物等功能。

圖5 主程序流程圖

■3.1 UWB 定位算法

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

■3.2 地圖構建子程序

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

圖6 地圖構建子程序流程圖

■3.3 避障跟隨子程序

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

圖7 避障子程序流程圖

4 結語

本系統以UWB 技術為基礎，以樹莓派4B 為主控，采用FOC 三相電機控制器，搭配霍爾傳感器和1000 線光電編碼器，實現了機器人的自身定位、環境地圖構建、路徑規劃以及障礙物閃避等功能。

經測試，跟隨機器人運行平穩，能夠實現避障跟隨等功能，同時在實驗室、教室等復雜場景工作下，也有良好的表現。系統整機測試如圖8 所示。

圖8 UWB 基站與跟隨機器人實物測試圖

在本系統中，UWB 技術用于實現機器人的位置和姿態的確定，樹莓派4B 作為主控負責控制機器人的運動，FOC三相電機控制器用于控制電機的轉速和轉向，霍爾傳感器和1000 線光電編碼器用于獲取機器人的運動狀態。通過這些技術的組合，本系統能夠實現高精度的定位和運動控制，同時能夠構建環境地圖、規劃路徑并避開障礙物，具有較高的實用性和可靠性。通過實際測試，機器人在實驗室環境下運行良好，能夠自主定位與避障，提高工作效率，減少人力成本，保障人員安全等，是未來智能化，自動化的發展趨勢，具有一定的實際應用價值和可觀的市場前景。