CN105824003A - 一种基于轨迹平滑的室内移动目标定位方法 - Google Patents

Abstract

本发明提出一种基于轨迹平滑的室内移动目标定位方法，所述方法包括(1)移动UWB节点通过轮询的方式获得与各个锚节点之间的距离；(2)将某一时刻锚节点测得距离通过无迹卡尔曼滤波器进行修正；(3)将时刻滤波后的UWB测量值采用三边测量定位法获得移动目标的位置坐标；(4)通过无迹卡尔曼滤波对定位结果进行修正；(5)采用自适应平滑算法对滤波后的定位结果进行平滑，从而到达移动目标当前时刻的位置估计值。本发明充分利用当前时刻与先前时刻的测量值和估计值，采用无迹卡尔曼滤波方式，减小了由于环境、器件等因素对测量结果的影响，提高了三边测量法的定位精度，并对定位结果进行二次滤波和自适应平滑，进一步提高定位精度。

Description

一种基于轨迹平滑的室内移动目标定位方法

技术领域

本发明涉及一种目标定位方法，具体讲涉及一种基于轨迹平滑的室内移动目标定位方法。

背景技术

如何实现对室内移动目标的高精度定位是室内定位系统研究的重点之一。现有的定位技术中，UWB技术可以实现厘米级的测距精度。UWB模块通过测量信号在空中的飞行时间(TOF，TimeofFlight)来计算模块之间的距离。但是在实际应用中发现，复杂的室内环境使得电磁波在视距条件和非视距条件下传输存在较大差异，此外，当受到外界因素的影响，UWB器件会使得测距结果产生较大的偏差，直接影响到三边测量的结果。现有的方法是假设噪声为零均值的高斯噪声，对测量的结果进行卡尔曼滤波，但实际的测量噪声并不服从标准正太分布，需要使用无迹卡尔曼滤波。此外，三边测量法获得的是满足条件的最小二乘解，当某些锚节点的出现微小的偏差，其定位结果即会产生较大的偏差。因此需要提出一种滤波平滑算法，对出现较大偏差的定位结果进行修正。

发明内容

针对现有技术的不足，本发明提出一种基于轨迹平滑的室内移动目标定位方法，在超宽带(UltraWideband，UWB)测距中将滤波算法与自适应平滑算法相结合，通过在多个阶段分别采用无迹卡尔曼滤波，将相邻时刻定位结果之间的距离建模为与权重系数相关的函数，使得权重系数可以自适应调整，再对定位结果进行加权平均，从而实现对移动目标轨迹的平滑。对定位过程中不同阶段的测量值和估计值分别进行滤波。一方面，通过对UWB测量结果进行无迹卡尔曼滤波，减小由于环境和器件等因素引起的测量误差，提高三边测量法的精度；另一方面，通过对定位结果进行二次滤波和自适应平滑，进一步减少定位误差。

如图2所示，为本发明适用的定位场景，在定位的区域内，至少需要三个UWB锚节点和一个移动UWB节点。

通过在定位过程中不同的阶段，对UWB测量值和三边测量结果分别采用无迹卡尔曼滤波进行误差修正，其算法适用于不服从高斯分布的噪声的影响。最终结合自适应平滑算法，对移动目标的轨迹进行平滑，从而减小环境和UWB器件对测量值的影响，提高了对移动目标的定位精度。

本发明的目的是采用下述技术方案实现的：

一种基于轨迹平滑的室内移动目标定位方法，其改进之处在于，所述方法包括

(1)移动UWB节点通过轮询的方式获得与各个锚节点之间的距离；

(2)将某一时刻锚节点测得距离通过无迹卡尔曼滤波器进行修正；

(3)将时刻滤波后的UWB测量值采用三边测量定位法获得移动目标的位置坐标；

(4)通过无迹卡尔曼滤波对定位结果进行修正；

(5)采用自适应平滑算法对滤波后的定位结果进行平滑，从而到达移动目标当前时刻的位置估计值。

优选的，所述步骤(1)包括轮询的间隔可自适应调整，当移动目标快速运动时，采用较小的轮询间隔；而当移动目标慢速移动或静止时，设定较大的轮询间隔，以降低功耗。

