CN105044666A - 面向矿井运动目标的双标签高精度定位方法 - Google Patents

Abstract

一种面向矿井运动目标的双标签高精度定位方法，属于双标签高精度定位方法。该双标签高精度定位方法包括：第一类矿井运动目标定位方法和第二类矿井运动目标定位方法；所述方法在运动目标上水平或垂直安装两个定位标签，并与沿巷道顶板安装的两个定位基站通信，通过构造标签与定位基站RSSI距离与估计距离之间的优化函数并求解其最小值，实时得到运动目标的位置；优化函数的求解通过迭代的方式完成，它包括迭代初值确定、左/右迭代两个主要步骤。本发明仅需在待定位目标上添加一个定位标签即可大幅提高定位精度，升级成本低，部署容易，适合于与巷道平行的长条状对象(如矿车、采煤机)，也适用于与巷道垂直的长条状对象(如人员)。

Description

面向矿井运动目标的双标签高精度定位方法

技术领域

本发明涉及一种双标签高精度定位方法，特别是一种面向矿井运动目标的双标签高精度定位方法。

背景技术

在现有矿井定位系统中，一般通过RSSI的方式测得定位目标与定位基站之间的距离，然后通过几何的方法(如三边定位法)求得目标位置。然而，RSSI测距的精确度后衰落效应的影响非常明显，致使测距精度很小，致使采用单标签的矿井定位系统的精度低下，定位结果不稳定，存在严重的位置漂移。

矿井运动目标可根据其外形分成两类，第一类矿井运动目标是与巷道平行的长条状对象，(如矿车、采煤机)；第二类矿井运动目标是与巷道垂直的长条状对象，(如人员)。

发明内容

本发明的目的是要提供一种面向矿井运动目标的双标签高精度定位方法，解决单标签的矿井定位系统的精度低下，定位结果不稳定，存在严重的位置漂移的问题。

本发明的目的是这样实现的：该双标签高精度定位方法包括：第一类矿井运动目标定位方法和第二类矿井运动目标定位方法；所述方法在运动目标上水平或垂直安装两个定位标签，并与沿巷道顶板安装的两个定位基站通信，通过构造标签与定位基站RSSI距离与估计距离之间的优化函数并求解其最小值，实时得到运动目标的位置；

所述第一类矿井运动目标定位方法：在待定位目标的头部和尾部各安装一个定位标签U₁和U₂，定位标签U₁和U₂之间的距离用L表示；定位出任何一个标签，即可知道目标在巷道中的位置；每个标签都能同时与两个安装巷道顶板中线的定位基站B₁和B₂通信，将定位基站B₁和B₂二点相连构成直线B₁B₂，定位标签U₁和U₂到直线B₁B₂的垂足分别为P₁和P₂，|U₁P₁|＝|U₂P₂|＝H为标签到顶板中心的高度；目标在巷道中宽度维上忽略不计，建模为一维定位，将定位标签U₁和U₂二点相连构成直线U₁U₂，以直线U₁U₂为横轴，U₁的横坐标为x，则U₂的横坐标为x+L；纵轴为与巷道纵向中分平面向上的方向；求解出x在满足

x_opt＝minf(x)

条件下的x_opt值，实现目标的定位。

所述第一类矿井运动目标定位方法的具体定位过程：

1)在待定位目标的头部和尾部各安装一个定位标签U₁和U₂；

2)构造优化函数

定位基站的坐标(x_B,H)和(x_B+L_B,H)、基站之间的距离|B₁B₂|＝L_B是已知的；当定位标签U₁和U₂的坐标分别为(x,0)和(x+L,0)；x_B≤x≤x_B+L_B；设B₁与定位标签U₁和U₂的距离分别为d₁₁和d₁₂，B₂与定位标签U₁和U₂的距离分别为d₂₁和d₂₂，∠B₁U₁P₁＝θ，∠P₁U₁B₂＝α；

构造优化函数f(x)：

