舞台多灯多目标自动跟踪系统的研究与设计

蒋伟楷

(广州市浩洋电子股份有限公司，广东 广州 511450)

引言

在演唱会、话剧、杂技等演艺活动中，为营造丰富的舞台艺术效果，通常利用灯具对移动目标进行跟踪追光。目前，舞台灯追光主要通过人工手动控制、事先规划和固定演员行走路线、速度等方法实现，不但演出成本高、费时费力，而且追光准确性、实时性难以满足要求[1，2]。因此，采用定位技术实现舞台灯光的自动跟踪功能具有较重要的意义。

目前，全球卫星定位系统(GNSS)应用室外定位效果较好[3]，但是卫星信号无法穿透建筑物等遮挡物，当舞台灯在室内使用时无法利用GNSS进行定位。因此，选取室内定位技术有利于扩大舞台灯自动跟踪功能的应用范围。当前，应用成熟的室内定位技术主要有：红外线定位、Wi-Fi定位、蓝牙定位、超声波定位、ZigBee定位、超宽带(Ultra Wide Band, UWB)定位、蜂窝移动网络定位、惯性导航、地磁导航等[4]。其中，UWB技术具有定位精度高、功耗低、穿透性好、抗干扰能力强等优点[5]。对UWB室内定位技术的研究主要集中在以下三个方面[6]：

1)UWB定位的相关原理和算法研究，可分为基于到达时间/到达时间差(TOA/TDOA)定位[7]、基于信号角度(AOA)定位[8]、基于信号强度(RSS)定位[9]等。

2)UWB信道传播建模，主要采用时域和频域测量方法。

3)定位误差建模，在非视距环境下定位技术的研究成为一个热点[10]。

文献[1]和[2]中选取UWB定位技术设计了舞台自动跟踪系统并取得一定的成效。但是，二者的定位系统均采用基于TOA的4基站三边/三维定位算法：将3个基站作为几何球心，基站与标签之间的距离作为半径求解球的交点，经解算后得到两个对称交点，选择与第4个基站距离最近的交点作为移动目标的最佳坐标。由于测距误差的存在，当基站数量大于4个时，该算法得到的移动目标定位解是一个区域，偏差较大[11]。另外，由于三边/三维定位算法要求3个基站不能共线，这限制了舞台基站布局的方便性和灵活性。在实际舞台应用中，当舞台场地较大、场景较复杂时一般需要布置多于4个基站才能满足信号覆盖要求。因此，三边/三维定位算法在实际舞台追光应用中存在局限性，实用化水平较低。

为解决上述问题，本文采用UWB定位技术和基于TDOA的Chan和Ho[12]定位算法，提高舞台多灯多目标自动跟踪系统的实用化水平。当基站数量大于4个时，CHAN定位算法利用超定方程组的冗余数据进行加权最小二乘(WLS)计算，能获得精准的移动目标定位估计[13]，其估计结果的误差达到克拉美罗界。另外，CHAN算法是对双曲线方程组进行解算，多个基站可以共线，布局较方便。本文设计的舞台自动跟踪系统静态定位精度高，动态定位跟踪实时性好，能够达到我司客户关于舞台自动追光场景对追光准确性、实时性的要求标准，有较好的应用价值。

1 基于UWB目标定位原理

为实现UWB定位跟踪功能，首先要测量移动标签与各个基站的距离，再求解移动标签的定位坐标。本系统将舞台灯、基站和移动标签的位置坐标建立在同一个坐标系内。为避免追光过程中出现非视距现象，基站的安装位置要比移动标签的位置高，同时，演员需将移动标签佩戴在头顶上。

当用户通过上位机开启自动追光模式之后，上位机实时接收来自主基站的测距数据，首先由CHAN算法计算出移动标签的坐标，利用卡尔曼滤波对坐标数据进行优化，再通过舞台灯与标签之间的坐标关系，得出舞台灯步进电机需要转动的水平、垂直扫描信号数值，最后通过DMX512控制器发送DMX数据控制舞台灯转动到相应位置，将灯光投射到移动目标身上实现追光。自动追光实时连续地运行，直至用户通过上位机退出自动追光模式。