优选的，所述步骤(1)包括通过设定合适的轮询间隔保证在同一位置上，移动目标至少获得与三个锚节点之间的距离。

优选的，所述步骤(2)包括

设n时刻，通过UWB测得的移动目标与其中一个锚节点之间距离的真实值为r(n)，速度的真实值为v(n)；将r(n)和v(n)作为系统的状态矢量，表示为

$x\left(n\right)=\left[\begin{array}{c}r\left(n\right)\\ v\left(n\right)\end{array}\right]---\left(1\right)$

将系统的状态方程建模为

x(n)＝F₁x(n-1)+Gα(2)

其中：F₁为状态转移矩阵，表示为 $F_1=\left[\begin{array}{cc}1& T\\ 0& 1\end{array}\right];$ α为加速度项，将其建模为系统噪声，其系数矩阵为 $G=\left[\begin{array}{c}{T}^2/2\\ T\end{array}\right];$ T为状态更新的时间间隔；

观测方程建模为

y(n)＝H₁(n)x(n)+v(n)(3)

其中： $y\left(n\right)=\left[\begin{array}{c}{r}_m\left(n\right)\\ v_m\left(n\right)\end{array}\right],$ r_m(n)为距离的测量值，v_m(n)为速度的测量值，表示为 $v_m\left(n\right)=\frac{r_m\left(n\right)-r_m(n-1)}{T};$ 观测矩阵 $H_1\left(n\right)=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right];$ v(n)为零均值单位方差的观测噪声。

优选的，所述步骤(4)包括

n时刻系统状态矢量为u(n)＝[x(n)，y(n)，v_x(n)，v_y(n)]^T，其中：x(n)，y(n))为n时刻的坐标，v_x(n)和v_y(n)为x轴和y轴方向上的速度；

将系统的状态方程建模为

u(n)＝F₂u(n-1)+w(n)(4)

其中：F2为状态转移矩阵，表示为 $F_2=\left[\begin{array}{cccc}1& 0& T& 0\\ 0& 1& 0& T\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right];$ w(n)为零均值单位方差的系统噪声；T为状态更新的时间间隔；

观测方程建模为

z(n)＝H₂(n)u(n)+σ(n)(5)

其中：z(n)＝[x_t(n)，y_t(n)，v_xt(n)，v_yt(n)]^T，x_t(n)和y_t(n)为n时刻通过三边测量法得到的定位结果坐标，v_xt(n)和v_yt(n)分别为x轴和y轴方向上的速度的测量值；观测矩阵 $H_2\left(n\right)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],$ σ(n)为零均值单位方差的观测噪声。

优选的，所述步骤(5)包括

设n时刻对三边测量的定位结果进行滤波后，得到移动目标的位置为s_r(n)，其坐标为(x_r(n)，y_r(n))，则n-1时刻滤波后移动目标的定位结果为s_r(n-1)；n时刻平滑后的移动目标的定位结果为坐标表示为(x_est(n)，y_est(n))，则n-1时刻平滑后的移动目标的定位结果为

定义相邻两个时刻三边测量结果的距离D_r(n)为

$\begin{array}{c}{D}_r\left(n\right)=|s_r\left(n\right)-s_r(n-1)|\\ =\sqrt{{[x_r\left(n\right)-x_r(n-1)]}^2+{{[y}_r\left(n\right)-y_r(n-1)]}^2}\end{array}$

设n时刻平滑后的定位结果为n-1时刻平滑后的定位结果和n时刻滤波后的定位结果的加权平均，定义为

${\hat{s}}^{\left(1\right)}\left(k\right)=\mathrm{\ρ}\left(n\right)\·s_r\left(n\right)+(1-\mathrm{\ρ}\left(n\right))\·{\hat{s}}^{\left(1\right)}(n-1)---\left(6\right)$

其中ρ应满足

$\mathrm{\ρ}\left(n\right)=e^{-D_r\left(n\right)}---\left(7\right).$

