本发明涉及一种基于chan算法的改进天牛须探索算法。
背景技术：
随着经济社会的发展，以及人工智能的发展，定位的重要性越发的得到展现。同样，也出现了越来越多的定位算法及其优化算法，如粒子群算法、卡尔曼滤波算法等。chan算法作为最基本的定位算法，虽然有着在非视距环境下定位误差较大的缺点，但是由于其经济性强、非常适合工程实际，一直以来都被研究者进行研究。
现有技术中采用最常用的蜂窝型系统，包括六个副基站和一个主基站，通过tdoa建立方程，定位系统示意图如图1和图2所示。
其中，图1的五角星表示目标点的位置，图2的星点表示目标点的位置，di表示目标与第i个基站的距离。接下来以三维环境下的进行讨论。
用方程对上式进行描述：
其中，(x,y,z)表示目标位置，(xi,yi,zi)表示地面站坐标。ri表示目标点到第i站间的距离，ri,1表示目标点到达主站与第i副站间的距离差。c表示无线电磁波传播速度，τi,1表示飞机发出的信号到达主站与第i副站间的时间差。
目标位置的求解也就是用定位算法求解(x,y,z)的过程。
一、chan算法
chan氏算法由y.t.chan提出，它是一种较优的求解双曲线的非递归算法。该算法使用两次最小二乘法来计算目标位置。
对式(1)进行整理可以得到：
展开可得：
其中
设bs1为定位系统中的主站，
由于地面站的位置坐标和信号到达地面站的时间差是已知的，即r2，1、r3,1、r4,1是已知量，因此式(4)是关于x、y、z和r1的线性方程。设xi,1＝xi-x1,yi,1＝yi-y1,zi,1＝zi-z1，代入上式可得：
对于求解上式中的未知量，可以利用最小二乘(ls：leastsquares)法和球面插值(si：sphericalinterpolation)算法进行求解。而chan氏算法利用将r1视为已知量，则上式为x,y,z的线性方程，当存在四台基站时，测量得到三组时间差，可以得到三组关于x,y,z的线性方程，如下所示：
当i＝1,
令：
其中：
求得：
将式(8)代入
其中，
当n2-4mk＝0时，即r1解唯一，定位有效，且目标位置唯一。
当n2-4mk＞0时，r1有两个解，如果两个解一正一负，取正解作为r1的真实值，如果同为正或同为负，即存在模糊解，则另需要根据其他条件进行判定。
当n2-4mk＜0时，无解，定位不能实现。
求解出r1的值后，根据r1的方程和bs1坐标就可以求得目标t(x,y,z)的值。
当基站的个数大于四个的时候，得到的时间差将多于三个，采用加权最小二乘法(wls)来充分利用冗余数据，可以获得更加好的待定点位置。
chan氏算法具有明确的解析表达式，不需要目标位置的初始预测值。通过引入中间变量将目标方程线性化，其缺点是在非视距环境下测量的各个tdoa值存在较大误差。
二、chan-taylor算法
chan算法的特点是计算量小，在噪声服从高斯分布的环境下，定位精度高。但是在信道环境较差的情况下其定位精度会显著下降。taylor算法，即泰勒级数展开法，也可以用于目标位置的求解，但是这种方法必须有一个初始猜测值，通过求解测量误差的局部线性最小二乘解来改进估计位置。这种方法的问题在于需要初始猜测值，以及并不能保证算法的收敛性和计算量很大算法。因此有学者将chan算法得到的目标估计位置作为taylor方法的初始解，通过迭代确定目标位置，也就是chan-taylor算法。具体流程图如图3所示。
chan-taylor算法的定位精度要比传统的chan算法高，但是其迭代次数多收敛速度漫，定位效率要比传统的chan算法低。
三、卡尔曼-chan算法
为了克服非视距误差的影响，有学者提出采用卡尔曼滤波的方式，对采集到的tdoa值进行处理，然后再将处理后的数据送入chan算法进行定位。具体的流程图如图4所示。
当目标长时间被遮挡时会存在目标跟踪丢失的情况，同时卡尔曼滤波适用于线性、离散和有限维系统，而实际生活是一个复杂环境。
技术实现要素：
本发明的目的是提供一种定位精度高、收敛速度快的基于chan算法的改进天牛须探索算法。
本发明采用如下技术方案：
一种基于chan算法的改进天牛须探索算法，其特征在于，其包括如下步骤：
(1)采集tdoa测量值，确定目标函数；
(2)通过chan-taylor算法和卡尔曼-chan算法对目标进行定位，分别设定为一次定位结果和天牛的初始位置；
(3)利用aoa算法对比一次定位结果与天牛左右两须的角度，利用tdoa算法对比一次定位结果与天牛左右两须的距离差；
(4)重复步骤(3)，更新天牛的位置，直到找到最优解或者迭代结束。
步骤(1)中，所述目标函数为：
其中，dqi表示一次定位点q到其他基站的距离减去到参考基站的距离，相当于该定位点的tdoa测量值，di表示天牛位置到其他基站的距离减去到参考基站的距离，相当于该须所在位置点的tdoa测量值。
步骤(2)中，将天牛的初始方向始终设置为朝着y轴的正方向。
步骤(3)中，利用aoa算法，根据左右两须的连线与左右两须各自与待定点的一次定位结果q之间的夹角zl和zr进行判定：
如果zl＞zr，则向左须方向行进一步，a＝a-step*t/cos(θ)；
如果zl＜zr，则天牛朝着右须方向行进一步，a＝a step*t/cos(θ)；
如果zl与zr相等，则利用tdoa算法，通过一次定位结果q到其他基站和到参考基站的距离差dqi再减去天牛左右两须各自到其他基站和到参考基站的距离差dli和dri，对比
如果p(l)＜p(r)，则a＝a-step*t/cos(θ)；
如果p(l)＞p(r)，则a＝a step*t/cos(θ)。
步骤(3)中，天牛的移动方程为：
a＝a step*(-1)*t*sign(p(l)-p(r))/cos(θ)(11)；
其中，t为(1,1,...,1)，即天牛的初始方向；step为每次移动的步长。θ为移动前天牛当前位置与一次定位点与水平线的夹角；目标函数为：
步骤(4)中，通过步骤(3)计算出天牛的位置a，并将其带入
中进行比较，直到找到最优解或者迭代结束。
本发明的有益效果在于：本发明较chan算法与chan-taylor算法或其他定位算法，首先是在对一个目标的定位中融合了两种不同的定位算法，定位精度得到很大提高。其次天牛须探索算法本身因为仅仅只有一个天牛，加之将整个区域进行缩小，整个程序的运行时间并没有明显的增加。
附图说明
图1为三维环境下的蜂窝型基站系统示意图。
图2为二维环境下的蜂窝型基站系统示意图。
图3为chan-taylor算法流程图。
图4为卡尔曼-chan算法流程图。
图5为本发明算法的流程图。
图6为迭代200次天牛位置的适应度函数(角度)变化曲线图。
图7为迭代20次天牛位置的适应度函数(角度)变化曲线图。
图8和图9为七基站时两种优化算法与chan结合后的定位结果图。
图10和图11为六基站时两种优化算法与chan结合后的定位结果图。
图12和图13为五基站时两种优化算法与chan结合后的定位结果图。
图14和图15为四基站时两种优化算法与chan结合后的定位结果图。
具体实施方式
下面将结合本申请实施例和附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本申请及其应用或使用的任何限制。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
天牛须探索算法是一种生物仿生的优化算法，当天牛在搜寻食物的过程中，根据左右两须探测到的气味的强弱程度进而决定向左还是向右前进，并最终找到整个区域内气味最浓的点，即为所求点。因为仅仅有一个天牛探索，所以收敛速度很快。在本发明中采用该优化算法对chan-taylor和卡尔曼-chan联合算法进行优化，结合了两种改进的chan算法，通过增加一个tdoa和aoa环节，以及天牛与一次定位点之间的夹角θ，不断修改天牛的位置a，提高了定位精度，又因为天牛须探索算法自身的特点，运行时间并没有过多的增加。
本发明的算法流程图如图5所示。具体包括如下步骤。
(1)确定目标函数：
其中dqi表示一次定位点q到其他基站的距离减去到参考基站的距离，相当于该定位点的tdoa测量值，而di表示天牛位置到其他基站的距离减去到参考基站的距离，相当于该须所在位置点的tdoa测量值。
(2)通过两种改进的chan算法对目标点进行一次定位，将其中一个结果定为目标点的一次定位结果q，将另一个结果定为天牛的初始位置a，这样就可以直接把大空间定位转换为小空间定位。
在k维空间中，待定点的初始方向可以用一个矩阵t表示，本发明中，将该矩阵设为t＝[1,1,...]，即将天牛的初始方向始终设置为朝着y轴的正方向。
(3)首先是利用aoa算法，根据左右两须的连线与左右两须各自与待定点的一次定位结果q之间的夹角zl和zr进行判定。
如果zl＞zr，则向左须方向行进一步，a＝a-step*t/cos(θ)。
如果zl＜zr，则天牛朝着右须方向行进一步，a＝a step*t/cos(θ)。
如果zl与zr相等，则利用tdoa算法，通过一次定位结果q到其他基站和到参考基站的距离差dqi再减去天牛左右两须各自到其他基站和到参考基站的距离差dli和dri，对比
如果p(l)＜p(r)，则a＝a-step*t/cos(θ)。
如果p(l)＞p(r)，则a＝a step*t/cos(θ)。
天牛的移动方程如下所示。
a＝a step*(-1)*t*sign(p(l)-p(r))/cos(θ)(11)；
其中，t为(1,1,...,1)，即天牛的初始方向；step为每次移动的步长。θ为移动前天牛当前位置与一次定位点与水平线的夹角；目标函数为：
通过步骤(3)计算出天牛的位置a，并将其带入
该算法不仅可以有效融合两种不同精度的chan算法，提高了定位的精度，同时因为天牛须探索算法自身的收敛速度快的优点，并没有增加过多的运行时间。相对于精度的大幅度提升是完全可以接受的。
理论证明：当前多数优化算法为群优化算法，如粒子群算法、蚁群算法等，以粒子群算法为例，设粒子数量为a，迭代次数为n1 n2(其中n1表示朝左移动的次数，n2表示朝右移动的次数)，各个粒子的初始位置为ai(其中i为粒子数量)，各个粒子的速度为vi，则粒子群算法最终一共进行了a*(n1 n2)次迭代，假设粒子向左移动时速度下降，向右移动时速度上升，则该算法对a*[ai vi*(n2-n1)]个位置点进行了探索，但是由于粒子的随机性，其多数粒子对应的位置属于无效探索，仅部分粒子的探索属于有效探索，因此消耗的时间多数为无用时间。天牛须探索算法是由一个天牛进行探索的，同样设迭代次数为n1 n2(其中n1表示朝左移动的次数，n2表示朝右移动的次数)，天牛的初始位置为a，每次移动步长为l，天牛左右两须长d，则天牛须探索算法一共进行了n1 n2次迭代，迭代次数相对于粒子群算法有着很大的降低，根据天牛的形状，天牛须探索算法对a l*(n2-n1)个位置点及与每个位置点相距d的圆形范围进行了探索，通过增大对每个位置点的探索范围来降低迭代次数，进而降低运行时间。
仿真证明：首先对天牛须探索算法进行试验，选择角度作为适应度函数。设天牛起始位置位于一次定位点的左侧，角度为正时，意味着天牛当前位置也位于一次定位点的左侧，角度为负时意味着当前位置位于一次定位点的右侧。当天牛当前位置的角度突然由正变负时，意味着天牛经过这次迭代后所在位置穿过了设为参考点的一次定位点。当角度处于一个相对稳定的状态，意味着天牛通过探索确定了目标点的位置，在目标点周围进行运动。由下图6和图7可以看出，该算法的在迭代20次左右即可达到最优的效果。因此暂定迭代次数为20次。
在二维环境下对粒子群-chan与天牛须-chan进行对比，假设迭代次数相同，均为20次，对基站采用七基站、六基站、五基站、四基站这四种类型的方式，仿真结果如下所示。对每种基站排布进行单独考虑，待测的目标点坐标随机产生。误差采用高斯白误差随机产生。仿真选择的空间为50k*50k的空间。
可以看出，虽然天牛须探索算法的提出时间不长，但是天牛须探索算法与chan定位可以很好的结合，效果比提出多年的粒子群算法与chan定位的结合算法好。多数情况下，天牛须-chan的融合算法误差与运行时间均优于粒子群-chan的融合算法。
图8和图9表示七基站的仿真结果，其中，图8是整体的结果图，图9是将定位结果所在的区间进行放大后的图像；图10和图11表示六基站的仿真结果，其中，图10是整体的结果图，图11是将定位结果所在的区间进行放大后的图像；图12和图13表示五基站的仿真结果，其中，图12是整体的结果图，图13是将定位结果所在的区间进行放大后的图像；图14和图15表示四基站的仿真结果，其中，图14是整体的结果图，图15是将定位结果所在的区间进行放大后的图像。