1.1 SDS-TWR测距法

通过测量电磁波信号在空中传播的时间并乘以光速可计算出两个设备之间的距离。在没有公共时钟的情况下，通信双方时钟难以严格同步，而对称双边双向测距法(symmetrical double sided two way ranging，SDS-TWR)通过在通信双方各测量一次来避免时钟漂移对测距带来的影响，可以消除计时误差和同步时延[14]。

SDS-TWR测距流程如图1所示。

1)在时刻t1，设备a向设备b发出测距包。

2)电磁波信号在空中传播，经过时间Tf之后在时刻t2被设备b收到。

3)设备b经过时延Tbreply后，在时刻t3发出应答数据包，其中，Tbreply=t3-t2。

4)经过时间Tf后设备a在时刻t4收到应答包，第一次测量完成，所用时间Taround=t4-t1。

5)设备a经过时延Tareply后，在时刻t5发出结束包，其中Tareply=t5-t4。

6)经过时间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定位

为适用和满足大部分的舞台场景需求，本文的舞台多灯多目标自动跟踪测试系统由6台电脑摇头灯L1～L6、8个UWB基站BSi(i∈[1，8])和3个移动标签Pm(m∈[1，3])组成。基站、标签和舞台灯在舞台三维空间的位置分布示意图如图2所示。

图2 系统空间分布示意图Fig.2 Schematic diagram of the system in space

在自动追光场景中，上位机首先通过UWB定位系统确定移动标签Pm的坐标位置，再控制舞台灯水平、垂直转动，将灯光投射到被跟踪目标身上，实现跟踪追光。

为确定移动标签Pm的坐标，上位机通过TCP/IP网络接口与主基站BS1连接，实时接收来自BS1的数据包并从中提取Pm与各个基站BSi的距离信息，计算出Pm到各个基站之间的距离之差，构成一组基于TDOA的双曲线方程组，通过CHAN算法求解该方程组的最优估计值，即可确定Pm的坐标。

假设舞台自动跟踪系统仅对一个移动标签进行定位，移动标签的坐标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，并求出误差的协方差矩阵Ψ，代入式(5)得到第一次加权最小二乘估计：

(7)

(8)

(9)

通过上述两次加权最小二乘估计，移动标签最终的位置估计：

(10)

CHAN定位算法的精度与环境有关：在理想视距LOS环境下，当噪声服从理想高斯分布时TDOA的误差较小，得到的定位精度较高；在非视距NLOS环境下，TDOA的误差较大，求解得到的定位精度较差[15]。

1.3 卡尔曼滤波

在舞台自动跟踪系统中，UWB定位精度受到各种环境噪声的干扰。为降低噪声对定位精度的影响，采用卡尔曼滤波对噪声进行处理。

卡尔曼滤波算法是一种基于状态方程、观测方程对系统状态进行最优估计的滤波算法，具有线性无偏最小均方差的特点[16]。卡尔曼滤波的性能在很大程度上取决于系统的建模方式[17]。系统在t时刻的卡尔曼滤波模型可用式(11)表示[18]：

(11)

其中，X(t|t-1)是t时刻的系统状态预测；P(t|t-1)是t时刻的系统状态协方差预测；Kg为卡尔曼增益；X(t|t)是t时刻的系统状态估计结果；Z(t)是t时刻的观测结果；P(t|t)是t时刻的系统状态协方差估计结果；U(t)是t时刻系统的控制量；A和B是系统参数矩阵；H是系统观测矩阵；Q、R是系统过程噪声、观测噪声协方差，两者都是均值为0的高斯白噪声；I为单位矩阵。为了减少噪声对系统的影响，在舞台灯自动跟踪系统中采用卡尔曼滤波对移动标签的坐标数据进行滤波处理。

