基于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 和气压计的自动跟踪定位系统，能够实现舞台灯光传统手动控制的基本功能，也能够满足舞台人员在低速移动过程实时性和准确性的要求，并且可以实现多个舞台灯光对多名移动人员的精准自动跟踪，很大程度上降低了相关行业的人力成本。