进一步地，通过自适应平滑算法对滤波后的定位结果进行轨迹平滑，将当前时刻的定位估计值与前一时刻的定位估计值进行比较，当两者的距离越大，则当前时刻的定位估计值的权重越小；而当两者的距离较小，则当前时刻定位估计值的权重越大，通过不同时刻的权重对相邻时刻的定位估计值进行加权平均，从而实现自适应的过程。

与现有技术比，本发明的有益效果为：

本发明充分利用当前时刻与先前时刻的测量值和估计值，采用无迹卡尔曼滤波方式，减小了由于环境、器件等因素对测量结果的影响，提高了三边测量法的定位精度，并对定位结果进行二次滤波和自适应平滑，进一步提高定位精度。

附图说明

图1为本发明提供的一种基于轨迹平滑的室内移动目标定位方法流程图。

图2为本发明提供的定位场景示意图。

具体实施方式

下面结合附图对本发明的具体实施方式作进一步的详细说明。

如图1所示，为本发明提出的一种基于轨迹平滑的室内移动目标定位方法，该方法使用多阶段无迹卡尔曼滤波和自适应平滑算法，包括如下步骤：

(1)移动UWB节点通过轮询的方式获得与各个锚节点之间的距离；

轮询的间隔可以自适应调整，当移动目标快速运动时，采用较小的轮询间隔；而当移动目标慢速移动或静止时，设定较大的轮询间隔，以降低功耗；

(2)将某一时刻所有锚节点测得距离通过无迹卡尔曼滤波器进行修正；

其中，对UWB测量值进行无迹卡尔曼滤波具体为：

假设n时刻，通过UWB测得的移动目标与其中一个锚节点之间距离的真实值为r(n)，速度的真实值为v(n)。将r(n)和v(n)作为系统的状态矢量，表示为

$x\left(n\right)=\left[\begin{array}{c}r\left(n\right)\\ v\left(n\right)\end{array}\right]---\left(1\right)$

将系统的状态方程建模为

x(n)＝F₁x(n-1)+Gα(2)

其中：F₁为状态转移矩阵，表示为 $F_1=\left[\begin{array}{cc}1& T\\ 0& 1\end{array}\right];$ α为加速度项，将其建模为系统噪声，其系数矩阵为 $G=\left[\begin{array}{c}{T}^2/2\\ T\end{array}\right];$ T为状态更新的时间间隔。

观测方程建模为

y(n)＝H₁(n)x(n)+v(n)(3)

其中： $y\left(n\right)=\left[\begin{array}{c}{r}_m\left(n\right)\\ v_m\left(n\right)\end{array}\right],$ r_m(n)为距离的测量值，v_m(n)为速度的测量值，表示为 $v_m\left(n\right)=\frac{r_m\left(n\right)-r_m(n-1)}{T};$ 观测矩阵 $H_1\left(n\right)=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right];$ v(n)为零均值单位方差的观测噪声。

在实际的测量过程中，对测得的与各个锚节点之间的距离分别进行无迹卡尔曼滤波的修正，从而提高各个锚节点测量的精度。

(3)将(2)中该时刻滤波后的UWB测量值采用三边测量定位法，获得移动目标的位置坐标；

(4)通过无迹卡尔曼滤波对(3)中的定位结果进行修正；

其中，对三边测量法的定位结果进行滤波具体为：

n时刻系统状态矢量为u(n)＝[x(n)，y(n)，v_x(n)，v_y(n)]^T，其中：(x(n)，y(n)为n时刻的坐标，v_x(n)和v_y(n)为x轴和y轴方向上的速度。

将系统的状态方程建模为

u(n)＝F₂u(n-1)+w(n)(4)

其中：F₂为状态转移矩阵，表示为 $F_2=\left[\begin{array}{cccc}1& 0& T& 0\\ 0& 1& 0& T\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right];$ w(n)为零均值单位方差的系统噪声；T为状态更新的时间间隔。

观测方程建模为

z(n)＝H₂(n)u(n)+σ(n)(5)

其中：z(n)＝[x_t(n)，y_t(n)，v_xt(n)，v_yt(n)]^T，x_t(n)和y_t(n)为n时刻通过三边测量法得到的定位结果坐标，v_xt(n)和v_yt(n)分别为x轴和y轴方向上的速度的测量值；观测矩阵 $H_2\left(n\right)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],$ σ(n)为零均值单位方差的观测噪声。

