定位算法——TDOA的Chan算法推导与Matlab实现

阅读本书更多章节>>>>

TDOA算法原理

TDOA(Time Difference of Arrival)——时间差到达算法，利用了几何数学中双曲线的特点—— 双曲线上的任意点到达两焦点的距离差是固定值。 这个距离差它天然可以抹去用户设备(UE)和基站的之间时钟误差。

P 1 C 1 = c ⋅ ( t 11 + Δ t ) P_1C_1 = c·(t_{11}+\Delta t) P1​C1​=c⋅(t11​+Δt)其中 Δ t \Delta t Δt是UE和基站之间的钟差(在UE与基站不完全同步的情况下)，这个钟差我们没法直接获得。
P 1 C 2 = c ⋅ ( t 12 + Δ t ) P_1C_2 = c·(t_{12}+\Delta t) P1​C2​=c⋅(t12​+Δt) 则 ∣ P 1 C 1 − P 1 C 2 ∣ = c ⋅ ( t 11 − t 12 ) 则|P_1C_1-P_1C_2 |=c·(t_{11}-t_{12}) 则∣P1​C1​−P1​C2​∣=c⋅(t11​−t12​)可见这里的钟差 Δ t \Delta t Δt被消除了，之后使用数学方法求出两个双曲线的焦点。但这同时也暗示着 基站的时钟需要同步才能被消除。 所以TDOA算法特性：UE和基站无需同步，基站之间需要同步，最少三个基站能测得焦点。

Chan算法介绍

在TDOA的解算方法上，有直接求解析解的Chan算法、Fang算法。也有迭代算法如Taylor算法(它是通过不断计算当前误差来调整参数，这个误差需要真实的位置标签来对比，但我们有真实标签后为什么还需要估计呢？这个是我对Taylor算法的疑惑，欢迎大家一起探讨👏)。基于解析解的方差理论上是没有误差的，它只受限于计算机的计算精度。

Chan算法公式推导

上述算法中， A x = r 0 C + D Ax=r_0C+D Ax=r0​C+D，先求 A x = C Ax=C Ax=C的解 x a x_a xa​，再求 A x = D Ax=D Ax=D的解 x b x_b xb​，再求 r 0 r_0 r0​，最后按照 x = r 0 x a + x b x=r_0x_a+x_b x=r0​xa​+xb​组合起来。

Chan算法实现2D