$f\left(x\right)=\underset{i=1}{\overset{2}{\Σ}}\underset{j=1}{\overset{2}{\Σ}}{\[{(x_{U_i}-x_{B_j})}^2+{(y_{U_i}-y_{B_j})}^2-d_{ij}^2\]}^2---\left(9\right)$

(9)式右边先平方再开方的目的是为了保证每一项都为正，以免求和的时候正负抵消；其中，为标签U_i,i＝1,2的坐标，且 $x_{U_1}=x,x_{U_2}=x_{U_1}+L,y_{U_1}=y_{U_2}=0;$ 为定位基站B_j,j＝1,2的坐标，且这些量中，是已知条件，d_ij可用RSSI的方式得到；节点i、j之间的RSS值遵循对数正态阴影模型：

${\mathrm{RSS}}_{ij}=P_0-10{\mathrm{\ηlog}}_{10}\left(\frac{d_{ij}}{d_0}\right)+\mathrm{\χ}---\left(10\right)$

其中，P^dBm是收发节点之间以dB为单位的功率路径损耗，是参考距离d₀处测量到的功率，通常d₀＝1m；χ为阴影效应导致的零均值高斯随机变量，在实际计算中可以将其忽略；η是路径损耗因子；因此，

$d_{ij}={10}^{(P_0-{\mathrm{RSS}}_{ij})/10\mathrm{\η}}---\left(11\right)$

于是，(9)式只有x未知。将U₁、U₂、B₁、B₂和d_ij代入(9)式，得：

$\begin{array}{c}f\left(x\right)={\[{(x-x_B)}^2+H^2-d_{11}^2\]}^2+{\[{(x-x_B-L_B)}^2+H^2-d_{21}^2\]}^2\\ ={\[{(x+L-x_B)}^2+H^2-d_{12}^2\]}^2+{\[{(x+L-x_B-L_B)}^2+H^2-d_{22}^2\]}^2\end{array}---\left(12\right)$

如果定位结果是无偏估计，则|U_iB_j|＝d_ij，从而使得f(x)＝0；如果是有偏估计，应取能够使得f(x)最小的x，即待定位目标位置x_opt可以通过求解使得f(x)最小的x值获得：

x_opt＝minf(x)(13)

3)优化函数的求解

(1)通过单标签矿井目标定位方法获得迭代初值x₀，

sinθ＝(x-x_B)/d₁₁，cosθ＝H/d₁₁，sinα＝(x_B+L_B-x)/d₂₁，cosα＝H/d₂₁，

$\begin{array}{c}\cos (\∠B_1{U}_1{B}_2)=\cos (\mathrm{\θ}+\mathrm{\α})\cos \mathrm{\θ}\cos \mathrm{\α}-\sin \mathrm{\θ}\sin \mathrm{\α}\\ =(H/d_{11})(H/d_{21})-\[(x-x_B)/d_{11}\]\[(x_B+L_B-x)/d_{21}\]\\ =\[x^2-(2x_B+L_B)x+L_B{x}_B+x_B^2+H^2\]/\left(d_{11}{d}_{21}\right)\end{array}$

cosα＝H/d₂₁，sinα＝(x_B+L_B-x)/d₂₁，因此，

$cos(\∠B_1{U}_1{B}_2)=cos(\mathrm{\θ}+\mathrm{\α})=\[x^2-(2x_B+L_B)x+L_B{x}_B+x_B^2+H^2\]/\left(d_{11}{d}_{21}\right)---\left(14\right)$

针对ΔB₁U₁B₂，根据余弦定理，有：

$\begin{array}{c}{L}_B^2=d_{11}^2+d_{21}^2-2d_{11}{d}_{21}cos(\mathrm{\θ}+\mathrm{\α})\\ =d_{11}^2+d_{21}^2-2\[x^2-(2x_B+L_B)x+L_B{x}_B+x_B^2+H^2\]\end{array}---\left(15\right)$

利用一元二次方程求根公式解方程(15)，并令其为迭代初值x₀，得：

$x_0=\frac{-b\±\sqrt{b^2-4ac}}{2a}=\frac{1}{2}\·(2x_B+L_B\±\sqrt{2d_{11}^2+2d_{21}^2-4H^2-L_B^2})---\left(16\right)$

