CN106123892A - 一种基于无线传感器网络与地磁地图的机器人定位方法 - Google Patents

Abstract

本发明涉及一种基于无线传感器网络与地磁地图的机器人定位方法，设定个无线传感器网络节点，机器人移动过程中，通过激活个无线传感器网络节点，利用RSSI得出无线传感器网络节点与机器人的观测模型；再借助PSO及多传感器信息融合来得到粒子似然度；同时得出粒子群优化方法中粒子的权重；之后进行粒子重采样及适应度函数判断，得到最优粒子的位置，获得局部地磁图，并利用地磁匹配方法实现精准定位，最后得到绝对位置。本发明数据关联容易，观测时不会出现错误的关联并且提高了网络的鲁棒性与精确性；同时获得更加真实的系统状态分布预测，使得机器人定位更加精确，减少了地磁匹配的运算量，增加了方法的实时性。

Description

一种基于无线传感器网络与地磁地图的机器人定位方法

技术领域

本发明专利涉及移动机器人室内定位领域，具体涉及一种基于无线传感器网络与地磁地图的机器人定位方法。

背景技术

随着我国现代化技术的不断发展，机器人技术得到了迅速发展，机器人的应用也越来越广泛。目前现有的机器人定位系统定位计算的复杂性大、实时性有效性不高、存在定位误差、相对观测量的增加与定位精度不成比例。无线传感器网络(wireless sensornetwork，后续简称WSN)环境下实时定位是移动机器人导航领域的一项关键技术，WSN引入机器人定位的优点为传感器网络节点可以作为机器人定位的特定标识，简化了传统SLAM(simultaneous localization and mapping，即时定位与地图构建)中的数据关联问题。

无线传感器网络是计算机、通信和传感器多领域技术相结合的产物，也是将信息获取(传感)、信息传递与信息处理的产物，它是将部署在监测区域，大量的具有有限计算能力的微型传感器节点相互协作构成的一个多跳自组织网络。传统无线传感器网络环境下的移动机器人定位方法，大多是结合无线传感器进行通信定位，但由于室内环境复杂多变，通过无线定位存在包括无线通信丢包、室内桌椅摆设各异、人走动随机性强等众多因素造成定位效果不佳，信号传播实时特征难以扑获、数据包丢失严重等问提，因而这种定位方法定位精度较低，定位效果不佳。地球近地表及表层分布的地磁场随不同的区域而变化，但这种特性构成了不同地域的一种特有的性质。地磁场匹配作为一种无源、隐蔽的导航技术，具有跨平原、抗干扰、全天时的特点，同时具有误差不随时间积累的特点。

发明内容

本发明要解决的技术问题是：为克服移动机器人定位过程中数据丢失和数据随机性强等问题，提供一种基于无线传感器网络与地磁地图的机器人定位方法，以获得较高的移动机器人室内定位精度。

为解决上述问题，本发明采用如下技术方案：

一种基于无线传感器网络与地磁地图的机器人定位方法，其特征在于：首先设定n个无线传感器网络节点，机器人移动过程中，通过激活n个无线传感器网络节点，利用RSSI得出无线传感器网络节点与机器人的观测模型；再借助粒子群优化方法及多传感器信息融合来得到粒子似然度，同时得出粒子群优化方法中粒子的权重；之后进行粒子重采样及适应度函数判断，得到最优粒子的位置，获得局部地磁图，并利用地磁匹配方法实现精准定位，最后得到绝对位置。

上述技术方案，具体包括如下步骤：

步骤S1：建立地磁基准图：利用地磁采集模块采集地磁数据，遍历室内整个区域，采用网格状方式进行测量地磁基准图；采集地磁数据时相邻采集点之间的间隔形成一个具有长度和宽度的网格，在网格的顶点处采集地磁数据，每个点以“地磁数据+位置坐标”的形式存入数据库，为机器人定位时地磁匹配过程做准备；

步骤S2：设定粒子群优化方法中涉及的各参数：包括最大迭代次数gen_max，空间维数d，学习因子c₁,c₂，权系数最大最小值(min,max)，设置一个最大速度v_max，随机初始化粒子的位置及速度，设粒子群中第i个粒子的位置表示为xⁱ＝(xⁱ¹,xⁱ²,…,x^id),第i个粒子的速度表示为vⁱ＝(vⁱ¹,vⁱ²,…,v^id)；

步骤S3：在室内区域布置n个无线传感网络节点，利用无线网络传感器对移动机器人进行定位采样，在k时刻，机器人X_k(x_k,y_k,z_k)在移动的过程中，包括机器人本身无线传感器节点在内共有n个无线传感器节点被激活，且位置已知，只在机器人到达周围无线传感器网络感知范围时节点才被激活，x_k,y_k,z_k分别表示k时刻机器人的横坐标，纵坐标，和高度；