2 多灯多目标跟踪控制方法

为实现舞台灯自动追光控制，在得到标签的定位坐标之后，首先根据标签坐标与灯具坐标之间的关系计算出舞台灯要转动的角度，再控制舞台灯光转动到相应位置，实现追光。由于移动目标运动状态、运行轨迹变化不定，为确保舞台灯平稳、实时进行追光，本系统采用预测算法和PID算法对跟踪过程进行控制优化。

2.1 舞台多灯多目标跟踪

根据追光场景需求可分为以下四种跟踪方式：

1)单台灯跟踪单个移动目标。

2)多台灯跟踪单个移动目标。

3)单台灯跟踪多个目标。

4)多台灯跟踪多个目标。

为了实现多灯多目标跟踪控制，首先研究单灯单目标跟踪控制方法，然后在此基础上进一步研究多灯多目标的跟踪控制方法。

1)对于单台灯跟踪单个移动目标的场景，需要建立该灯的步进电机在水平、垂直方向上扫描信号数值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)可知，步进电机的扫描信号数值V水平、V垂直与移动标签的坐标对应关系为

(13)

因此，求出V水平、V垂直之后通过驱动步进电机将舞台灯转动到指定的位置，即可实现追光功能。

2)对于多台灯跟踪单个移动目标的场景，需建立每台灯的V水平、V垂直与移动标签坐标之间的对应关系。在追光过程中每台灯的V水平、V垂直均单独求出。

3)对于单台灯跟踪多个移动目标的场景，系统需要用户事先输入跟踪顺序，然后按照该顺序依次切换并建立该灯的V水平、V垂直与当前被追光的移动标签坐标之间的对应关系。上位机根据编号顺序控制舞台灯对目标轮流追光。

为了确保多个标签同时工作时不相互干扰，需要把标签设备配置成不同地址。UWB定位系统的标签与基站之间采用时分多址(TDMA)通信技术，有效防止了不同标签设备之间的通信干扰。

4)对于多台灯跟踪多个目标的场景，每台灯各自跟踪一个或一个以上的目标。在追光过程中，系统建立每台灯的V水平、V垂直与各自跟踪的移动标签坐标之间的一一对应关系。当被跟踪的目标越多，在单位时间内基站要处理的标签测距请求越多，系统的运算复杂度越高。为保证追光的准确性和实时性，多灯跟踪多个目标的场景对硬件性能的要求比其他场景的更高。本场景中，由于多目标之间的距离关系无法预知，当目标之间距离过近时可能会存在遮挡导致非视距现象，降低定位精度。

2.2 移动目标运动状态预测

由于被跟踪目标的运动状态、运动轨迹变化不定，单纯的定位跟踪控制常常造成追光滞后。为了实时跟踪移动目标，需要对目标的下一步运动状态做出预测。

假设移动标签在很小的时间片段内处于匀速直线运动状态，则根据移动标签的运动速度可提前预测该时间片段内目标的位置，从而提高系统的响应速度，控制舞台灯光实时地跟踪移动标签。本系统以0.1 s为采样周期，先由标签的当前坐标(Xcurrent,Ycurrent)、前一个坐标(Xlast,Ylast)求出标签在X、Y方向的运动速度，再预测0.5 s后标签可能的坐标(Xnext,Ynext)，如式(14)所示：

(14)

2.3 舞台追光过程PID控制优化

为确保舞台灯在追光过程中步进电机运行的稳定性，避免抖动引起灯光晃动，利用PID算法优化舞台灯步进电机的运行和位置控制。在第k次采样时位置式PID控制如式(15)所示[19]：

(15)

其中，u(k)表示输出值，ek表示偏差值，Kp、Ki和Kd分别代表比例、积分和微分系数,u0表示PID控制开始时对应的初值。

在舞台灯追光过程中，根据演员的移动速度修改比例系数，微分系数可以减少至0，去掉微分环节的作用，减少超前调节给系统带来的不稳定性。

3 舞台灯目标跟踪实验

