一种联合最小二乘和三边定位的超宽带室内定位方法与流程

本发明涉及一种联合最小二乘和三边定位的超宽带室内定位方法，属于信号处理领域。

背景技术：

随着室内定位技术的发展，超宽带定位技术已经被广泛地应用到各种室内场所中。超宽带技术具有穿透能力强、抗多径衰减能力强、抗干扰能力好以及安全性好等特点。在使用超宽带技术进行室内定位时，最重要的指标则是对待测目标的定位精度，如果不能够实时获得待测目标的精确位置，室内定位技术也就失去了意义。

传统的超宽带定位算法有最小二乘法、chan算法以及taylor级数算法等，但是它们在对目标位置进行解算时具有计算量大、定位误差较大等缺点。

技术实现要素：

本发明提出了一种联合最小二乘和三边定位的超宽带室内定位方法，为了解决超宽带定位系统对待测目标的定位问题。

一种联合最小二乘和三边定位的超宽带室内定位方法，所述定位方法包括以下步骤：

步骤一：在对标签进行定位时，4个锚节点的位置固定且已知，设4个锚节点的坐标分别为a1(x1,y1,z1),a2(x2,y2,z2),a3(x3,y3,z3),a4(x4,y4,z4)，标签的坐标设为(x,y,z)，得到z1＝z2＝z3＝z4，

利用锚节点a1、a2、a3对标签进行定位，设标签到锚节点a1、a2、a3的距离分别为d1、d2、d3，则标签到锚节点a1、a2、a3的距离di可以建立的方程如式(1)所示：

di²＝(x-xi)²+(y-yi)²+(z-zi)²

＝(x²+y²+z²)-2xxi-2yyi-2zzi+(xi²+yi²+zi²)

＝r-2xxi-2yyi-2zzi+ki(i＝1,2,3)(1)

其中：r＝(x²+y²+z²)，

步骤二：将i＝1,2,3分别带入式(1)，组成由3个方程构成的方程组，并且将前2个方程依次减去第3个方程，得到下列矩阵关系如式(2)所示：

将公式(2)写成：ax＝b，

其中：

步骤三：由最小二乘估计：

x＝(a^ta)^-1a^tb(3)

求出当前标签的x、y坐标，由于锚节点a1的位置为所建立坐标系的原点，则可以根据式：

求得关于当前4个锚节点构成平面的2个对称解：

步骤四：在基于锚节点a1、a2、a3求解出的定位结果的基础上，利用第四个锚节点a4对坐标结果进行优化，设2个对称解分别为x1，x2，sc为锚节点a4的位置，r为球的半径，接收到锚节点a4与标签t0间的距离值，

假设球与线段有交点，设球与线段相交的点为x＝x1+λ(x2-x1)，将该点带入球面方程：

(sc-x)²＝r²(6)

将球面方程转换为式(7)的形式：

αλ²+βλ+γ＝0(7)

其中，α＝(x2-x1)²，β＝2(x1-sc)(x2-x1)，

得到方程的2个根如式(8)所示：

步骤五：针对不同情况对应的λ的取值对z轴坐标进行优化；

步骤六：每当接收到新的测距信息时，重复步骤一到步骤五，计算出当前时刻标签的精确位置，实现对标签运动轨迹的精确追踪。

进一步的，具体的，球面与线段的位置关系分为以下几种：

情况一：线段x1x2在球体之外，没有交点，此时λ1＜0且λ2＜0，令λ＝min{|λ1|,|λ2|}，则定位结果x＝x2+0.5λ(x2-x1)，

情况二：线段x1x2在球体之内，没有交点，此时λ1＜0,λ2＞1或者λ2＜0,λ1＞1，则定位结果x＝x2+0.5(x2-x1)，

情况三：线段x1x2与球体只有一个交点，此时0＜λ1＜1,λ2＜0||λ2＞1或者0＜λ2＜1,λ1＜0||λ1＞1，令λ＝{λ1，λ2|λ∈(0,1)}，如果λ≤0.5，λ＝0.5λ；如果λ＞0.5，λ＝λ-0.5(1-λ)，则定位结果x＝x2+λ(x2-x1)，

情况四：线段x1x2与球体有两个交点，此时0＜λ1＜1并且0＜λ2＜1，令

λ＝(λ1+λ2)/2，则定位结果x＝x2+λ(x2-x1)，

情况五：线段x1x2与球体相切，只有一个交点，此时λ1＝λ2，令λ＝λ1＝λ2，当λ≤0.25，λ＝0.5λ；当0.25＜λ≤0.5，λ＝λ-0.5(0.5-λ)；当0.5＜λ≤0.75，λ＝λ-0.5(λ-0.5)；当λ＞0.75，λ＝λ-0.5(1-λ)，则定位结果x＝x2+λ(x2-x1)。

本发明的主要优点是：本发明提出了一种联合最小二乘和三边定位的超宽带室内定位方法，该定位方法通过利用所获取到的距离信息，利用改进的最小二乘和三边定位的联合算法对测距结果进行两步解算处理，从而得到更加精确的定位结果，并且耗时较短。

附图说明

图1为超宽带定位系统示意图；

