纯方位无源定位：从几何原理到无人机编队协同的算法实现-北京尧图网络

1. 项目背景与核心挑战当无人机“看不见”时如何定位在无人机集群协同作业特别是遂行编队飞行这个场景里有一个经典且极具挑战性的问题如果一架或多架无人机失去了GPS、北斗等有源定位信号甚至机载的视觉、激光雷达等主动感知设备也因故障或干扰失效它们该如何仅凭彼此间“听”到的方位信息来确定自己的精确位置从而维持编队队形、完成既定任务这就是“纯方位无源定位”问题的现实背景。它听起来有点像我们在一个完全黑暗、没有地图的房间里仅凭听到的几个同伴的声音方向来推断自己所在的位置。2022年的全国大学生数学建模竞赛B题正是将这个极具工程价值和理论深度的前沿问题抽象成了一个可供数学建模的赛题。题目要求参赛者针对无人机编队中部分无人机“失明”的情况建立数学模型仅利用它们接收到的来自编队中其他正常无人机发出的方位角信息即方向没有距离来求解这些“失明”无人机的坐标。这不仅仅是解一道数学题其背后对应的是无人机在复杂电磁环境、峡谷或室内等GNSS拒止条件下的生存与协同能力。我之所以对这个题目印象深刻是因为它完美地将几何、优化和状态估计理论融合在了一起并且有非常明确的工程落地场景。那些热搜词如“无人机编队”、“无源定位”、“定位模型”、“规划模型”都精准地指向了这个问题的不同侧面。简单来说核心挑战在于信息的不完备性。我们只知道角度方位线不知道距离。这就好比从一点看向另一点你只知道那条视线的方向但对方究竟在100米外还是1000米外是未知的。单条方位线无法定位一个点它定义了一条从观测点出发的射线。因此必须依赖多个不同位置的观测者或者同一观测者在不同时间点的移动来获得多条方位线它们的交点理论上就是目标的位置。在无人机编队中我们拥有的正是多个“观测者”正常无人机对多个“目标”失联无人机的方位观测。问题的核心就转化为如何从这些带有噪声的方位线观测数据中鲁棒、高效地反演出最可能的目标位置坐标。2. 问题拆解从现实场景到数学模型框架面对这样一个工程问题直接上手写代码是行不通的。我们必须先进行严谨的问题拆解和数学抽象。整个问题可以分解为几个层次2.1 场景与假设定义首先我们需要明确物理场景和建模假设这是所有模型的基石。坐标系通常建立二维平面直角坐标系如果考虑高度则是三维但国赛题常简化在二维。将整个编队飞行的空域投影到平面上。无人机角色基准无人机或导引机假设其位置精确已知通过自身完好的GPS等系统。它们是整个定位系统的“锚点”。待定位无人机失联机其位置未知是需要求解的对象。观测信息已知基准无人机对待定位无人机的方位角测量值。方位角通常定义为从正北方向或X轴正方向顺时针旋转到观测者与目标连线的角度。观测噪声真实的传感器如电子罗盘、视觉系统测量存在误差。因此我们得到的方位角数据是带有噪声的通常假设为高斯白噪声。这意味着我们建立的模型必须具备一定的抗噪声能力。问题目标给定所有基准无人机的位置坐标(x_i, y_i)以及它们对待定位无人机j的方位角测量值θ_ij求解待定位无人机j的最优位置估计(x_j, y_j)。2.2 核心数学模型方位交汇定位Bearings-Only Localization最基本的数学模型源于三角测量原理。对于单个待定位无人机j如果有一个基准无人机i观测到它那么它们满足以下几何关系tan(θ_ij) (y_j - y_i) / (x_j - x_i)或者更常用的是正弦/余弦形式sin(θ_ij) * (x_j - x_i) - cos(θ_ij) * (y_j - y_i) 0这个方程描述了一条通过基准点(x_i, y_i)、方向角为θ_ij的直线。待定位点(x_j, y_j)理论上应该位于这条直线上。当有多个至少两个基准无人机观测同一个目标时我们就得到了多条这样的直线方程。在无噪声的理想情况下这些直线应交于一点该点即为目标位置。这就是方位交汇定位的直观理解。2.3 从理想交汇到实际优化然而由于观测噪声的存在多条方位线往往不会精确交于一点而是形成一个小的“误差三角形”区域。因此我们不能直接求解方程组因为无解而是需要寻找一个点使得它到所有这些方位线的“距离”之和最小。这就将一个几何问题转化为了一个非线性最小二乘优化问题。定义代价函数Cost Function为所有观测的方位角误差的平方和F(x_j, y_j) Σ [ (测量的方位角 - 根据当前估计位置计算的理论方位角) ^ 2 ] Σ [ θ_ij_measured - arctan2( (y_j - y_i), (x_j - x_i) ) ] ^ 2其中arctan2是四象限反正切函数能给出正确的角度值。我们的目标就是找到一组(x_j, y_j)使得代价函数F的值最小。注意这里有一个关键的建模细节。直接使用角度差平方和作为代价函数在角度值接近圆周跳变点如359°和1°时会出现不连续问题。更稳健的做法是处理角度的正弦和余弦值。我们可以将代价函数定义为基于向量投影的误差F(x_j, y_j) Σ || ( [x_j - x_i, y_j - y_i]^T / d_ij ) × u_ij ||^2其中d_ij是估计距离u_ij [sin(θ_ij), -cos(θ_ij)]是垂直于观测方向线的单位向量。这个形式的物理意义是目标位置到每条方位线的垂直距离的平方和它避免了角度周期性问题是工程上更常用的形式。3. 求解策略与算法选型从理论到代码实现建立了优化模型后接下来就是选择求解算法。这是一个典型的无约束非线性最小二乘问题。我们可以根据问题的规模无人机数量、对实时性的要求以及初始化质量来选择不同的求解器。3.1 梯度下降法家族稳健的通用选择对于这类问题基于梯度的迭代优化方法是主流。它们的核心思想是从一个初始猜测位置出发沿着代价函数下降最快的方向负梯度方向逐步调整位置直至收敛到局部极小值点。最速下降法最简单但收敛速度慢尤其在“山谷”地形中容易 zig-zag。高斯-牛顿法Gauss-Newton专门为最小二乘问题设计。它利用目标函数的特殊结构误差项的平方和用雅可比矩阵J近似代替海森矩阵H大大减少了计算量。其迭代公式为Δp - (J^T * J)^(-1) * J^T * e(p)其中p [x_j, y_j]是待估参数e(p)是误差向量J是误差向量对参数的雅可比矩阵。高斯-牛顿法在初始点靠近真值、且近似成立时收敛速度非常快。列文伯格-马夸尔特法Levenberg-Marquardt, LM这可以说是解决非线性最小二乘问题的“瑞士军刀”。它综合了最速下降法和高斯-牛顿法的优点。当迭代点远离解时LM算法更像最速下降法保证收敛当接近解时它自动切换到高斯-牛顿法加速收敛。LM算法通过引入一个阻尼因子 λ 来调节(J^T * J λ * I) * Δp - J^T * e(p)当 λ 很大时方程近似为λ * I * Δp - J^T * e(p)即最速下降当 λ 很小时方程退化为高斯-牛顿法。LM算法具有非常好的鲁棒性是很多数学库如SciPy, MATLAB的lsqnonlin的默认选择。3.2 初始化的重要性如何给算法一个“好起点”非线性优化算法严重依赖于初始值。一个糟糕的初始猜测可能导致算法收敛到错误的局部极小点甚至发散。在纯方位定位中由于没有距离信息初始值的选择需要一些技巧。几何重心法一种简单有效的方法是将每条方位线向外延伸一段合理的估计距离例如根据编队通常的间距设定一个值如50米得到一个估计点。对所有基准无人机产生的估计点取几何平均作为初始值。虽然粗糙但通常能提供一个位于真实位置附近的起点。最小二乘直接解伪线性化对方位观测方程tan(θ) (y - y_i)/(x - x_i)进行变形可以得到一个关于x和y的线性方程x * sinθ - y * cosθ x_i * sinθ - y_i * cosθ。对于每个观测我们都能得到这样一个方程。将所有观测的方程联立就构成了一个线性方程组A * [x, y]^T b。我们可以用最小二乘法直接求解这个线性方程组得到的结果可以作为非线性优化的一个优质初始值。这个方法被称为“伪线性估计PLE”它计算速度快但需要注意由于对方程进行了非线性变换其噪声统计特性发生了变化在噪声较大时可能会有偏。不过作为初始化步骤它非常出色。3.3 代码实现骨架Python示例下面我将结合上述思路给出一个使用PythonSciPy库实现的核心代码骨架。这里假设我们已经有了基准无人机位置列表anchors形状为[n_anchors, 2]和对应的方位角测量列表bearings形状为[n_anchors]单位为弧度。import numpy as np from scipy.optimize import least_squares def bearing_residuals(params, anchors, bearings): 计算代价函数的残差误差向量。 params: 待估参数 [x, y] anchors: 基准点数组形状 (n, 2) bearings: 方位角测量值数组形状 (n,)单位弧度返回残差向量形状 (n,) x, y params dx x - anchors[:, 0] dy y - anchors[:, 1] # 计算理论方位角 pred_bearings np.arctan2(dy, dx) # 处理角度周期性问题确保误差在 [-pi, pi] 之间 residuals np.unwrap(pred_bearings - bearings) # 另一种更稳定的残差定义点到直线的垂直距离 # unit_vec_perp np.column_stack([np.sin(bearings), -np.cos(bearings)]) # 垂直单位向量 # vec_to_target np.column_stack([dx, dy]) # residuals np.sum(vec_to_target * unit_vec_perp, axis1) # 投影长度即距离 return residuals def pseudo_linear_estimator(anchors, bearings): 伪线性估计器用于提供优化的初始值。求解 A * [x, y]^T b 的最小二乘解。 sin_t np.sin(bearings) cos_t np.cos(bearings) A np.column_stack([sin_t, -cos_t]) # 系数矩阵 b anchors[:, 0] * sin_t - anchors[:, 1] * cos_t # 常数项 # 最小二乘解 x_est, y_est np.linalg.lstsq(A, b, rcondNone)[0] return np.array([x_est, y_est]) def locate_target(anchors, bearings, initial_guessNone): 主定位函数。 anchors: 基准点坐标二维数组 bearings: 方位角测量一维数组弧度 initial_guess: 可选初始猜测返回优化后的位置 [x, y] if initial_guess is None: # 使用伪线性估计提供初始值 initial_guess pseudo_linear_estimator(anchors, bearings) # 或者使用简单的几何方法将方位线平移一段距离后求平均 # est_distance 50.0 # 假设的初始距离 # est_points anchors est_distance * np.column_stack([np.cos(bearings), np.sin(bearings)]) # initial_guess np.mean(est_points, axis0) # 使用Levenberg-Marquardt算法求解非线性最小二乘问题 result least_squares(bearing_residuals, initial_guess, args(anchors, bearings), methodlm) if result.success: return result.x else: raise RuntimeError(fOptimization failed: {result.message}) # 模拟数据生成与测试 if __name__ __main__: np.random.seed(42) # 1. 设置真实目标位置 true_target np.array([30.0, 40.0]) # 2. 生成4个基准无人机位置假设已知 anchors np.array([[0, 0], [100, 0], [0, 100], [100, 100]]) # 3. 计算真实的方位角并加入高斯噪声 true_vecs true_target - anchors true_bearings np.arctan2(true_vecs[:, 1], true_vecs[:, 0]) noise_std np.deg2rad(2.0) # 假设方位角测量噪声标准差为2度 noisy_bearings true_bearings np.random.randn(len(anchors)) * noise_std # 4. 调用定位函数 estimated_target locate_target(anchors, noisy_bearings) # 5. 输出结果 print(f真实目标位置: {true_target}) print(f估计目标位置: {estimated_target}) error np.linalg.norm(estimated_target - true_target) print(f定位误差: {error:.4f} 米)这段代码提供了一个完整的从数据生成、初始化、优化到结果评估的流程。least_squares函数是SciPy中强大的优化器其methodlm指定使用列文伯格-马夸尔特算法。在实际比赛中你需要根据题目给出的具体数据格式调整数据读取和预处理部分。4. 模型进阶、误差分析与实战避坑指南基础的定位模型搭建起来后要让它真正实用、鲁棒还需要考虑更多现实因素。这也是区分优秀模型和普通模型的关键。4.1 观测几何与精度稀释GDOP并不是任意分布的基准无人机都能提供好的定位精度。想象一下如果所有基准无人机都挤在同一个方向那么它们发出的方位线几乎平行交汇区域会是一个又长又窄的带状定位在垂直方向上的误差就会非常大。这种由于观测几何构型导致的定位精度下降称为精度稀释Geometric Dilution of Precision, GDOP。在纯方位定位中GDOP与基准点相对于目标的张角有关。理想的情况是基准点均匀分布在目标周围张角接近360度。在建模时我们可以通过Fisher信息矩阵FIM或克拉美-罗下界CRLB来理论分析给定几何构型下所能达到的最佳定位精度。这可以帮助我们在编队规划阶段就有意识地优化基准无人机的布局从源头上提升定位性能。例如在解题时如果题目允许调整部分无人机的位置那么一个优化模型可以是如何调整这些“锚点”的位置使得整体编队的定位误差的某种统计量如最大值或均方根最小化。4.2 异常观测检测与鲁棒估计我们的模型假设观测噪声是高斯分布。但现实中可能存在粗大误差Outliers例如某个无人机的罗盘突然受到强磁干扰给出了一个完全错误的方位角。这种“野值”会严重扭曲最小二乘的结果因为最小二乘对大的残差给予平方倍的权重非常敏感。为了提高模型的鲁棒性我们需要引入鲁棒估计技术RANSAC随机采样一致性这是一种非常经典的方案。其基本思想是随机从所有观测中选取最小子集对于二维定位最少需要2个观测计算一个候选解然后用这个解去测试其他观测点统计符合该解的“内点”数量。重复这个过程多次最终选择内点最多的那个候选解并用所有内点重新进行精细的最小二乘估计。RANSAC能有效剔除局外点。使用更鲁棒的损失函数将最小二乘中的L2范数平方和替换为对异常值不敏感的范数例如L1范数绝对值和或Huber损失函数。Huber损失在误差较小时是二次的像L2在误差较大时是线性的像L1从而兼具对高斯噪声的效率和对抗异常值的鲁棒性。在SciPy的least_squares中可以通过设置loss参数如losshuber来轻松实现。4.3 多目标与数据关联问题当有多架待定位无人机时问题变得更加复杂。我们不仅需要估计每个目标的位置还需要解决数据关联问题即判断哪个方位角观测是属于哪个目标的。如果基准无人机只能测量方位角而无法区分目标ID这就成了一个复杂的组合优化问题。在2022年国赛题中通常假设观测数据是已经关联好的即知道哪个方位角对应哪架无人机。但如果是更一般的情况可能需要引入诸如多假设跟踪MHT或基于聚类的方法来解决。4.4 实战编程中的关键细节与避坑点角度单位与周期处理这是最容易出错的地方。数学库如np.arctan2,math.atan2返回的角度单位是弧度。题目数据或传感器数据很可能是度。务必在计算前统一转换为弧度。另外角度误差pred - meas要使用np.unwrap或手动加减2π来处理359°和1°这种跳变否则代价函数会不连续导致优化失败。我在代码中使用了np.unwrap这是最安全的方式。优化算法的收敛性与调参scipy.optimize.least_squares功能强大但需要合理设置参数。ftol函数值变化容忍度、xtol参数变化容忍度和max_nfev最大函数评估次数是常用的调参项。如果优化不收敛首先检查初始值是否太差其次可以适当放宽容忍度或增加迭代次数。使用verbose2参数可以打印迭代过程帮助调试。雅可比矩阵的提供默认情况下least_squares会使用有限差分法数值计算雅可比矩阵。对于性能要求高的问题我们可以手动提供解析的雅可比矩阵这能显著提升优化速度和精度。对于我们的残差函数r_i sin(θ_i)*(x - x_i) - cos(θ_i)*(y - y_i)其对x和y的偏导数是dr_i/dx sin(θ_i),dr_i/dy -cos(θ_i)。将这个雅可比矩阵通过jac参数传入优化效率会更高。结果验证与不确定性评估优化得到点估计后如何评估这个估计的可信度我们可以计算估计值的协方差矩阵。对于最小二乘估计在最优解附近参数估计的协方差矩阵可以近似为Cov σ^2 * (J^T * J)^{-1}其中σ^2是观测噪声方差的估计可通过残差均方和估算J是最优解处的雅可比矩阵。这个协方差矩阵的对角线元素就是x和y坐标的估计方差其平方根即为标准差可以理解为定位误差的“置信区间”。5. 从定位到编队控制模型的闭环与应用延伸解决了单点、单时刻的定位问题只是第一步。在无人机遂行编队飞行的动态场景中我们需要的是连续、实时的位置估计并以此为基础进行队形保持控制。这就引出了更高级的模型。5.1 基于滤波的时序定位跟踪静态定位模型处理的是“快照”数据。而在飞行中无人机是连续运动的。我们可以利用运动模型如匀速模型、匀加速模型来预测无人机下一时刻的位置然后将方位观测作为量测信息通过滤波算法来更新和修正预测实现跟踪。最经典的算法是扩展卡尔曼滤波EKF。状态预测根据上一时刻的位置和速度预测当前时刻的位置。观测更新用当前时刻获得的多个方位角观测来修正预测的位置。由于方位观测方程是非线性的EKF需要对其进行线性化求雅可比矩阵。 EKF将纯方位定位从一个静态批处理优化问题变成了一个动态的、递归的序贯估计问题计算效率高非常适合实时系统。这也是“无人机飞控”、“无人机控制算法”等热词背后涉及的核心技术之一。5.2 编队队形重构与协同路径规划当部分无人机失联被重新定位后整个编队需要调整以恢复到期望的几何队形如三角形、菱形、一字形。这涉及到编队控制问题。一种常见的思路是“领航-跟随”法指定一架或几架无人机为领航者其余跟随者根据与领航者的期望相对位置Δx, Δy以及自身估计出的绝对位置计算出控制指令速度、航向驱使自己到达期望的相对位置。这里的控制律可以是简单的P比例控制也可以是更复杂的模型预测控制MPC。更进一步如果整个编队需要在有障碍物的环境中飞行那么结合了定位信息的协同路径规划就至关重要。这正对应了热词中的“无人机路径规划算法”、“无人机自主路径规划仿真”。我们可以将重新集结的编队视为一个整体为其规划一条安全、高效的全局路径同时内部各成员根据避障和队形保持要求进行局部调整。5.3 仿真验证从MATLAB/Python到更逼真环境在数学建模竞赛中用MATLAB或Python进行数值仿真验证模型是标准流程。但若要贴近工程实践可以考虑在更高级的仿真环境中测试例如ROS Gazebo可以搭建一个包含无人机动力学模型、传感器噪声模型和物理引擎的仿真环境用你写的定位算法作为ROS节点订阅虚拟的方位传感器话题发布估计的位置再通过控制器去控制无人机形成一个完整的闭环。这能极大地验证算法在更真实扰动下的鲁棒性。PX4/Gazebo仿真直接使用PX4开源飞控的软件在环SITL仿真配合Gazebo环境可以测试算法与真实飞控软件的集成能力。这些仿真平台的学习曲线较陡但它们是连接理论模型与真实无人机系统的桥梁。对于有志于从事无人机、机器人相关研究的同学尽早接触这些工具非常有价值。回顾整个从问题理解、模型建立、算法实现到进阶应用的过程纯方位无源定位就像是为无人机赋予了一种“盲人听声辨位”的协同感知能力。它的魅力在于用看似简单的角度信息通过精巧的数学和算法构建出对空间位置的精确感知。在实际操作中我最大的体会是模型的鲁棒性往往比单纯的精度更重要。一个能处理异常数据、对初始值不敏感、在恶劣几何条件下仍能给出可用结果的算法远比一个只在理想仿真中精度高但脆弱的算法有价值。因此在实现基本定位功能后务必投入精力到初始化策略、异常值处理和不确定性分析这些“非核心”但至关重要的环节上这才是从“模型跑通”到“系统可靠”的关键一跃。

