，，

(1.浙江樹(shù)人大學(xué) 信息科技學(xué)院，杭州 310015;2.史陶比爾(杭州)精密機(jī)械電子有限公司)

引 言

室內(nèi)移動(dòng)機(jī)器人定位是移動(dòng)機(jī)器人設(shè)計(jì)的關(guān)鍵技術(shù)，由于室內(nèi)環(huán)境GPS定位無(wú)法正常運(yùn)行，主要采用激光導(dǎo)航SLAM定位方法、視覺(jué)定位方法。激光導(dǎo)航SLAM定位、視覺(jué)定位傳感器價(jià)格昂貴，數(shù)據(jù)處理計(jì)算量大，實(shí)時(shí)性較差，目前應(yīng)用成本較高，而且對(duì)于復(fù)雜環(huán)境，這兩種方法不能提供很好的定位精度。所以，這里提出了采用UWB技術(shù)來(lái)實(shí)現(xiàn)室內(nèi)復(fù)雜環(huán)境下的移動(dòng)機(jī)器人定位。

1 UWB定位基本原理

UWB(超寬帶)定位是近年來(lái)發(fā)展起來(lái)的一種定位方式，使用極窄的脈沖信號(hào)或者極寬的頻譜帶寬信號(hào)進(jìn)行信息傳遞，具有穿透能力強(qiáng)、定位精度高等優(yōu)點(diǎn)，具有厘米級(jí)的定位能力[1-2]。

UWB定位的常用方法有基于到達(dá)時(shí)間(TOA)、基于到達(dá)時(shí)間差(TDOA)、基于到達(dá)角度(AOA)、基于接收到信號(hào)強(qiáng)度指示(RSSI)4種[3-5]。這里采用TDOA作為UWB的定位方法。

到達(dá)時(shí)間差法TDOA的原理是通過(guò)測(cè)量不同基站接收同一個(gè)移動(dòng)站定位信號(hào)的時(shí)間差，從而計(jì)算出同一個(gè)移動(dòng)站到不同基站的距離差。知道移動(dòng)點(diǎn)到不同基站的距離差，就可以通過(guò)三邊定位法求得移動(dòng)點(diǎn)的位置。

UWB定位的基本原理采用三邊定位法[6]，已知三點(diǎn)位置(x1, y1)、(x2, y2)、(x3, y3)，已知未知點(diǎn)(x0, y0)到三點(diǎn)距離d1、d2、d3,以 d1、d2、d3為半徑作三個(gè)圓，根據(jù)畢達(dá)哥拉斯定理，得出交點(diǎn)即未知點(diǎn)的位置計(jì)算公式：

上式通過(guò)最小二乘法求解最小方差得到坐標(biāo)信息。

可以得到矩陣Hx=b,其中:

最終解為：x=(HTH)-1HTb。

2 系統(tǒng)總體架構(gòu)

本設(shè)計(jì)為UWB定位系統(tǒng)。系統(tǒng)以STM32F103C8為主控制器，使用DWM1000模塊進(jìn)行通信，使用TOF算法測(cè)得兩模塊之間的距離。本系統(tǒng)由一個(gè)標(biāo)簽和三個(gè)基站組成，在標(biāo)簽分別得到三個(gè)基站的距離后，通過(guò)三邊定位用最小二乘法求解得到坐標(biāo)。

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

3.1 UWB定位模塊設(shè)計(jì)

UWB定位模塊采用DWM1000模塊，可以通過(guò)控制相關(guān)寄存器實(shí)現(xiàn)工作狀態(tài)的切換；采用SPI總線(xiàn)與主控芯片接口，速率最高達(dá)20 MHz；具有很好的抗多徑干擾能力，非常適合室內(nèi)復(fù)雜環(huán)境的定位應(yīng)用。DWM1000模塊由收發(fā)器、模式變換器、狀態(tài)控制器、SPI接口電路組成，其原理圖如圖1所示。

圖1 DWM1000模塊原理圖

DWM1000模塊核心控制引腳有CS、SI、SO、SCLK，功能定義如表1所列，這里DWM1000模塊為從設(shè)備，STM32F103為主設(shè)備。

表1 DWM1000模塊核心控制引腳

3.2 主控制模塊設(shè)計(jì)

主控制模塊采用STM32F103C8芯片，工作頻率為72 MHz；具有36個(gè)快速I(mǎi)/O端口，所有I/O口均可以映像到16個(gè)外部中斷；配置有3個(gè)USART接口、1個(gè)CAN總線(xiàn)接口、2個(gè)I2C接口、2個(gè)SPI接口、2個(gè)I2S接口、1個(gè)USB接口；擁有2個(gè)12位模/數(shù)轉(zhuǎn)換器，可以實(shí)現(xiàn)單次或者多次掃描轉(zhuǎn)換；內(nèi)部包含8個(gè)16位的定時(shí)器；采用LQFP48封裝，具有體積小、易于集成的優(yōu)勢(shì)。

這里采用STM32F103C8的SPI總線(xiàn)接口實(shí)現(xiàn)對(duì)DWM1000模塊的控制，主控制模塊的原理圖如圖2所示。其中CS、SCLK、SO、SI為STM32F103C8的SPI數(shù)據(jù)總線(xiàn)；SW1、SW2、SW3、SW4為撥碼輸入，設(shè)置UWB定位模塊的節(jié)點(diǎn)號(hào)；RXD、TXD用來(lái)實(shí)現(xiàn)外接無(wú)線(xiàn)模塊，實(shí)現(xiàn)與上位機(jī)的無(wú)線(xiàn)通信；CAN_TX、CAN_RX為STM32F103C8的CAN總線(xiàn)接口，用來(lái)實(shí)現(xiàn)板級(jí)互聯(lián)；SWCLK、SWDIO用來(lái)實(shí)現(xiàn)控制程序的SWD方式下載。

