CN105068043A - 一种无线传感器网络节点和移动机器人同时定位方法 - Google Patents

Abstract

一种无线传感器网络节点和移动机器人同时定位方法，该方法采用序贯平方根容积卡尔曼滤波算法实现节点位置未知的无线传感器网络覆盖环境中移动机器人的自定位。移动机器人通过内部传感器信息不断地进行状态预测，然后，根据移动机器人与传感器节点之间的距离对两者的预测状态进行修正。本发明提供一种无线传感器网络节点位置未知情况下的移动机器人自定位方法，避免了环境特征提取、数据关联，提高了移动机器人的对环境适应能力。

Description

一种无线传感器网络节点和移动机器人同时定位方法

技术领域

本发明涉及移动机器人自定位领域，尤其是一种基于无线传感器网络的移动机器人自定位方法。

背景技术

移动机器人自身携带丰富的传感器，对加速度计、陀螺仪、里程计等内部传感器数据进行积分运算，可以便捷地推算出移动机器人的位置信息。但是由于受累积误差的影响，移动机器人的位置估计误差将随着时间的推移不断增大，而且对初始位置的要求高，传感器数据的短暂失效都会对其产生很大影响。因此，在实际应用中，往往需要结合激光、红外、声纳、视觉等外部传感器进行位置估计的修正。但使用外部传感器存在很多困难，未知环境中环境特征的提取难度大，尤其是环境中往往存在静态特征和动态特征，这大大增加了数据关联的难度。在对未知空旷环境进行探索时，环境特征少，这时外部传感器的作用将十分有限。移动机器人对陌生环境的适应能力差，这严重阻碍了移动机器人的实际应用。无线传感器网络具有良好的自组织能力，环境适应力强，可有效地协助移动机器人进行定位以及增强其环境感知的能力，同时每个传感器节点都具有固定特有的ID，避免了复杂的数据关联过程。只要在未知环境中随机地播撒无线传感器节点，将可有效地协助移动机器人进行定位，消除累积误差的影响。目前，还没有技术可简单有效地解决移动机器人在未知环境中的自定位问题。

发明内容

为了克服已有移动机器人自定位方法对环境适应力差，环境特征难以提取以及数据关联难度大、计算复杂等不足。本发明提供一种无线传感器网络节点和移动机器人同时定位方法，避免了环境特征提取、数据关联，提高了移动机器人对环境的适应能力，且无需节点位置信息。

本发明解决其技术问题所采用的技术方案是：

一种无线传感器网络节点和移动机器人同时定位方法，所述方法包括以下步骤：

步骤1)移动机器人向单跳传输半径内传感器节点发送“定位请求”数据包；

步骤2)无线传感器网络节点收到请求后，解析收到的数据包，提取该数据包的RSSI值，随后，对该RSSI信号进行一次预滤波，得到该RSSI信号的估计值；

步骤3)通过该RSSI信号的估计值，判断是否满足发送条件。若满足发送条件，以单跳的方式发送响应数据包给移动机器人；否则，忽略该服务请求；

步骤4)移动机器人收到周围节点返回的响应数据包后，解析该数据包中的节点信息、RSSI信号；若该响应节点为已知节点，提取该节点位置信息；否则，初始化该节点位置信息并加入已知节点表；

步骤5)通过RSSI模型转换成距离信息，根据该距离信息，采用基于平方根容积卡尔曼的序贯融合估计方法估计出当前的移动机器人和响应节点的位置。

进一步，所述步骤1)中，无线传感器网络采用网状拓扑形式，所有节点都具有路由功能，协助定位节点为固定节点且节点位置未知，移动机器人为其中一个路由节点。

所述步骤1)中，所述的发送“定位请求”数据包，通过单跳的方式发送给周围的节点，即移动节点和周围节点直接通信，不经过其他路由节点的转发。单跳半径，即可直接通信的最大半径。

所述步骤2)中，所述的预滤波为其中，为k时刻传感器节点接收到的“定位请求”数据包的RSSI值，为滤波器初值，α为滤波器系数，0≤α≤1。

所述步骤3)中，所述的发送条件为：且其中，y_T，ε₁，ε₂为预先设定的阈值参数，阈值y_T为服务半径，ε₁用于判断目标位置是否变化，ε₂用于判断噪声大小，若满足上述条件，响应移动机器人请求；否则，忽略该请求。

所述步骤4)中，所述的已知节点表，其包含节点ID、节点位置及其协方差，所述的节点信息，即节点的ID，所述的节点位置信息，即节点的位置及其协方差。

