基于UWB 和氣壓計(jì)的自動(dòng)跟蹤定位系統(tǒng)研究

王波

（廣東省機(jī)械技師學(xué)院，廣東廣州，510450）

0 引言

燈光作為舞臺(tái)藝術(shù)中不可或缺部分，舞臺(tái)燈光對(duì)移動(dòng)人員的跟蹤可以創(chuàng)造更好的舞臺(tái)環(huán)境，營(yíng)造更好的舞臺(tái)氛圍，對(duì)舞臺(tái)人員的表演和情感的變化，都起到十分重要。然而，傳統(tǒng)的舞臺(tái)燈光跟蹤控制方法長(zhǎng)期停留在人工操作的層面，主要有工手動(dòng)操作和遙控操作這兩種手段[1,2]。隨著舞臺(tái)燈光技術(shù)的不斷發(fā)展，上述的手段顯然已無(wú)法經(jīng)濟(jì)高效率地滿足舞臺(tái)燈光對(duì)移動(dòng)人員的跟蹤要求。因此開(kāi)發(fā)一種低成本、高效率、高精度的舞臺(tái)燈光自動(dòng)跟蹤定位系統(tǒng)，對(duì)舞臺(tái)渲染效果的提高和氣氛的營(yíng)造，增強(qiáng)舞臺(tái)燈光行業(yè)企業(yè)的進(jìn)一步向前發(fā)展具有重要意義。

目前主流的室內(nèi)定位技術(shù)包括藍(lán)牙、WIFI、Zigbee、RFID 和UWB 等[3,4]，通過(guò)以上這幾種技術(shù)中的一種或多種技術(shù)的融合構(gòu)建室內(nèi)定位系統(tǒng)。超寬帶技術(shù)（Ultra Wide Band，UWB）是無(wú)線電通信技術(shù)的一種新方向，被認(rèn)為是當(dāng)今最具發(fā)展的通信技術(shù)之一[5]。UWB 帶寬極大，具備短距離、高速率的數(shù)據(jù)傳輸能力，具備時(shí)間分辨率高、抗多徑干擾能力強(qiáng)和信道衰落不敏感的特點(diǎn)[6]，因此在室內(nèi)大型倉(cāng)儲(chǔ)、智能車庫(kù)、導(dǎo)航購(gòu)物等室內(nèi)定位領(lǐng)域應(yīng)用廣泛。UWB 定位技術(shù)可以實(shí)現(xiàn)高精度的二維定位，但是在三維定位時(shí)其高度數(shù)據(jù)的精度就差強(qiáng)人意，往往不能滿足實(shí)際應(yīng)用需求，而且為獲取UWB 定位的高度數(shù)據(jù)，對(duì)UWB 基站的安裝也提出了很高的要求，大大增加了定位系統(tǒng)搭建的成本。

基于UWB 和氣壓計(jì)的自動(dòng)跟蹤定位系統(tǒng)，由于引進(jìn)了氣壓計(jì)，可以很好地彌補(bǔ)UWB 定位時(shí)高度數(shù)據(jù)精度不夠的缺點(diǎn)，從而獲得高精度的三維定位數(shù)據(jù)。本系統(tǒng)設(shè)計(jì)了兩個(gè)部分：一個(gè)是融合了氣壓計(jì)的UWB 定位子系統(tǒng)，另外一個(gè)是二自由的射燈控制子系統(tǒng)。定位子系統(tǒng)可以實(shí)時(shí)獲取三維定位數(shù)據(jù)，定位數(shù)據(jù)進(jìn)入到射燈控制子系統(tǒng)，控制射燈方向，實(shí)現(xiàn)舞臺(tái)移動(dòng)人員的自動(dòng)跟蹤。

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

■1.1 系統(tǒng)整體框架

基于UWB 和氣壓計(jì)的自動(dòng)跟蹤定位系統(tǒng)由定位子系統(tǒng)和射燈控制子系統(tǒng)兩部分組成。系統(tǒng)整體框架如圖1 所示。

