基于UWB 和氣壓計的自動跟蹤定位系統研究

王波

（廣東省機械技師學院，廣東廣州，510450）

0 引言

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

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

基于UWB 和氣壓計的自動跟蹤定位系統，由于引進了氣壓計，可以很好地彌補UWB 定位時高度數據精度不夠的缺點，從而獲得高精度的三維定位數據。本系統設計了兩個部分：一個是融合了氣壓計的UWB 定位子系統，另外一個是二自由的射燈控制子系統。定位子系統可以實時獲取三維定位數據，定位數據進入到射燈控制子系統，控制射燈方向，實現舞臺移動人員的自動跟蹤。

1 系統整體方案設計

■1.1 系統整體框架

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

（1）定位子系統。該子系統是舞臺燈光自動跟蹤系統的關鍵所在，負責實時提供三維定位數據。該部分的核心主要包括：1 個PC 基站、4 個定位基站、若干個移動標簽（佩戴在移動人員身上）、氣壓計以及上位機系統等。其主要工作是測量移動標簽到A、B、C、D 四個定位基站的距離，并通過氣壓計獲取高度數據，最后移動標簽將相關數據返回給PC 基站。PC 基站與上位機連接，上位機收到PC 基站數據后，計算得到實時的三維定位數據，再將定位數據換算成角度值，下發至射燈控制子系統。移動標簽模塊安裝在舞臺移動人員身上，隨著人員移動位置不斷發生變化。

（2）射燈控制子系統。該子系統主要負責控制舞臺燈光對移動人員的精準自動跟蹤，確保燈光實時照射在移動人員身上。該部分的核心主要包括：STM32 主控制器、兩個步進電機及其驅動器。主控制設計主要負責4 路PWM 波形的輸出控制，根據上位機下發的角度數據，計算步進電機運行數據，射燈控制子系統實時控制射燈運動，實現射燈對舞臺移動人員的自動跟蹤。

■1.2 定位子系統硬件設計

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

圖2 定位子系統硬件設計原理圖

如圖3 所示，可以通過BUTTON_SET 和BUTTON_MODE 兩個按鍵，設置該模塊為定位基站或者移動標簽，并可以設置地址編號，并將設置好的參數在液晶屏上進行顯示，使用較為方便。

圖3 定位子系統用到的UWB 模塊

■1.3 射燈控制子系統硬件構成

本部分由兩個步進電機共同組成了一個二自由度的運動控制系統，其中一個步進電機控制射燈的水平旋轉（在此稱之為水平旋轉電機），一個電機控制射燈的俯仰擺動（在此稱之為俯仰擺動電機），可以滿足射燈在舞臺空間的全方位照射。射燈控制子系統的外觀及硬件構成如圖4 所示。

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

射燈控制子系統的主控制器選擇了STM32F107VCT6 芯片來實現。其核心部分是4 路PWM 波形的輸出控制。同時，設計了兩種通訊接口，一種是RS232 有線方式，另外一種是LORA 無線方式。可根據現場布線復雜程度和用戶需求，選擇不同的通訊方式。射燈控制子系統的主控制器部分原理圖如圖5所示。

圖5 射燈控制子系統的主控制器電路原理圖

2 定位算法的實現

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

移動標簽X，依次與A、B、C、D 四個定位基站進行測距后，可得到移動標簽X 到四個定位基站的距離：d1，d2，d3，d4。此時，需要根據三邊定位法，計算出唯一的交點作為標簽的位置坐標。三邊定位法原理如圖6 所示。

圖6 三邊定位法原理

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

在（1）式中，任意一個方程與最后一個方程進行減運算，轉換成矩陣的形式，結果如下：

最終的X即為二維平面定位數據，此時再結合氣壓計的高度數據，組成了三維空間定位。

■2.2 卡爾曼濾波算法

通過上述的三邊定位算法計算得到的維平面定位數據，其波動性較大。為了進一步優化定位數據，引入了卡爾曼濾波算法。系統的控制輸入和測量數據作為卡爾曼濾波估計狀態的關鍵，用TOF 測距算法得到的距離值與距離變化率可以作為系統狀態向量[7],得到卡爾曼濾波的測距模型為：

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

上式中，k和k-1 分別表示當前狀態和前一狀態，X為先驗估計的k時刻狀態矩陣，Zk為實際觀察量，Ak-1為狀態變換矩陣，Hk為實際觀測矩陣，Wk-1和Vk為噪聲協方差矩陣。Kk為卡爾曼增益，Pk|k-1預測協方差矩陣。當Pk|k-1增大時，Kk增大，Vk增大，隨之Kk相應減小。

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

■2.3 射燈控制子系統燈光追蹤算法

為了實現最終本射燈控制子系統燈光對移動人員的追蹤，系統對舞臺設備的安裝有一定的要求，舞臺布局及硬件安裝示意圖如圖7 所示。

圖7 舞臺布局及硬件安裝示意圖

其中：X 為移動標簽，A、B、C、D 為四個定位基站，基站安裝高度1 米以上。射燈安裝時水平旋轉軸與z 軸方向平行，確保該軸旋轉時，燈光始終是沿著y 軸方向左右平移，再配合制俯仰擺動軸的上下平移，即可追蹤舞臺三維空間的中任意位置點。具體的計算過程如下：

（1）移動標簽X，通過依次與四個定位基站測距定位得到四個距離數據d1，d2，d3，d4，如圖7 中的線段AX，BX，CX 和DX。

（2）通過三邊定位算法和卡爾曼濾波計算后，再結合氣壓計數據，可得到移動標簽X 較為穩定的三維坐標數據X(x,y,z)，如圖7 中的線段綠色線段x，y 和z。

（3）此時，可得到移動標簽X 坐標實為圖7 中矩形abcd 和矩形efgh 相交線ij 上的某個點X(x,y,z)，再結合舞臺燈安裝位置實際，將X(x,y,z)坐標數據換算為射燈控制子系統兩個步進電機的絕對角度數據ω1和ω2。

（4）控制水平旋轉步進電機旋轉至絕對角度數據ω1位置，可使得射燈在沿圖7 中y 軸方向平移到直線ij 上某個位置；再控制俯仰擺動步進電機擺動到絕對角度數據ω2位置,可使得射燈在沿圖7 中直線ij 方向上下擺動，最終使得燈光移動到直線ij 上移動標簽所在的實際位置X(x,y,z)上。

3 結論

針對傳統的舞臺燈光跟蹤控制手動方式實現所存在的缺點，設計了一種基于UWB 和氣壓計的自動跟蹤定位系統，為舞臺燈光實現自動跟蹤進行了探索和實踐。研究成果表明，基于UWB 和氣壓計的自動跟蹤定位系統，能夠實現舞臺燈光傳統手動控制的基本功能，也能夠滿足舞臺人員在低速移動過程實時性和準確性的要求，并且可以實現多個舞臺燈光對多名移動人員的精準自動跟蹤，很大程度上降低了相關行業的人力成本。