基于TDOA的chan算法(定位算法)

您所在的位置:网站首页 双曲线要求 基于TDOA的chan算法(定位算法)

基于TDOA的chan算法(定位算法)

2023-09-05 06:06| 来源: 网络整理| 查看: 265

Chan算法原理

TDOA(TDOA,the time differences of arrival,到达时间差),Chan算法是TDOA定位方法的一个很好用的方法。

Chan算法是非递归双曲线方程组解法,具有解析表达式解,主要特点是:在测量误差服从理想高斯分布时,它的定位精度高、计算量小,并且可以通过增加已确定点的数量来提高算法精度。

该算法的推导前提是基于测量误差为零均值高斯随机变量,对于实际环境中误差较大的测量值,比如在有非视距误差的环境下,该算法的性能会有显著下降。二维情况下,可分为只有三个点参与定位和三个点以上参与定位。 在这里插入图片描述 已知坐标 ( x 1 , y 1 ) , ( x 2 , y 2 ) , ( x 3 , y 3 ) (x_1, y_1), (x_2, y_2), (x_3, y_3) (x1​,y1​),(x2​,y2​),(x3​,y3​),假设第未知点的坐标是 ( x , y ) (x, y) (x,y)根据几何关系定义一下关系表达式:

r i = ( x i − x ) 2 + ( y i − y ) 2 r_i = \sqrt{(x_i-x)^2+(y_i-y)^2} ri​=(xi​−x)2+(yi​−y)2 ​ … … … ( 1 ) \dots\dots\dots(1) ………(1)

r i , 1 = r i − r 1 = ( x i − x ) 2 + ( y i − y ) 2 − ( x 1 − x ) 2 + ( y 1 − y ) 2 r_{i,1}=r_i-r_1=\sqrt{(x_i-x)^2+(y_i-y)^2}-\sqrt{(x_1-x)^2+(y_1-y)^2} ri,1​=ri​−r1​=(xi​−x)2+(yi​−y)2 ​−(x1​−x)2+(y1​−y)2 ​ … … … ( 2 ) \dots\dots\dots(2) ………(2)

根据(1)另有如下关系:先令 K i = x i 2 + y i 2 K_i=x_i^2+y_i^2 Ki​=xi2​+yi2​

r i 2 = x i 2 + y i 2 = K i − 2 x i x − 2 y i y + x 2 + y 2 r_i^2=x_i^2+y_i^2=K_i-2x_ix-2y_iy+x^2+y^2 ri2​=xi2​+yi2​=Ki​−2xi​x−2yi​y+x2+y2 … … … ( 3 ) \dots\dots\dots(3) ………(3)

根据(2)另外有如下关系:

r i 2 = ( r i , 1 + r i ) 2 r_i^2=(r_{i,1}+r_i)^2 ri2​=(ri,1​+ri​)2 … … … ( 4 ) \dots\dots\dots(4) ………(4)

将(3)代入(4)可推出如下关系(关键):

r i , 1 2 + 2 r i , 1 r 1 = x i 2 + y i 2 − 2 x i x − 2 y i y + 2 x 1 x + 2 y 1 y − ( x 1 2 + y 1 2 ) = r i 2 − r 1 2 r_{i,1}^2+2r_{i,1}r_1=x_i^2+y_i^2-2x_ix-2y_iy+2x_1x+2y_1y-(x_1^2+y_1^2)=r_i^2-r_1^2 ri,12​+2ri,1​r1​=xi2​+yi2​−2xi​x−2yi​y+2x1​x+2y1​y−(x12​+y12​)=ri2​−r12​ … … … ( 5 ) \dots\dots\dots(5) ………(5)

进一步,令 x i , 1 = x i − x 1 x_{i,1}=x_i-x_1 xi,1​=xi​−x1​,有

r i , 1 2 + 2 r i , 1 r 1 = ( K i − K 1 ) − 2 x i , 1 x − 2 y i , 1 y r_{i,1}^2+2r_{i,1}r_1=(K_i-K_1)-2x_{i,1}x-2y_{i,1}y ri,12​+2ri,1​r1​=(Ki​−K1​)−2xi,1​x−2yi,1​y

这里(5)是关键一步:消除了未知数的平方项,仅保留一系列的线性方程。当 i = 1 , 2 , 3 i = 1, 2, 3 i=1,2,3时,有如下表达式: 在这里插入图片描述 通过消元法进行求解: 在这里插入图片描述 接下来就是如何求解 ( x , y ) (x,y) (x,y)