（1）定位子系統(tǒng)。該子系統(tǒng)是舞臺(tái)燈光自動(dòng)跟蹤系統(tǒng)的關(guān)鍵所在，負(fù)責(zé)實(shí)時(shí)提供三維定位數(shù)據(jù)。該部分的核心主要包括：1 個(gè)PC 基站、4 個(gè)定位基站、若干個(gè)移動(dòng)標(biāo)簽（佩戴在移動(dòng)人員身上）、氣壓計(jì)以及上位機(jī)系統(tǒng)等。其主要工作是測(cè)量移動(dòng)標(biāo)簽到A、B、C、D 四個(gè)定位基站的距離，并通過(guò)氣壓計(jì)獲取高度數(shù)據(jù)，最后移動(dòng)標(biāo)簽將相關(guān)數(shù)據(jù)返回給PC 基站。PC 基站與上位機(jī)連接，上位機(jī)收到PC 基站數(shù)據(jù)后，計(jì)算得到實(shí)時(shí)的三維定位數(shù)據(jù)，再將定位數(shù)據(jù)換算成角度值，下發(fā)至射燈控制子系統(tǒng)。移動(dòng)標(biāo)簽?zāi)K安裝在舞臺(tái)移動(dòng)人員身上，隨著人員移動(dòng)位置不斷發(fā)生變化。

（2）射燈控制子系統(tǒng)。該子系統(tǒng)主要負(fù)責(zé)控制舞臺(tái)燈光對(duì)移動(dòng)人員的精準(zhǔn)自動(dòng)跟蹤，確保燈光實(shí)時(shí)照射在移動(dòng)人員身上。該部分的核心主要包括：STM32 主控制器、兩個(gè)步進(jìn)電機(jī)及其驅(qū)動(dòng)器。主控制設(shè)計(jì)主要負(fù)責(zé)4 路PWM 波形的輸出控制，根據(jù)上位機(jī)下發(fā)的角度數(shù)據(jù)，計(jì)算步進(jìn)電機(jī)運(yùn)行數(shù)據(jù)，射燈控制子系統(tǒng)實(shí)時(shí)控制射燈運(yùn)動(dòng)，實(shí)現(xiàn)射燈對(duì)舞臺(tái)移動(dòng)人員的自動(dòng)跟蹤。

■1.2 定位子系統(tǒng)硬件設(shè)計(jì)

定位子系統(tǒng)模塊的主控制器采用ST公司的STM32F103C8T6芯片，超寬帶收發(fā)器芯片采用DecaWave 公司DWM1000模塊，該模塊集成了射頻無(wú)線電設(shè)計(jì)所必需的 DW1000 芯片、天線、電源管理和時(shí)鐘電路等器件。定位子系統(tǒng)模塊主控芯片及超寬帶收發(fā)器芯片引腳分配和外圍器件設(shè)計(jì)如圖2所示。

圖2 定位子系統(tǒng)硬件設(shè)計(jì)原理圖

如圖3 所示，可以通過(guò)BUTTON_SET 和BUTTON_MODE 兩個(gè)按鍵，設(shè)置該模塊為定位基站或者移動(dòng)標(biāo)簽，并可以設(shè)置地址編號(hào)，并將設(shè)置好的參數(shù)在液晶屏上進(jìn)行顯示，使用較為方便。

圖3 定位子系統(tǒng)用到的UWB 模塊

■1.3 射燈控制子系統(tǒng)硬件構(gòu)成

本部分由兩個(gè)步進(jìn)電機(jī)共同組成了一個(gè)二自由度的運(yùn)動(dòng)控制系統(tǒng)，其中一個(gè)步進(jìn)電機(jī)控制射燈的水平旋轉(zhuǎn)（在此稱之為水平旋轉(zhuǎn)電機(jī)），一個(gè)電機(jī)控制射燈的俯仰擺動(dòng)（在此稱之為俯仰擺動(dòng)電機(jī)），可以滿足射燈在舞臺(tái)空間的全方位照射。射燈控制子系統(tǒng)的外觀及硬件構(gòu)成如圖4 所示。

