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

蔣偉楷

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

引言

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

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

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

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

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

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

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

1 基于UWB目標(biāo)定位原理

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

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

1.1 SDS-TWR測距法

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

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

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

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

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

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

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

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

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

1.2 基于CHAN算法的UWB定位

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

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

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

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

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

(2)

由式(2)可得

(3)

(4)

由文獻(xiàn)[12]可知za的最大似然估計為：

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

(5)

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

(6)

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

(7)

(8)

(9)

通過上述兩次加權(quán)最小二乘估計，移動標(biāo)簽最終的位置估計：

(10)

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

1.3 卡爾曼濾波

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

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

(11)

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

2 多燈多目標(biāo)跟蹤控制方法

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

2.1 舞臺多燈多目標(biāo)跟蹤

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

1)單臺燈跟蹤單個移動目標(biāo)。

2)多臺燈跟蹤單個移動目標(biāo)。

3)單臺燈跟蹤多個目標(biāo)。

4)多臺燈跟蹤多個目標(biāo)。

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

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

圖3 舞臺燈與移動標(biāo)簽坐標(biāo)關(guān)系Fig.3 Coordinate relationship between stage light and tag

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

(12)

由式(12)可知，步進(jìn)電機的掃描信號數(shù)值V水平、V垂直與移動標(biāo)簽的坐標(biāo)對應(yīng)關(guān)系為

(13)

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

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

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

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

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

2.2 移動目標(biāo)運動狀態(tài)預(yù)測

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

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

(14)

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

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

(15)

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

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

3 舞臺燈目標(biāo)跟蹤實驗

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

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

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

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

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

4 結(jié)論

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