1. 从一道赛题看无人机编队定位的实战挑战2022年的数学建模竞赛B题把无人机编队飞行中的一个硬核问题摆在了所有参赛者面前纯方位无源定位。这个题目听起来很学术但它背后指向的是一个在军事侦察、灾害救援、协同作业等领域都极具现实意义的场景。想象一下一个无人机编队在空中执行任务其中几架“小弟”无人机我们称之为跟随无人机的精确位置信息丢失了它们只剩下一个能力——能“看到”编队里其他几架已知位置的“大哥”无人机我们称之为参考无人机并且只能测量出“大哥”相对于自己的方向角方位角而不知道距离。我们的任务就是仅凭这些零散的方向角信息像解一个复杂的空间几何谜题一样把这些“迷路”的无人机在三维空间中的坐标给算出来。这本质上是一个非线性优化问题更具体地说是多站纯方位目标定位问题在无人机编队场景下的一个特化。它难就难在“纯方位”和“无源”这两个词上。“纯方位”意味着信息极度匮乏只有角度线没有距离信息这会导致定位解算存在固有的模糊性比如著名的“左右舷模糊”问题。“无源”则意味着跟随无人机自身不主动发射任何信号如雷达波、激光完全依靠被动接收参考无人机的信号可能是视觉、射频信号等来测向这提升了隐蔽性但也对算法的抗噪能力和鲁棒性提出了更高要求。这道赛题的价值远不止于寻求一个数学上的最优解。它迫使我们去思考在资源观测无人机数量、测量精度受限的真实环境中如何设计一个稳定、高效且实用的定位算法如何将抽象的数学模型转化为可以应对测量误差、模型偏差甚至部分数据缺失的工程方案接下来我将结合常见的解决思路和工程实践中的考量拆解这个问题的核心并分享一套从建模到求解再到误差分析的完整实战框架。2. 问题本质与数学模型构建把现实约束转化为数学语言面对“纯方位无源定位”第一步也是最关键的一步是建立一个精确的数学模型。这个模型必须严格反映题目中的所有约束条件并将物理世界中的观测转化为计算机可以理解和处理的数学关系。2.1 坐标系定义与基本观测方程首先我们需要一个统一的“舞台”。通常建立东北天坐标系以地面某点为原点X轴指向正东Y轴指向正北Z轴垂直向上指向天顶。假设有m架参考无人机其位置坐标已知记为 ( \mathbf{p}_i (x_i, y_i, z_i)^T, i1,2,...,m )。有一架待定位的跟随无人机其真实位置未知记为 ( \mathbf{u} (x, y, z)^T )。所谓“纯方位”观测通常指的是跟随无人机测量得到从自身位置指向参考无人机方向的方位角azimuth和高低角elevation。方位角 ( \alpha_i ) 是在水平面上的投影与正北方向的夹角顺时针或逆时针需统一高低角 ( \beta_i ) 是视线向量与水平面的夹角。从几何关系上参考无人机i相对于跟随无人机的方向向量为 ( \mathbf{v}_i \mathbf{p}_i - \mathbf{u} (x_i - x, y_i - y, z_i - z)^T )。那么观测方位角 ( \hat{\alpha}_i ) 和观测高低角 ( \hat{\beta}_i ) 与这个方向向量的关系为 [ \hat{\alpha}_i \arctan2(x_i - x, y_i - y) \quad \text{(注意反正切函数参数顺序与坐标系定义有关)} ] [ \hat{\beta}_i \arcsin\left(\frac{z_i - z}{|\mathbf{v}_i|}\right) \quad \text{或} \quad \arctan\left(\frac{z_i - z}{\sqrt{(x_i-x)^2 (y_i-y)^2}}\right) ] 这里( |\mathbf{v}_i| ) 是方向向量的模即两者之间的真实距离。关键点来了我们的观测值 ( (\hat{\alpha}_i, \hat{\beta}_i) ) 是带有噪声的记为 ( (\alpha_i^{obs}, \beta_i^{obs}) )。而真实值依赖于未知数 ( \mathbf{u} )。这就构成了我们的基本观测方程。2.2 最小二乘模型的建立与局限性最直观的思路是最小二乘法。既然观测值和模型预测值之间存在偏差那么我们就寻找一个跟随无人机位置 ( \mathbf{u} )使得所有观测角度与模型计算出的角度之差的平方和最小。构建目标函数 [ F(\mathbf{u}) \sum_{i1}^{m} \left[ w_{\alpha} (\alpha_i^{obs} - \hat{\alpha}i(\mathbf{u}))^2 w{\beta} (\beta_i^{obs} - \hat{\beta}i(\mathbf{u}))^2 \right] ] 其中 ( w{\alpha} ) 和 ( w_{\beta} ) 是权重系数可以用来平衡方位角和高低角观测精度的差异通常高低角观测误差更大权重可适当调低。我们的任务就是求解 ( \min_{\mathbf{u}} F(\mathbf{u}) \。然而这个模型直接求解非常困难。因为 ( \hat{\alpha}_i ) 和 ( \hat{\beta}_i ) 都是关于 ( \mathbf{u} ) 的非线性函数包含了arctan、arcsin等导致目标函数 ( F(\mathbf{u}) ) 是一个非凸函数可能存在多个局部极小值。直接用梯度下降、牛顿法等常规优化算法极易陷入局部最优而得不到全局正确解特别是当初始猜测值离真实位置较远时。2.3 转化为伪线性方程极大似然估计与约束最小二乘为了规避非凸性工程上常采用一种“伪线性化”的技巧。我们注意到方向向量 ( \mathbf{v}_i ) 的单位向量 ( \mathbf{u}_i^{obs} ) 可以从观测角度直接计算 [ \mathbf{u}_i^{obs} (\sin\beta_i^{obs}\cos\alpha_i^{obs}, \sin\beta_i^{obs}\sin\alpha_i^{obs}, \cos\beta_i^{obs})^T ] 这个单位向量代表了观测到的方向。而理论上这个单位向量也应该等于 ( (\mathbf{p}_i - \mathbf{u}) / |\mathbf{p}_i - \mathbf{u}| )。于是有 [ \mathbf{u}_i^{obs} \times (\mathbf{p}_i - \mathbf{u}) \mathbf{0} ] 这里“×”表示叉乘。这个等式在无噪声时成立。有噪声时等式右边不为零。我们可以将其转化为一个最小二乘问题寻找 ( \mathbf{u} ) 使得所有叉乘的范数平方和最小。但叉乘方程仍然是非线性的。更进一步的技巧是将方程改写为 [ (\mathbf{I} - \mathbf{u}_i^{obs} {\mathbf{u}_i^{obs}}^T) (\mathbf{p}_i - \mathbf{u}) \mathbf{0} ] 因为 ( \mathbf{I} - \mathbf{u}\mathbf{u}^T ) 是一个投影到垂直于 ( \mathbf{u} ) 的子空间上的投影矩阵。这个方程是线性的整理后得到 [ (\mathbf{I} - \mathbf{u}_i^{obs} {\mathbf{u}_i^{obs}}^T) \mathbf{u} (\mathbf{I} - \mathbf{u}_i^{obs} {\mathbf{u}_i^{obs}}^T) \mathbf{p}_i ] 对每一架参考无人机i我们都能得到这样一个线性方程实际上是三个标量方程但秩为2。将所有m架无人机的方程堆叠起来就形成了一个超定线性方程组( \mathbf{A} \mathbf{u} \mathbf{b} )。我们可以用线性最小二乘法 ( \min |\mathbf{A}\mathbf{u} - \mathbf{b}|^2 ) 来求解 ( \mathbf{u} )。这种方法称为正交向量法或伪线性估计器。注意这种方法求解速度快且能提供一个不错的初始解。但它存在一个显著问题它隐含地假设了各观测噪声是独立同分布且较小的当噪声较大时其估计结果是有偏的并非最优。因此它通常作为下一步更精确非线性优化的初始值。2.4 考虑地球曲率与高度约束进阶在真实的大范围无人机编队中如果考虑地球曲率东北天坐标系就不再适用需要引入地心地固坐标系。此外无人机飞行高度通常在一个相对固定的层或者其气压高度计能提供一个粗糙的高度信息 ( z_{baro} )。这可以作为一个软约束或硬约束加入优化模型。例如在目标函数中加入惩罚项( \lambda (z - z_{baro})^2 )其中 ( \lambda ) 是一个很大的数硬约束或根据高度计精度调整软约束。这能有效降低问题的维度提升解算稳定性尤其是在参考无人机几何分布不佳例如所有参考机几乎在同一高度导致高低角观测对垂直方向定位能力弱时这个高度先验信息至关重要。3. 核心算法实现从理论到可运行的代码有了数学模型接下来就是算法实现。一套稳健的定位算法通常分为两步1用伪线性方法求初始解2用非线性优化方法进行精炼。3.1 伪线性估计器快速获取初始解我们首先实现伪线性估计器。假设我们已经有了obs_angles一个m x 2的数组每一行是[方位角_弧度, 高低角_弧度]以及ref_positions一个m x 3的数组每一行是参考无人机的[x, y, z]坐标。import numpy as np def pseudo_linear_estimator(obs_angles, ref_positions): 使用伪线性估计器计算跟随无人机位置的初始估计。 参数: obs_angles: m x 2 数组 [azimuth, elevation] 单位弧度 ref_positions: m x 3 数组 参考无人机位置 [x, y, z] 返回: u_init: 估计的跟随无人机位置 [x, y, z] m obs_angles.shape[0] A_list [] b_list [] for i in range(m): az, el obs_angles[i] # 计算观测单位向量 u_obs np.array([ np.sin(el) * np.cos(az), np.sin(el) * np.sin(az), np.cos(el) ]) # 构建投影矩阵 P I - u_obs * u_obs^T P np.eye(3) - np.outer(u_obs, u_obs) # 累积系数矩阵和右侧向量 A_list.append(P) b_list.append(P ref_positions[i]) # 堆叠成超定方程组 A np.vstack(A_list) # 形状 (3m, 3) b np.concatenate(b_list) # 形状 (3m,) # 线性最小二乘求解 u_init, residuals, rank, s np.linalg.lstsq(A, b, rcondNone) return u_init这个函数计算效率很高能瞬间给出一个解。但正如之前所说这个解在噪声下是有偏的。我们可以将其作为下一步非线性优化的起点。3.2 非线性优化精炼高斯-牛顿法与Levenberg-Marquardt为了得到统计意义上更优在高斯噪声假设下为最大似然估计的解我们需要回到最初的非线性最小二乘模型并使用迭代优化算法。这里介绍高斯-牛顿法及其稳健变种Levenberg-Marquardt算法。首先定义残差函数。我们使用角度差作为残差这比使用单位向量叉乘更直观也更容易赋予权重。def angle_residuals(u, obs_angles, ref_positions): 计算对于位置u所有观测的角度残差。 参数: u: 待估计的3维位置向量 obs_angles: m x 2 数组观测角度[azimuth, elevation] ref_positions: m x 3 数组 返回: residuals: 残差向量 (2m,)顺序为 [az_res1, el_res1, az_res2, el_res2, ...] m obs_angles.shape[0] residuals np.zeros(2 * m) for i in range(m): delta ref_positions[i] - u range_i np.linalg.norm(delta) # 计算理论角度 az_pred np.arctan2(delta[0], delta[1]) # 注意这里x对应东y对应北azatan2(x,y) el_pred np.arcsin(delta[2] / range_i) if range_i 1e-9 else 0.0 # 观测角度 az_obs, el_obs obs_angles[i] # 方位角残差处理圆周性 az_diff az_obs - az_pred az_diff (az_diff np.pi) % (2 * np.pi) - np.pi # 规整到 [-pi, pi] # 高低角残差 el_diff el_obs - el_pred residuals[2*i] az_diff residuals[2*i 1] el_diff return residuals这里有一个至关重要的细节方位角残差的处理。方位角是一个圆周量差值直接相减可能得到大于180度的值例如真实值355度观测值5度实际差10度但直接减得-350度。必须通过模运算将其规整到[-π, π]区间否则优化算法会因残差不连续而失效。接下来我们可以使用scipy.optimize.least_squares这个强大的工具它内置了Levenberg-Marquardt算法能自动处理雅可比矩阵的近似计算非常方便。from scipy.optimize import least_squares def refine_position_with_lm(u_init, obs_angles, ref_positions, weight_az1.0, weight_el0.5): 使用Levenberg-Marquardt算法精炼位置估计。 参数: u_init: 初始估计位置 obs_angles, ref_positions: 同前 weight_az, weight_el: 方位角和高低角残差的权重 返回: result: scipy优化结果对象包含最优解等信息 # 构建权重向量 m obs_angles.shape[0] weights np.ones(2 * m) weights[0::2] weight_az # 偶数索引为方位角残差 weights[1::2] weight_el # 奇数索引为高低角残差 def weighted_residuals(u): res angle_residuals(u, obs_angles, ref_positions) return res * weights # 对残差进行加权 # 设置边界可选例如高度非负 bounds ([-np.inf, -np.inf, 0], [np.inf, np.inf, np.inf]) # z 0 result least_squares(weighted_residuals, u_init, boundsbounds, methodtrf, losslinear) return resultweight_az和weight_el参数非常实用。在实际传感器中方位角例如来自视觉或射频测向的精度往往高于高低角例如来自单站测向。通过给方位角更高的权重可以引导优化器更信任精度更高的观测。bounds参数可以加入先验知识比如无人机高度不可能为负。3.3 多解性与全局优化策略纯方位定位的一个根本性难题是多解性。在二维平面上两个观测站只能确定目标位于两条方位线的交点上但无法区分真实目标和它的“镜像点”关于基线对称的点。在三维空间中问题更复杂。当参考无人机几何分布不佳时例如所有参考机近似共线或共面优化问题可能存在多个局部极小值算法可能收敛到错误的“镜像解”上。应对策略增加观测数量这是最有效的方法。多于3个不共面的参考无人机可以在一定程度上抑制虚假解但无法完全根除。利用运动信息滤波如果无人机是连续运动的我们可以结合动力学模型如匀速模型使用卡尔曼滤波或粒子滤波利用时间序列的观测数据来平滑轨迹滤波器的预测功能可以帮助跳出局部极小值并识别出物理上不可能的跳跃解如瞬间穿越到镜像点。多初始点搜索在优化时不仅仅使用伪线性解作为初始值而是在解空间内随机采样多个初始点分别进行优化最后选择目标函数值最小的解作为最终输出。虽然计算量大但能显著提高找到全局最优解的概率。利用高度先验如前所述高度约束能极大降低模糊性。在代码中实现一个多初始点的方案def global_refinement(obs_angles, ref_positions, n_trials20): 使用多初始点策略进行全局优化。 best_u None best_cost np.inf # 首先伪线性解作为一个高质量初始点 u_linear pseudo_linear_estimator(obs_angles, ref_positions) init_guesses [u_linear] # 生成随机初始点在参考点包围盒附近 pos_min ref_positions.min(axis0) pos_max ref_positions.max(axis0) for _ in range(n_trials - 1): random_guess pos_min (pos_max - pos_min) * np.random.rand(3) # 可以给高度一个更合理的随机范围比如100-500米 random_guess[2] 100 400 * np.random.rand() init_guesses.append(random_guess) for init_guess in init_guesses: result refine_position_with_lm(init_guess, obs_angles, ref_positions) if result.cost best_cost: best_cost result.cost best_u result.x return best_u, best_cost4. 误差分析与算法鲁棒性仿真与实战检验算法写好了但它到底靠不靠谱这需要通过系统的仿真来检验其性能边界和鲁棒性。我们不能只满足于“看起来能跑”必须知道它在什么情况下会失效误差大概有多大。4.1 设计蒙特卡洛仿真实验蒙特卡洛仿真的核心思想是在设定的真实位置和参考无人机布局下人为地为观测角度添加符合特定分布通常是高斯分布的随机噪声然后用我们的算法进行定位解算将解算结果与真实位置对比统计误差。重复成千上万次就能得到误差的统计分布。def monte_carlo_simulation(true_u, ref_positions, angle_std_deg, num_trials1000): 蒙特卡洛仿真评估定位算法性能。 参数: true_u: 跟随无人机的真实位置 ref_positions: 参考无人机位置 angle_std_deg: 角度观测噪声的标准差度 num_trials: 仿真次数 返回: errors: 每次仿真的定位误差欧氏距离列表 solutions: 每次仿真的解算位置列表 angle_std_rad np.deg2rad(angle_std_deg) m ref_positions.shape[0] errors [] solutions [] for _ in range(num_trials): # 生成带噪声的观测角度 noisy_angles [] for i in range(m): # 计算真实角度 delta ref_positions[i] - true_u az_true np.arctan2(delta[0], delta[1]) el_true np.arcsin(delta[2] / np.linalg.norm(delta)) # 添加高斯噪声 az_noisy az_true np.random.randn() * angle_std_rad el_noisy el_true np.random.randn() * angle_std_rad # 确保高低角在合理范围内[-pi/2, pi/2] el_noisy np.clip(el_noisy, -np.pi/2 1e-3, np.pi/2 - 1e-3) noisy_angles.append([az_noisy, el_noisy]) noisy_angles np.array(noisy_angles) # 使用我们的算法进行定位 u_est, _ global_refinement(noisy_angles, ref_positions, n_trials10) # 仿真时减少尝试次数以加速 # 计算误差 error np.linalg.norm(u_est - true_u) errors.append(error) solutions.append(u_est) return np.array(errors), np.array(solutions)4.2 关键性能指标与影响因素分析运行仿真后我们可以计算几个核心指标均方根误差rmse np.sqrt(np.mean(errors**2))综合衡量精度。圆概率误差CEP50/CEP95即50%或95%的误差落在多大的半径内更能反映误差分布。收敛成功率可以定义一个误差阈值如100米低于该阈值的算作成功定位计算成功率。通过改变仿真条件我们可以系统分析影响定位精度的因素几何稀释精度这是最重要的因素。它描述了参考无人机相对于跟随无人机的几何布局对定位误差的放大效应。理想情况是参考无人机从各个方向包围跟随无人机且不在同一个平面上。最差的情况是所有参考无人机和跟随无人机近乎共线此时GDOP值趋于无穷大定位结果完全不可信。在仿真中可以通过设计不同的ref_positions布局来直观感受GDOP的影响。例如将参考机排成一条直线即使噪声很小定位误差也可能非常大且算法可能完全收敛到错误的镜像点。观测噪声水平噪声越大误差自然越大。但更重要的是噪声增大到一定程度非线性优化可能无法收敛到全局最优解导致定位失败。我们的多初始点策略就是为了对抗这个问题。参考无人机数量数量越多提供的约束越多理论上精度越高鲁棒性越强。但边际效益递减且增加一架无人机意味着成本和数据通信负担。仿真可以帮助我们找到满足特定精度要求所需的最少参考机数量。算法对比我们可以将伪线性估计器、单初始点LM算法、多初始点全局优化算法的结果放在一起对比。通常会发现伪线性解在有噪声时存在系统性偏差其RMSE可能比非线性优化后的结果还要大。而多初始点策略的成功率显著高于单初始点。4.3 实战中的非理想情况处理仿真假设噪声是高斯分布但现实更骨感野值某个传感器的某次观测可能完全错误。这会导致优化目标函数出现一个巨大的残差项将解“拉偏”。应对方法是使用鲁棒损失函数。在scipy.optimize.least_squares中loss参数可以设置为soft_l1或cauchy它们能降低大残差对总代价的影响相当于实现了M-估计。result least_squares(weighted_residuals, u_init, losssoft_l1, f_scale0.1)f_scale参数控制了损失函数的尺度需要根据残差的典型大小进行调整。部分数据缺失飞行中可能暂时丢失对某架参考无人机的观测。我们的算法框架天然支持不同数量的观测。只要剩余的参考无人机数量不少于3且几何分布良好算法依然可以工作。但在滤波框架下需要对观测模型进行动态调整。时钟同步与坐标系对齐这是工程实现中极易忽略但致命的问题。所有参考无人机的位置信息、跟随无人机的观测数据必须在同一时间戳和同一坐标系下。如果数据存在时间延迟或坐标系未统一例如有的用WGS-84有的用局部坐标系那么再精巧的算法也是徒劳。必须在数据预处理环节进行严格的时间和空间同步。5. 从模型到系统工程化扩展思考解决一道赛题可能止步于一个在特定数据集上表现良好的算法。但要将其应用于真实的无人机编队系统还有很长的路要走。5.1 分布式与协同定位上述模型是集中式处理所有观测数据传回一个中心节点地面站或长机进行解算。在去中心化的编队中每架无人机可能既是“跟随者”也是“参考者”。这就演变为协同定位问题。每架无人机根据自己的观测对其他无人机方向的测量和接收到的邻居无人机的位置估计通常也带有不确定性共同迭代优化整个编队所有无人机的位置。这通常使用分布式一致性算法或基于因子图的优化来实现。其通信和计算复杂度更高但鲁棒性和可扩展性更好。5.2 与惯性导航的松耦合纯方位定位是间断性的更新率取决于测向传感器的数据输出频率且在几何条件差时可能完全失效。而惯性测量单元可以提供高频、连续但会随时间发散的位姿信息。一个成熟的方案是松耦合组合导航将纯方位定位算法的输出结果位置和位置不确定性作为一个外部观测量与IMU的惯性导航解算结果进行卡尔曼滤波融合。这样在方位定位可用时用它来校正IMU的累积误差在方位定位不可用或质量差时系统可以依靠IMU自主推算一段时间。这大大提升了系统的可用性和可靠性。5.3 传感器选型与误差标定算法依赖高质量的方位观测。在工程中这涉及到传感器选型视觉方案使用机载摄像头识别特定图案的僚机或使用特征点匹配。优点是成本低、信息丰富可能还能估计距离但对光照、天气敏感计算量大且视角有限。射频方案使用定向天线阵列测量射频信号到达角。作用距离远不受光照影响但易受多径效应干扰且需要额外的射频硬件。 无论哪种方案传感器都必须经过严格的标定。摄像头需要标定内参焦距、畸变和外参相对于无人机机体的安装角度射频阵列需要标定通道间幅度和相位误差。这些标定参数的不准确会直接转化为系统性的观测误差必须在算法中予以考虑或补偿。5.4 通信链路的设计编队无人机之间需要交换两类关键数据1参考无人机广播自身的高精度位置信息2跟随无人机上传或共享自己的观测数据在协同定位中。这对通信链路的实时性、可靠性和带宽提出了要求。需要考虑使用专用的数据链设计抗干扰的通信协议并处理好通信延迟对定位结果的影响通常在滤波器中作为状态延迟进行处理。回过头看2022年的这道赛题它就像一把钥匙打开了一扇通往复杂系统设计的大门。从抽象的数学公式到考虑噪声和误差的算法实现再到应对真实世界各种非理想因素的工程化方案每一步都充满了挑战和权衡。真正的价值不在于求出某个特定数据集的答案而在于掌握这一套从问题定义、数学建模、算法实现、仿真验证到工程扩展的完整方法论。当你下次再看到“纯方位无源定位”这几个字时脑海里浮现的不应再是冰冷的公式而是一个在三维空间中穿梭的无人机编队以及维系它们之间精确相对位置的那条看不见的、由数学和算法编织成的纽带。