圖4 二自由度射燈外觀及其主控制器硬件

射燈控制子系統(tǒng)的主控制器選擇了STM32F107VCT6 芯片來(lái)實(shí)現(xiàn)。其核心部分是4 路PWM 波形的輸出控制。同時(shí)，設(shè)計(jì)了兩種通訊接口，一種是RS232 有線方式，另外一種是LORA 無(wú)線方式?？筛鶕?jù)現(xiàn)場(chǎng)布線復(fù)雜程度和用戶需求，選擇不同的通訊方式。射燈控制子系統(tǒng)的主控制器部分原理圖如圖5所示。

圖5 射燈控制子系統(tǒng)的主控制器電路原理圖

2 定位算法的實(shí)現(xiàn)

■2.1 UWB 定位的三邊定位算法

移動(dòng)標(biāo)簽X，依次與A、B、C、D 四個(gè)定位基站進(jìn)行測(cè)距后，可得到移動(dòng)標(biāo)簽X 到四個(gè)定位基站的距離：d1，d2，d3，d4。此時(shí)，需要根據(jù)三邊定位法，計(jì)算出唯一的交點(diǎn)作為標(biāo)簽的位置坐標(biāo)。三邊定位法原理如圖6 所示。

圖6 三邊定位法原理

假設(shè)4 個(gè)定位基站的位置坐標(biāo)為：A(x1,y1)，B(x2,y2)，C(x3,y3)，D(x4,y4)，采用TOF 方式進(jìn)行測(cè)距，并分別記標(biāo)簽與定位基站的距離為：d1，d2，d3，d4，整合后得到如下的方程組：

在（1）式中，任意一個(gè)方程與最后一個(gè)方程進(jìn)行減運(yùn)算，轉(zhuǎn)換成矩陣的形式，結(jié)果如下：

最終的X即為二維平面定位數(shù)據(jù)，此時(shí)再結(jié)合氣壓計(jì)的高度數(shù)據(jù)，組成了三維空間定位。

■2.2 卡爾曼濾波算法

通過(guò)上述的三邊定位算法計(jì)算得到的維平面定位數(shù)據(jù)，其波動(dòng)性較大。為了進(jìn)一步優(yōu)化定位數(shù)據(jù)，引入了卡爾曼濾波算法。系統(tǒng)的控制輸入和測(cè)量數(shù)據(jù)作為卡爾曼濾波估計(jì)狀態(tài)的關(guān)鍵，用TOF 測(cè)距算法得到的距離值與距離變化率可以作為系統(tǒng)狀態(tài)向量[7],得到卡爾曼濾波的測(cè)距模型為：

卡爾曼濾波的增益矩陣如下：

上式中，k和k-1 分別表示當(dāng)前狀態(tài)和前一狀態(tài)，X為先驗(yàn)估計(jì)的k時(shí)刻狀態(tài)矩陣，Zk為實(shí)際觀察量，Ak-1為狀態(tài)變換矩陣，Hk為實(shí)際觀測(cè)矩陣，Wk-1和Vk為噪聲協(xié)方差矩陣。Kk為卡爾曼增益，Pk|k-1預(yù)測(cè)協(xié)方差矩陣。當(dāng)Pk|k-1增大時(shí)，Kk增大，Vk增大，隨之Kk相應(yīng)減小。

由三邊定位法直接解算的定位數(shù)據(jù)誤差較大，引入了卡爾曼濾波之，經(jīng)過(guò)多次反復(fù)實(shí)驗(yàn)，位置誤差基本上保持在±10cm 范圍，可以明顯看到卡爾曼濾波很大程度上提高了定位的精度。

