本发明涉及高精度的距离估计技术和无线定位技术。
背景技术：
基于测距的三维无线定位技术广泛应用到军事、工业、交通、民生等领域，是即将到来的人工智能、物联网时代的基础技术之一，有着不可估量的应用前景。基于测距的三维定位精度依赖于无线传感器网络节点间的距离估计精度。在实际定位应用中，多径传播、非视距误差、环境干扰及测量误差对距离估计精度和定位精度具有较大的负面影响，给提高三维定位精度带来很大挑战。为了提高基于测距的最小二乘三维定位精度，本发明通过距离估计值的筛选策略，能够有效降低误差、噪声等对距离估计精度和三维最小二乘定位精度的负面影响，从而提高定位精度。
技术实现要素：
本发明的目的是为了解决由于多径传播、非视距误差、环境干扰及测量误差对距离估计精度和定位精度具有较大的负面影响，导致定位精度较低的问题，提供一种基于距离估计值筛选的最小二乘三维定位方法。
本发明所述的一种基于距离估计值筛选的最小二乘三维定位方法包括以下步骤：
步骤一、基于距离估计值筛选的三维定位系统由m个锚节点、1个未知节点、1个通信节点和1个上位机组成，其中m为整数，且10≤m≤20，所有锚节点和未知节点都具有nanoloc无线射频收发模块，采用双边对等方法来估计节点间距离，m个锚节点位置是固定的，它们的坐标分别为(x1，y1，z1)，(x2，y2，z2)，…，(xj，yj，zj)，…，(xm，ym，zm)，其中j表示锚节点的序号，j为整数，且1≤j≤m，未知节点可以部署到三维定位空间中的任意位置，其坐标待估计；通信节点与上位机连接，用于实现上位机与定位系统中锚节点和未知节点间的通信桥接功能，上位机中安装有定位软件客户端；
步骤二、锚节点、未知节点、通信节点和上位机经过上电初始化后构建无线局域网络，在上位机一端，三维定位空间被按照长、宽、高等距切割为l、w、h份，其中l、w、h均为正整数，分别表示按照长、宽、高切割的份数，并且有4≤l≤10、4≤w≤10、4≤h≤10，三维定位空间被切割后形成n个大小相同的立方体，n的值如公式(1)所示，每个立方体的几何中心被设定为三维定位空间中的一个特征点，则特征点的数量也为n，三维定位空间中所有特征点的坐标都可以由锚节点坐标简单计算出来，将n个特征点坐标表示为(x1，y1，z1)，(x2，y2，z2)，…，(xi，yi，zi)，…，(xn，yn，zn)，其中i表示特征点的序号，i为整数，且1≤i≤n；
n＝l*w*h(1)
步骤三、上位机定位客户端启动定位任务，通过通信节点向未知节点发送定位请求数据包，未知节点收到定位请求数据包后通过其nanoloc无线射频收发模块向所有锚节点发送测距请求数据包，锚节点收到测距请求数据包后采用双边对等测量方法对节点间距离进行估计，并将距离估计值反馈给未知节点，未知节点收到m个锚节点的距离估计值后将其打包发送到上位机客户端，其中，m个锚节点的距离估计值构成的序列表示为{d1,d2,...,dj,...,dm}，初始化i＝1；
步骤四、根据公式(2)分别计算出第i个特征点(xi，yi，zi)与m个锚节点之间的距离，对应得到m个计算距离，将它们表示为计算距离序列{di1,di2,...,dij,...,dim}，其中，i为正整数，代表特征点的序号，且1≤i≤n，j表示锚节点的序号，j为整数，且1≤j≤m；
步骤五、根据公式(3)分别计算第i个特征点(xi，yi，zi)到m个锚节点的计算距离dij与未知节点到m个锚节点的距离估计值序列dj之间的绝对误差eij，将得到的m个绝对误差表示为绝对误差序列{ei1,ei2,...,eij,...,eim}；
eij＝|dij-dj|(3)
步骤六、将步骤五中得到的关于第i个特征点的绝对误差序列{ei1,ei2,...,eij,...,eim}分别代入到公式(4)中，得到关于第i个特征点的绝对误差校正序列{ei1,ei2,...,eij,...,eim}；
步骤七、将步骤六中得到的关于第i个特征点的绝对误差校正序列{ei1,ei2,...,eij,...,eim}代入到公式(5)中可以得到λi，λi被定义为第i个特征点的评估因子，它的值越大，代表第i个特征点的位置与未知节点的位置越接近；
步骤八、令i＝i+1，判断i的值是否等于n+1，若i不等于n+1，则跳转执行步骤四；若i等于n+1，则执行步骤九；
步骤九、经过以上步骤可以得到n个特征点的评估因子序列{λ1,λ2,...,λi,...,λn}，从评估因子序列中找出最大值λo，则它所对应的特征点(xo，yo，zo)为最优特征点，其中o为最优特征点编号，且1≤o≤n，在本发明中，最优特征点意味着根据现有已知条件，特征点(xo，yo，zo)为所有特征点中与未知节点位置最接近的那个；
步骤十、以最优特征点代替未知节点来反向考查距离估计值的有效性，由公式(6)计算m个测量距离的相对误差，得到相对误差序列{ε1,ε2,...,εj,...,εm}，其中，doj代表最优特征点(xo,yo,zo)与第j个锚节点之间的计算距离，该数值已在步骤四中计算过；
步骤十一、从相对误差序列{ε1,ε2,...,εj,...,εm}中选取数值最小的k个元素，它们所对应的距离估计值及锚节点坐标将参与到下一步定位运算中，其余m-k个距离估计值及锚节点坐标将被舍弃，不参与定位运算，其中，正整数k表示选取距离估计值的数量，应根据实际情况设定，并且4≤k<m；
步骤十二、将经过筛选后的k个距离估计值重新排序为{dz1,dz2,...,dzk}，对应的锚节点坐标为(xz1，yz1，zz1)，(xz2，yz2，zz2)，…(xzk，yzk，zzk)，应用最小二乘定位算法对未知节点坐标进行估计，如公式(7)所示，其中未知节点坐标为
步骤十三、上位机根据用户指令判断是否已经完成定位任务，若已经完成，则执行步骤十四；若尚未完成，则跳转执行步骤三；
步骤十四、结束整个定位流程。
附图说明
图1为一种基于距离估计值筛选的最小二乘三维定位方法的流程图。
具体实施方式
步骤一、基于距离估计值筛选的三维定位系统由m个锚节点、1个未知节点、1个通信节点和1个上位机组成，其中m为整数，且10≤m≤20，所有锚节点和未知节点都具有nanoloc无线射频收发模块，采用双边对等方法来估计节点间距离，m个锚节点位置是固定的，它们的坐标分别为(x1，y1，z1)，(x2，y2，z2)，…，(xj，yj，zj)，…，(xm，ym，zm)，其中j表示锚节点的序号，j为整数，且1≤j≤m，未知节点可以部署到三维定位空间中的任意位置，其坐标待估计；通信节点与上位机连接，用于实现上位机与定位系统中锚节点和未知节点间的通信桥接功能，上位机中安装有定位软件客户端；
步骤二、锚节点、未知节点、通信节点和上位机经过上电初始化后构建无线局域网络，在上位机一端，三维定位空间被按照长、宽、高等距切割为l、w、h份，其中l、w、h均为正整数，分别表示按照长、宽、高切割的份数，并且有4≤l≤10、4≤w≤10、4≤h≤10，三维定位空间被切割后形成n个大小相同的立方体，n的值如公式(1)所示，每个立方体的几何中心被设定为三维定位空间中的一个特征点，则特征点的数量也为n，三维定位空间中所有特征点的坐标都可以由锚节点坐标简单计算出来，将n个特征点坐标表示为(x1，y1，z1)，(x2，y2，z2)，…，(xi，yi，zi)，…，(xn，yn，zn)，其中i表示特征点的序号，i为整数，且1≤i≤n；
n＝l*w*h(1)
步骤三、上位机定位客户端启动定位任务，通过通信节点向未知节点发送定位请求数据包，未知节点收到定位请求数据包后通过其nanoloc无线射频收发模块向所有锚节点发送测距请求数据包，锚节点收到测距请求数据包后采用双边对等测量方法对节点间距离进行估计，并将距离估计值反馈给未知节点，未知节点收到m个锚节点的距离估计值后将其打包发送到上位机客户端，其中，m个锚节点的距离估计值构成的序列表示为{d1,d2,...,dj,...,dm}，初始化i＝1；
步骤四、根据公式(2)分别计算出第i个特征点(xi，yi，zi)与m个锚节点之间的距离，对应得到m个计算距离，将它们表示为计算距离序列{di1,di2,...,dij,...,dim}，其中，i为正整数，代表特征点的序号，且1≤i≤n，j表示锚节点的序号，j为整数，且1≤j≤m；
步骤五、根据公式(3)分别计算第i个特征点(xi，yi，zi)到m个锚节点的计算距离dij与未知节点到m个锚节点的距离估计值序列dj之间的绝对误差eij，将得到的m个绝对误差表示为绝对误差序列{ei1,ei2,...,eij,...,eim}；
eij＝|dij-dj|(3)
步骤六、将步骤五中得到的关于第i个特征点的绝对误差序列{ei1,ei2,...,eij,...,eim}分别代入到公式(4)中，得到关于第i个特征点的绝对误差校正序列{ei1,ei2,...,eij,...,eim}；
步骤七、将步骤六中得到的关于第i个特征点的绝对误差校正序列{ei1,ei2,...,eij,...,eim}代入到公式(5)中可以得到λi，λi被定义为第i个特征点的评估因子，它的值越大，代表第i个特征点的位置与未知节点的位置越接近；
步骤八、令i＝i+1，判断i的值是否等于n+1，若i不等于n+1，则跳转执行步骤四；若i等于n+1，则执行步骤九；
步骤九、经过以上步骤可以得到n个特征点的评估因子序列{λ1,λ2,...,λi,...,λn}，从评估因子序列中找出最大值λo，则它所对应的特征点(xo，yo，zo)为最优特征点，其中o为最优特征点编号，且1≤o≤n，在本发明中，最优特征点意味着根据现有已知条件，特征点(xo，yo，zo)为所有特征点中与未知节点位置最接近的那个；
步骤十、以最优特征点代替未知节点来反向考查距离估计值的有效性，由公式(6)计算m个测量距离的相对误差，得到相对误差序列{ε1,ε2,...,εj,...,εm}，其中，doj代表最优特征点(xo,yo,zo)与第j个锚节点之间的计算距离，该数值已在步骤四中计算过；
步骤十一、从相对误差序列{ε1,ε2,...,εj,...,εm}中选取数值最小的k个元素，它们所对应的距离估计值及锚节点坐标将参与到下一步定位运算中，其余m-k个距离估计值及锚节点坐标将被舍弃，不参与定位运算，其中，正整数k表示选取距离估计值的数量，应根据实际情况设定，并且4≤k<m；
步骤十二、将经过筛选后的k个距离估计值重新排序为{dz1,dz2,...,dzk}，对应的锚节点坐标为(xz1，yz1，zz1)，(xz2，yz2，zz2)，…(xzk，yzk，zzk)，应用最小二乘定位算法对未知节点坐标进行估计，如公式(7)所示，其中未知节点坐标为
步骤十三、上位机根据用户指令判断是否已经完成定位任务，若已经完成，则执行步骤十四；若尚未完成，则跳转执行步骤三；
步骤十四、结束整个定位流程。
具体实施方案二，本实施方式是对具体实施方式一所述的一种基于距离估计值筛选的最小二乘三维定位方法作进一步说明，本实施方式一步骤一中，m为锚节点个数，本发明中m＝5。
具体实施方案三，本实施方式是对具体实施方式一所述的一种基于距离估计值筛选的最小二乘三维定位方法作进一步说明，本实施方式一步骤二中三维定位空间被按照长、宽、高等距切割为l、w、h份，本发明中l＝6，w＝6，h＝6，根据公式(1)可得n取值为216。
具体实施方案四，本实施方式是对具体实施方式一所述的一种基于距离估计值筛选的最小二乘三维定位方法作进一步说明，本实施方式一步骤十一中k表示选取距离估计值的数量，本发明中k＝8。
具体实施方案五，本实施方式是对具体实施方式一所述的一种基于距离估计值筛选的最小二乘三维定位方法作进一步说明，本实施方式中，在三维定位运算之前对作为输入变量的距离估计值进行筛选，将非视距误差较大的距离估计值舍弃，不参与到定位运算中，从而减弱距离估计过程中非视距误差给三维定位精度带来的影响。
具体实施方案六，本实施方式是对具体实施方式一所述的一种基于距离估计值筛选的最小二乘三维定位方法作进一步说明，本实施方式中，将三维定位空间切割为大小相同的若干立方体，并以每个立方体的几何中心为一个特征点，特征点的数量要根据实际情况设定，选取数量过少则影响筛选效果，选取数量过多会增加计算量。
具体实施方案七，本实施方式是对具体实施方式一所述的一种基于距离估计值筛选的最小二乘三维定位方法作进一步说明，本实施方式中，采用一种逆向评估策略，以三维定位空间内最优特征点代替目标节点对距离估计值进行反向考查，从而达到选择高质量的距离估计值，提高三维定位精度的目的。