舞臺多燈多目標自動跟蹤系統(tǒng)的研究與設計

蔣偉楷

(廣州市浩洋電子股份有限公司，廣東 廣州 511450)

引言

在演唱會、話劇、雜技等演藝活動中，為營造豐富的舞臺藝術效果，通常利用燈具對移動目標進行跟蹤追光。目前，舞臺燈追光主要通過人工手動控制、事先規(guī)劃和固定演員行走路線、速度等方法實現(xiàn)，不但演出成本高、費時費力，而且追光準確性、實時性難以滿足要求[1，2]。因此，采用定位技術實現(xiàn)舞臺燈光的自動跟蹤功能具有較重要的意義。

目前，全球衛(wèi)星定位系統(tǒng)(GNSS)應用室外定位效果較好[3]，但是衛(wèi)星信號無法穿透建筑物等遮擋物，當舞臺燈在室內(nèi)使用時無法利用GNSS進行定位。因此，選取室內(nèi)定位技術有利于擴大舞臺燈自動跟蹤功能的應用范圍。當前，應用成熟的室內(nèi)定位技術主要有：紅外線定位、Wi-Fi定位、藍牙定位、超聲波定位、ZigBee定位、超寬帶(Ultra Wide Band, UWB)定位、蜂窩移動網(wǎng)絡定位、慣性導航、地磁導航等[4]。其中，UWB技術具有定位精度高、功耗低、穿透性好、抗干擾能力強等優(yōu)點[5]。對UWB室內(nèi)定位技術的研究主要集中在以下三個方面[6]：

1)UWB定位的相關原理和算法研究，可分為基于到達時間/到達時間差(TOA/TDOA)定位[7]、基于信號角度(AOA)定位[8]、基于信號強度(RSS)定位[9]等。

2)UWB信道傳播建模，主要采用時域和頻域測量方法。

3)定位誤差建模，在非視距環(huán)境下定位技術的研究成為一個熱點[10]。

文獻[1]和[2]中選取UWB定位技術設計了舞臺自動跟蹤系統(tǒng)并取得一定的成效。但是，二者的定位系統(tǒng)均采用基于TOA的4基站三邊/三維定位算法：將3個基站作為幾何球心，基站與標簽之間的距離作為半徑求解球的交點，經(jīng)解算后得到兩個對稱交點，選擇與第4個基站距離最近的交點作為移動目標的最佳坐標。由于測距誤差的存在，當基站數(shù)量大于4個時，該算法得到的移動目標定位解是一個區(qū)域，偏差較大[11]。另外，由于三邊/三維定位算法要求3個基站不能共線，這限制了舞臺基站布局的方便性和靈活性。在實際舞臺應用中，當舞臺場地較大、場景較復雜時一般需要布置多于4個基站才能滿足信號覆蓋要求。因此，三邊/三維定位算法在實際舞臺追光應用中存在局限性，實用化水平較低。

為解決上述問題，本文采用UWB定位技術和基于TDOA的Chan和Ho[12]定位算法，提高舞臺多燈多目標自動跟蹤系統(tǒng)的實用化水平。當基站數(shù)量大于4個時，CHAN定位算法利用超定方程組的冗余數(shù)據(jù)進行加權最小二乘(WLS)計算，能獲得精準的移動目標定位估計[13]，其估計結果的誤差達到克拉美羅界。另外，CHAN算法是對雙曲線方程組進行解算，多個基站可以共線，布局較方便。本文設計的舞臺自動跟蹤系統(tǒng)靜態(tài)定位精度高，動態(tài)定位跟蹤實時性好，能夠達到我司客戶關于舞臺自動追光場景對追光準確性、實時性的要求標準，有較好的應用價值。

1 基于UWB目標定位原理

為實現(xiàn)UWB定位跟蹤功能，首先要測量移動標簽與各個基站的距離，再求解移動標簽的定位坐標。本系統(tǒng)將舞臺燈、基站和移動標簽的位置坐標建立在同一個坐標系內(nèi)。為避免追光過程中出現(xiàn)非視距現(xiàn)象，基站的安裝位置要比移動標簽的位置高，同時，演員需將移動標簽佩戴在頭頂上。