■2.3 射燈控制子系統(tǒng)燈光追蹤算法

為了實(shí)現(xiàn)最終本射燈控制子系統(tǒng)燈光對(duì)移動(dòng)人員的追蹤，系統(tǒng)對(duì)舞臺(tái)設(shè)備的安裝有一定的要求，舞臺(tái)布局及硬件安裝示意圖如圖7 所示。

圖7 舞臺(tái)布局及硬件安裝示意圖

其中：X 為移動(dòng)標(biāo)簽，A、B、C、D 為四個(gè)定位基站，基站安裝高度1 米以上。射燈安裝時(shí)水平旋轉(zhuǎn)軸與z 軸方向平行，確保該軸旋轉(zhuǎn)時(shí)，燈光始終是沿著y 軸方向左右平移，再配合制俯仰擺動(dòng)軸的上下平移，即可追蹤舞臺(tái)三維空間的中任意位置點(diǎn)。具體的計(jì)算過(guò)程如下：

（1）移動(dòng)標(biāo)簽X，通過(guò)依次與四個(gè)定位基站測(cè)距定位得到四個(gè)距離數(shù)據(jù)d1，d2，d3，d4，如圖7 中的線段AX，BX，CX 和DX。

（2）通過(guò)三邊定位算法和卡爾曼濾波計(jì)算后，再結(jié)合氣壓計(jì)數(shù)據(jù)，可得到移動(dòng)標(biāo)簽X 較為穩(wěn)定的三維坐標(biāo)數(shù)據(jù)X(x,y,z)，如圖7 中的線段綠色線段x，y 和z。

（3）此時(shí)，可得到移動(dòng)標(biāo)簽X 坐標(biāo)實(shí)為圖7 中矩形abcd 和矩形efgh 相交線ij 上的某個(gè)點(diǎn)X(x,y,z)，再結(jié)合舞臺(tái)燈安裝位置實(shí)際，將X(x,y,z)坐標(biāo)數(shù)據(jù)換算為射燈控制子系統(tǒng)兩個(gè)步進(jìn)電機(jī)的絕對(duì)角度數(shù)據(jù)ω1和ω2。

（4）控制水平旋轉(zhuǎn)步進(jìn)電機(jī)旋轉(zhuǎn)至絕對(duì)角度數(shù)據(jù)ω1位置，可使得射燈在沿圖7 中y 軸方向平移到直線ij 上某個(gè)位置；再控制俯仰擺動(dòng)步進(jìn)電機(jī)擺動(dòng)到絕對(duì)角度數(shù)據(jù)ω2位置,可使得射燈在沿圖7 中直線ij 方向上下擺動(dòng)，最終使得燈光移動(dòng)到直線ij 上移動(dòng)標(biāo)簽所在的實(shí)際位置X(x,y,z)上。

3 結(jié)論

針對(duì)傳統(tǒng)的舞臺(tái)燈光跟蹤控制手動(dòng)方式實(shí)現(xiàn)所存在的缺點(diǎn)，設(shè)計(jì)了一種基于UWB 和氣壓計(jì)的自動(dòng)跟蹤定位系統(tǒng)，為舞臺(tái)燈光實(shí)現(xiàn)自動(dòng)跟蹤進(jìn)行了探索和實(shí)踐。研究成果表明，基于UWB 和氣壓計(jì)的自動(dòng)跟蹤定位系統(tǒng)，能夠?qū)崿F(xiàn)舞臺(tái)燈光傳統(tǒng)手動(dòng)控制的基本功能，也能夠滿足舞臺(tái)人員在低速移動(dòng)過(guò)程實(shí)時(shí)性和準(zhǔn)確性的要求，并且可以實(shí)現(xiàn)多個(gè)舞臺(tái)燈光對(duì)多名移動(dòng)人員的精準(zhǔn)自動(dòng)跟蹤，很大程度上降低了相關(guān)行業(yè)的人力成本。