发明内容
本发明的目的是在区域定位领域提出运算量小、快速收敛、对测量误差不敏感的方法,提供一种离散节点的区域无参考节点定位方法。
离散节点的区域无参考节点定位方法包括如下步骤:
1)测量有效范围内离散节点之间的距离;
2)选择有效范围内离散节点中的三个离散节点作为形成饱满三角形的备选离散节点;
3)将饱满三角形的备选离散节点作为搜索起点,决定主节点、X节点以及翻转节点,主节点与X节点的连线将构成饱满三角形的X轴,过主节点垂直于X轴的直线构成饱满三角形的Y轴;
4)选择主节点、X节点和翻转节点之外的离散节点加入饱满三角形构成的相对坐标系中;
5)对加入饱满三角形构成的相对坐标系的离散节点位置进行修正;
6)重复步骤4)到步骤5)直至所有离散节点加入以饱满三角形为基础的相对坐标系中,定位过程完成。
步骤2)中所述选择有效范围内离散节点中的三个离散节点作为形成饱满三角形的备选离散节点,步骤为:
a)在离散节点中,任选一对点作为主节点备选节点和X节点备选节点;
b)寻找距离主节点备选节点和X节点备选节点构成的X轴距离最远的离散节点作为翻转节点备选节点。
步骤3)中所述将饱满三角形的备选离散节点作为搜索起点,决定主节点、X节点以及翻转节点,主节点与X节点的连线将构成饱满三角形的X轴,过主节点垂直于X轴的直线构成饱满三角形的Y轴,步骤为:
c)测量主节点备选节点、X节点备选节点以及翻转节点备选节点三者之间的相互距离;
d)如果X节点备选节点和翻转节点备选节点之间的距离大于主节点备选节点与X节点备选节点之间距离,则交换X节点备选节点和翻转节点备选节点的身份;
e)如果X节点备选节点和翻转节点备选节点进行交换,则以主节点备选节点和X节点备选节点组成X轴,抛弃翻转节点备选节点,寻找离X轴垂直距离最远的点作为翻转节点备选节点;
f)重复步骤d)到步骤e),从所有离散节点中选出的主节点备选节点、X节点备选节点和翻转节点备选节点变为主节点、X节点和翻转节点;
g)主节点、X节点以及翻转节点构成饱满三角形,形成区域定位相对坐标系。
步骤4)所述选择主节点、X节点和翻转节点之外的离散节点加入饱满三角形构成的相对坐标系中,步骤为:
h)从有效区域内,选择主节点、X节点和翻转节点之外的离散节点;
i)测量选择的离散节点与主节点、X节点以及翻转节点之间的距离;
j)在饱满三角形构成的相对坐标系中,计算选择的离散节点在饱满三角形构成相对坐标系中的位置,用如下公式计算选择的离散节点在饱满三角形构成的相对坐标系中的位置,
X坐标
x=(L12+L32-L22)/(2×L3)
Y坐标
其中L1为选择的离散节点与主节点之间的距离,L2为选择的离散节点与X 节点之间的距离,L3为主节点和X节点之间的距离,x为选择的离散节点在相对坐标系中的x坐标,y为选择的离散节点在相对坐标系中的y坐标。
步骤5)所述对加入饱满三角形构成的相对坐标系的离散节点位置进行修正,步骤为:
k)计算离散节点到相对坐标系的X轴垂直距离,如果离散节点到相对坐标系的X轴垂直距离与主节点和X节点之间距离比在10%以内,对加入饱满三角形构成的相对坐标系的离散节点位置进行修正,离散节点到相对坐标系的X轴垂直距离计算公式如下
x=(L12+L32-L22)/(2×L3)
其中L1为离散节点与主节点之间的距离,L2为离散节点与X节点之间的距离,L3为主节点和X节点之间的距离,x为离散节点在相对坐标系中的x坐标,y的绝对值为离散节点到相对坐标系的X轴垂直距离。
l)如果步骤k)中需要对加入饱满三角形构成的相对坐标系的离散节点位置进行修正,交换X节点和翻转节点的身份,获得X轴和相对坐标系,在相对坐标系中计算加入饱满三角形构成的相对坐标系的离散节点位置,计算离散节点在相对坐标系中位置使用公式如下
x=(L12+L32-L22)/(2×L3)
其中L1为离散节点与主节点之间的距离,L2为离散节点与X节点之间的距离,L3为主节点和X节点之间的距离,x为离散节点在相对坐标系中的x坐标,y为离散节点在相对坐标系的y坐标。
m)如果步骤k)中需要对加入饱满三角形构成的相对坐标系的离散节点定位进行修正,并且在完成步骤l)后,以X节点和翻转接点的连线作为X轴,主节点变成翻转节点,构成相对坐标系,在相对坐标系中计算加入饱满三角形构成的相对坐标系的离散节点位置,计算离散节点位置使用的公式如下
x=(L12+L32-L22)/(2×L3)
其中L1为离散节点与主节点之间的距离,L2为离散节点与X节点之间的距离,L3为主节点和X节点之间的距离,x为离散节点在相对坐标系中的x坐标,y的绝对值为离散节点到相对坐标系的X轴垂直距离。
n)如果步骤k)中需要对加入饱满三角形构成的相对坐标系的离散节点定 位进行修正,并且完成步骤l)和步骤m)后,加权加入饱满三角形构成的相对坐标系的离散节点在步骤l)和步骤m)相对坐标系中位置,完成对加入饱满三角形构成的相对坐标系的离散节点坐标的修正。
本发明具有运算量小,快速收敛的优点。在区域定位过程中,对底层测量带来的距离误差不敏感且不会产生误差累积问题。在定位过程中通过优化的方法构建饱满三角形形成一个强健饱满的区域相对坐标体系。方法具有很好的鲁棒性能。
具体实施方式
离散节点的区域无参考节点定位方法包括如下步骤:
1)测量有效范围内离散节点之间的距离;
该距离信息组是整个定位过程的基础。利用底层CSS测距技术,结合TDOA方式,按照顺序测定有效区域内点对间的物理距离,该距离受到噪声的影响,测量得到的数值按照信道特点偏离实际真值。
2)选择有效范围内离散节点中的三个离散节点作为形成饱满三角形的备选离散节点;
在所有点对距离中,任意选择一对点作为主节点备选节点和X节点备选节点。同时遍历所有节点寻找离现定主节点备选节点和X节点备选节点组成的X轴垂直距离最远的点作为翻转节点备选节点。
计算任意点到x轴垂直距离公式,如图3所示
x=(L12+L32-L22)/(2×L3)
其中L1为离散节点与主节点之间的距离,L2为离散节点与X节点之间的距离,L3为主节点和X节点之间的距离,x为离散节点在相对坐标系中的x坐标,y的绝对值为离散节点到相对坐标系的X轴垂直距离。
3)将饱满三角形的备选离散节点作为搜索起点,决定主节点、X节点以及翻转节点,主节点与X节点的连线将构成饱满三角形的X轴,过主节点垂直于X轴的直线构成饱满三角形的Y轴;
计算主节点备选节点、X节点备选节点和翻转节点备选节点之间的相互距离,如果发现主节点备选节点和翻转接点备选节点间的距离或者X节点备选节点与翻转接点备选节点的距离大于主节点备选节点和X节点备选节点的距离的2.5倍,则交换X节点备选节点和翻转节点备选节点的身份。
如图1所示上述情况的交换目的在于不允许短边做x轴,交换后如图2所示。
对于新的主节点备选节点和X节点备选节点构成的x轴寻找离x轴垂直距离最远的点做为新的翻转节点备选节点,如果发现主节点备选节点和X节点备选节点距离是短边则仍做上述交换,如此循环
c程序如下:
flag=0;
mainpoint=0;//任选一个点作为主节点备选节点,此处选择零号节点
for(i=0;i<num;i++)
if(i!=mainpoint)
if(a[mainpoint][i]>=a[xpoint][mainpoint])
xpoint=i;//遍历所有点选择距离主节点最远的点作为X节点备选节点
while(flag==0)
{
temppp=0;
tt=0;//直线形状判断参数,1表示所有点共线,0表示非共线
temp=0;
temppointnum=10000;//暂时点号,用于找到最大值
turnpoint=10000;//实际点号
for(i=0;i<num;i++)
{
l1=a[mainpoint][i];
l2=a[xpoint][i];
l3=a[mainpoint][xpoint];
temp=0;
sum=l1+l2+l3;
if(l1>=temp)
temp=l1;
if(l2>=temp)
temp=l2;
if(l3>=temp)
temp=l3;//选择L1、L2、L3中最长距离作为temp
if(i!=mainpoint&&i!=xpoint//取离现有x轴垂直距离最远点
if(sum-2*temp>0)//判断L1、L2、L3距离数据是否满足构成三角形
if(sqrt(l1*l1-(l1*l1-l2*l2+l3*l3)/(2*l3)*(l1*l1-l2*l2+l3*l3)/(2*l3))>temppp)
{
temppp=sqrt(l1*l1-(l1*l1-l2*l2+l3*l3)/(2*l3)*(l1*l1-l2*l2+l3*l3)/(2*l3));
temppointnum=i;
}//遍历寻找距离X轴垂直距离最远的点以及其距离
}
{
if(temppointnum==10000)
{
turnpoint=2;
tt=1;//如果没有取到则说明图形一条直线,tt=1
}
else
turnpoint=temppointnum;//非共线的话取垂直距离最大的点作为翻转接点
备选节点
}
flag=1;//如果没有取大边的交换操作则跳出循环
//发现主节点备选节点和X节点的距离不是定位三角形的较大边//则取大边
作为X轴,即交换X节点备选节点和翻转节点备选节点的//身份
if(a[mainpoint][turnpoint]/a[mainpoint][xpoint]>2.5‖a[xpoint][turnpoint]/a[mainpoin
t][xpoint]>2.5)
{
temppointnum=xpoint;
xpoint=turnpoint;
turnpoint=temppointnum;
flag=0;//保持循环继续第二次选择X轴垂直距离最远点
}
}
4)选择主节点、x节点和翻转节点之外的离散节点加入饱满三角形构成的相对坐标系中;
利用主节点和x节点构成的x轴,计算未定位的离散节点节点x坐标X值计算:
计算未定位节点x坐标为:
x=(L12+L32-L22)/(2×L3)
利用主节点、X节点和翻转节点计算未定位节点的Y轴坐标,通过上面计算出来的未定位节点x值可以获得为定位节点Y轴坐标y的绝对值:
为了获得y的正负性,需要引入通过翻转节点获得。计算得到红色线长度和绿色线长度,和实测距离比较获得y正负性。如图4所示
C程序如下:
xx=(l1*l1-l2*l2+l3*l3)/(2*l3);
bijiao=sqrt(pow((xd-xx),2)+pow((yd-sqrt(l1*l1-xx*xx)),2));//红色线
bijiao1=sqrt(pow((xd-xx),2)+pow((yd+sqrt(l1*l1-xx*xx)),2));//绿色线
if(abs(bijiao-a[i][turnpoint])<abs(bijiao1-a[i][turnpoint]))//实测距离和红线比较接近则认为Y值为正
y[i]=sqrt(l1*l1-xx*xx);
else if(abs(bijiao-a[i][turnpoint])>abs(bijiao1-a[i][turnpoint]))//实测距离和绿色线比较接近则认为Y值为负
y[i]=-sqrt(l1*l1-xx*xx);//得到距离
5)对加入饱满三角形构成的相对坐标系的离散节点位置进行修正;
在X轴附近,距离x轴距离小于X节点和主节点之间距离的20%的点,需要对其定位进行修正:
第一步交换X节点和翻转接点的身份,获得新的X轴和新的相对坐标系,用这个坐标系定位一部分原有X轴附近点的坐标(这些点不能是新的X轴附近点,否则仍有误差)。第二步继续交换,取原始X节点和翻转节点的连线作为新 X轴,定位该点的相对坐标。过程如图5~图7所示
图6中交换X节点和翻转节点身份后,更换X轴之后确定方框中的点的坐标。
图7中以原始X节点和翻转节点作为新的X轴重新定位,更换X轴之后确定方框中的点的坐标。
将经过身份替换获得的两组新的相对坐标系位置值,进行坐标变换到原始坐标系后,加权平均得到靠近X轴位置的修正值。坐标平移翻转如下公式所示
X′=cosθ×(X-X0)-sinθ(Y-Y0)
Y′=sinθ×(X-X0)+cosθ(Y-Y0)
C程序如下
double X0=0;//设置X0值
double Y0=0;//设置Y0值
double angle=0;//设置旋转角度
double oldX=0;//需平移旋转的原始坐标X值
double oldY=0;//需平移旋转的原始坐标Y值
double newX=0;//平移旋转后的X值
double newY=0;//平移旋转后的Y值
//---------交换X节点和翻转节点----------------------------------
x1=(l1*l1-l2*l2+l3*l3)/(2*l3);
bijiao=sqrt(pow((xd-xx),2)+pow((yd-sqrt(l1*l1-xx*xx)),2));//红色线
bijiao1=sqrt(pow((xd-xx),2)+pow((yd+sqrt(l1*l1-xx*xx)),2));//绿色线
if(abs(bijiao-a[i][turnpoint])<abs(bijiao1-a[i][turnpoint]))//实测距离和红线比较
接近则认为Y值为正
y1=sqrt(l1*l1-xx*xx);
else if(abs(bijiao-a[i][turnpoint])>abs(bijiao1-a[i][turnpoint]))//实测距离和绿色线比
较接近则认为Y值为负
y1=-sqrt(l1*l1-xx*xx);//得到距离
//---------------------------------------------
//---------以老X节点和翻转节点作为新X轴----------------------------------
x2=(l4*l4-l5*l5+l6*l6)/(2*l6);
bijiao=sqrt(pow((xd-xx),2)+pow((yd-sqrt(l4*l4-xx*xx)),2));//红色线
bijiao1=sqrt(pow((xd-xx),2)+pow((yd+sqrt(l4*l4-xx*xx)),2));//绿色线
if(abs(bijiao-a[i][turnpoint])<abs(bijiao1-a[i][turnpoint]))//实测距离和红线比较
接近则认为Y值为正
y2=sqrt(l4*l4-xx*xx);
else if(abs(bijiao-a[i][turnpoint])>abs(bijiao1-a[i][turnpoint]))//实测距离和绿色线比
较接近则认为Y值为负
y2=-sqrt(l4*l4-xx*xx);//得到距离
//-------------------------------------------
oldX=0.5*(x1+x2);
oldY=0.5*(y1+y2);
X0=mainX;
Y0=mainY;
newX=cos(angle)*(oldX-X0)-sin(angle)*(oldY-Y0);//计算新X值
newY=sin(angle)*(oldX-X0)+cos(angle)*(oldY-Y0);//计算新Y值
6)重复步骤4)到步骤5)直至所有离散节点加入以饱满三角形为基础的相对坐标系中,定位过程完成。
步骤2)中所述选择有效范围内离散节点中的三个离散节点作为形成饱满三角形的备选离散节点,步骤为:
a)在离散节点中,任选一对点作为主节点备选节点和X节点备选节点;
b)寻找距离主节点备选节点和X节点备选节点构成的X轴距离最远的离散节点作为翻转节点备选节点。
步骤3)中所述将饱满三角形的备选离散节点作为搜索起点,决定主节点、X节点以及翻转节点,主节点与X节点的连线将构成饱满三角形的X轴,过主节点垂直于X轴的直线构成饱满三角形的Y轴,步骤为:
c)测量主节点备选节点、X节点备选节点以及翻转节点备选节点三者之间的相互距离;
d)如果X节点备选节点和翻转节点备选节点之间的距离大于主节点备选节点与X节点备选节点之间距离,则交换X节点备选节点和翻转节点备选节点的身份;
e)如果X节点备选节点和翻转节点备选节点进行交换,则以主节点备选节点和X节点备选节点组成X轴,抛弃翻转节点备选节点,寻找离X轴垂直距离最远的点作为翻转节点备选节点;
f)重复步骤d)到步骤e),从所有离散节点中选出的主节点备选节点、X节点备选节点和翻转节点备选节点变为主节点、X节点和翻转节点;
g)主节点、X节点以及翻转节点构成饱满三角形,形成区域定位相对坐标系。
步骤4)所述选择主节点、X节点和翻转节点之外的离散节点加入饱满三角形构成的相对坐标系中,步骤为:
h)从有效区域内,选择主节点、X节点和翻转节点之外的离散节点;
i)测量选择的离散节点与主节点、X节点以及翻转节点之间的距离;
j)在饱满三角形构成的相对坐标系中,计算选择的离散节点在饱满三角形构成相对坐标系中的位置,用如下公式计算选择的离散节点在饱满三角形构成的相对坐标系中的位置,
X坐标
x=(L12+L32-L22)/(2×L3)
Y坐标
其中L1为选择的离散节点与主节点之间的距离,L2为选择的离散节点与X节点之间的距离,L3为主节点和X节点之间的距离,x为选择的离散节点在相对坐标系中的x坐标,y为选择的离散节点在相对坐标系中的y坐标。
步骤5)所述对加入饱满三角形构成的相对坐标系的离散节点位置进行修正,步骤为:
k)计算离散节点到相对坐标系的X轴垂直距离,如果离散节点到相对坐标系的X轴垂直距离与主节点和X节点之间距离比在10%以内,对加入饱满三角形构成的相对坐标系的离散节点位置进行修正,离散节点到相对坐标系的X轴垂直距离计算公式如下
x=(L12+L32-L22)/(2×L3)
其中L1为离散节点与主节点之间的距离,L2为离散节点与X节点之间的距离,L3为主节点和X节点之间的距离,x为离散节点在相对坐标系中的x坐标,y的绝对值为离散节点到相对坐标系的X轴垂直距离。
l)如果步骤k)中需要对加入饱满三角形构成的相对坐标系的离散节点位置进行修正,交换X节点和翻转节点的身份,获得X轴和相对坐标系,在相对坐标系中计算加入饱满三角形构成的相对坐标系的离散节点位置,计算离散节点在相对坐标系中位置使用公式如下
x=(L12+L32-L22)/(2×L3)
其中L1为离散节点与主节点之间的距离,L2为离散节点与X节点之间的距离,L3为主节点和X节点之间的距离,x为离散节点在相对坐标系中的x坐标,y为离散节点在相对坐标系的y坐标。
m)如果步骤k)中需要对加入饱满三角形构成的相对坐标系的离散节点定位进行修正,并且在完成步骤l)后,以X节点和翻转接点的连线作为X轴,主节点变成翻转节点,构成相对坐标系,在相对坐标系中计算加入饱满三角形构成的相对坐标系的离散节点位置,计算离散节点位置使用的公式如下
x=(L12+L32-L22)/(2×L3)
其中L1为离散节点与主节点之间的距离,L2为离散节点与X节点之间的距离,L3为主节点和X节点之间的距离,x为离散节点在相对坐标系中的x坐标,y的绝对值为离散节点到相对坐标系的X轴垂直距离。
n)如果步骤k)中需要对加入饱满三角形构成的相对坐标系的离散节点定位进行修正,并且完成步骤l)和步骤m)后,加权加入饱满三角形构成的相对坐标系的离散节点在步骤l)和步骤m)相对坐标系中位置,完成对加入饱满三角形构成的相对坐标系的离散节点坐标的修正。