當用戶通過上位機開啟自動追光模式之后，上位機實時接收來自主基站的測距數(shù)據(jù)，首先由CHAN算法計算出移動標簽的坐標，利用卡爾曼濾波對坐標數(shù)據(jù)進行優(yōu)化，再通過舞臺燈與標簽之間的坐標關系，得出舞臺燈步進電機需要轉動的水平、垂直掃描信號數(shù)值，最后通過DMX512控制器發(fā)送DMX數(shù)據(jù)控制舞臺燈轉動到相應位置，將燈光投射到移動目標身上實現(xiàn)追光。自動追光實時連續(xù)地運行，直至用戶通過上位機退出自動追光模式。

1.1 SDS-TWR測距法

通過測量電磁波信號在空中傳播的時間并乘以光速可計算出兩個設備之間的距離。在沒有公共時鐘的情況下，通信雙方時鐘難以嚴格同步，而對稱雙邊雙向測距法(symmetrical double sided two way ranging，SDS-TWR)通過在通信雙方各測量一次來避免時鐘漂移對測距帶來的影響，可以消除計時誤差和同步時延[14]。

SDS-TWR測距流程如圖1所示。

1)在時刻t1，設備a向設備b發(fā)出測距包。

2)電磁波信號在空中傳播，經(jīng)過時間Tf之后在時刻t2被設備b收到。

3)設備b經(jīng)過時延Tbreply后，在時刻t3發(fā)出應答數(shù)據(jù)包，其中，Tbreply=t3-t2。

4)經(jīng)過時間Tf后設備a在時刻t4收到應答包，第一次測量完成，所用時間Taround=t4-t1。

5)設備a經(jīng)過時延Tareply后，在時刻t5發(fā)出結束包，其中Tareply=t5-t4。

6)經(jīng)過時間Tf后設備b在時刻t6收到結束包，第二次測量完成，所用時間Tbround=t6-t3。SDS-TWR測距雙方記錄下t1～t6各個時刻，由這些時刻之差求解出信號在空中的飛行時間Tf。兩個設備之間的距離S等于Tf乘以光速c，如式(1)所示。

圖1 對稱雙邊雙向測距(SDS-TWR)流程Fig.1 Symmetrical double sided two-way ranging process

1.2 基于CHAN算法的UWB定位

為適用和滿足大部分的舞臺場景需求，本文的舞臺多燈多目標自動跟蹤測試系統(tǒng)由6臺電腦搖頭燈L1～L6、8個UWB基站BSi(i∈[1，8])和3個移動標簽Pm(m∈[1，3])組成?；尽撕灪臀枧_燈在舞臺三維空間的位置分布示意圖如圖2所示。

圖2 系統(tǒng)空間分布示意圖Fig.2 Schematic diagram of the system in space

在自動追光場景中，上位機首先通過UWB定位系統(tǒng)確定移動標簽Pm的坐標位置，再控制舞臺燈水平、垂直轉動，將燈光投射到被跟蹤目標身上，實現(xiàn)跟蹤追光。

為確定移動標簽Pm的坐標，上位機通過TCP/IP網(wǎng)絡接口與主基站BS1連接，實時接收來自BS1的數(shù)據(jù)包并從中提取Pm與各個基站BSi的距離信息，計算出Pm到各個基站之間的距離之差，構成一組基于TDOA的雙曲線方程組，通過CHAN算法求解該方程組的最優(yōu)估計值，即可確定Pm的坐標。

假設舞臺自動跟蹤系統(tǒng)僅對一個移動標簽進行定位，移動標簽的坐標zp=[x，y]T為未知量，第i個基站的坐標(xi，yi)為已知量，移動標簽與第i個基站的測量距離為ri，移動標簽到基站i、基站1的距離之差為ri,1，由幾何關系得到如下方程組：

(2)

由式(2)可得

(3)

(4)

由文獻[12]可知za的最大似然估計為：

za=arg min{(h-)TΨ-1(h-Gaza)}

(5)