(5)采用自适应平滑算法对(4)中滤波后的定位结果进行平滑，从而到达移动目标当前时刻的位置估计值。

其中，自适应平滑过程具体为：

假设n时刻对三边测量的定位结果进行滤波后，得到移动目标的位置为s_r(n)，其坐标为(x_r(n)，y_r(n))，则n-1时刻滤波后移动目标的定位结果为s_r(n-1)。n时刻平滑后的移动目标的定位结果为坐标表示为(x_est(n)，y_est(n))，则n-1时刻平滑后的移动目标的定位结果为

定义相邻两个时刻三边测量结果的距离D_r(n)为

$\begin{array}{c}{D}_r\left(n\right)=|s_r\left(n\right)-s_r(n-1)|\\ =\sqrt{{[x_r\left(n\right)-x_r(n-1)]}^2+{{[y}_r\left(n\right)-y_r(n-1)]}^2}\end{array}$

设n时刻平滑后的定位结果为n-1时刻平滑后的定位结果和n时刻滤波后的定位结果的加权平均，定义为

${\hat{s}}^{\left(1\right)}\left(k\right)=\mathrm{\ρ}\left(n\right)\·s_r\left(n\right)+(1-\mathrm{\ρ}\left(n\right))\·{\hat{s}}^{\left(1\right)}(n-1)---\left(6\right)$

其中ρ应满足

$\mathrm{\ρ}\left(n\right)=e^{-D_r\left(n\right)}---\left(7\right).$

由此可见，将当前时刻的定位估计值与前一时刻的定位估计值进行比较，当两者的距离越大，则当前时刻的定位估计值的权重越小；而当两者的距离较小，则当前时刻定位估计值的权重越大，通过不同时刻的权重对相邻时刻的定位估计值进行加权平均，从而实现自适应的过程。

本发明通过设定合适的轮询间隔保证在同一位置上，移动目标至少获得与三个锚节点之间的距离。

本发明对移动目标接收的与各个锚节点之间距离进行无迹卡尔曼滤波，再将滤波后的距离测量值采用三边测量法进行定位。对三边测量法的定位结果再次利用无迹卡尔曼滤波，从而进一步提高定位的精度。

最后应当说明的是：以上实施例仅用以说明本发明的技术方案而非对其限制，所属领域的普通技术人员参照上述实施例依然可以对本发明的具体实施方式进行修改或者等同替换，这些未脱离本发明精神和范围的任何修改或者等同替换，均在申请待批的本发明的权利要求保护范围之内。

Claims (7)

1.一种基于轨迹平滑的室内移动目标定位方法，其特征在于，所述方法包括

(1)移动UWB节点通过轮询的方式获得与各个锚节点之间的距离；

(2)将某一时刻锚节点测得距离通过无迹卡尔曼滤波器进行修正；

(3)将时刻滤波后的UWB测量值采用三边测量定位法获得移动目标的位置坐标；

(4)通过无迹卡尔曼滤波对定位结果进行修正；

(5)采用自适应平滑算法对滤波后的定位结果进行平滑，从而到达移动目标当前时刻的位置估计值。

2.如权利要求1所述的一种基于轨迹平滑的室内移动目标定位方法，其特征在于，所述步骤(1)包括轮询的间隔可自适应调整，当移动目标快速运动时，采用较小的轮询间隔；而当移动目标慢速移动或静止时，设定较大的轮询间隔，以降低功耗。

3.如权利要求1所述的一种基于轨迹平滑的室内移动目标定位方法，其特征在于，所述步骤(1)包括通过设定合适的轮询间隔保证在同一位置上，移动目标至少获得与三个锚节点之间的距离。

4.如权利要求1所述的一种基于轨迹平滑的室内移动目标定位方法，其特征在于，所述步骤(2)包括

设n时刻，通过UWB测得的移动目标与其中一个锚节点之间距离的真实值为r(n)，速度的真实值为v(n)；将r(n)和v(n)作为系统的状态矢量，表示为

$x\left(n\right)=\left[\begin{array}{c}r\left(n\right)\\ v\left(n\right)\end{array}\right]---\left(1\right)$