本文的舞台自动跟踪系统选用DecaWave公司的DW1000芯片作为UWB测距模块。基站坐标采用CEM公司LDM-100型激光测距仪进行观测测量，其测量精度为±1.5 mm。为满足大部分舞台追光场景的应用需求，本文采用6台电脑摇头灯、8个基站、3个移动标签(目标)进行测试验证。8个基站安装在长约为25 m、宽约为10 m的长方形舞台正上方，距离地面高度约为5 m。在舞台自动跟踪的四个应用场景中，单灯跟踪单目标、多灯跟踪单目标、单灯跟踪多目标都是单一的功能应用，而较复杂的多灯跟踪多目标场景由前三种场景组合构成。因此，本文重点对多灯跟踪多目标的场景进行测试验证。

为测试系统在静态条件下的定位精度，将3个标签静止放置于地面，6台灯分别跟踪各自标签。对多次实验的数据进行处理，结果如图4(a)、(b)所示。从图4(a)可看出，基于TDOA的CHAN算法三个基站之间可以共线，在基站布局灵活性方面比三边/三维定位算法高。从图4(b)可看出，通过CHAN定位算法和卡尔曼滤波处理得到移动标签坐标位置的估计结果与实际观测值的均方根误差(RMSE)小于20 cm。因此，UWB静态定位精度误差<±20 cm，能够满足舞台追光对定位精度的要求。实验过程中，6台灯的光斑均能准确地投射到各自跟踪目标的标签上。

图4 三个标签静态定位测试Fig.4 Static positioning test of three tags

为验证卡尔曼滤波对系统动态定位的影响，由三个测试人员在头顶各佩戴一个标签进行多目标自动追光实验。测试人员以1 m/s的速度分别沿不同路线匀速前进。对多次实验的数据进行处理，得到有、无卡尔曼滤波算法的动态轨迹曲线，如图5(a)、(b)所示。可见，在大约12 s的跟踪时间内，无滤波的算法得到的3目标动态轨迹曲线有较多的毛刺，曲线不光滑，说明UWB定位过程中受到环境噪声影响，而仅通过CHAN定位算法无法消除这些噪声的影响；有卡尔曼滤波的算法得到的3目标动态轨迹曲线毛刺较少，曲线较光滑，说明卡尔曼滤波有效降低了环境噪声的影响，提高了动态定位的精度，而光滑的动态定位曲线有利于提高自动追光过程的顺滑效果和稳定性。实验过程中，6台灯的灯光均能投射到各自跟踪的测试目标(人员)身上，在测试目标(人员)移动过程中投射的光斑均能无滞后、无超前且无抖动地平稳跟随测试目标(人员)，保证测试目标(人员)始终处于投射光斑范围内，达到我司客户关于舞台多灯多目标自动追光准确性、实时性的要求标准，满足了客户需求。

图5 三个目标动态定位轨迹曲线Fig.5 Dynamic positioning trajectory curve of three targets

4 结论

本文设计的舞台多灯多目标自动跟踪系统，采用基于CHAN算法的UWB室内定位技术，提高舞台自动跟踪系统的实用化水平；通过卡尔曼滤波对多目标的坐标数据进行优化，提高系统的静态、动态定位精度。实验结果表明，在视距条件下，3个标签静态定位的均方根误差小于20 cm，满足舞台追光对定位精度的要求；以大约1 m/s匀速行走约12 s，3个移动目标动态定位轨迹曲线毛刺较少，曲线较光滑。在单灯跟踪单目标、单灯跟踪多目标、多灯跟踪单目标和多灯跟踪多目标这4种追光场景实验中，舞台灯光均能无滞后、无超前且无抖动地平稳跟随测试人员，保证测试人员始终处于投射光斑范围内。因此，本系统能够满足关于舞台灯自动追光准确性、实时性的要求，解决了舞台灯人工追光费时费力的问题，提高了舞台灯光的自动化控制程度，具有较好的应用前景。