其中a＝1，b＝-(2x_B+L_B)，根据x_B≤x≤x_B+L_B，可以消除一个解，得到唯一的迭代初值；

(2)通过双向迭代法获得最优解

以x₀为起始点，令x_i+1＝x_i±Δx，i＝0,1,2,…,N，代入(12)式求得第i+1次迭代的f(x)值f_i+1(x_i)；其中，N为预设的最大迭代次数，Δx为迭代步长，若Δx取正号，则向B₂的方向迭代(右向迭代)，反之则向B₁的方向迭代即左向迭代；迭代起始的时候，x_opt＝x₀；在迭代过程中，若f_i+1(x_i)＞f_i(x_i)，则令x_opt＝x_i+1，否则保持不变；为了加快迭代速度，这里同时进行双向迭代；令分别对应右向迭代和左向迭代；

迭代遇到下列条件结束：(1)超过迭代次数超过阈值N，整个迭代过程终止；(2)若但则右向迭代结束，只进行左向迭代；若但则左向迭代结束，只进行右向迭代；(3)且整个迭代过程终止；(4)当f(x)≤f_th的时候，整个迭代过程终止，其中f_th是给定的距离误差阈值。

所述第二类矿井运动目标，步骤如下：

1)分别在待定位目标的头部、中部各安装一个定位标签U₁和U₂，并令|U₁U₂|＝L，U₁到B₁B₂的垂足P的距离|U₁P|＝H，|B₁B₂|＝L_B；

2)构造优化函数，待定位目标位置x_opt＝minf(x)；

3)最优解的求解：

(1)利用 $x_0=\frac{-b\±\sqrt{b^2-4ac}}{2a}=\frac{1}{2}\·(2x_B+L_B\±\sqrt{2d_{11}^2+2d_{21}^2-4H^2-L_B^2})$ 获得迭代初值；

其中a＝1，b＝-(2x_B+L_B)，根据x_B≤x≤x_B+L_B，可以消除一个解，得到唯一的迭代初值；

(2)通过双向迭代法获得最优解x_opt。

有益效果，由于采用了上述方案，该方法在运动目标上水平或垂直安装两个定位标签，并与沿巷道顶板安装的两个定位基站通信，通过构造标签与定位基站RSSI距离与估计距离之间的优化函数并求解其最小值，实时得到运动目标的位置。优化函数的求解通过迭代的方式完成，它包括迭代初值确定、左/右迭代两个步骤。克服单标签定位受矿井环境因素较大的缺陷，显著提高定位精度。这些装备和人员完全可以安装两个甚至多个定位标签，利用多个标签之间的空间约束提高定位精度。解决了单标签的矿井定位系统的精度低下，定位结果不稳定，存在严重的位置漂移的问题，达到了本发明的目的。

优点：本发明采用双标签的方法进行矿井运动目标定位，仅需在待定位目标上添加一个定位标签即可大幅提高定位精度，升级成本低，部署容易，适合于与巷道平行的长条状对象(如矿车、采煤机)，也适用于与巷道垂直的长条状对象(如人员)。

附图说明：

图1是本发明的第一类矿井运动目标双标签定位图。

图2是本发明的第二类矿井运动目标双标签定位图。

具体实施方式

实施例1：该双标签高精度定位方法包括：第一类矿井运动目标定位方法和第二类矿井运动目标定位方法；所述方法在运动目标上水平或垂直安装两个定位标签，并与沿巷道顶板安装的两个定位基站通信，通过构造标签与定位基站RSSI距离与估计距离之间的优化函数并求解其最小值，实时得到运动目标的位置；

第一类矿井运动目标定位方法：在待定位目标的头部和尾部各安装一个定位标签U₁和U₂，定位标签U₁和U₂之间的距离用L表示；定位出任何一个标签，即可知道目标在巷道中的位置；每个标签都能同时与两个安装巷道顶板中线的定位基站B₁和B₂通信，将定位基站B₁和B₂二点相连构成直线B₁B₂，定位标签U₁和U₂到直线B₁B₂的垂足分别为P₁和P₂，|U₁P₁|＝|U₂P₂|＝H为标签到顶板中心的高度；目标在巷道中宽度维上忽略不计，建模为一维定位，将定位标签U₁和U₂二点相连构成直线U₁U₂，以直线U₁U₂为横轴，U₁的横坐标为x，则U₂的横坐标为x+L；纵轴为与巷道纵向中分平面向上的方向；求解出x在满足