将系统的状态方程建模为

x(n)＝F₁x(n-1)+Gα(2)

其中：F₁为状态转移矩阵，表示为 $F_1=\left[\begin{array}{cc}1& T\\ 0& 1\end{array}\right];$ α为加速度项，将其建模为系统噪声，其系数矩阵为 $F_1=\left[\begin{array}{c}{T}^2/2\\ T\end{array}\right];$ T为状态更新的时间间隔；

观测方程建模为

y(n)＝H₁(n)x(n)+v(n)(3)

其中： $y\left(n\right)=\left[\begin{array}{c}{r}_m\left(n\right)\\ v_m\left(n\right)\end{array}\right],$ r_m(n)为距离的测量值，v_m(n)为速度的测量值，表示为 $v_m\left(n\right)=\frac{r_m\left(n\right)-r_m(n-1)}{T};$ 观测矩阵 $H_1\left(n\right)=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right];$ v(n)为零均值单位方差的观测噪声。

5.如权利要求1所述的一种基于轨迹平滑的室内移动目标定位方法，其特征在于，所述步骤(4)包括

n时刻系统状态矢量为u(n)＝[x(n)，y(n)，v_x(n)，v_y(n)]^T其中x(n),y(n))为n时刻的坐标，v_x(n)和v_y(n)为x轴和y轴方向上的速度；

将系统的状态方程建模为

u(n)＝F₂u(n-1)+w(n)(4)

其中：F₂为状态转移矩阵，表示为 $F_2=\left[\begin{array}{cccc}1& 0& T& 0\\ 0& 1& 0& T\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right];$ w(n)为零均值单位方差的系统噪声；T为状态更新的时间间隔；

观测方程建模为

z(n)＝H₂(n)u(n)+σ(n)(5)

其中：z(n)＝[x_t(n)，y_t(n)，v_xt(n)，v_yt(n)]^Tx_t(n)和y_t(n)为n时刻通过三边测量法得到的定位结果坐标，v_xt(n)和v_yt(n)分别为x轴和y轴方向上的速度的测量值；观测矩阵 $H_2\left(n\right)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right];$ σ(n)为零均值单位方差的观测噪声。

6.如权利要求1所述的一种基于轨迹平滑的室内移动目标定位方法，其特征在于，所述步骤(5)包括

设n时刻对三边测量的定位结果进行滤波后，得到移动目标的位置为s_r(n)，其坐标为(x_r(n)，y_r(n))，则n-1时刻滤波后移动目标的定位结果为s_r(n-1)；n时刻平滑后的移动目标的定位结果为坐标表示为(x_est(n)，y_est(n))，则n-1时刻平滑后的移动目标的定位结果为

定义相邻两个时刻三边测量结果的距离D_r(n)为

$\begin{array}{c}{D}_r\left(n\right)={|s}_r\left(n\right)-s_r(n-1)|\\ =\sqrt{[x_r\left(n\right)-x_r(n-1){]}^2+[y_r\left(n\right)-y_r(n-1){]}^2}\end{array}$

设_n时刻平滑后的定位结果为n-1时刻平滑后的定位结果和n时刻滤波后的定位结果的加权平均，定义为

${\hat{s}}^{\left(1\right)}\left(k\right)=\mathrm{\ρ}\left(n\right)\·s_r\left(n\right)+(1-\mathrm{\ρ}\left(n\right))\·{\hat{s}}^{\left(1\right)}(n-1)---\left(6\right)$

其中ρ应满足

$\mathrm{\ρ}\left(n\right)=e^{-D_r\left(n\right)}---\left(7\right).$

7.如权利要求6所述的一种基于轨迹平滑的室内移动目标定位方法，其特征在于，通过自适应平滑算法对滤波后的定位结果进行轨迹平滑，将当前时刻的定位估计值与前一时刻的定位估计值进行比较，当两者的距离越大，则当前时刻的定位估计值的权重越小；而当两者的距离较小，则当前时刻定位估计值的权重越大，通过不同时刻的权重对相邻时刻的定位估计值进行加权平均，从而实现自适应的过程。