蒙特卡罗算法在室内三维实时定位系统的应用

    1 引言

    为满足公安、武警反恐模拟训练中的室内人员实时三维定位的需求,需要研制一种体积小、功耗低、作用距离远,定位精度高,刷新速度快的实时三维定位系统。与普通的定位系统相比,它要求同一时刻、在同一区域内对多个目标进行定位。因此,对系统的实时性、多任务性以及定位的精确性都提出了很高的要求。

    2 主动式定位方法

    常用的定位方法有很多种,包括到达时延定位方法、到达信号相位差定位方法、到达信号声压差定位方法等。本文采用超声传感器电路到达时延定位方法,超声波定位具有成本低、抗电磁干扰能力强、不受光线烟雾影响等特点。常规的超声定位过程是待定位物体发射超声波,在局部范围内的几个固定位置处放置超声波接收传感器(接收基站),接收并处理从定位物体上发出的超声波信号,然后将数据送往数据处理中心解算,最终得到定位物体的位置。这种定位系统的缺陷是:当定位空间中存在多于两个定位物体时,基站需要区分哪个定位物体发射的信号,当需要定位的物体增加时,基站需区分定位物体的难度及整个系统的复杂程度将成指数增加,系统难以实现。

    本文采用主动式三维超声定位,即用分布在几个固定位置上的基站来发射不同频率的超声波,待定位物体接收超声信号(类似于GPS的工作原理),待定位物体将所接收到的数据发送到数据处理中心,进而定出物体的位置。理论上该系统可以定出无限多个定位物体,且它们之间互不影响。

    3 三维定位模型

    由于上述应用目的,不能使用普通的脉冲回波法测距,只能使用基站发射,定位物体接收的方法来进行测距。

    设在同一空间中,分别放置四个发射基站A、B、C、D,其位置为已知(为便于计算,现设四个基站成正方形布置,间距为M),待定位的物体位置假设为P(x,y,z),如图1所示:

    由图1可知四个基站的坐标分别为A(0,0,0)、B(M,0,0)、C(M,M,0)、D(0,M,0),Tab、Tbc、Tcd分别为待定位物体接收到基站AB、BC、CD同一时刻发出的声波的时间间隔,则有下列关系成立:

    其中,c为声速。c主要受气温影响,可通过以下修正式得到精确值:

    式中:c0)25e时声速;Dt)环境温度。

    通过求解以上方程组便可以得到所要求的x、y、z,从而可以确定待定位物体的空间位置。

    本文由于考虑到在室内环境下,在计算的过程中没有加入环境中的风速。为增强系统的通用性,如果在方程中速度项上加入风速矢量在各路基站与待定位物体方向连线上的投影,设其分别为v1、v2、v3,则方程变为:

    方程组(1)是关于变量x、y、z的高次方程组,对于测定的时间间隔Tab、Tbc、Tcd,可以通过求解方程组来求待定位的物体位置P(x,y,z)。这种方程组没有解析解,需要用数值法求解。