%%clc; clear;close all; format long;figure;%设置UE位置for i=1:100    ue_x = randi(100);     ue_y = randi(100);        scatter(ue_x, ue_y, '*');    hold on;        %注意注意！！！基站的y不能全部相同，否则在第57行的矩阵A第二列元素全为0，Ax=C或Ax=Ds时求不出唯一解    stations = [-40 0; 20 0; 40 50; 10 10];     %第四个基站是为了提出伪解        hold on;    r0_real = distance(ue_x, ue_y,stations(1,1), stations(1,2));    r1_real = distance(ue_x, ue_y,stations(2,1), stations(2,2));    r2_real = distance(ue_x, ue_y,stations(3,1), stations(3,2));    r3_real = distance(ue_x, ue_y,stations(4,1), stations(4,2));    %ri_real只是用来计算tds，实际上它会带有时钟误差，而这个误差我们不能直接得到    tds = [r1_real-r0_real r2_real-r0_real, r3_real-r0_real];    position = TDOA(stations, tds);    scatter(position(1), position(2), 'o');    hold on;end%%function [position] = TDOA(stations, tds)        x0 = stations(1,1);y0 = stations(1,2);    x1 = stations(2,1);y1 = stations(2,2);    x2 = stations(3,1);y2 = stations(3,2);    x3 = stations(4,1);y3 = stations(4,2);        r10 = tds(1);    r20 = tds(2);    r30 = tds(3);   %ue对3号基站和0号基站的距离差，真实的        scatter(x0,y0,120,'d', 'filled'); text(x0,y0,'Anchor1');    scatter(x1,y1,120,'d', 'filled'); text(x1,y1,'Anchor2');    scatter(x2,y2,120,'d', 'filled'); text(x2,y2,'Anchor3');    hold on;        x10 = x1 - x0;    x20 = x2 - x0;    y10 = y1 - y0;    y20 = y2 - y0;    k0  = x0^2 + y0^2;    k1  = x1^2 + y1^2;    k2  = x2^2 + y2^2;    A = [x10 y10; x20 y20];    C = -[r10; r20];    D = [(k1-k0-r10^2)/2; (k2-k0-r20^2)/2];    %求解Ax = r0 * C + D    a = A\C;    b = A\D;    %求解r0    A_ = a(1)^2 + a(2)^2-1;    B_ = a(1) * (b(1) - x0) + a(2) * (b(2) - y0);    C_ = (x0 - b(1))^2 + (y0 - b(2))^2;    if B_^2-A_*C_ < 0        position = [Nan, Nan];    else        r0_1 = -(B_+sqrt(B_^2-A_*C_))/A_;        r0_2 = -(B_-sqrt(B_^2-A_*C_))/A_;        X1 = a * r0_1 + b;        X2 = a * r0_2 + b;        %剔除错误解：方法一：UE和基站时钟尽量同步。方法二：增加观测站(本例使用)        if abs(r30-(distance(X1(1),X1(2),x3,y3)-distance(X1(1),X1(2),x0,y0))) < 1e-8            position = X1;        else            position = X2;        end    endend%%function dist = distance(x1,y1,x2,y2)    dist = sqrt((x1-x2)^2 + (y1-y2)^2);    end

上述代码需要注意三个地方！！！

算距离差时，不加上绝对值，这样可以排除掉一半的解(两双曲线相交有2至4个交交点)计算 r 0 r_0 r0​时可能有伪解，需要增加观测站或牺牲一定精度来排除另外一个解。(本文是增加了一个基站)。二维定位时，不允许所有的基站在 y y y轴的数值相等。

上述代码得到的结果

上图中*表示UE的真实位置，o表示UE的计算位置，可以看到每个UE的位置都被正确解算了。

Chan算法实现3D