步骤S4：由机器人坐标x_k,y_k,z_k以及被激活的n个无线传感器节点的坐标得到机器人的观测模型，求得n个节点对应的似然度；

步骤S5：因激活n个无线传感器节点，用步骤S4的似然度得到一组权重值：{w_1k,w_2k,…,w_nk}；

步骤S6：判断是否进行重采样，通过重采样的方式将步骤S5中权重较大的粒子留下来，舍去权重较小的粒子，将粒子集中到高似然区域，以提高对将来状态的估计，重采样则回到步骤S3；

步骤S7：若不进行重采样，对于每一个粒子，评价其适应度Fitness，预估计的位置偏离机器人的真实位置，需要再次优化粒子的权重，返回步骤S5；

步骤S8：利用速度与位置关系对粒子新的预测位置进行更新；

步骤S9：判断更新是否结束：通过不断迭代判断更新是否结束，若迭代次数已达到最大迭代次数，则认为最后一次迭代的位置即为机器人的最优位置；

步骤S10：在获得最优位置后，采用地磁匹配方法进行局部地磁数据的遍历以进一步精确机器人的位置，通过地磁匹配方法求取相关性，相关性最高的为最优解，即机器人的精准位置。

上述技术方案中，步骤S1中，地磁采集模块的磁力计测得的磁场磁矢量的大小与方向的关系如下：

M(r,t)＝M_m(r,t)+M_c(r)+M_d(r,t) (1-1)

式中，M表示磁场总强度，M_m表示主地磁场、M_c表示地壳地磁场，M_d表示干扰地磁场；r表示载体所处位置，t表示时间)；将磁矢量从三维量转化为二维量：M、H、φ四要素与M在地球坐标系三个轴上投影Mx,My,Mz的关系如下(1-2)式：

(1-2)式中，磁场总强度用M表示，将它投射到东北平面得到H，称作水平强度；将H投射到东西向轴，得到东西向场强Mx；同理，将H投射到南北向轴，得到南北向场强My；其中表示磁偏角，φ表示磁倾角。

上述技术方案中，步骤S2中，涉及到的各参数如下：

(1)粒子数N及维数d：粒子数N的多少直接决定了方法的复杂性，空间维数决定了在二维还是在三维空间下；

(2)学习因子c₁，c₂：使粒子具有自我学习和参照群体优异值的能力，从而使粒子向局部和全体最优解移动，c₁＝c₂＝2；

(3)迭代次数gen_max：粒子在解空间内不断跟踪个体极值与全局极值并进行搜索,直到达到规定的最大迭代次数，也就是跟踪的次数，通常迭代次数越大对应的寻求精度越精确，但也要考虑时效性。

上述技术方案中，步骤S4中，被激活的这n个节点用表示，设第j个节点对应的坐标为则节点j与机器人的距离为：

$d_k^j={10}^{(-RSSI-C)/\left(10\mathrm{\ζ}\right)}---(2-1)$

其中，ζ为路径衰减因子(常数2～5)，RSSI为信号强度衰减值，C为在1m处的接受信号耗损，RSSI可由以下表达式表示(2-2)：

$RSSI=G\left(d_k^j\right)=p+g-\[pl\left(d_0\right)+10\mathrm{\ζ}lg\left(\frac{d_k^j}{d_0}\right)\]---(2-2)$

式(2-2)中，p为发射功率(dB)，g为天线的增益(dm),ζ为路径衰减因子(常数2～5)，pl(d₀)为距离节点d₀(参考距离1m)传播耗损(dB)；d_kj为机器人真实位置距节点M₁＝(x_m1,y_m1)的距离，pl(d₀)的求法可参考如下式所述(2-3)：

$pl\left(d_0\right)=10pl\[\frac{p}{w}\]=10lg{\[\frac{4{\mathrm{\πfd}}_0}{c}\]}^n=...10lg{\[\frac{4\mathrm{\π}f}{c}\]}^n+10nlg\left(d_0\right)---(2-3)$

w是接收信号功率dB，f是数据传送频率Hz，c标定位光速；式(2-3)中，传播耗损与传播距离的对数pl(d₀)是线性的关系，在d₀逐渐增大中，pl(d₀)逐渐趋于平缓，但实际情况中，由于阻碍、反射、天线增益等因素，pl(d₀)与距离的关系可能发生剧烈的震荡；由(2-1)，(2-2)，(2-3)得到后，再求出观测到的机器人X_k和节点j之间距离的均值和方差

