1. 无源定位到底做了一件什么事1.1 一个只知道听信号的定位系统无源定位这个概念拆开来看就是八个字我只收听我不发送。无线电侦测设备把天线架在那里接收目标辐射出来的信号然后通过多个观测站对同一信号的到达时间、到达角度、到达频率进行测量再经过算法换算就能把目标的位置、速度、航向全部还原出来。整个过程里我们不做任何应答也不主动发射任何电磁波目标完全感知不到有人在盯着它。这种定位方式在工程里的价值非常明显。你不要求目标主动配合也不用像雷达那样发射大功率信号隐蔽性极佳特别适合电子侦察、频谱监测、无人机管制、应急搜救这些场景。与之形成对比的是有源定位比如雷达测距或者信标定位原理简单、精度也高但代价是只要发射信号就等于把自己的位置和意图暴露给了对手在真实对抗场景里这往往是不能接受的。我接触无源定位这个方向最早就是从一次项目需求开始的要在某个区域内定位若干个非合作辐射源既不能打扰目标又要实时给出位置和运动状态。调研下来发现可用的观测量无非就是几大类——到达时间TOA、到达时间差TDOA、到达角度AOA、到达频率FOA、到达频率差FDOA以及它们的各种组合。这次我们重点展开的是TOA和FOA一个管位置一个管运动组合起来正好覆盖了最核心的定位需求。1.2 TOA和FOA能给什么不能给什么TOATime of Arrival定位字面意思是利用信号到达观测站的时间来定位。电磁波在空气或真空中以光速传播信号从目标到观测站花了多长时间乘以光速就是距离。只要三个以上观测站各自测出距离就能通过解方程组确定目标的位置。这个思路和手机GPS定位是一样的区别在于GPS是卫星发射信号、接收机被动接收而我们这个场景里目标是未知的发射时刻也不受控所以实际落地时往往要改造成TDOA到达时间差来消掉目标的发射时刻。FOAFrequency of Arrival定位用的是多普勒效应。当目标和观测站之间存在相对运动时接收到的信号频率会和发射频率产生偏差偏离的大小和方向取决于相对运动速度在视线方向上的投影。通过在不同位置的观测站测量频率就能反推出目标的位置和速度向量。这个观测量在运动目标定位和测速里非常关键尤其在目标以较高速度移动时多普勒频移比较明显FOA能提供TOA完全得不到的速度信息。所以说白了TOA和FOA不是竞争关系而是互补关系TOA负责回答“目标在哪”FOA负责回答“目标怎么运动”。把它们联合同步求解定位精度和鲁棒性都会明显提升。接下来我直接把原理、公式、MATLAB实现一路走完代码都是可以复制去跑的重点是让你理解每一步在做什么以及为什么这么做。2. TOA定位从到达时间到位置的三个关键步骤2.1 观测模型先搞清楚我们手里有什么假设我们有N个观测站第i个站的坐标是si (xi, yi)目标真实位置是u (x, y)。电磁波传播速度为c目标发射时刻为t0第i个站接收到信号的时刻为ti那么理论上的TOA满足ti - t0 ||u - si|| / c也就是接收时刻减去发射时刻等于距离除以光速。这里有一个无源定位里最基本的麻烦t0是未知的。目标不会告诉我们它什么时候发射的因此在真正的无源场景里我们不能直接用绝对到达时间算出单一距离更常用的做法是采用“到达时间差”TDOA把t0消掉。具体操作是选定第一个观测站作为参考站用其他站的到达时刻减去参考站的到达时刻Δt_i1 ti - t1 (||u - si|| - ||u - s1||) / c这样t0就被消掉了剩下唯一的未知量是目标位置u。整理一下得到r_i - r_1 c * Δt_i1 d_i1其中r_i ||u - si||d_i1就是我们常说的距离差。顺便说一句如果坚持用绝对TOA那就必须额外估计t0未知量变成[x, y, t0]方程数量也需要增加求解稳定性更差工程上很少这样干。所以这篇文章里说的TOA定位在实现层面都是以TDOA约束为基础这一点请务必记牢。2.2 线性最小二乘第一次落地下面从r_i r_1 d_i1出发做一次两边平方||u - si||² (r_1 d_i1)²展开左边u² - 2u·si si² r_1² 2r_1·d_i1 d_i1²对于参考站也有u² - 2u·s1 s1² r_1²两个式子相减消掉u²和r_1²-2u·si 2u·s1 si² - s1² 2r_1·d_i1 d_i1²整理成关于未知数u [x, y]和r_1的线性方程2u·(s1 - si) - 2d_i1·r_1 d_i1² - si² s1²注意这个式子里已经没有平方根了是关于“x、y、r_1”三个未知量的线性方程。当有N个观测站时可以得到N-1个这样的方程堆成矩阵形式A * z b其中z [x; y; r_1]。然后用最小二乘求解z (A * A) \ (A * b)这个方法最大的优点是完全不需要初值不会发散代码十分钟就能跑通非常适合作为入门第一版。缺点是它对测量噪声做了简单的等权处理而实际上不同站的测量质量可能不同在噪声较大时平方运算会放大误差精度会比迭代法略差。但作为初始解它非常可靠后面的泰勒迭代、粒子滤波、卡尔曼滤波都可以拿它做初值。下面是这个算法的核心代码我建议所有入门无源定位的人都亲手写一遍不依赖任何工具箱function u_est tdoa_ls(BS, del_t, c) % BS: N x 2 观测站坐标 % del_t: (N-1) x 1 相对参考站的到达时间差 N size(BS, 1); d c * del_t; % 时差转距离差 A zeros(N-1, 3); b zeros(N-1, 1); for i 2:N A(i-1, 1:2) 2 * (BS(1, :) - BS(i, :)); A(i-1, 3) -2 * d(i-1); b(i-1) d(i-1)^2 - norm(BS(i, :))^2 norm(BS(1, :))^2; end z (A * A) \ (A * b); u_est z(1:2); end代码里最需要注意的地方就是构造A和b时的符号方向稍微错一个符号结果就会完全不对。我当年第一次写这十几行代码就因为在b的公式里把正负号搞反跑出来的定位结果落在几千公里之外排查了一整个下午。2.3 Chan算法和泰勒迭代精度还能往上提线性最小二乘虽然稳定但精度有上限。想要进一步提升有两个主流方向。第一个方向是Chan算法。这个算法本质上是一种两步最小二乘第一步先用上面那样的线性模型解出一个粗位置第二步再利用TDOA噪声的统计协方差特性和“位置与r_1之间必须满足真实几何距离”这个约束关系做一次加权最小二乘修正。当测量噪声不大、TDOA误差近似服从高斯分布时Chan算法可以达到克拉美劳下界CRLB也就是说精度已经逼近理论极限而且它不需要迭代计算速度快特别适合实时定位系统。在无线定位的文献里Chan算法几乎是TDOA定位的标准答案。第二个方向是泰勒级数迭代。思路很直接先有一个初始估计位置u0把非线性方程在u0处做一阶泰勒展开得到一个线性化的修正量δ然后迭代更新u0 ← u0 δ直到位置修正量小于某个门限为止。泰勒迭代收敛后的精度也可以逼近CRLB而且它不挑观测模型无论是TOA、TDOA、AOA还是FOA都能套进去。缺点是它对初始值敏感初值离真实位置太远时迭代可能发散。泰勒迭代的MATLAB实现如下function u_est taylor_tdoa(BS, del_t, c, u_init, tol) u u_init; d c * del_t; % 距离差 N size(BS, 1); for iter 1:100 r sqrt(sum((BS - u).^2, 2)); % 各站到当前估计位置的距离 r1 r(1); h zeros(N-1, 1); G zeros(N-1, 2); for i 2:N h(i-1) r(i) - r1 - d(i-1); G(i-1, :) (u - BS(i, :)) / r(i) - (u - BS(1, :)) / r1; end delta (G * G) \ (G * h); u u - delta; if norm(delta) tol break; end end u_est u; end实际仿真中我通常的组合是先用线性最小二乘或Chan算法得到一个可靠的初值然后用泰勒迭代精修两三步。初值质量有保证时泰勒迭代基本上几步就收敛精度也不错。注意GD矩阵的构造里分母都有距离r如果某个观测站离目标非常近距离趋近于零时会出现数值不稳定所以要加个eps保护。3. FOA定位用多普勒频移反推运动状态3.1 多普勒频移公式背后的直觉多普勒效应大家都有生活直觉火车鸣笛朝你开过来的时候音调变高离开的时候音调变低。无线电信号也一样。目标朝观测站运动时观测到的信号频率高于发射频率目标远离时观测到的频率低于发射频率。写成公式f_obs f0 * (1 v_r / c)其中v_r是目标相对观测站沿视线方向的速度分量朝向观测站运动时取正。光速c是个极大的数所以无线电频率的绝对偏移量往往非常小在2.4 GHz的载频上即使目标以100 m/s的速度径向运动多普勒频移也只有大约800 Hz。对于窄带通信信号这个频移只能通过长时间的相干积累才能精确测量。v_r的严格计算方式是目标速度与观测站速度的差向量在目标到观测站的视线方向上的投影。v_r (v_u - v_s) · (u - s) / ||u - s||把v_r代回频率公式得到第i个观测站的频率观测方程f_obs_i f0 * [1 ((v_u - v_si) · (u - si)) / (c * ||u - si||)]这个方程一看就是高度非线性的而且未知量不仅有目标位置u还有目标速度v_u和发射频率f0。3.2 FOA观测方程与求解策略在二维场景里目标位置有2个未知数速度有2个未知数加上发射频率f0一共5个未知量。每个观测站能提供一个频率测量值所以至少需要5个站才能把方程解封闭。但在很多实际场景中观测站数量凑不够或者目标发射频率本身可以预先估计出来这时可以把f0当作已知量未知量降到4个站数要求也随之降到4个。求解这类非线性方程组比较直接的办法是用非线性最小二乘目标函数为min J(u, v_u) Σ [f_obs_i - f0 * (1 v_r_i / c)]²我通常不手写复杂的闭式解而是直接用Gauss-Newton迭代或者MATLAB的lsqnonlin工具箱。手写Gauss-Newton的好处是每一步的雅可比矩阵怎么更新都看得清清楚楚方便调试和理解。下面是一个简化的迭代框架function x_est foa_gauss_newton(BS, f_obs, f0, c, x_init) % x [x; y; vx; vy] x x_init; for iter 1:50 [F, J] foa_residual_jacobian(BS, f_obs, f0, c, x); delta (J * J) \ (J * F); x x - delta; if norm(delta) 1e-8 break; end end x_est x; end其中foa_residual_jacobian需要计算出每个观测站的频率残差以及对位置和速度的偏导数。如果你用MATLAB的lsqnonlin写起来更快但需要传入一个残差函数句柄。在实际调试中我的感受是FOA定位比TOA定位要“娇气”得多。因为多普勒频率对位置变化不敏感尤其目标低速运动时频率偏移只有几赫兹稍微有点噪声估计就完全跑偏。所以FOA在低速场景下单独使用效果不佳通常要和TOA/AOA联合使用才靠谱。3.3 TOAFOA联合定位为什么值得做单独用TOA时我们拿到的是目标在一个时间切片上的位置快照如果目标在运动那么观测期间目标的位置其实一直在变相当于给定位结果叠加了一层动态误差。单独用FOA时频率信息对位置不敏感几何布局稍微差一点结果就会剧烈抖动。两者放在一起相当于同时用“距离差”和“径向速度”两个维度的信息去约束目标的位置和速度状态。这个优势在跟踪场景里尤其明显先用TOA解出位置初值再用FOA修正速度方向甚至可以用卡尔曼滤波器把TOA和FOA统一建进观测模型里形成完整的目标跟踪闭环。我后面的仿真结果会展示联合定位在目标运动速度较高时的均方根误差明显低于单独使用任何一种观测量。4. MATLAB仿真全流程实录4.1 仿真场景设定与参数列表为了让结果可复现我固定使用下面这组参数参数数值说明观测站数量4二维场景站围绕目标分布观测站坐标(0,0), (10000,0), (10000,10000), (0,10000)单位m正方形布站目标真实位置(3500, 4200)单位m目标速度(60, 30)单位m/sFOA仿真用发射频率f02.4 GHz相当于WiFi/ISM频段光速c3e8 m/sTOA测量噪声标准差10~100 ns对应距离误差3~30 mFOA测量噪声标准差1~100 Hz对应速度误差约0.125~12.5 m/s蒙特卡洛次数500统计RMSE这里要提醒一下TOA噪声的单位是纳秒1 ns对应约0.3米的光速距离。所以实际系统要做到亚米级定位精度时间测量分辨率必须在纳秒以内这直接取决于接收机带宽、信噪比和站间时间同步水平。FOA噪声则直接关系到接收机频率估计的准确度信号持续时间越长、信噪比越高频率估计越准。4.2 TOA定位仿真代码与运行结果有了场景参数我们生成含噪的TOA测量值再用前面写的tdoa_ls和taylor_tdoa分别求解跑500次蒙特卡洛统计RMSEclear; clc; rng(42); c 3e8; BS [0 0; 10000 0; 10000 10000; 0 10000]; u_true [3500 4200]; N size(BS, 1); mc 500; sigma_t 30e-9; % 30 ns 时间测量噪声 rmse_ls 0; rmse_taylor 0; for k 1:mc r_true sqrt(sum((BS - u_true).^2, 2)); t_true r_true / c; t_meas t_true sigma_t * randn(N, 1); del_t t_meas(2:end) - t_meas(1); % 线性最小二乘 u_ls tdoa_ls(BS, del_t, c); rmse_ls rmse_ls norm(u_ls - u_true)^2; % Taylor迭代用LS解做初值 u_taylor taylor_tdoa(BS, del_t, c, u_ls, 1e-4); rmse_taylor rmse_taylor norm(u_taylor - u_true)^2; end rmse_ls sqrt(rmse_ls / mc); rmse_taylor sqrt(rmse_taylor / mc); fprintf(LS RMSE %.2f m\nTaylor RMSE %.2f m\n, rmse_ls, rmse_taylor);我实测的结果是当TOA噪声标准差为30 ns约9米距离误差时LS定位的RMSE大约在13~18米Taylor迭代能压到9~12米。噪声越大两者差距越明显。这个现象很好理解LS没有利用噪声的统计信息做最优加权平方运算又放大了大误差样本的权重所以噪声增大时它的性能衰减更快。4.3 FOA定位仿真代码与运行结果FOA仿真稍微复杂因为涉及目标速度和发射频率的耦合。为了把问题聚焦在定位方法上我把发射频率当作已知量直接在目标位置和速度上产生频率观测再用非线性最小二乘求解f0 2.4e9; v_true [60 30]; sigma_f 10; % 频率测量标准差单位Hz mc 500; rmse_foa 0; for k 1:mc f_obs zeros(N, 1); for i 1:N r_vec u_true - BS(i, :); r_norm norm(r_vec); v_r v_true * (r_vec / r_norm); f_obs(i) f0 * (1 v_r / c) sigma_f * randn; end % 使用手写Gauss-Newton或lsqnonlin求解 x_init [3000 4000 50 40]; % 给一个接近真实值的初值 x_est foa_gauss_newton(BS, f_obs, f0, c, x_init); rmse_foa rmse_foa norm(x_est(1:2) - u_true)^2; end rmse_foa sqrt(rmse_foa / mc);跑下来的结果让我印象很深当频率测量标准差为10 Hz时对应的径向速度误差约1.25 m/sFOA单独定位的位置RMSE通常在几十到上百米量级具体数值受几何布局影响非常大。这个数字看起来比TOA差不少但你要理解FOA单独本来就更擅长测速对位置的敏感度天然不如专门测距的TOA真正发挥作用是在联合定位里。4.4 定位误差随测量噪声的变化曲线我做了几组对照实验把TOA噪声从10 ns变化到100 nsFOA噪声从1 Hz变化到100 Hz记录三种方案的RMSE画成对数坐标曲线。结果非常直观TOA定位的RMSE基本与TOA噪声标准差成正比。FOA定位的RMSE与频率噪声强相关而且易受布站几何影响单独使用时曲线抖动较大。联合定位的RMSE在大多数噪声水平下都低于单独方案尤其是目标运动速度较高时优势更加明显。联合定位的代码是把位置和速度统一成状态向量[x, y, vx, vy]把TOA方程和FOA方程合并成一个残差向量再一起丢给Gauss-Newton迭代。实现起来并不复杂关键是设计好残差函数和雅可比矩阵这一步完成后剩下的就是调初值和观测权重的问题了。5. 误差分析与几何布局的影响5.1 GDOP为什么你的站摆成一条线算法再好也白搭GDOP几何精度因子是一个衡量观测几何布局对定位精度影响的指标。核心思想是即使每个观测量的测量噪声完全相同如果观测站和目标之间的几何关系不好最终定位误差也会被成倍放大。举个最典型的例子观测站和目标几乎在一条直线上时各个站测到的距离差或频率差之间的差异非常小方程组在几何上处于“病态”一个小小的测量噪声就能把解推出十万八千里。反过来观测站均匀分布在目标四周测量约束在多个方向上起作用定位结果就稳定得多。GDOP的计算方式是根据观测矩阵H构造(H * H)^{-1}然后对对角线元素求平方根得到的数值可以理解为“测量误差到定位误差的放大倍数”。在布站规划阶段用MATLAB画一幅GDOP分布图一眼就能看出哪些区域定位精度高、哪些是盲区。这个步骤我强烈建议做它能帮你省掉大量试错时间。5.2 时间同步误差和频率估计误差的影响无源定位系统里TOA精度并不完全取决于接收机的时间分辨率更关键的是站间时间同步。如果两个观测站之间存在100 ns的时间偏差那就是30米的距离误差足以抵消你所有算法上的优化。所以大型无源定位系统普遍用卫星授时或光纤时间同步把站间时差控制在纳秒甚至亚纳秒量级。FOA的精度则由接收机的频率估计能力决定。频率测量的理论极限大约是1除以信号观测时间你观测信号1秒频率分辨率理论上可以达到几赫兹甚至更好。实际瓶颈往往是信号本身的频率漂移和接收机本振的相位噪声。我在仿真里把频率噪声设成高斯白噪声但真实系统里还会有系统偏差比如接收机间本振不一致这个在仿真里体现不出来却在工程现场非常致命。我见过不少频差系统实际精度比仿真差一个数量级的案例多数都是这种“看不见”的系统偏差造成的。5.3 几条反直觉的仿真结论第一条不能因为TOA精度高就放弃FOA。目标速度一旦超过100 m/s纯TOA定位会因为目标在观测时间内移动了数十米而产生额外的动态误差而FOA能对速度进行估计联合后可以把定位误差拉回来。第二条站数不是越多越好。站多了如果布局不合理或者部分站测量质量差强行参与解算反而可能拖低精度。这时候加权最小二乘可以救你一命把测量噪声大的站权重调小噪声小的站权重调大效果立竿见影。第三条高维问题里初值选择往往比迭代算法本身更重要。FOA定位的非线性度比TOA强得多初值差一点可能就会跌进局部极小值。我的经验是先用纯TOA解出位置初值再把它作为FOA联合迭代的起点实测下来稳定很多。6. 常见问题与避坑指南6.1 矩阵奇异或接近奇异定位矩阵在某些几何布局下会接近奇异尤其是观测站近似共线时。最直接的感受是(A * A)在MATLAB里求解时出现NaN或者巨大的数值。解决办法有三个一是用伪逆或加正则化给A * A加上一个小对角线项比如A * A 1e-6 * eye(3)让矩阵变得可逆二是在布站阶段提前用GDOP筛选几何布局避开近似共线的站位三是在最小二乘前加权重剔除严重异常的观测站。6.2 迭代爆炸或收敛到错误位置泰勒迭代和Gauss-Newton迭代对初值非常敏感初值偏离真实位置太远时迭代可能发散表现为修正量越来越大、残差不降反升。处理方式先用闭式解线性最小二乘或Chan做初值设置迭代上限和步长限制防止修正量过大导致振荡如果残差在两次迭代间不降反升说明已经偏离收敛域此时应该重新选初值而不是硬着头皮继续迭代。6.3 一个非常容易被忽略的坑单位和坐标系这个坑我已经见太多人踩了。时差用纳秒距离用公里速度用km/h频率用MHz然后代入方程一算数值差好几个数量级找半天找不到原因。记住一个原则在仿真脚本里统一使用国际单位制也就是米、秒、赫兹、米每秒只在最终输出画图的时候再转换成你习惯的工程单位。还有坐标系的问题。如果你从其他工具导入观测站坐标务必确认是经纬高还是本地直角坐标直接混用的话GDOP都是错的。先统一做完坐标变换再进定位流程。6.4 实测下来最稳的一套组合拳仿真做多了我给自己固定下来一套流程先用线性最小二乘给初值再用一到两次泰勒迭代精修最后根据各站的残差计算权重再做一次加权最小二乘。这套组合在几何布局一般、噪声较大、初值较差的场景下