基于TDOA的chan算法（定位算法）-Toy模板网

这篇具有很好参考价值的文章主要介绍了基于TDOA的chan算法（定位算法）。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

Chan算法原理

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

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

该算法的推导前提是基于测量误差为零均值高斯随机变量，对于实际环境中误差较大的测量值，比如在有非视距误差的环境下，该算法的性能会有显著下降。二维情况下，可分为只有三个点参与定位和三个点以上参与定位。
基于TDOA的chan算法（定位算法）
已知坐标 $x_1, y_1), (x_2, y_2), (x_3, y_3)$ ，假设第未知点的坐标是 $(x, y)$ 根据几何关系定义一下关系表达式：

$r_i = \sqrt{(x_i-x)^2+(y_i-y)^2}$ $\dots\dots\dots(1)$

$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}$ $\dots\dots\dots(2)$

根据(1)另有如下关系：先令 $K_i=x_i^2+y_i^2$

$r_i^2=x_i^2+y_i^2=K_i-2x_ix-2y_iy+x^2+y^2$ $\dots\dots\dots(3)$

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

$r_i^2=(r_{i,1}+r_i)^2$ $\dots\dots\dots(4)$

将(3)代入(4)可推出如下关系（关键）：

$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$ $\dots\dots\dots(5)$

进一步，令 $x_{i,1}=x_i-x_1$ ，有

$r_{i,1}^2+2r_{i,1}r_1=(K_i-K_1)-2x_{i,1}x-2y_{i,1}y$

这里（5）是关键一步：消除了未知数的平方项，仅保留一系列的线性方程。当 $i = 1, 2, 3$ 时，有如下表达式：
基于TDOA的chan算法（定位算法）
通过消元法进行求解：

接下来就是如何求解 $(x, y)$

首先，令
基于TDOA的chan算法（定位算法）
有如下非常简洁的形式：

其中只有 $r_1$ 是未知的。因此要想求出未知坐标 $(x, y)$ ，由式（9）可知，首先要求出 $r_1$ 。

接下来，将式(9)代入到式(3)， $r_i^2=x_i^2+y_i^2=K_i-2x_ix-2y_iy+x^2+y^2, 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\dots\dots\dots(10)$

→ $a*r^2+b*r+c=0，r = r_1$

式子（10）是 $r_1$ 的一元二次方程。求解式(10)可得 $r$ 的两个根，根据先验信息可以舍去一个无效的根，将有效的 $r$ 代会到式（9）中可求出未知点的坐标。文章来源地址https://www.toymoban.com/news/detail-406165.html

代码

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)