%%clc; clear;close all;format long; % tmp = unifrnd(0,255,4,2);% x1 = tmp(1,1); y1 = tmp(1,2);  % Anchor1% x2 = tmp(2,1); y2 = tmp(2,2);  % Anchor2% x3 = tmp(3,1); y3 = tmp(3,2);  % Anchor3figure;%随机生成100个UE位置，并对其进行TDOA计算correct_sum = 0;uncorrect_sum = 0;for i=1:100    ue_x = randi(60)+randn();    ue_y = randi(60)+randn();    ue_z = randi(60)+randn();        scatter3(ue_x, ue_y, ue_z, 120, '*');    hold on;    %注意注意！！！基站的z不能全部相同，否则在第67行的矩阵A第三列元素全为0，Ax=C或Ax=Ds时求不出唯一解    stations = [-40 0 5; 20 0 15; 40 50 5; 0 0 5; 10 10 10];      r0_real = distance(ue_x, ue_y, ue_z, stations(1,1), stations(1,2), stations(1,3));    r1_real = distance(ue_x, ue_y, ue_z, stations(2,1), stations(2,2), stations(2,3));    r2_real = distance(ue_x, ue_y, ue_z, stations(3,1), stations(3,2), stations(3,3));    r3_real = distance(ue_x, ue_y, ue_z, stations(4,1), stations(4,2), stations(4,3));    r4_real = distance(ue_x, ue_y, ue_z, stations(5,1), stations(5,2), stations(5,3));    tds = [r1_real-r0_real r2_real-r0_real r3_real-r0_real r4_real-r0_real];    position = TDOA(stations, tds);    if distance(position(1), position(2), position(3), ue_x, ue_y, ue_z) < 1e-8        correct_sum = correct_sum + 1;    else        uncorrect_sum = uncorrect_sum + 1;    end    scatter3(position(1), position(2), position(3), 'o', 'r');    hold on;        end%%function [position] = TDOA(stations, tds)        x0 = stations(1,1);y0 = stations(1,2);z0 = stations(1,3);    x1 = stations(2,1);y1 = stations(2,2);z1 = stations(2,3);    x2 = stations(3,1);y2 = stations(3,2);z2 = stations(3,3);    x3 = stations(4,1);y3 = stations(4,2);z3 = stations(4,3);    x4 = stations(5,1);y4 = stations(5,2);z4 = stations(5,3);        r10 = tds(1);    r20 = tds(2);    r30 = tds(3);    r40 = tds(4);            scatter3(x0,y0,z0,120,'d', 'filled'); text(x0,y0,z0,'Station1');hold on;    scatter3(x1,y1,z1,120,'d', 'filled'); text(x1,y1,z1,'Station2');hold on;    scatter3(x2,y2,z2,120,'d', 'filled'); text(x2,y2,z2,'Station3');hold on;    scatter3(x3,y3,z3,120,'d', 'filled'); text(x3,y3,z3,'Station4');hold on;    hold on;    % r21 represents the TDOA between anchor1 and anchor2    % r31 represents the TDOA between anchor1 and anchor3        x10 = x1 - x0;    x20 = x2 - x0;    x30 = x3 - x0;        y10 = y1 - y0;    y20 = y2 - y0;    y30 = y3 - y0;        z10 = z1 - z0;    z20 = z2 - z0;    z30 = z3 - z0;    k0  = x0^2 + y0^2 + z0^2;    k1  = x1^2 + y1^2 + z1^2;    k2  = x2^2 + y2^2 + z2^2;    k3  = x3^2 + y3^2 + z3^2;    A = [x10 y10 z10; x20 y20 z20; x30 y30 z30];    C = -[r10; r20; r30];    D = [(k1-k0-r10^2)/2; (k2-k0-r20^2)/2; (k3-k0-r30^2)/2];    %求解Ax = r0 * C + D    a = A\C;    b = A\D;    %求解r0    A_ = a(1)^2 + a(2)^2 + a(3)^2 -1;    B_ = a(1) * (b(1) - x0) + a(2) * (b(2) - y0) + a(3) * (b(3) - z0);    C_ = (x0 - b(1))^2 + (y0 - b(2))^2 + (z0 - b(3))^2;        r0_1 = -(B_+sqrt(B_^2-A_*C_))/A_;    r0_2 = -(B_-sqrt(B_^2-A_*C_))/A_;    X1 = a * r0_1 + b;    X2 = a * r0_2 + b;    %剔除错误解：方法一：UE和基站时钟尽量同步。方法二：增加观测站(本例使用)    if abs(r40-(distance(X1(1),X1(2),X1(3),x4,y4,z4)-distance(X1(1),X1(2),X1(3),x0,y0,z0))) < 1e-8        position = X1;    else        position = X2;    endend%%function dist = distance(x1,y1,z1,x2,y2,z2)    dist = sqrt((x1-x2)^2 + (y1-y2)^2 + (z1-z2)^2);    end

这份代码是随机生成了100个三维点，然后使用Chan算法解算位置。
上述代码也需要注意三个地方！！！

算距离差时，不加上绝对值，这样可以排除掉一半的解(两双曲线相交有2至4个交交点)计算 r 0 r_0 r0​时可能有伪解，需要增加观测站或牺牲一定精度来排除另外一个解。(本文是增加了一个基站)。二维定位时，不允许所有的基站在 z z z轴的数值相等。

上述代码的运行结果

经过测试，所有的解算误差都小于 1 0 − 8 10^{-8} 10−8

阅读本书更多章节>>>>

本文链接：https://www.kjpai.cn/gushi/2024-04-06/154777.html，文章来源：网络cs，作者：胡椒，版权归作者所有，如需转载请注明来源和作者，否则将追究法律责任！