首先,令 在这里插入图片描述 有如下非常简洁的形式: 在这里插入图片描述 其中只有 r 1 r_1 r1​是未知的。因此要想求出未知坐标 ( x , y ) (x,y) (x,y),由式(9)可知,首先要求出 r 1 r_1 r1​。

接下来,将式(9)代入到式(3), r i 2 = x i 2 + y i 2 = K i − 2 x i x − 2 y i y + x 2 + y 2 , i = 1 r_i^2=x_i^2+y_i^2=K_i-2x_ix-2y_iy+x^2+y^2, i = 1 ri2​=xi2​+yi2​=Ki​−2xi​x−2yi​y+x2+y2,i=1可以化为如下形式:

( q 1 2 + q 2 2 − 1 ) r 1 2 − 2 [ q 1 ( x 1 − p 1 ) + q 2 ( y 1 − p 2 ) ] r 1 + [ ( x 1 − p 1 ) 2 + ( y 1 − p 2 ) 2 ] = 0 … … … ( 10 ) (q_1^2+q_2^2-1)r_1^2-2[q_1(x_1-p_1)+q_2(y_1-p_2)]r_1+[(x_1-p_1)^2+(y_1-p_2)^2]=0\dots\dots\dots(10) (q12​+q22​−1)r12​−2[q1​(x1​−p1​)+q2​(y1​−p2​)]r1​+[(x1​−p1​)2+(y1​−p2​)2]=0………(10)

→ a ∗ r 2 + b ∗ r + c = 0 , r = r 1 a*r^2+b*r+c=0,r = r_1 a∗r2+b∗r+c=0,r=r1​

式子(10)是 r 1 r_1 r1​的一元二次方程。求解式(10)可得 r r r的两个根,根据先验信息可以舍去一个无效的根,将有效的 r r r代会到式(9)中可求出未知点的坐标。

代码 clear all; clc; %% 初始化数值 c = 3120.77; % 标准值 t1 = 0.22 * 0.0001; % 传感器1的时间延迟 t2 = 0.58 * 0.0001; % 传感器2的时间延迟 x1 = 0; y1 = 0.5; % 传感器1 x2 = 0; y2 = 0; % 传感器2 x3 = 0.5; y3 = 0; % 传感器3 s1 = c * t1; % 时延距离 s1 = r21 s2 = c * t2; % 试验距离 s2 = r31 %% 求解未知点 k1 = x1^2 + y1^2; % 中间值 k2 = x2^2 + y2^2; k3 = x3^2 + y3^2; p1_molecule = (y2 - y1)*s2^2 - (y3 - y1)*s1^2 + (y3 - y1)*(k2 - k1) - (y2 - y1)*(k3 - k1); p1_denominator = (x2 - x1)*(y3 - y1) - (x3 - x1)*(y2 - y1); p1 = p1_molecule / (p1_denominator*2); q1 = ((y2 - y1)*s2 - (y3 - y1)*s1)/((x2 - x1)*(y3 - y1)-(x3 - x1)*(y2 - y1)); p2_molecule = (x2 - x1)*s2^2 - (x3 - x1)*s1^2 + (x3 - x1)*(k2 - k1) - (x2 - x1)*(k3 - k1); p2_denominator = (x3 - x1)*(y2 - y1) - (x2 - x1)*(y3 - y1); p2 = p2_molecule / (p2_denominator * 2); q2 = ((x2 - x1)*s2 - (x3 - x1)*s1)/((x3 - x1)*(y2 - y1)-(x2 - x1)*(y3 - y1)); %% 求取方程 a = q1^2 + q2^2 -1; b = -2*((x1 - p1)*q1 + (y1 - p2)*q2); c = (x1 - p1)^2 + (y1 - p2)^2; r1 = (-b + sqrt(b^2 - 4*a*c))/(2*a); r2 = (-b - sqrt(b^2 - 4*a*c))/(2*a); if r1 > 0 x = p1 + q1*r1; y = p2 + q2*r1; else x = p1 + q1*r2; y = p2 + q2*r2; end disp('未知点坐标: ') disp(x), disp(y)


【本文地址】


今日新闻


推荐新闻


CopyRight 2018-2019 办公设备维修网 版权所有 豫ICP备15022753号-3