纯方位无源定位：从几何原理到无人机编队协同的算法实现

相关推荐

一致性哈希不是平均分配：一次扩容把缓存命中率从 92% 打到 61% 的事故

Proxima优化vLLM KV Cache内存管理，提升GPU并发推理能力

数学建模竞赛B题解题全攻略：从预测优化到论文写作

更多资讯

UDP之上实现可靠传输：停等、GBN与SR协议详解与避坑指南

从扩散模型到DiT：FLUX.2/Seedream/Qwen/GLM/Z-image选型指南

Artcraft手作实战：热缩片与树脂的材料工艺及工具搭配指南

具身智能安全控制：当AI拥有四肢，如何握紧电源线

Spring Boot 3.4集成YOLOv12：Java实现工业缺陷检测实战

MySQL性能调优实战：执行计划、锁等待与主从延迟排查

今日更新

悟空开源无代码平台实战：从实体建模到应用设计避坑指南

基于.NET9的多租户权限系统：数据权限与动态API落地实践

场地预约小程序毕设实战：SSM+微信小程序+MySQL数据库设计到避坑

本周精选

金融AI落地指南：从调研报告到技术选型与工程实践

大模型上下文成本与注意力优化技术深度解析——为何O(n²)平方复杂度无法被常规优化消除｜TaoToken 统一 Key 通道实测

基于GMM的语音识别系统原理与MATLAB实现详解

本月热门

我发现了一个新思路：用 Remotion + Claude Code 像写代码一样自动化生成短视频

Windows下 Codex 中 Chrome 和 Computer Use 插件不可用问题排查及解决参考方式：TaoToken 统一 Key 配置与验证

2026 大模型集体涨价：用 Python 做企业 Token 成本测算与选型避坑（附配置）

读完文章，想聊聊您的网站？