圖2 STM32F103C8原理圖

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

系統(tǒng)軟件采用的開(kāi)發(fā)平臺(tái)為Keil公司的Keil5，STM32F103采用SPI總線(xiàn)控制UWB定位模塊DWM1000，主要軟件模塊有串口通信模塊、SPI通信模塊、時(shí)針模塊、中斷模塊、定時(shí)器模塊。STM32F103控制UWB定位模塊的SPI控制時(shí)序圖如圖3所示。

圖3 UWB模塊SPI控制時(shí)序圖

通過(guò)SPI時(shí)鐘極性(CPOL)和時(shí)鐘相位(CPHA)，STM32F103可以實(shí)現(xiàn)4種SPI模式。時(shí)鐘極性CPOL=0時(shí)，SCLK在空閑狀態(tài)為低電平；時(shí)鐘極性CPOL=1時(shí)，SCLK在空閑狀態(tài)為高電平。時(shí)鐘相位CPHA=0時(shí)，數(shù)據(jù)線(xiàn)信號(hào)會(huì)在SCLK的奇數(shù)邊沿采樣；時(shí)鐘相位CPHA=1時(shí)，數(shù)據(jù)線(xiàn)信號(hào)會(huì)在SCLK的偶數(shù)邊沿采樣。

這里采用時(shí)鐘極性CPOL=0，時(shí)鐘相位CPHA=0模式，SCLK在空閑狀態(tài)為低電平，數(shù)據(jù)線(xiàn)信號(hào)會(huì)在SCLK的奇數(shù)邊沿采樣。SPI總線(xiàn)單次傳輸8位，高位MSB在前，這個(gè)可以通過(guò)STM32F103的SPI初始化程序配置，配置程序如下：

SPI_InitStructure.SPI_Direction=SPI_Direction_2Lines_FullDuplex;

SPI_InitStructure.SPI_Mode=SPI_Mode_Master;

SPI_InitStructure.SPI_DataSize=SPI_DataSize_8b;

SPI_InitStructure.SPI_CPOL=SPI_CPOL_Low;

SPI_InitStructure.SPI_CPHA=SPI_CPHA_1Edge;

SPI_InitStructure.SPI_NSS=SPI_NSS_Soft;

SPI_InitStructure.SPI_BaudRatePrescaler=SPI_BaudRatePrescaler_32;

SPI_InitStructure.SPI_FirstBit=SPI_FirstBit_MSB;

SPI_InitStructure.SPI_CRCPolynomial=7;

SPI_Init(SPI1, &SPI_InitStructure);

其中SPI_Mode 設(shè)置STM32F103為主設(shè)備模式，SPI_DataSize 設(shè)置單次輸出8位，SPI_CPOL設(shè)置SPI時(shí)鐘極性為低電平，SPI_CPHA設(shè)置SPI為奇數(shù)邊采樣，SPI_FirstBit設(shè)置SPI通信為高位MSB在前。配置完這些SPI_InitStructure的結(jié)構(gòu)體成員后，只需要SPI_Init()函數(shù)就可以把這些參數(shù)寫(xiě)入寄存器中。

5 實(shí)驗(yàn)與結(jié)果

通過(guò)在室內(nèi)實(shí)驗(yàn)場(chǎng)地每隔2 m畫(huà)一個(gè)測(cè)試點(diǎn)，測(cè)試距離范圍為0～40 m，UWB定位節(jié)點(diǎn)安裝在移動(dòng)機(jī)器人上，測(cè)量到的數(shù)據(jù)通過(guò)板子里面的無(wú)線(xiàn)模塊發(fā)送到電腦，電腦采集定位數(shù)據(jù)并同時(shí)顯示在上位機(jī)界面，測(cè)試數(shù)據(jù)如表2所列。上位機(jī)界面通過(guò)C#軟件開(kāi)發(fā)。

提取到距離數(shù)據(jù)后，就可以通過(guò)相應(yīng)算法實(shí)現(xiàn)定位點(diǎn)坐標(biāo)測(cè)量，測(cè)試數(shù)據(jù)如表3所列，可以保證±5 cm的定位精度。

表2 測(cè)距測(cè)試數(shù)據(jù)

表3 定位測(cè)試數(shù)據(jù)

坐標(biāo)測(cè)試效果圖如圖4所示，○為設(shè)計(jì)坐標(biāo)點(diǎn)，*為系統(tǒng)定位結(jié)果。

圖4 UWB定位點(diǎn)測(cè)試效果圖

結(jié) 語(yǔ)

激光SLAM定位、視覺(jué)SLAM定位的計(jì)算復(fù)雜、傳感器成本過(guò)高、實(shí)時(shí)性差，本文針對(duì)這些缺陷，提出了通過(guò)UWB定位技術(shù)來(lái)實(shí)現(xiàn)室內(nèi)移動(dòng)機(jī)器人的定位，構(gòu)建了實(shí)際的軟硬件平臺(tái)，完成了相關(guān)的實(shí)驗(yàn)，驗(yàn)證了UWB定位技術(shù)在室內(nèi)移動(dòng)機(jī)器人方面的可行性。實(shí)驗(yàn)結(jié)果表明，本系統(tǒng)具有高穩(wěn)定性、高精度，可以很好地滿(mǎn)足室內(nèi)移動(dòng)機(jī)器人的定位要求。