在步骤5)中，所述的RSSI模型为其中，z_k为y_k对应的距离值，z_r，y_r分别为参考距离和相应的RSSI值，n为电磁波传播衰减系数。

所述步骤5)中，所述的基于平方根容积卡尔曼的序贯融合估计方法，根据接收到数据包的先后顺序，采用平方根容积卡尔曼方法逐一进行移动机器人的状态融合估计；通过内部传感器信息得到移动机器人状态的预测值，然后利用距离值对移动机器人和响应节点的位置等状态进行修正。

本发明的有益效果主要表现在：由于每个传感器网络节点具有特有的ID，避免了复杂的数据关联过程。同时，通过移动机器人和传感器节点间的主动通信，避免了对环境特征的提取过程。无线传感器网络节点可随意播散在环境中，用于环境信息采集，同时可协助移动机器人进行定位。通过RSSI信号测距，单个节点的覆盖范围大，不需要增加额外的成本，增加了节点生存周期。无需对节点做特殊的布置，适用于对未知环境的探索。相对于现有的通过自身携带的激光、视觉等外部传感器的自定位方法，该方法对环境适应性强，难度低，易于实施。

附图说明

图1是移动机器人对无线传感器节点定位的示意图。

图2是无线传感器网络协助移动机器人定位的示意图。

图3是无线传感器固定节点响应服务流程图。

图4是无线传感器网络协助移动机器人自定位流程图。

图5是基于平方根容积卡尔曼的序贯融合估计流程图。

图6是移动机器人和传感器节点状态序贯融合估计时序图。

图7是移动机器人内部传感器和响应数据包时序图。

具体实施方式

下面结合附图对本发明作进一步描述。

参照图1～图6，一种无线传感器网络协助的移动机器人自定位方法，所述方法包括以下步骤：

步骤1)移动机器人向单跳半径内传感器节点发送“定位请求”数据包；

步骤2)无线传感器网络节点收到请求后，解析响应数据包，提取该数据包的RSSI值，随后，对该RSSI信号进行一次预滤波，得到该数据包RSSI信号的估计值；

步骤3)通过该RSSI信号的估计值，判断是否满足发送条件。若满足发送条件，以单跳的方式发送响应数据包给移动机器人；否则，忽略该服务请求；

步骤4)移动机器人收到周围节点返回的响应数据包后，解析该数据包中的节点信息、RSSI信号，若该响应节点为已知节点，提取该节点位置等信息；否则，初始化该节点位置信息并加入已知节点表；

步骤5)通过RSSI模型转换成距离信息，根据该距离信息，按照数据包的先后顺序采用平方根容积卡尔曼方法逐一进行移动机器人的状态融合估计。平方根容积卡尔曼方法分为两部分，首先，通过内部传感器信息得到移动机器人状态的预测值；然后，利用得到距离值对移动机器人和响应节点的位置等状态进行修正。

重复执行步骤1)-5)，实现对移动机器人和传感器节点的同时定位。

所述步骤2)中，所述的预滤波为其中，为k时刻传感器节点接收到的“定位请求”数据包的RSSI值，为滤波器初值，α为滤波器系数，0≤α≤1。

所述步骤3)中，所述的发送条件为：且其中，y_T，ε₁，ε₂为预先设定的阈值参数，阈值y_T为服务半径，ε₁用于判断目标位置是否变化，ε₂用于判断噪声大小，若满足上述条件，响应移动机器人请求；否则，忽略该请求。

在步骤5)中，所述的RSSI模型为其中，z_k为y_k对应的距离值，z_r，y_r分别为参考距离和相应的RSSI值，n为电磁波传播衰减系数。

如图1和图2所示，无线传感器网络节点播撒在环境中，可用于采集环境信息，同时协助移动机器人进行自定位。图1描述了移动机器人对传感器节点进行定位的过程。移动机器人根据内部传感器获得自身位姿信息，然后通过与传感器节点的多次通信测距，对传感器节点进行定位。随着测量次数的不断增多，节点位置的不确定性不断减少。图2描述了无线传感器网络协助的移动机器人自定位的过程。该系统采用网状拓扑结构，所有节点都具有路由功能，其中移动机器人为传感器网络中移动节点，其他节点为固定节点，固定节点位置未知。移动节点通过路由探索，以单跳的方式发送给周围固定节点。图3描述了无线传感器节点响应移动机器人请求的流程图。当固定节点接收的定位请求满足响应条件，发送定位服务数据包给相应的移动机器人。移动机器人和传感器节点定位流程图如图4-5所示。移动机器人和无线传感器网络系统的模型可由如式(1)和(2)所示的状态空间模型描述：