$u\left(d_k^j\right)=\underset{j=1}{\overset{n}{\Σ}}\sqrt{{(X_K^j-x_k)}^2+{(Y_K^j-y_k)}^2+{(Z_K^j-z_k)}^2}/n---(2-4)$

$\mathrm{\σ}\left(d_k^j\right)=E\left\{{\[d_k^j-u\left(d_k^j\right)\]}^2\right\}---(2-5)$

得到机器人的观测模型为：

$p\left(z_k^j\right|x_k^i)=\frac{1}{\mathrm{\σ}\left(d_k^j\right)\sqrt{2\mathrm{\π}}}{e}^{-({\mathrm{RSSI}}_j-u(d_k^j)2/(2\mathrm{\σ}{\left(d_k^j\right)}^2)}---(2-6)$

由此可得这n个节点对应的似然度为：

$p\left(Z_k\right|x_k^i)=\underset{j=1}{\overset{n}{\Π}}p\left(z_k^j\right|x_k^i)---(2-7)$

其中为k时刻粒子的坐标；

上述技术方案中，步骤S5中，利用一组带有权重的采样点来趋近状态的后验概率密度函数；粒子的权重表示为：

$w_k^i=w_{k-1}^i\frac{p\left(Z_k\right|x_k^i)p\left(x_k^i\right|x_{k-1}^i)}{\mathrm{\π}\left(x_k^i\right|x_{k-1}^i,Z_{1:k})}---(3-1)$

其中，为重要性函数，表示粒子群优化中粒子权重与机器人空间位置及节点观测模型的关系，并将式(2-7)代入式(3-1)，可知激活的节点更新粒子的权重表示为：

$w_{jk}^i\∝w_{jk-1}^{i}p\left(z_k^j\right|x_k^i),i\∈\[1,N\]---(3-2)$

总共选取n节点，每个节点都对粒子产生影响，那么每个粒子的得到一组权重：

上述技术方案中，步骤S6中利用重采样驱动粒子集向高似然区域运动，得到粒子的权重后，很多粒子的权值过大或过小，这些权值过小的粒子在计算中，增加了计算时间且对结果无益，判断条件为N_eff是否在权系数最大最小值(min，max)内：

$N_{eff}=1/{\Σ}_{N_i=1}^N{\left({\tilde{w}}^{\left(N_i\right)}\right)}^2---(4-1)$

N为粒子的个数，为粒子权重的归一化，进行重采样我们选定的采样函数为用来对粒子进行重采样，其中i为粒子序号，重采样后粒子的权值都设定为1/N。

上述技术方案中，步骤S7中，当高权重粒子在多次迭代过程中逐步趋近高似然区域的时候，用适应度函数来判断粒子群优化方法对移动机器人预测位置的趋近真实位置的程度，适应度函数如下：

$Fitness=\exp \{-sqrt\[(z_k-z_{ik}^j)\·R_k^{-1}\·{(z_k-z_{ik}^j)}^T/c_3\]\}---(5-1)$

其中R_k表示机器人的观测噪声协方差矩阵，表示j时刻机器人对粒子i观测值的预测，可通过预测位置值和已经观测到的粒子位置m_k计算得到，在估计移动机器人位置的基础上，计算出该特征相对于预估计位置的预测观测值设定一个阈值δ：当Fitness≥δ，则可以判断更新后的位置已经在机器人的真实位置附近；若Fitness≤δ，则表明预估计的位置偏离机器人的真实位置，需要再次优化粒子的权重。

上述技术方案中，步骤S8中，粒子群中第i个粒子的位置表示为x_i＝(x_i1,x_i2,…,x_id),第i个粒子的速度表示为v_i＝(v_i1,v_i2,…,v_id)，粒子适应度最接近阈值的作为最好，第i个粒子迄今为止搜索的最好位置记为p_i＝(p_i1,p_i2,…,p_id)，整个粒子迄今为止搜索到的最好位置记为p_g＝(p_g1,p_g2,…,p_gd)，对于每一个粒子，速度与位置方程对粒子新的预测位置进行更新，根据如下等式进行d维的位置和速度的更新：

v_i(t+1)＝wv_i(t)+c₁r₁(p_i-x_i(t))+c₂r₂(p_g-x_i(t)) (6-1)

x_i(t+1)＝x_i(t)+v_i(t+1) (6-2)

r₁，r₂表示(0,1)之间均匀分布的随机数；v_i∈[-v_max,v_max]，v_max是先前设定的最大移动速度。

上述技术方案中，步骤S10中，在获得最优位置后，在无线传感网络粒子群优化作用下，机器人获得一个相对位置(x_i1,x_i2)，由此相对位置计算出需要匹配的局部地磁图的遍历范围为遍历中，使用MSD地磁匹配进行相关性匹配，找到相关性最高的一组地磁数据，即D(u,v)最大的一组数据，对应的位置坐标即为机器人的精准位置，其中MSD地磁匹配方法如下：

$D(u,v)=\frac{1}{N}\underset{l=1}{\overset{N}{\Σ}}{(N_{uv+1}-m_l)}^2---(6-3)$

N_uv表示基准图中位置(u,v)上的N维特征向量；m表示实时测量的N维特征向量；N表示相关数据的总点数；D(u,v)为地磁匹配相关函数，N_{u v+l}表示基准数据库的位置(u,v+l)上的特征量，m_l表示实时测量的第l个特征量。

本发明设定n个无线传感器网络节点，其中包括机器人本身配置的一个无线传感器节点；机器人移动过程中，通过激活n个无线传感器网络节点，利用RSSI(Receive SignalStrength Indicator接收信号强度指示)得出无线传感器网络节点与机器人的观测模型再借助PSO及多传感器信息融合来得到粒子似然度；同时得出粒子群优化方法中粒子的权重表达式之后进行粒子重采样及适应度函数判断，得到最优粒子的位置，获得局部地磁图，并利用地磁匹配方法实现精准定位，最后得到绝对位置。该方法在无线传感器网络环境下结合粒子群优化与地磁地图机器人定位方法，引入重采样，解决了粒子耗尽问题，保证了种群的多样性，并且引入了惯性权重解决了粒子退化的问题。采用无线传感器网络进行机器人位置初定位，同时结合地磁地图和地磁匹配方法进行绝对定位，即在无线传感网络获得的相对位置上进行局部地磁地图搜索并通过MSD地磁定位方法匹配最佳磁场点，以获取较准确的移动个体所处位置参数，可以降低求解问题的空间维数，在高斯噪声下有效提高了地磁信息匹配效率，利用地球磁场在不同点的差异化信息，并通过选择适当的地磁匹配方法，能获得较高的室内定位的精度移动机器人定位精度。

相对于现有技术，本发明用无线传感器进行辅助定位，数据关联容易，观测时不会出现错误的关联并且观测时不再需要节点来进行定位，提高了网络的鲁棒性与精确性；同时，利用PSO驱动所有的粒子向高似然概率区域运动，多次迭代更新粒子状态，在粒子估计时充分考虑个体粒子和群体粒子共同的影响，从而获得更加真实的系统状态分布预测，使得机器人定位更加精确，运用这种手段获得机器人的相对位置，进而从地磁图中更新出局部地磁图用于地磁匹配过程，减少了地磁匹配的运算量，增加了方法的实时性。

附图说明

图1为本发明涉及的地磁模型的关系图；

图2为本发明涉及RSSI移动机器人观测模型图；

图3为本发明涉及的PSO方法流程图；

图4为本发明涉及的PSO与地磁匹配定位方案图；

图5为本发明涉及的PSO与地磁图定位方法流程图。

具体实施方式

为使本发明的上述目的、特征和优点能够更加明显易懂，下面对照附图1-5对本发明的基于无线传感器网络与地磁地图的机器人定位方法详细说明：

根据本发明实施的基于无线传感器网络与地磁地图的机器人定位方法总体流程如图5。其特征在于包括如下步骤：

步骤S1：对室内区域利用地磁采集模块MPU6050采集地磁数据，并保存至机器人SQLserver数据库中，其中，采集点之间的间隔为1m，即形成一个的网络，在网格的顶点处采集地磁数据，每个点以地磁数据+位置坐标的形式存入数据库，为机器人定位时地磁匹配过程做准备，地磁传感器用于感知载体磁场强度和方向，还能定位载体的方位，其实磁力计的工作原理跟指南针的工作原理在很多方面都很类似，可以感知出当前载体与东南西北四个方向上的夹角，本发明涉及的地磁场是自然场它们都为矢量，有大小以及方向之分，而这两个矢量会由所处空间及时间的改变而不断变化。通常描述地磁场时，只研究总磁场强以及每个分量。中国地处地球北极即地理南极，地磁强度由太空斜向下，地理坐标系的创立及各个矢量如图1；

步骤S2：根据粒子群优化方法(简称PSO)流程图3设定粒子群优化方法中涉及的各参数：(1)粒子数N及维数d：粒子数PopSize的多少直接决定了方法的复杂性，较简单的问题通常10个粒子足够，较复杂的问题粒子数有时可取到100，空间维数决定了是在二维还是三围环境下。

(2)学习因子c₁，c₂：使粒子具有自我学习和参照群体优异值的能力，从而使粒子向局部和全体最优解移动，通常c₁＝c₂＝2。

(3)迭代次数gen_max：粒子就在解空间内不断跟踪个体极值与全局极值并进行搜索,直到达到规定的最大迭代次数，也就是跟踪的次数，通常迭代次数越大对应的寻求精度越精确，但也要考虑方法时效性。

本实施例中，设置重要参数为：c₁＝2，c₂＝2，种群大小pop_size＝40，最大迭代次数gen_max＝80，权系数最大值0.9，最小为0.4，v_max＝1m/s步骤S3：机器人移动时采用的观测噪声和状态噪声分别是高斯白噪声和均匀分布的噪声，噪声由通带截止频率为wp＝[2.5e63.5e6]，阻带截止频率为ws＝[2e64e6],通带最大衰减1dB和阻带最小衰减60dB的一个巴特沃斯滤波器产生。放置7个节点并测量节点所处的位置坐标，接着保证移动机器人在二维无障碍事先选定区域中运动，每个节点和机器人配备的无线传感器为ZigBee2430，其精度达到0.24m，如图2；

步骤S4：利用无线网络传感器对移动机器人进行定位采样，在k时刻，机器人在移动的过程中，周围有n个无线传感器节点被激活(包括周围机器人身上的节点)且位置已知，只在机器人到达周围无线传感器网络感知范围时节点才被激活，得到机器人的观测模型，求得n个节点对应的似然度；

步骤S5：利用一组带有权重的采样点来趋近状态的后验概率密度函数，因激活n个无线传感器节点，用上述似然度得到一组权重值：

步骤S6：判断是否进行重采样，通过重采样的方式将S3中权重较大的粒子留下来，舍去权重较小的粒子，将粒子集中到高似然区域，以提高对将来状态的估计，重采样则回到步骤S3；

步骤S7：若不进行重采样，对于每一个粒子，若不进行重采样，对于每一个粒子，评价其适应度Fitness，预估计的位置偏离机器人的真实位置，需要再次优化粒子的权重，返回S5；

步骤S8：速度与位置方程对粒子新的预测位置进行更新；

步骤S9：判断是否迭代结束，若迭代次数已达到最大迭代次数，则将最后一次的位置；

步骤S10：获得最优位置后，采用地磁匹配方法进行局部地磁数据的遍历，通过地磁匹配方法求取相关性，相关性最高的为机器人的精准位置。

上述技术方案中，步骤S1中得到r位置、t时刻时磁场总强度M，地磁采集模块的磁力计测得的磁场磁矢量的大小与方向的关系如下：

M(r,t)＝M_m(r,t)+M_c(r)+M_d(r,t) (1-1)

主地磁场M_m、表示地壳地磁场M_c，M、H、φ四要素在三个分轴上投影(Mx,My,Mz)的联系如下：

上述技术方案中，步骤S4中，机器人周围被激活的这7个节点第j个节点对应的坐标为分别得到为观测到的机器人X_k和节点j之间距离的均值和方差

节点j与机器人的距离为：

$d_k^j={10}^{(-RSSI-C)/\left(10\mathrm{\ζ}\right)}---(2-1)$

$RSSI=G\left(d_k^j\right)=p+g-\[pl\left(d_0\right)+10\mathrm{\ζ}lg\left(\frac{d_k^j}{d_0}\right)\]---(2-2)$

其中:

$pl\left(d_0\right)=10pl\[\frac{p}{w}\]=10lg{\[\frac{4{\mathrm{\πfd}}_0}{c}\]}^n=...10lg{\[\frac{4\mathrm{\π}f}{c}\]}^n+10nlg\left(d_0\right)---(2-3)$

由(2-1)，(2-2)，(2-3)得到后，再求出观测到的机器人X_k和节点j之间距离的均值和方差

$u\left(d_k^j\right)=\underset{j=1}{\overset{n}{\Σ}}\sqrt{{(X_K^j-x_k)}^2+{(Y_K^j-y_k)}^2+{(Z_K^j-z_k)}^2}/n---(2-4)$

$\mathrm{\σ}\left(d_k^j\right)=E\left\{{\[d_k^j-u\left(d_k^j\right)\]}^2\right\}---(2-5)$

得到机器人的观测模型为：

$p\left(z_k^j\right|x_k^i)=\frac{1}{\mathrm{\σ}\left(d_k^j\right)\sqrt{2\mathrm{\π}}}{e}^{-({\mathrm{RSSI}}_j-u(d_k^j)2/(2\mathrm{\σ}{\left(d_k^j\right)}^2)}---(2-6)$

由此可得7个节点对应的似然度：

$p\left(Z_k\right|X_k^i)=\underset{j=1}{\overset{7}{\Π}}p\left(z_k^j\right|X_k^i)---(2-7)$

上述技术方案中，步骤S5中，利用一组带有权重的采样点来趋近状态的后验概率密度函数。得到粒子的权重：

$w_k^i\∝\frac{p\left(X_{0\·k}^i\right|Z_{0\·k})}{\mathrm{\π}\left(X_{0:k}^i\right|Z_{0:k})}=w_{k-1}^i\frac{p\left(Z_k\right|X_k^i)p\left(X_k^i\right|X_{k-1}^i)}{\mathrm{\π}\left(X_k^i\right|X_{k-1}^i,Z_{1:k})}---(3-1)$

接着可知激活的节点更新粒子的权重为：

$w_{jk}^i\∝w_{jk-1}^{i}p\left(z_k^j\right|X_k^i),i\∈\[1,80\]---(3-2)$

因激活n个无线传感器节点，总共选取n节点，每个节点都对粒子产生影响，那么每个粒子的得到一组权重：本实施例中总共选取80个节点，每个节点都对粒子产生影响，那么每个粒子的得到一组权重：上述技术方案中，步骤S6中利用重采样驱动粒子集向高似然区域运动，更新完粒子的权重后，判断条件为N_eff是否在范围(0.4,0.9)内，其中:

$N_{eff}=1/{\Σ}_{N_i=1}^N{\left({\tilde{w}}^{\left(N_i\right)}\right)}^2---(4-1)$

重采样后粒子的权值都设定为

上述技术方案中，步骤S7中，搜索空间为d＝2维，粒子群中第i个粒子的位置表示为x_i＝(x_i1,x_i2)，第i个粒子的速度为v_i＝(v_i1,v_i2)，第i个粒子迄今为止搜索的最好位置记为p_i＝(p_i1,p_i2)，整个粒子迄今为止搜索到的最好位置记为p_g＝(p_g1,p_g2)。评价每个粒子的适应度，适应度函数如下：

$Fitness=\exp \{-sqrt\[(z_k-z_{ik}^j)\·R_k^{-1}\·{(z_k-z_{ik}^j)}^T/c_3\]\}---(5-1)$

将当前各粒子的位置和适应度存储在各微粒的pbest中，将所有的pbest中适应值最优个体的位置和适应值存储于gbest中，对于每一个粒子速度与位置方程对粒子新的预测位置进行更新。更新过程根据如下式变化：

v_i(k+1)＝wv_i(t)+c₁r₁(p_i-x_i(t))+c₂r₂(p_g-x_i(t)) (6-1)

x_i(t+1)＝x_i(t)+v_i(t+1) (6-2)

上述技术方案中，步骤S9中，在无线传感网络粒子群优化作用下，机器人获得一个相对位置(x_i1,x_i2)，由此位置计算出需要匹配的局部地磁图的遍历范围为遍历中，使用MSD地磁匹配进行相关性匹配，找到相关性最高的一组地磁数据，对应的位置坐标即为机器人的精准位置，其中MSD地磁匹配方法如下：

$D(u,v)=\frac{1}{N}\underset{l=1}{\overset{N}{\Σ}}{(N_{uv+1}-m_l)}^2---(6-3)$

当进行多次迭代后，粒子汇集于一点，即机器人最优的位置，在一定误差的允许下基本上满足了室内环境下的定位精度，解决了粒子重采样时经常出现的粒子退化问题。为了获得更高的定位精度，采用局部地磁匹配的定位方法，在已知机器人相对位置的情况下，结合地磁图使用MSD地磁匹配方法能进一步提高机器人的定位精度，无线传感器网络与地磁图的机器人定位，驱动所有的粒子向高似然概率区域运动，多次迭代更新粒子状态，减少了冗余节点的影响，实现了节点间的信息融合，为地磁匹配提供了一个基准点，减少了地磁匹配的计算量，提高了系统的实时性，实验仿真表明本方法有效的抑制了系统的累积误差，同时地磁匹配方法提高了定位的精度，能从一定程度上解决室内定位的问题。

Claims (10)

1.一种基于无线传感器网络与地磁地图的机器人定位方法，其特征在于：首先设定n个无线传感器网络节点，机器人移动过程中，通过激活n个无线传感器网络节点，利用RSSI得出无线传感器网络节点与机器人的观测模型；再借助粒子群优化方法及多传感器信息融合来得到粒子似然度，同时得出粒子群优化方法中粒子的权重；之后进行粒子重采样及适应度函数判断，得到最优粒子的位置，获得局部地磁图，并利用地磁匹配方法实现精准定位，最后得到绝对位置。

2.根据权利要求1所述的基于无线传感器网络与地磁地图的机器人定位方法，其特征在于具体包括如下步骤：

步骤S1：建立地磁基准图：利用地磁采集模块采集地磁数据，遍历室内整个区域，采用网格状方式进行测量地磁基准图，相邻采集点之间的间隔形成一个具有长度和宽度的网格；在网格的顶点处采集地磁数据，每个点以“地磁数据+位置坐标”的形式存入数据库，为机器人定位时地磁匹配过程做准备；

步骤S2：设定粒子群优化方法中涉及的各参数：包括最大迭代次数gen_max，空间维数d，学习因子c₁,c₂，权系数最大最小值(min,max)，设置一个最大速度v_max，随机初始化粒子的位置及速度，设粒子群中第i个粒子的位置表示为xⁱ＝(xⁱ¹,xⁱ²,…,x^id),第i个粒子的速度表示为vⁱ＝(vⁱ¹,vⁱ²,…,v^id)；

步骤S3：在室内区域布置n个无线传感网络节点，利用无线网络传感器对移动机器人进行定位采样，在k时刻，机器人X_k(x_k,y_k,z_k)在移动的过程中，包括机器人本身无线传感器节点在内共有n个无线传感器节点被激活，且位置已知，只在机器人到达周围无线传感器网络感知范围时节点才被激活，x_k,y_k,z_k分别表示k时刻机器人的横坐标，纵坐标，和高度；

步骤S4：由机器人坐标x_k,y_k,z_k以及被激活的n个无线传感器节点的坐标得到机器人的观测模型，求得n个节点对应的似然度；

步骤S5：因激活n个无线传感器节点，用步骤S4的似然度得到一组权重值：{w_1k,w_2k,…,w_nk}；

步骤S6：判断是否进行重采样，通过重采样的方式将步骤S5中权重较大的粒子留下来，舍去权重较小的粒子，将粒子集中到高似然区域，以提高对将来状态的估计，重采样则回到步骤S3；

步骤S7：若不进行重采样，对于每一个粒子，评价其适应度Fitness，预估计的位置偏离机器人的真实位置，需要再次优化粒子的权重，返回步骤S5；

步骤S8：利用速度与位置关系对粒子新的预测位置进行更新；

步骤S9：判断更新是否结束：通过不断迭代判断更新是否结束，若迭代次数已达到最大迭代次数，则认为最后一次迭代的位置即为机器人的最优位置；

步骤S10：在获得最优位置后，采用地磁匹配方法进行局部地磁数据的遍历以进一步精确机器人的位置，通过地磁匹配方法求取相关性，相关性最高的为最优解，即机器人的精准位置，完成机器人定位。

3.根据权利要求2所述的基于无线传感器网络与地磁地图的机器人定位方法，其特征在于：

步骤S1中，地磁采集模块的磁力计测得的磁场磁矢量的大小与方向的关系如下：

M(r,t)＝M_m(r,t)+M_c(r)+M^d(r,t) (1-1)

式中，M表示磁场总强度，M_m表示主地磁场、M_c表示地壳地磁场，M_d表示干扰地磁场；r表示载体所处位置，t表示时间)；将磁矢量从三维量转化为二维量：M、H、φ四要素与M在地球坐标系三个轴上投影Mx,My,Mz的关系如下(1-2)式：

(1-2)式中，磁场总强度用M表示，将它投射到东北平面得到H，称作水平强度；将H投射到东西向轴，得到东西向场强Mx；同理，将H投射到南北向轴，得到南北向场强My；其中表示磁偏角，φ表示磁倾角。

4.根据权利要求2所述的基于无线传感器网络与地磁地图的机器人定位方法，其特征在于：

步骤S2中，涉及到的各参数如下：

(1)粒子数N及维数d：粒子数N的多少直接决定了方法的复杂性，空间维数决定了在二维还是在三维空间下；

(2)学习因子c₁，c₂：使粒子具有自我学习和参照群体优异值的能力，从而使粒子向局部和全体最优解移动，c₁＝c₂＝2；

(3)迭代次数gen_max：粒子在解空间内不断跟踪个体极值与全局极值并进行搜索,直到达到规定的最大迭代次数，也就是跟踪的次数，通常迭代次数越大对应的寻求精度越精确，但也要考虑时效性。

5.根据权利要求3所述的基于无线传感器网络与地磁地图的机器人定位方法，其特征在于：

步骤S4中，被激活的这n个节点用表示，设第j个节点对应的坐标为则节点j与机器人的距离为：

其中，ζ为路径衰减因子(常数2～5)，RSSI为信号强度衰减值，C为在1m处的接受信号耗损，RSSI可由以下表达式表示(2-2)：

式(2-2)中，p为发射功率(dB)，g为天线的增益(dm),ζ为路径衰减因子(常数2～5)，pl(d₀)为距离节点d₀(参考距离1m)传播耗损(dB)；d_kj为机器人真实位置距节点M₁＝(x_m1,y_m1)的距离，pl(d₀)的求法可参考如下式所述(2-3)：

w是接收信号功率dB，f是数据传送频率Hz，c标定位光速；式(2-3)中，传播耗损与传播距离的对数pl(d₀)是线性的关系，在d₀逐渐增大中，pl(d₀)逐渐趋于平缓，但实际情况中，由于阻碍、反射、天线增益等因素，pl(d₀)与距离的关系可能发生剧烈的震荡；由(2-1)，(2-2)，(2-3)得到后，再求出观测到的机器人X_k和节点j之间距离的均值和方差

得到机器人的观测模型为：

由此可得这n个节点对应的似然度为：

其中为k时刻粒子的坐标。

6.根据权利要求2所述的基于无线传感器网络与地磁地图的机器人定位方法，其特征在于：

步骤S5中，利用一组带有权重的采样点来趋近状态的后验概率密度函数；粒子的权重表示为：

其中，为重要性函数，表示粒子群优化中粒子权重与机器人空间位置及节点观测模型的关系，并将式(2-7)代入式(3-1)，可知激活的节点更新粒子的权重表示为：

总共选取n节点，每个节点都对粒子产生影响，那么每个粒子的得到一组权重：

7.根据权利要求2所述的基于无线传感器网络与地磁地图的机器人定位方法，其特征在于：

步骤S6中利用重采样驱动粒子集向高似然区域运动，得到粒子的权重后，很多粒子的权值过大或过小，这些权值过小的粒子在计算中，增加了计算时间且对结果无益，判断条件为N_eff是否在权系数最大最小值(min，max)内：

N为粒子的个数，为粒子权重的归一化，进行重采样我们选定的采样函数为用来对粒子进行重采样，其中i为粒子序号，重采样后粒子的权值都设定为1/N。

8.根据权利要求2所述的基于无线传感器网络与地磁地图的机器人定位方法，其特征在于：

步骤S7中，当高权重粒子在多次迭代过程中逐步趋近高似然区域的时候，用适应度函数来判断粒子群优化方法对移动机器人预测位置的趋近真实位置的程度，适应度函数如下：

其中R_k表示机器人的观测噪声协方差矩阵，表示j时刻机器人对粒子i观测值的预测，可通过预测位置值和已经观测到的粒子位置m_k计算得到，在估计移动机器人位置的基础上，计算出该特征相对于预估计位置的预测观测值设定一个阈值δ：当Fitness≥δ，则可以判断更新后的位置已经在机器人的真实位置附近；若Fitness≤δ，则表明预估计的位置偏离机器人的真实位置，需要再次优化粒子的权重。

9.根据权利要求2所述的基于无线传感器网络与地磁地图的机器人定位方法，其特征在于：

步骤S8中，粒子群中第i个粒子的位置表示为x_i＝(x_i1,x_i2,…,x_id),第i个粒子的速度表示为 v_i＝(v_i1,v_i2,…,v_id)，粒子适应度最接近阈值的作为最好，第i个粒子迄今为止搜索的最好位置记为p_i＝(p_i1,p_i2,…,p_id)，整个粒子迄今为止搜索到的最好位置记为p_g＝(p_g1,p_g2,…,p_gd)，对于每一个粒子，速度与位置方程对粒子新的预测位置进行更新，根据如下等式进行d维的位置和速度的更新：

v_i(t+1)＝wv_i(t)+c₁r₁(p_i-x_i(t))+c₂r₂(p_g-x_i(t)) (6-1)x_i(t+1)＝x_i(t)+v_i(t+1) (6-2)

r₁，r₂表示(0,1)之间均匀分布的随机数；v_i∈[-v_max,v_max]，v_max是先前设定的最大移动速度。

10.根据权利要求2所述的基于无线传感器网络与地磁地图的机器人定位方法，其特征在于：

步骤S10中，在获得最优位置后，在无线传感网络粒子群优化作用下，机器人获得一个相对位置(x_i1,x_i2)，由此相对位置计算出需要匹配的局部地磁图的遍历范围为遍历中，使用MSD地磁匹配进行相关性匹配，找到相关性最高的一组地磁数据，即D(u,v)最大的一组数据，对应的位置坐标即为机器人的精准位置，其中MSD地磁匹配方法如下：

N_uv表示基准图中位置(u,v)上的N维特征向量；m表示实时测量的N维特征向量；N表示相关数据的总点数；D(u,v)为地磁匹配相关函数，N_{u v+l}表示基准数据库的位置(u,v+l)上的特征量，m_l表示实时测量的第l个特征量。