首先，假設za各個元素之間相互獨立。移動標簽的初始解可通過式(6)求出：

(6)

用初始解構造矩陣B，并求出誤差的協(xié)方差矩陣Ψ，代入式(5)得到第一次加權最小二乘估計：

(7)

(8)

(9)

通過上述兩次加權最小二乘估計，移動標簽最終的位置估計：

(10)

CHAN定位算法的精度與環(huán)境有關：在理想視距LOS環(huán)境下，當噪聲服從理想高斯分布時TDOA的誤差較小，得到的定位精度較高；在非視距NLOS環(huán)境下，TDOA的誤差較大，求解得到的定位精度較差[15]。

1.3 卡爾曼濾波

在舞臺自動跟蹤系統(tǒng)中，UWB定位精度受到各種環(huán)境噪聲的干擾。為降低噪聲對定位精度的影響，采用卡爾曼濾波對噪聲進行處理。

卡爾曼濾波算法是一種基于狀態(tài)方程、觀測方程對系統(tǒng)狀態(tài)進行最優(yōu)估計的濾波算法，具有線性無偏最小均方差的特點[16]?？柭鼮V波的性能在很大程度上取決于系統(tǒng)的建模方式[17]。系統(tǒng)在t時刻的卡爾曼濾波模型可用式(11)表示[18]：

(11)

其中，X(t|t-1)是t時刻的系統(tǒng)狀態(tài)預測；P(t|t-1)是t時刻的系統(tǒng)狀態(tài)協(xié)方差預測；Kg為卡爾曼增益；X(t|t)是t時刻的系統(tǒng)狀態(tài)估計結果；Z(t)是t時刻的觀測結果；P(t|t)是t時刻的系統(tǒng)狀態(tài)協(xié)方差估計結果；U(t)是t時刻系統(tǒng)的控制量；A和B是系統(tǒng)參數(shù)矩陣；H是系統(tǒng)觀測矩陣；Q、R是系統(tǒng)過程噪聲、觀測噪聲協(xié)方差，兩者都是均值為0的高斯白噪聲；I為單位矩陣。為了減少噪聲對系統(tǒng)的影響，在舞臺燈自動跟蹤系統(tǒng)中采用卡爾曼濾波對移動標簽的坐標數(shù)據(jù)進行濾波處理。

2 多燈多目標跟蹤控制方法

為實現(xiàn)舞臺燈自動追光控制，在得到標簽的定位坐標之后，首先根據(jù)標簽坐標與燈具坐標之間的關系計算出舞臺燈要轉動的角度，再控制舞臺燈光轉動到相應位置，實現(xiàn)追光。由于移動目標運動狀態(tài)、運行軌跡變化不定，為確保舞臺燈平穩(wěn)、實時進行追光，本系統(tǒng)采用預測算法和PID算法對跟蹤過程進行控制優(yōu)化。

2.1 舞臺多燈多目標跟蹤

根據(jù)追光場景需求可分為以下四種跟蹤方式：

1)單臺燈跟蹤單個移動目標。

2)多臺燈跟蹤單個移動目標。

3)單臺燈跟蹤多個目標。

4)多臺燈跟蹤多個目標。

為了實現(xiàn)多燈多目標跟蹤控制，首先研究單燈單目標跟蹤控制方法，然后在此基礎上進一步研究多燈多目標的跟蹤控制方法。

1)對于單臺燈跟蹤單個移動目標的場景，需要建立該燈的步進電機在水平、垂直方向上掃描信號數(shù)值V水平、V垂直與移動標簽坐標x、y之間的對應關系，如圖3所示。

圖3 舞臺燈與移動標簽坐標關系Fig.3 Coordinate relationship between stage light and tag

假設舞臺燈在水平、垂直方向總轉動角度分別為540°、270°，行程分辨率16位，共65 536等分，舞臺燈與移動標簽在水平、垂直方向的夾角分別為α、β。舞臺燈坐標(Lx,Ly,Lz)為已知量，移動標簽的坐標P(x,y,0)通過UWB定位求出。由幾何關系可確定舞臺燈轉動角度、β與移動標簽坐標關系：