x_opt＝minf(x)

条件下的x_opt值，实现目标的定位。

所述第一类矿井运动目标定位方法的具体定位过程：

1)在待定位目标的头部和尾部各安装一个定位标签U₁和U₂；

2)构造优化函数

定位基站的坐标(x_B,H)和(x_B+L_B,H)、基站之间的距离|B₁B₂|＝L_B是已知的；当定位标签U₁和U₂的坐标分别为(x,0)和(x+L,0)；x_B≤x≤x_B+L_B；设B₁与定位标签U₁和U₂的距离分别为d₁₁和d₁₂，B₂与定位标签U₁和U₂的距离分别为d₂₁和d₂₂，∠B₁U₁P₁＝θ，∠P₁U₁B₂＝α；

构造优化函数f(x)：

$f\left(x\right)=\underset{i=1}{\overset{2}{\Σ}}\underset{j=1}{\overset{2}{\Σ}}{\[{(x_{U_i}-x_{B_j})}^2+{(y_{U_i}-y_{B_j})}^2-d_{ij}^2\]}^2---\left(17\right)$

(9)式右边先平方再开方的目的是为了保证每一项都为正，以免求和的时候正负抵消；其中，为标签U_i,i＝1,2的坐标，且 $x_{U_1}=x,x_{U_2}=x_{U_1}+L,y_{U_1}=y_{U_2}=0;$ 为定位基站B_j,j＝1,2的坐标，且这些量中，是已知条件，d_ij可用RSSI的方式得到；节点i、j之间的RSS值遵循对数正态阴影模型：

${\mathrm{RSS}}_{ij}=P_0-10{\mathrm{\ηlog}}_{10}\left(\frac{d_{ij}}{d_0}\right)+\mathrm{\χ}---\left(18\right)$

其中，P^dBm是收发节点之间以dB为单位的功率路径损耗，是参考距离d₀处测量到的功率，通常d₀＝1m；χ为阴影效应导致的零均值高斯随机变量，在实际计算中可以将其忽略；η是路径损耗因子；因此，

$d_{ij}={10}^{(P_0-{\mathrm{RSS}}_{ij})/10\mathrm{\η}}---\left(19\right)$

于是，(9)式只有x未知。将U₁、U₂、B₁、B₂和d_ij代入(9)式，得：

$\begin{array}{c}f\left(x\right)={\[{(x-x_B)}^2+H^2-d_{11}^2\]}^2+{\[{(x-x_B-L_B)}^2+H^2-d_{21}^2\]}^2\\ ={\[{(x+L-x_B)}^2+H^2-d_{12}^2\]}^2+{\[{(x+L-x_B-L_B)}^2+H^2-d_{21}^2\]}^2\end{array}---\left(20\right)$

如果定位结果是无偏估计，则|U_iB_j|＝d_ij，从而使得f(x)＝0；如果是有偏估计，应取能够使得f(x)最小的x，即待定位目标位置x_opt可以通过求解使得f(x)最小的x值获得：

x_opt＝minf(x)(21)

3)优化函数的求解

(1)通过单标签目标定位方法获得迭代初值x₀，

在图1中，sinθ＝(x-x_B)/d₁₁，cosθ＝H/d₁₁，sinα＝(x_B+L_B-x)/d₂₁，cosα＝H/d₂₁，因此，

$\begin{array}{c}\cos (\∠B_1{U}_1{B}_2)=\cos (\mathrm{\θ}+\mathrm{\α})\cos \mathrm{\θ}\cos \mathrm{\α}-\sin \mathrm{\θ}\sin \mathrm{\α}\\ =(H/d_{11})(H/d_{21})-\[(x-x_B)/d_{11}\]\[(x_B+L_B-x)/d_{21}\]\\ =\[x^2-(2x_B+L_B)x+L_B{x}_B+x_B^2+H^2\]/\left(d_{11}{d}_{21}\right)\end{array}$

