基于樽海鞘算法的井下3D定位方法研究

王端义，李艾民

(1.江苏建筑职业技术学院 智能制造学院，江苏 徐州 221116；2.中国矿业大学 机电工程学院，江苏 徐州 221116)

煤炭是我国重要的基础能源，煤矿安全高效开采已成为许多学者研究的重点。煤矿井下作业环境复杂，近年来矿难事故时有发生，作为煤矿井下安全避险“六大系统”之一的井下人员定位系统[1]，其定位精度的提升为矿井安全基础工程提供有力保障。基于无线电的定位技术发展至今已经非常成熟，以GPS为代表的GNSS已经被广泛应用于人们的生活之中，但是在环境特殊的矿井下，GNSS已经不再适用，而无线传感器网络(Wireless Sensor Network，WSN)以其功耗低、体积小且定位精度高的优点，被广泛应用于井下定位系统中[2-4]。其中，基于超宽带(Ultra Wide Band，UWB)的井下定位方法因具有很高的定位精度而备受关注[5]。

利用无线电进行导航定位，其本质在于获取无线电某一物理参量，并据此结合算法实现位置解析，这些物理参量包括电磁波信号强度值、入射角、到达时间差以及飞行时间等，因此，常用的无线电定位技术可以分为基于接收信号强度(Received Signal Strength Indication，RSSI)、信号到达角(Angle of Arrival，AOA)、信号到达时间差(Time Difference of Arrival，TDOA)和信号到达时间(Time of Arrival，TOA)等几种方式。

基于RSSI的定位方式，包括离线采集和在线匹配两个部分[6]，根据电磁波在空间中传输的路径损耗模型，利用检测到的接收信号强度可以计算出信号源到接收机的距离，由多个基站提供的距离信息可以解算出目标的位置。这种定位方式实现容易，但是精度往往很低。基于AOA的定位方式利用信号达到角实现定位[7]，需要依赖体型庞大的天线来检测电磁波的入射角度信息，检测过程较为复杂，且容易受到外部电磁环境的影响。TDOA定位技术最少需要三个基站辅助定位，接收机检测到每两个基站信号到达的时间差，可以确定一组双曲线，两组双曲线的交点即为目标的位置[8]。该技术能够提供很高的定位精度，但是其对基站的时间同步具有很高的要求。TOA定位技术根据电磁波传输的时间计算出基站到目标的距离，以基站为圆心，电磁波传输距离为半径确定一个圆，在2D定位中，三个基站确定的三个圆的交点则是目标的位置[9]，要实现3D定位则最少需要四个基站的辅助，且四个基站不能都在同一平面[10]。

TOA技术因其定位精度较高、成本较低的特点被广泛应用于井下定位环境中[11]。在工程实践中，由于误差因素的存在，多个圆相交得到的往往是一个范围，这就需要使用优化算法进行寻优解算，获取最优估计，实现目标的定位。文献[12]基于最小二乘法，选择从二维到三维分步计算来提高三维定位精度。文献[13]则是在最小二乘法的基础上构建加权矩阵实现精确定位。文献[14]采用最小二乘法实现多基站的三维定位，但是四基站的定位误差较大，只能通过提高基站数量来提高定位精度。

无论是硬件上的增加基站还是算法上的改进处理，最小二乘法的局限性始终影响着定位解算的精度。本文以井下四个测量基站的三维TOA定位模式为应用背景，引用樽海鞘算法(Salp Swarm Algorithm，SSA)[15]对非线性方程组确定的最小均方误差函数进行最优估计。该算法简单易用，收敛速度快，具有很高的收敛性，其特有的链群机制有效地避免了在运算过程中陷入局部最优的困境。该算法避免了最小二乘法的降维运算，能够提供更高的定位精度，在工程实践中具有一定的应用价值。

1 井下定位3D模型

井下环境较为复杂，为了实现三维定位，UWB基站的布设需要覆盖矿工的活动范围，矿工随身携带一个UWB标签。本文采用四个基站用于采集矿工的距离信息，基站在矿井中设置如图1所示，基站A、基站B、基站C和基站D的位置已知，且不在同一平面。并以该四个基站的位置建立空间坐标系，矿工活动于四个基站覆盖的竖井范围之内。

图1 UWB基站在竖井中分布

假设在该坐标系中，四个基站的坐标已知，分别为A(x1，y1，z1)、B(x2，y2，z2)、C(x3，y3，z3)、D(x4，y4，z4)，矿工的坐标未知，设为(x，y，z)。那么根据矿工到各基站的距离关系，可得：

式中，d1、d2、d3、d4分别为四个基站到矿工的量测距离，m。

对此方程组求解，即可获得矿工的位置。但是在实际过程中，因为误差的存在，此非线性方程组无法求解，对目标的确定，可引入等价方程组进行近似估计，考虑到在实际测量中，测量值往往大于真值，即可得：

式中，ε1、ε2、ε3、ε4为引入的误差值，m。

对上述非线性方程组进行误差最小化解算，即可得到目标位置的近似估计。

2 最优化估计算法