图2为标签位置解算流程图；

图3为z轴坐标优化示意图；

图4为步骤六中各个情况的优化示意图，其中，图4(a)为情况一的示意图；图4(b)为情况二的示意图；图4(c)为情况三的示意图；图4(d)为情况四的示意图；图4(e)为情况五的示意图。

具体实施方式

下面将结合本发明实施例中的附图对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明公开了一种联合最小二乘和三边定位的超宽带室内定位方法的一实施例，所述定位方法包括以下步骤：

步骤一：在对标签进行定位时，4个锚节点的位置固定且已知，设4个锚节点的坐标分别为a1(x1,y1,z1),a2(x2,y2,z2),a3(x3,y3,z3),a4(x4,y4,z4)，标签的坐标设为(x,y,z)，由于4个锚节点在实际配置时要求处在同一高度，因此可得z1＝z2＝z3＝z4，

利用锚节点a1、a2、a3对标签进行定位，设标签到锚节点a1、a2、a3的距离分别为d1、d2、d3，则标签到锚节点a1、a2、a3的距离di可以建立的方程如式(1)所示：

di²＝(x-xi)²+(y-yi)²+(z-zi)²

＝(x²+y²+z²)-2xxi-2yyi-2zzi+(xi²+yi²+zi²)

＝r-2xxi-2yyi-2zzi+ki(i＝1,2,3)(1)

其中：r＝(x²+y²+z²)，

步骤二：将i＝1,2,3分别带入式(1)，组成由3个方程构成的方程组，并且将前2个方程依次减去第3个方程，得到下列矩阵关系如式(2)所示：

将公式(2)写成：ax＝b，

其中：

步骤三：由最小二乘估计：

x＝(a^ta)^-1a^tb(3)

可以求出当前标签的x、y坐标，由于锚节点a1的位置为所建立坐标系的原点，则可以根据式：

可以求得关于当前4个锚节点构成平面的2个对称解：

步骤四：在基于锚节点a1、a2、a3求解出的定位结果的基础上，再利用第四个锚节点a4对坐标结果进行优化，主要是对于z轴上面的优化。设2个对称解分别为x1，x2，sc为锚节点a4的位置，r为球的半径，也就是接收到锚节点a4与标签t0间的距离值，z轴坐标优化如图3所示。

假设球与线段有交点，设球与线段相交的点为x＝x1+λ(x2-x1)，将该点带入球面方程：

(sc-x)²＝r²(6)

将球面方程转换为式(7)的形式：

αλ²+βλ+γ＝0(7)

其中，α＝(x2-x1)²，β＝2(x1-sc)(x2-x1)，

得到方程的2个根如式(8)所示：

步骤五：球面与线段x1x2的位置关系决定了λ的值，因此需要针对不同情况对应的λ的取值对z轴坐标进行优化；

步骤六：每当接收到新的测距信息时，重复步骤一到步骤五，计算出当前时刻标签的精确位置，最终实现对标签运动轨迹的精确追踪。

参照图4所示，在本部分优选实施例中，具体的，球面与线段的位置关系分为以下几种：

情况一：线段x1x2在球体之外，没有交点，此时λ1＜0且λ2＜0，令λ＝min{|λ1|,|λ2|}，则定位结果x＝x2+0.5λ(x2-x1)，

情况二：线段x1x2在球体之内，没有交点，此时λ1＜0,λ2＞1或者λ2＜0,λ1＞1，则定位结果x＝x2+0.5(x2-x1)，

情况三：线段x1x2与球体只有一个交点，此时0＜λ1＜1,λ2＜0||λ2＞1或者0＜λ2＜1,λ1＜0||λ1＞1，令λ＝{λ1，λ2|λ∈(0,1)}，如果λ≤0.5，λ＝0.5λ；如果λ＞0.5，λ＝λ-0.5(1-λ)，则定位结果x＝x2+λ(x2-x1)，

情况四：线段x1x2与球体有两个交点，此时0＜λ1＜1并且0＜λ2＜1，令λ＝(λ1+λ2)/2，则定位结果x＝x2+λ(x2-x1)，

情况五：线段x1x2与球体相切，只有一个交点，此时λ1＝λ2，令λ＝λ1＝λ2，当λ≤0.25，λ＝0.5λ；当0.25＜λ≤0.5，λ＝λ-0.5(0.5-λ)；当0.5＜λ≤0.75，λ＝λ-0.5(λ-0.5)；当λ＞0.75，λ＝λ-0.5(1-λ)，则定位结果x＝x2+λ(x2-x1)。

具体的，本发明主要针对超宽带定位系统对待测目标的定位问题而提出。超宽带定位系统中一共有5个节点，其中包括4个锚节点和1个标签。4个锚节点的位置坐标已知，并以第一个锚节点a1作为该系统的坐标原点，而标签的位置坐标未知，需要利用其与4个锚节点的信息交互来解算出标签的位置坐标。超宽带定位系统示意图如图1所示，其中a1、a2、a3、a4为4个锚节点，t0为标签。本发明在对标签进行位置解算的流程如图2所示，其中第二步是利用改进的最小二乘算法实现的，第三步是利用改进的三边定位算法实现的。