cosα＝H/d₂₁，sinα＝(x_B+L_B-x)/d₂₁，因此，

$cos(\∠B_1{U}_1{B}_2)=cos(\mathrm{\θ}+\mathrm{\α})=\[x^2-(2x_B+L_B)x+L_B{x}_B+x_B^2+H^2\]/\left(d_{11}{d}_{21}\right)---\left(22\right)$

针对ΔB₁U₁B₂，根据余弦定理，有：

$\begin{array}{c}{L}_B^2=d_{11}^2+d_{21}^2-2d_{11}{d}_{21}cos(\mathrm{\θ}+\mathrm{\α})\\ =d_{11}^2+d_{21}^2-2\[x^2-(2x_B+L_B)x+L_B{x}_B+x_B^2+H^2\]\end{array}---\left(23\right)$

利用一元二次方程求根公式解方程(15)，并令其为迭代初值x₀，得：

$x_0=\frac{-b\±\sqrt{b^2-4ac}}{2a}=\frac{1}{2}\·(2x_B+L_B\±\sqrt{2d_{11}^2+2d_{21}^2-4H^2-L_B^2})---\left(24\right)$

其中a＝1，b＝-(2x_B+L_B)，根据x_B≤x≤x_B+L_B，可以消除一个解，得到唯一的迭代初值；

(2)通过双向迭代法获得最优解

以x₀为起始点，令x_i+1＝x_i±Δx，i＝0,1,2,…,N，代入(12)式求得第i+1次迭代的f(x)值f_i+1(x_i)；其中，N为预设的最大迭代次数，Δx为迭代步长，若Δx取正号，则向B₂的方向迭代(右向迭代)，反之则向B₁的方向迭代即左向迭代；迭代起始的时候，x_opt＝x₀；在迭代过程中，若f_i+1(x_i)＞f_i(x_i)，则令x_opt＝x_i+1，否则保持不变；为了加快迭代速度，这里同时进行双向迭代；令分别对应右向迭代和左向迭代；

迭代遇到下列条件结束：(1)超过迭代次数超过阈值N，整个迭代过程终止；(2)若但则右向迭代结束，只进行左向迭代；若但则左向迭代结束，只进行右向迭代；(3)且整个迭代过程终止；(4)当f(x)≤f_th的时候，整个迭代过程终止，其中f_th是给定的距离误差阈值。

所述第二类矿井运动目标，步骤如下：

1)分别在待定位目标的头部、中部各安装一个定位标签U₁和U₂，并令|U₁U₂|＝L，U₁到B₁B₂的垂足P的距离|U₁P|＝H，|B₁B₂|＝L_B；

2)构造优化函数，待定位目标位置x_opt＝minf(x)；

3)最优解的求解：

(1)利用 $x_0=\frac{-b\±\sqrt{b^2-4ac}}{2a}=\frac{1}{2}\·(2x_B+L_B\±\sqrt{2d_{11}^2+2d_{21}^2-4H^2-{L^2}_B})$ 获得迭代初值；

其中a＝1，b＝-(2x_B+L_B)，根据x_B≤x≤x_B+L_B，可以消除一个解，得到唯一的迭代初值；

(2)通过双向迭代法获得最优解x_opt。

Claims (2)

1.一种面向矿井运动目标的双标签高精度定位方法，其特征是：该双标签高精度定位方法包括：第一类矿井运动目标定位方法和第二类矿井运动目标定位方法；所述方法在运动目标上水平或垂直安装两个定位标签，并与沿巷道顶板安装的两个定位基站通信，通过构造标签与定位基站RSSI距离与估计距离之间的优化函数并求解其最小值，实时得到运动目标的位置；