2.1 最小二乘法

在求解此类基于TOA定位技术的最优解时，最小二乘法通过最小化误差平方的方法寻优，在导航定位中应用广泛。使用最小二乘法进行最优化估计，对式(2)进一步处理可得：

上式可表示为：

HX-b=ε

(4)

根据最小二乘原理，求解目标函数的最小均方误差函数为：

JLS=(HX-b)T(HX-b)

(5)

其中，对式(3)的一次项的提取的矩阵形式表示为：

矿工的位置是未知的，其坐标表示为：

对式(3)的二次项的提取的矩阵形式表示为：

误差的二次项的，差的矩阵形式可表示为：

根据最小二乘法，可得位置解算的最优估计为：

X=(HTH)-1HTb

(10)

最小二乘法在解算此类最优解时，为了消除原四维方程组中的二次方未知项，将四维方程组转化为三维方程组，通过最小化误差平方和的方式求解最优值。

2.2 樽海鞘算法

受樽海鞘群捕食方法的启发，Seyedali Mirjalili等人提出了樽海鞘寻优算法，该算法能够在有限次的迭代计算后得到最优解，表现出较高的收敛特性。算法的基本思想如图2所示，在设置一个樽海鞘搜索种群后，每一个樽海鞘都在对食物进行搜索，并计算出所有个体的适应度，适应度可以视为一个寻优效果的体现，具有最佳适应度的樽海鞘链所获得的食物信息将用于更新其它链群，其它链群在此基础上进行第二轮搜索，如此往复，逐步逼近，直到获取食物的位置为止。

图2 SSA算法流程

每一次迭代都将进行一次数据更新以传递食物信息，其中排在链群首位的领导者的更新方法根据式(11)进行：

可见，每个链群中的领导者的位置信息的更新只和食物的位置有关，不受其它樽海鞘的直接影响。

作为该算法中唯一的控制参数，c1的取值反应了算法对全局搜索和局部优化的权衡，其定义为：

式中，l是当前迭代次数；L是总迭代数。

跟随者的位置更新只和上一级樽海鞘有关，它们会一直跟随领导者运动，这在一定程度上防止了算法陷入局部最优的困境，其更新方程为：

根据式(2)可得最小均方误差函数为：

JSSA=(AX-B)T(AX-B)

(14)

其中，对式(2)的一次项的提取的矩阵形式可表示为：

目标的坐标仍然表示为：

对式(2)的二次项的提取的矩阵形式可表示为：

搜索空间的确定是樽海鞘算法运算的前提，目标的三个位置变量(x，y，z)的约束条件即为算法的搜索界限，可根据实际情况设定。在确定搜索空间的上下界之后，即可对式(14)的目标函数进行最优化求解，解算出目标位置的最优估计。

3 系统仿真

为了检验樽海鞘算法在对矿工位置解算过程中的应用效果，仿真验证是有必要的。首先生成30个矿工位置的随机值作为真值参考，分别使用最小二乘法和樽海鞘算法对该30个位置进行解算。使用樽海鞘算法进行30次计算，其中一次位置解算的迭代过程的收敛曲线如图3所示，可见其收敛速度很快，大约进行200次迭代，算法就可以获取最优值。整个搜索过程没有局部最优的困境出现。最小二乘法的运算则不存在迭代计算的收敛问题，因其和樽海鞘算法的目标函数不同，二者对矿工位置的解算结果精度也不尽相同。樽海鞘算法可直接对式(2)进行解算，而最小二乘法则需要消除二次项后，才可以实现位置解算。

图3 SSA收敛曲线

为了对比位置解算的误差，分别从矿工在坐标系中的距离和方位角两个方面进行，即将最小二乘法和樽海鞘算法解算的目标坐标估计值转换成相应距离和角度信息。其中，距离误差分析包括3D空间几何距离误差、平面距离误差和垂直距离误差三个部分，如图4所示，樽海鞘算法相比最小二乘法，整体上距离解算误差更小，表现更优。方位角误差的对比包括航向角和俯仰角两个部分，角度误差对比如图5所示，从图5中两个对比结果可见，樽海鞘算法和最小二乘法解算的航向角误差基本持平，而在俯仰角方面，樽海鞘算法的误差更小，表现更优。

图4 距离误差对比

图5 角度误差对比

4 结 语

对基于TOA定位技术的非线性方程组的位置解算，一般采用结构简单且计算量较小的最小二乘法。为了消除公共二次项变量而采用的降维处理，在一定程度上引入了二次误差。本文引入的樽海鞘算法是一种具有较高收敛特性的寻优算法，能够在指定搜索空间内迅速找到最优估计值，其独特的链群机制能够有效避免陷入局部最优的困境。在对非线性方程组解算时，能够在不改变方程组维数的情况下实现最优估计。仿真结果表明，基于樽海鞘算法的定位误差整体上小于最小二乘法的解算，算法快速收敛，应用于煤矿井下的无线传感器网络定位中，能够从软件算法上进一步提高定位精度，更加准确地获取井下矿工的三维位置信息。