x_k+1＝x_k+u_k+w_k(1)

$z_k^{Si}=h^{Si}(x_k,x^{Si})+v_k---\left(2\right)$

其中，为系统状态，为移动机器人的位置和方向，为传感器节点Si位置状态。w_k为系统过程噪声， $u_k=\left[\begin{array}{c}{u}_k^R\\ 0\end{array}\right],$ 为移动机器人的内部传感器信息。为移动机器人对传感器节点Si的观测值(距离)，为其观测方程。n_x为系统状态的维数，n_z为观测向量的维数。w_k，v_k为零均值、协方差阵分别为Q和R的不相关高斯白噪声，其中， $Q=\left[\begin{array}{cc}{Q}^R& \\ & Q^s\end{array}\right],$ Q^R为移动机器人过程噪声协方差，Q^s为传感器节点过程噪声协方差。初始状态和初始方差分别为和P_0|0，且与w_k，v_k统计独立。

移动机器人进行序贯融合估计流程图如图5所示，其中，平方根容积卡尔曼方法描述如下：

移动机器人状态预测过程：

第1步：生成k-1系统状态估计值的容积点

$X_{i,k-1|k-1}=S_{k-1|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k-1|k-1},i=1,2,...,2n_x---\left(3\right)$

其中， P_k-1|k-1为k-1时刻系统状态估计值及其估计协方差。

第2步：计算过程传播容积点

$X_{i,k-1|k-1}^{*}=X_{i,k-1|k-1}+u_{k-1}---\left(4\right)$

第3步：计算状态预测值

${\hat{x}}_{k|k-1}=\frac{1}{2n_x}\underset{i=1}{\overset{2n_x}{\Σ}}{X}_{i,k-1|k-1}^{*}---\left(5\right)$

第4步：计算状态预测协方差的平方根

$S_{k|k-1}=Tria\left(\left[\begin{array}{cc}{\mathrm{\χ}}_{k|k-1}^{*}& S_{Q,k-1}\end{array}\right]\right)---\left(6\right)$

其中， ${\mathrm{\χ}}_{k|k-1}^{*}=\frac{1}{\sqrt{2n_x}}\left[\begin{array}{cccc}{X}_{1,k|k-1}^{*}-{\hat{x}}_{k|k-1}& X_{2,k|k-1}^{*}-{\hat{x}}_{k|k-1}& \mathrm{...}& X_{2n_x,k|k-1}^{*}-{\hat{x}}_{k|k-1}\end{array}\right],$ Tria(·)为三角分解，即S_k|k-1是三角阵；

移动机器人和传感器节点状态修正过程：

第5步：生成系统状态预测值的容积点

$X_{i,k|k-1}=S_{k|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k|k-1},i=1,2,...,2n_x---\left(7\right)$

第6步：计算量测传播容积点

Z_i,k|k-1＝h^Si(X_i,k|k-1,x^Si)(8)

第7步：计算量测预测值

${\hat{z}}_{k|k-1}=\frac{1}{2n_x}\underset{i=1}{\overset{2n_x}{\Σ}}{Z}_{i,k-1|k-1}---\left(9\right)$

第8步：计算量测预测协方差

S_zz,k|k-1＝Tria([Z_k|k-1S_R,k])(10)

其中， $Z_{k|k-1}=\frac{1}{\sqrt{2n_x}}\left[\begin{array}{cccc}{Z}_{1,k|k-1}-{\hat{z}}_{k|k-1}& Z_{2,k|k-1}-{\hat{z}}_{k|k-1}& \mathrm{...}& Z_{2n_x,k|k-1}-{\hat{z}}_{k|k-1}\end{array}\right],$ $S_{R,k-1}=\sqrt{R}.$

第9步：计算状态和量测的互协方差

$P_{xz,k|k-1}={\mathrm{\χ}}_{k|k-1}{Z}_{k|k-1}^T---\left(11\right)$

其中， ${\mathrm{\χ}}_{k|k-1}=\frac{1}{\sqrt{2n_x}}\left[\begin{array}{cccc}{X}_{1,k|k-1}-{\hat{x}}_{k|k-1}& X_{2,k|k-1}-{\hat{x}}_{k|k-1}& \mathrm{...}& X_{2n_x,k|k-1}-{\hat{x}}_{k|k-1}\end{array}\right]$

第10步：计算滤波增益

$W_k=(P_{xz,k|k-1}/S_{xz,k|k-1}^T)/S_{xz,k|k-1}---\left(12\right)$

第12步：计算系统状态估计值

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+W_k(z_k-{\hat{z}}_{k|k-1})---\left(13\right)$