所述第一类矿井运动目标定位方法：在待定位目标的头部和尾部各安装一个定位标签U₁和U₂，定位标签U₁和U₂之间的距离用L表示；定位出任何一个标签，即可知道目标在巷道中的位置；每个标签都能同时与两个安装巷道顶板中线的定位基站B₁和B₂通信，将定位基站B₁和B₂二点相连构成直线B₁B₂，定位标签U₁和U₂到直线B₁B₂的垂足分别为P₁和P₂，|U₁P₁|＝|U₂P₂|＝H为标签到顶板中心的高度；目标在巷道中宽度维上忽略不计，建模为一维定位，将定位标签U₁和U₂二点相连构成直线U₁U₂，以直线U₁U₂为横轴，U₁的横坐标为x，则U₂的横坐标为x+L；纵轴为与巷道纵向中分平面向上的方向；求解出x在满足

x_opt＝minf(x)

条件下的x_opt值，实现目标的定位；

所述第二类矿井运动目标，步骤如下：

1)分别在待定位目标的头部、中部各安装一个定位标签U₁和U₂，并令|U₁U₂|＝L，U₁到B₁B₂的垂足P的距离|U₁P|＝H，|B₁B₂|＝L_B；

2)构造优化函数，待定位目标位置x_opt＝minf(x)；

3)最优解的求解：

(1)利用 $x_0=\frac{-b\±\sqrt{b^2-4ac}}{2a}=\frac{1}{2}\·(2x_B+L_B\±\sqrt{2d_{11}^2+2d_{21}^2-4H^2-{L^2}_B})$ 获得迭代初值；

其中a＝1，b＝-(2x_B+L_B)，根据x_B≤x≤x_B+L_B，可以消除一个解，得到唯一的迭代初值；

(2)通过双向迭代法获得最优解x_opt。

2.根据权利要求1所述的面向矿井运动目标的双标签高精度定位方法，其特征是：所述第一类矿井运动目标定位方法的具体定位过程：

1)在待定位目标的头部和尾部各安装一个定位标签U₁和U₂；

2)构造优化函数

定位基站的坐标(x_B,H)和(x_B+L_B,H)、基站之间的距离|B₁B₂|＝L_B是已知的；当定位标签U₁和U₂的坐标分别为(x,0)和(x+L,0)；x_B≤x≤x_B+L_B；设B₁与定位标签U₁和U₂的距离分别为d₁₁和d₁₂，B₂与定位标签U₁和U₂的距离分别为d₂₁和d₂₂，∠B₁U₁P₁＝θ，∠P₁U₁B₂＝α；

构造优化函数f(x)：

$f\left(x\right)=\underset{i=1}{\overset{2}{\Σ}}\underset{j=1}{\overset{2}{\Σ}}{\[{(x_{U_i}-x_{B_j})}^2+{(y_{U_i}-y_{B_j})}^2-d_{ij}^2\]}^2---\left(1\right)$

(9)式右边先平方再开方的目的是为了保证每一项都为正，以免求和的时候正负抵消；其中，为标签U_i,i＝1,2的坐标，且 $x_{U_1}=x,x_{U_2}=x_{U_1}+L,y_{U_1}=y_{U_2}=0;$ 为定位基站B_j,j＝1,2的坐标，且这些量中，是已知条件，d_ij可用RSSI的方式得到；节点i、j之间的RSS值遵循对数正态阴影模型：

${\mathrm{RSS}}_{ij}=P_0-10{\mathrm{\ηlog}}_{10}\left(\frac{d_{ij}}{d_0}\right)+\mathrm{\χ}---\left(2\right)$

其中，P^dBm是收发节点之间以dB为单位的功率路径损耗，是参考距离d₀处测量到的功率，通常d₀＝1m；χ为阴影效应导致的零均值高斯随机变量，在实际计算中可以将其忽略；η是路径损耗因子；因此，

$d_{ij}={10}^{(P_0-{\mathrm{RSS}}_{ij})/10\mathrm{\η}}---\left(3\right)$

于是，(9)式只有x未知。将U₁、U₂、B₁、B₂和d_ij代入(9)式，得：