(12)

由式(12)可知，步進電機的掃描信號數(shù)值V水平、V垂直與移動標簽的坐標對應關系為

(13)

因此，求出V水平、V垂直之后通過驅動步進電機將舞臺燈轉動到指定的位置，即可實現(xiàn)追光功能。

2)對于多臺燈跟蹤單個移動目標的場景，需建立每臺燈的V水平、V垂直與移動標簽坐標之間的對應關系。在追光過程中每臺燈的V水平、V垂直均單獨求出。

3)對于單臺燈跟蹤多個移動目標的場景，系統(tǒng)需要用戶事先輸入跟蹤順序，然后按照該順序依次切換并建立該燈的V水平、V垂直與當前被追光的移動標簽坐標之間的對應關系。上位機根據(jù)編號順序控制舞臺燈對目標輪流追光。

為了確保多個標簽同時工作時不相互干擾，需要把標簽設備配置成不同地址。UWB定位系統(tǒng)的標簽與基站之間采用時分多址(TDMA)通信技術，有效防止了不同標簽設備之間的通信干擾。

4)對于多臺燈跟蹤多個目標的場景，每臺燈各自跟蹤一個或一個以上的目標。在追光過程中，系統(tǒng)建立每臺燈的V水平、V垂直與各自跟蹤的移動標簽坐標之間的一一對應關系。當被跟蹤的目標越多，在單位時間內(nèi)基站要處理的標簽測距請求越多，系統(tǒng)的運算復雜度越高。為保證追光的準確性和實時性，多燈跟蹤多個目標的場景對硬件性能的要求比其他場景的更高。本場景中，由于多目標之間的距離關系無法預知，當目標之間距離過近時可能會存在遮擋導致非視距現(xiàn)象，降低定位精度。

2.2 移動目標運動狀態(tài)預測

由于被跟蹤目標的運動狀態(tài)、運動軌跡變化不定，單純的定位跟蹤控制常常造成追光滯后。為了實時跟蹤移動目標，需要對目標的下一步運動狀態(tài)做出預測。

假設移動標簽在很小的時間片段內(nèi)處于勻速直線運動狀態(tài)，則根據(jù)移動標簽的運動速度可提前預測該時間片段內(nèi)目標的位置，從而提高系統(tǒng)的響應速度，控制舞臺燈光實時地跟蹤移動標簽。本系統(tǒng)以0.1 s為采樣周期，先由標簽的當前坐標(Xcurrent,Ycurrent)、前一個坐標(Xlast,Ylast)求出標簽在X、Y方向的運動速度，再預測0.5 s后標簽可能的坐標(Xnext,Ynext)，如式(14)所示：

(14)

2.3 舞臺追光過程PID控制優(yōu)化

為確保舞臺燈在追光過程中步進電機運行的穩(wěn)定性，避免抖動引起燈光晃動，利用PID算法優(yōu)化舞臺燈步進電機的運行和位置控制。在第k次采樣時位置式PID控制如式(15)所示[19]：

(15)

其中，u(k)表示輸出值，ek表示偏差值，Kp、Ki和Kd分別代表比例、積分和微分系數(shù),u0表示PID控制開始時對應的初值。

在舞臺燈追光過程中，根據(jù)演員的移動速度修改比例系數(shù)，微分系數(shù)可以減少至0，去掉微分環(huán)節(jié)的作用，減少超前調(diào)節(jié)給系統(tǒng)帶來的不穩(wěn)定性。

3 舞臺燈目標跟蹤實驗

本文的舞臺自動跟蹤系統(tǒng)選用DecaWave公司的DW1000芯片作為UWB測距模塊?；咀鴺瞬捎肅EM公司LDM-100型激光測距儀進行觀測測量，其測量精度為±1.5 mm。為滿足大部分舞臺追光場景的應用需求，本文采用6臺電腦搖頭燈、8個基站、3個移動標簽(目標)進行測試驗證。8個基站安裝在長約為25 m、寬約為10 m的長方形舞臺正上方，距離地面高度約為5 m。在舞臺自動跟蹤的四個應用場景中，單燈跟蹤單目標、多燈跟蹤單目標、單燈跟蹤多目標都是單一的功能應用，而較復雜的多燈跟蹤多目標場景由前三種場景組合構成。因此，本文重點對多燈跟蹤多目標的場景進行測試驗證。