第13步：计算系统状态的估计协方差

S_k|k＝Tria([χ_k|k-1-W_kZ_k|k-1W_kS_R,k])(14)

注：其中，若n_x＝4，[1]_i为集合[1]中的一个元素，其中， $\[1\]=\left\{\left(\begin{array}{c}1\\ 0\\ 0\\ 0\end{array}\right),\left(\begin{array}{c}0\\ 1\\ 0\\ 0\end{array}\right),\left(\begin{array}{c}0\\ 0\\ 1\\ 0\end{array}\right),\left(\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right),\left(\begin{array}{c}-1\\ 0\\ 0\\ 0\end{array}\right),\left(\begin{array}{c}0\\ -1\\ 0\\ 0\end{array}\right),\left(\begin{array}{c}0\\ 0\\ -1\\ 0\end{array}\right),\left(\begin{array}{c}0\\ 0\\ 0\\ -1\end{array}\right)\right\}.$

如图7所示，当内部传感器速率大于外部传感器速率时，重复执行式(3)-(6)，即从k时刻预测到k₁时刻，经过多次迭代预测，直至预测到k+1时刻。其中，m是内部传感器速率相对于外部传感器速率的倍数。然后通过k+1时刻收到的RSSI值，执行式(7)-(14)对预测过程进行修正。如图6所示，移动机器人根据时间先后顺序，收到一个定位响应数据包，进行一次状态修正过程。

Claims (8)

1.一种无线传感器网络节点和移动机器人同时定位方法，其特征在于：所述方法包括以下步骤：

步骤1)移动机器人向单跳传输半径内传感器网络节点发送“定位请求”数据包；

步骤2)无线传感器网络节点收到请求后，解析收到的数据包，提取该数据包的RSSI值，随后，对该RSSI信号进行一次预滤波，得到该数据包RSSI信号的估计值；

步骤3)通过该RSSI信号的估计值，判断是否满足发送条件，若满足发送条件，以单跳的方式发送响应数据包给移动机器人；否则，忽略该服务请求；

步骤4)移动机器人收到周围节点返回的响应数据包后，解析该数据包中的节点信息、RSSI信号，若该响应节点为已知节点，提取该节点位置信息；否则，初始化该节点位置并加入已知节点表；

步骤5)通过RSSI模型转换成距离信息，根据该距离信息，采用基于平方根容积卡尔曼的序贯融合估计方法估计出当前的移动机器人和响应节点的位置信息。

2.如权利要求1所述的无线传感器网络协助的移动机器人自定位方法，其特征在于：所述步骤1)中，无线传感器网络采用网状拓扑形式，所有节点都具有路由功能，协助定位节点为固定节点且节点位置未知，移动机器人为其中一个路由节点；所述的发送“定位请求”，通过单跳的方式发送给周围的节点。

3.如权利要求1或2所述的无线传感器网络协助的移动机器人自定位方法，其特征在于：所述步骤2)中，所述的预滤波为其中，为k时刻传感器节点接收到的“定位请求”数据包的RSSI值，为滤波器初值，α为滤波器系数，0≤α≤1。

4.如权利要求3所述的无线传感器网络协助的移动机器人自定位方法，其特征在于：所述步骤3)中，所述的发送条件为：且其中，y_T，ε₁，ε₂为预先设定的阈值参数，阈值y_T为服务半径，ε₁用于判断目标位置是否变化，ε₂用于判断噪声大小。

5.如权利要求1或2所述的无线传感器网络协助的移动机器人自定位方法，其特征在于：所述步骤4)中，所述的已知节点表，包含节点ID、节点位置及其协方差。

6.如权利要求4所述的无线传感器网络协助的移动机器人自定位方法，其特征在于：在步骤5)中，所述的RSSI模型为其中，z_k为y_k对应的距离值，z_r，y_r分别为参考距离和相应的RSSI值，n为电磁波传播衰减系数。

7.如权利要求1或2所述的无线传感器网络协助的移动机器人自定位方法，其特征在于：所述步骤5)中，所述的基于平方根容积卡尔曼的序贯融合估计方法，根据接收到响应数据包的先后顺序，采用平方根容积卡尔曼方法对得到的距离信息逐一进行融合估计。

8.如权利要求7所述的无线传感器网络协助的移动机器人自定位方法，其特征在于：所述步骤5)中，所述的平方根容积卡尔曼方法，其可分为两部分，首先，通过内部传感器信息得到移动机器人状态的预测值或者多次迭代预测得到其状态的预测值；然后，利用距离值对移动机器人和响应节点的位置等状态进行修正。