$\begin{array}{c}f\left(x\right)={\[{\left(x-x_B\right)}^2+H^2-d_{11}^2\]}^2+{\[{\left(x-x_B-L_B\right)}^2+H^2-d_{21}^2\]}^2\\ ={\[{\left(x+L-x_B\right)}^2+H^2-d_{12}^2\]}^2+{\[{\left(x+L-x_B-L_B\right)}^2+H^2-d_{22}^2\]}^2\end{array}---\left(4\right)$

如果定位结果是无偏估计，则|U_iB_j|＝d_ij，从而使得f(x)＝0；如果是有偏估计，应取能够使得f(x)最小的x，即待定位目标位置x_opt可以通过求解使得f(x)最小的x值获得：

x_opt＝minf(x)(5)

3)优化函数的求解

(1)通过单标签矿井目标定位方法获得迭代初值x₀，

$\begin{array}{c}\sin \mathrm{\θ}=\left(x-x_B\right)/d_{11},\cos \mathrm{\θ}=H/d_{11},\sin \mathrm{\α}=\left(x_B+L_B-x\right)/d_{21},\cos \mathrm{\α}=H/d_{21},\\ \cos \left(\∠B_1{U}_1{B}_2\right)=\cos \left(\mathrm{\θ}+\mathrm{\α}\right)=\cos \mathrm{\θ}\cos \mathrm{\α}-\sin \mathrm{\θ}\sin \mathrm{\α}\\ =\left(H/d_{11}\right)\left(H/d_{21}\right)-\[\left(x-x_B\right)/d_{11}\]\[\left(x_B+L_B-x\right)/d_{21}\]\\ =\[x^2-\left(2x_B+L_B\right)x+L_B{x}_B+x_B^2+H^2\]/\left(d_{11}{d}_{21}\right)\end{array}$

cosα＝H/d₂₁，sinα＝(x_B+L_B-x)/d₂₁，因此，

$cos(\∠B_1{U}_1{B}_2)=cos(\mathrm{\θ}+\mathrm{\α})=\[x^2-(2x_B+L_B)x+L_B{x}_B+x_B^2+H^2\]/\left(d_{11}{d}_{21}\right)---\left(6\right)$

针对ΔB₁U₁B₂，根据余弦定理，有：

$\begin{array}{c}{L}_B^2=d_{11}^2+d_{21}^2-2d_{11}{d}_{21}\cos \left(\mathrm{\θ}+\mathrm{\α}\right)\\ =d_{11}^2+d_{21}^2-2\[x^2-\left(2x_B+L_B\right)x+L_B{x}_B+x_B^2+H^2\]\end{array}---\left(7\right)$

利用一元二次方程求根公式解方程(15)，并令其为迭代初值x₀，得：

$x_0=\frac{-b\±\sqrt{b^2-4ac}}{2a}=\frac{1}{2}\·(2x_B+L_B\±\sqrt{2d_{11}^2+2d_{21}^2-4H^2-L_B^2})---\left(8\right)$

其中a＝1，b＝-(2x_B+L_B)，根据x_B≤x≤x_B+L_B，可以消除一个解，得到唯一的迭代初值；

(2)通过双向迭代法获得最优解

以x₀为起始点，令x_i+1＝x_i±Δx，i＝0,1,2,…,N，代入(12)式求得第i+1次迭代的f(x)值f_i+1(x_i)；其中，N为预设的最大迭代次数，Δx为迭代步长，若Δx取正号，则向B₂的方向迭代(右向迭代)，反之则向B₁的方向迭代即左向迭代；迭代起始的时候，x_opt＝x₀；在迭代过程中，若f_i+1(x_i)＞f_i(x_i)，则令x_opt＝x_i+1，否则保持不变；为了加快迭代速度，这里同时进行双向迭代；令分别对应右向迭代和左向迭代；

迭代遇到下列条件结束：(1)超过迭代次数超过阈值N，整个迭代过程终止；(2)若但则右向迭代结束，只进行左向迭代；若但则左向迭代结束，只进行右向迭代；(3)且整个迭代过程终止；(4)当f(x)≤f_th的时候，整个迭代过程终止，其中f_th是给定的距离误差阈值。