為測試系統(tǒng)在靜態(tài)條件下的定位精度，將3個標簽靜止放置于地面，6臺燈分別跟蹤各自標簽。對多次實驗的數(shù)據(jù)進行處理，結果如圖4(a)、(b)所示。從圖4(a)可看出，基于TDOA的CHAN算法三個基站之間可以共線，在基站布局靈活性方面比三邊/三維定位算法高。從圖4(b)可看出，通過CHAN定位算法和卡爾曼濾波處理得到移動標簽坐標位置的估計結果與實際觀測值的均方根誤差(RMSE)小于20 cm。因此，UWB靜態(tài)定位精度誤差<±20 cm，能夠滿足舞臺追光對定位精度的要求。實驗過程中，6臺燈的光斑均能準確地投射到各自跟蹤目標的標簽上。

圖4 三個標簽靜態(tài)定位測試Fig.4 Static positioning test of three tags

為驗證卡爾曼濾波對系統(tǒng)動態(tài)定位的影響，由三個測試人員在頭頂各佩戴一個標簽進行多目標自動追光實驗。測試人員以1 m/s的速度分別沿不同路線勻速前進。對多次實驗的數(shù)據(jù)進行處理，得到有、無卡爾曼濾波算法的動態(tài)軌跡曲線，如圖5(a)、(b)所示?？梢?，在大約12 s的跟蹤時間內(nèi)，無濾波的算法得到的3目標動態(tài)軌跡曲線有較多的毛刺，曲線不光滑，說明UWB定位過程中受到環(huán)境噪聲影響，而僅通過CHAN定位算法無法消除這些噪聲的影響；有卡爾曼濾波的算法得到的3目標動態(tài)軌跡曲線毛刺較少，曲線較光滑，說明卡爾曼濾波有效降低了環(huán)境噪聲的影響，提高了動態(tài)定位的精度，而光滑的動態(tài)定位曲線有利于提高自動追光過程的順滑效果和穩(wěn)定性。實驗過程中，6臺燈的燈光均能投射到各自跟蹤的測試目標(人員)身上，在測試目標(人員)移動過程中投射的光斑均能無滯后、無超前且無抖動地平穩(wěn)跟隨測試目標(人員)，保證測試目標(人員)始終處于投射光斑范圍內(nèi)，達到我司客戶關于舞臺多燈多目標自動追光準確性、實時性的要求標準，滿足了客戶需求。

圖5 三個目標動態(tài)定位軌跡曲線Fig.5 Dynamic positioning trajectory curve of three targets

4 結論

本文設計的舞臺多燈多目標自動跟蹤系統(tǒng)，采用基于CHAN算法的UWB室內(nèi)定位技術，提高舞臺自動跟蹤系統(tǒng)的實用化水平；通過卡爾曼濾波對多目標的坐標數(shù)據(jù)進行優(yōu)化，提高系統(tǒng)的靜態(tài)、動態(tài)定位精度。實驗結果表明，在視距條件下，3個標簽靜態(tài)定位的均方根誤差小于20 cm，滿足舞臺追光對定位精度的要求；以大約1 m/s勻速行走約12 s，3個移動目標動態(tài)定位軌跡曲線毛刺較少，曲線較光滑。在單燈跟蹤單目標、單燈跟蹤多目標、多燈跟蹤單目標和多燈跟蹤多目標這4種追光場景實驗中，舞臺燈光均能無滯后、無超前且無抖動地平穩(wěn)跟隨測試人員，保證測試人員始終處于投射光斑范圍內(nèi)。因此，本系統(tǒng)能夠滿足關于舞臺燈自動追光準確性、實時性的要求，解決了舞臺燈人工追光費時費力的問題，提高了舞臺燈光的自動化控制程度，具有較好的應用前景。