1. 从一道赛题到真实世界的定位难题去年备赛期间我和团队反复琢磨的一道题就是2022年高教社杯数学建模竞赛的B题。题目本身不长但场景非常有意思假设我们有一架无人机它本身不发射任何信号无源只能被动地测量它与地面两个已知位置的固定信标之间的夹角。我们的任务就是仅凭这些时变的夹角观测数据去反推出无人机自身的实时位置和运动轨迹。这听起来像是一个纯粹的数学游戏但如果你对无人机、机器人或者任何需要自主定位的领域有所了解就会立刻意识到这恰恰是许多现实问题的核心抽象。比如无人机在GPS拒止环境如室内、峡谷、城市楼宇间下的视觉或射频定位本质上就是利用自身传感器相机、天线观测已知特征点信标、地标的角度或方向来解算自身位姿。题目中的“无源”特性更是强调了系统的隐蔽性和低功耗优势这在军事侦察或某些特定民用场景中至关重要。所以复盘这道题远不止是解一道数学题。它是一次绝佳的契机让我们深入理解“角度观测定位”这一经典问题的数学内核、求解的挑战以及从理想模型到工程实践需要跨越的鸿沟。本文将结合我自己的解题思路和后续的拓展研究详细拆解从模型构建、算法求解到误差分析与仿真验证的全过程并分享一些在仿真编程中容易踩的“坑”。2. 问题重述与坐标系建立一切计算的起点首先我们需要把模糊的自然语言描述转化为精确的数学模型。题目描述可以提炼为以下要素已知量两个地面信标的位置记为 ( A(x_A, y_A, z_A0) ) 和 ( B(x_B, y_B, z_B0) )。通常我们会将地面设为 ( z0 ) 平面。一系列离散时间点 ( t_k (k1,2,...,N) ) 上无人机观测到的与信标A、B的夹角 ( \theta_k )。这个夹角通常定义为从无人机位置看向量 ( \overrightarrow{UA} ) 与向量 ( \overrightarrow{UB} ) 之间的夹角。未知量无人机在每个观测时刻 ( t_k ) 的位置 ( U_k(x_k, y_k, z_k) )。目标根据所有时刻的观测夹角序列 ( {\theta_k} )估计出无人机的位置序列 ( {U_k} )。第一步也是至关重要的一步是建立合适的坐标系。一个清晰的坐标系能极大简化后续的向量运算。最直接的方式是建立三维直角坐标系东北天ENU。将地面视为XY平面东-北平面Z轴垂直向上天。两个信标A和B的坐标是已知的无人机的坐标是待求的。在这个坐标系下向量 ( \overrightarrow{U_kA} A - U_k (x_A - x_k, y_A - y_k, -z_k) )同理 ( \overrightarrow{U_kB} B - U_k (x_B - x_k, y_B - y_k, -z_k) )。根据向量夹角公式观测模型可以写为[ \cos\theta_k \frac{\overrightarrow{U_kA} \cdot \overrightarrow{U_kB}}{|\overrightarrow{U_kA}| |\overrightarrow{U_kB}|} ]其中( \cdot ) 表示向量点积( |\cdot| ) 表示向量的模长度。这就是我们最基本的观测方程。对于每一个时刻 ( k )我们都有一个这样的方程其中包含三个未知数 ( (x_k, y_k, z_k) )。注意这里隐含了一个重要的假设——观测夹角 ( \theta_k ) 是准确无误的。但现实中传感器如视觉系统、测向天线必然存在测量噪声。因此我们的模型从求解确定方程转变为在噪声干扰下的状态估计问题。3. 模型构建从几何约束到优化问题只有一个夹角观测方程却有三个未知数显然这是一个欠定问题单点无法定位。这也是题目设计的精妙之处无人机是运动的我们拥有一个时间序列的观测。我们需要利用无人机运动的连续性或动力学约束来增加约束条件使问题可解。常见的思路有两种3.1 基于运动模型的滤波方法这种方法假设无人机遵循某种运动模型例如匀速CV模型或匀加速CA模型。将无人机的状态位置、速度甚至加速度作为估计量将夹角观测作为量测量构建一个状态空间模型。然后采用卡尔曼滤波KF或其非线性变种如扩展卡尔曼滤波EKF、无迹卡尔曼滤波UKF进行递推估计。这种方法能很好地处理时序数据并给出平滑的轨迹。3.2 基于批量优化的方法这也是赛题中更常见、更直接的思路。我们不对运动做强假设而是直接估计每个时刻的位置。为了克服单点欠定的问题我们引入一个合理的假设无人机轨迹是平滑的。这可以通过在优化目标中加入一个轨迹平滑项来实现。具体来说我们可以构建一个非线性最小二乘问题定义待优化变量为所有时刻的无人机位置( \mathbf{X} [x_1, y_1, z_1, x_2, y_2, z_2, ..., x_N, y_N, z_N]^T )。目标函数由两部分组成数据拟合项衡量估计位置计算出的夹角与观测夹角的差异。 [ f_{data}(\mathbf{X}) \sum_{k1}^{N} \left( \cos\theta_k^{calc}(\mathbf{X}) - \cos\theta_k^{obs} \right)^2 ] 这里使用余弦值是为了避免直接处理角度360度跳变带来的周期性歧义。也可以使用角度差但需要注意归一化到 ([-π, π]) 区间。轨迹平滑项惩罚相邻时刻位置的变化鼓励轨迹平滑。最简单的是使用一阶差分速度近似恒定或二阶差分加速度近似恒定。 [ f_{smooth}(\mathbf{X}) \lambda \sum_{k2}^{N} | U_k - U_{k-1} |^2 \mu \sum_{k3}^{N} | U_k - 2U_{k-1} U_{k-2} |^2 ] 其中 ( \lambda ) 和 ( \mu ) 是正则化系数用于控制平滑项的权重。它们是需要调整的超参数。最终的总目标函数为 [ F(\mathbf{X}) f_{data}(\mathbf{X}) f_{smooth}(\mathbf{X}) ]我们的任务就是找到一组位置序列 ( \mathbf{X}^* )使得 ( F(\mathbf{X}) ) 最小。这是一个大规模3N维的非线性最小二乘问题。实操心得在比赛有限的时间内批量优化方法通常更直观代码实现也相对直接。选择平滑项时一阶差分速度平滑通常就够了除非你知道无人机机动性很强。正则化系数 ( \lambda ) 的选择非常关键太小平滑作用弱噪声影响大太大轨迹会被过度平滑可能丢失真实机动细节。一个实用的技巧是从一个较小的值如0.1开始根据仿真结果视觉调整。4. 核心算法Levenberg-Marquardt (LM) 实战详解面对上节构建的非线性最小二乘问题 ( \min F(\mathbf{X}) )我们需要一个稳健高效的求解器。Levenberg-Marquardt (LM) 算法正是为此类问题而生的利器它在高教社杯赛题中也被频繁使用。这里我们不只讲调用更要拆解其为何适合本题。4.1 为什么是LM算法LM算法是高斯-牛顿法和最速下降法的混合体。理解这一点至关重要高斯-牛顿法在最优解附近收敛速度极快二阶收敛。但它依赖于初始猜测要足够好并且要求残差函数接近线性或者说二阶项可忽略。如果初始点离真值太远其近似的Hessian矩阵可能不是正定的导致迭代失败。最速下降法非常稳健即使初始点很差也能保证向函数值下降的方向移动。但它的收敛速度很慢线性收敛尤其在谷底会 zig-zag 前进。LM算法的聪明之处在于它引入了一个阻尼因子 ( \mu ) 来动态调整。当当前估计远离解时增大 ( \mu )算法行为更像最速下降法保证稳健下降当接近解时减小 ( \mu )算法行为更像高斯-牛顿法加速收敛。这种自适应机制使其兼具了全局收敛性和局部快速收敛性非常适合我们这种初始位置未知、非线性程度较高的定位问题。4.2 LM算法的迭代步骤与核心公式假设我们的目标函数是 ( F(\mathbf{X}) \frac{1}{2} \sum_{i} r_i(\mathbf{X})^2 \frac{1}{2} \mathbf{r}(\mathbf{X})^T \mathbf{r}(\mathbf{X}) )其中 ( \mathbf{r}(\mathbf{X}) ) 是残差向量包含了数据拟合和平滑项的所有残差。在每次迭代中LM算法求解以下线性方程来更新参数 [ (\mathbf{J}^T\mathbf{J} \mu \mathbf{I}) \delta -\mathbf{J}^T \mathbf{r} ] 其中( \mathbf{J} ) 是残差向量 ( \mathbf{r} ) 关于参数 ( \mathbf{X} ) 的雅可比矩阵Jacobian。这是算法的核心计算量。( \mu 0 ) 是阻尼因子。( \mathbf{I} ) 是单位矩阵。( \delta ) 是本次迭代的参数更新步长。解得 ( \delta ) 后更新参数( \mathbf{X}{new} \mathbf{X}{old} \delta )。4.3 阻尼因子 ( \mu ) 的自适应更新策略LM算法的性能很大程度上取决于如何更新 ( \mu )。标准策略如下计算增益比 ( \rho ) [ \rho \frac{F(\mathbf{X}) - F(\mathbf{X} \delta)}{L(\mathbf{0}) - L(\delta)} ] 其中( L(\delta) ) 是我们在当前点用线性模型即高斯-牛顿近似预测的目标函数值( L(\delta) \frac{1}{2} |\mathbf{r} \mathbf{J}\delta|^2 )。分母 ( L(\mathbf{0}) - L(\delta) ) 是线性模型预测的下降量分子是实际下降量。根据 ( \rho ) 更新 ( \mu ) 和接受本次更新如果 ( \rho ) 很大比如 0.75说明线性模型拟合得很好可以减小 ( \mu )例如 ( \mu \mu / 3 )并接受这次更新 ( \mathbf{X}_{new} )。如果 ( \rho ) 很小比如 0.25说明线性模型拟合得很差需要增大 ( \mu )例如 ( \mu \mu * 2 )并拒绝这次更新保持 ( \mathbf{X}_{old} ) 不变用新的 ( \mu ) 重新求解 ( \delta )。如果 ( \rho ) 在中间范围则接受更新但保持 ( \mu ) 不变。4.4 雅可比矩阵 ( \mathbf{J} ) 的推导与计算对于我们的问题残差 ( r_i ) 有两类夹角余弦残差和平滑项残差。以夹角余弦残差为例对于第 ( k ) 时刻 [ r_k^{angle} \cos\theta_k^{calc} - \cos\theta_k^{obs} \frac{\overrightarrow{U_kA} \cdot \overrightarrow{U_kB}}{|\overrightarrow{U_kA}| |\overrightarrow{U_kB}|} - \cos\theta_k^{obs} ] 我们需要求 ( r_k^{angle} ) 对 ( x_k, y_k, z_k ) 的偏导数。这是一个链式求导过程虽然繁琐但必须精确推导因为解析的雅可比矩阵比数值差分如有限差分计算更快、更精确能极大提升LM算法的效率和稳定性。平滑项残差如一阶差分 ( r_k^{smooth} \sqrt{\lambda} (U_k - U_{k-1}) ) 的雅可比矩阵是稀疏的只有少数几个元素非零。这提示我们整个问题的雅可比矩阵 ( \mathbf{J} ) 是一个稀疏矩阵。踩坑实录在比赛或自己实现时最容易出错的地方就是雅可比矩阵的计算。一个笔误就可能导致算法不收敛。强烈建议先用符号计算工具如 MATLAB 的symsPython 的sympy推导出偏导公式确保正确。实现解析雅可比函数后用数值差分法如中心差分在初始点附近计算一个雅可比矩阵与你的解析结果对比验证。这是保证后续优化正确的关键一步。利用雅可比矩阵的稀疏性不要用稠密矩阵存储和运算。使用稀疏矩阵格式如 MATLAB 的sparse, Python SciPy 的scipy.sparse可以处理成千上万个状态变量N很大时而内存和计算时间只与非零元数量成正比。4.5 算法实现流程综合以上一个完整的LM求解流程如下初始化给定初始猜测 ( \mathbf{X}_0 )初始阻尼因子 ( \mu_0 )如 0.01最大迭代次数收敛阈值 ( \epsilon )。迭代循环 a. 计算当前残差 ( \mathbf{r}(\mathbf{X}) ) 和雅可比矩阵 ( \mathbf{J}(\mathbf{X}) )。 b. 求解线性方程 ( (\mathbf{J}^T\mathbf{J} \mu \mathbf{I}) \delta -\mathbf{J}^T \mathbf{r} )。对于大规模稀疏问题应使用稀疏求解器如 MATLAB 的\运算符会自动识别稀疏矩阵Python 可用scipy.sparse.linalg.spsolve。 c. 计算增益比 ( \rho )。 d. 根据 ( \rho ) 更新 ( \mu ) 和 ( \mathbf{X} )。 e. 检查收敛条件如 ( |\delta| \epsilon ) 或迭代次数超限则退出。输出最优参数 ( \mathbf{X}^* )。5. 仿真实验设计与结果分析验证与洞察理论模型和算法都需要通过仿真来验证。一个完整的仿真实验应该包含以下步骤5.1 生成仿真数据Ground Truth设定两个信标的位置例如 A(0, 0, 0), B(1000, 0, 0)单位米。设计一条无人机的真实飞行轨迹。可以是简单的直线、圆弧也可以是更复杂的“8”字形以测试算法对不同机动轨迹的适应性。生成每个时刻 ( t_k ) 的真实位置 ( U_k^{true} )。根据真实位置计算每个时刻的真实夹角 ( \theta_k^{true} \arccos\left( \frac{\overrightarrow{U_kA} \cdot \overrightarrow{U_kB}}{|\overrightarrow{U_kA}| |\overrightarrow{U_kB}|} \right) )。在真实夹角上添加高斯白噪声模拟传感器误差( \theta_k^{obs} \theta_k^{true} \epsilon_k, \quad \epsilon_k \sim \mathcal{N}(0, \sigma^2) )。这里 ( \sigma ) 是角度观测噪声的标准差例如 ( 1^\circ ) 或 ( 2^\circ )。5.2 算法求解与评估指标初始猜测给算法一个粗略的初始位置。可以设为轨迹起点附近的一个随机点或者更简单设为两个信标连线的中点上方某个高度。这模拟了我们对无人机初始位置只有大致了解的情况。运行LM算法使用上节实现的算法进行求解得到估计轨迹 ( {U_k^{est}} )。评估指标绝对位置误差( e_k | U_k^{est} - U_k^{true} | )绘制随时间变化的误差曲线。均方根误差( RMSE \sqrt{\frac{1}{N}\sum_{k1}^{N} e_k^2} )这是一个总体精度的标量指标。轨迹对比图在二维XY平面和三维空间中同时绘制真实轨迹和估计轨迹直观对比。5.3 关键影响因素分析What-If 分析通过改变仿真条件我们可以深入理解系统性能的边界噪声水平的影响固定其他条件逐步增大观测噪声 ( \sigma )观察 RMSE 如何增长。通常会得到近似线性的关系。这有助于确定该定位方案对传感器精度的要求。信标几何构型的影响这是极其重要的一点。尝试改变两个信标的位置。场景A信标A和B距离很近如相距100米。场景B信标A和B距离很远如相距5000米。场景C无人机飞行轨迹大部分时间位于两个信标的“侧方”或“后方”。 你会发现当无人机、信标A、信标B三者接近共线时定位精度会急剧下降甚至算法可能发散。这是因为此时的观测几何导致雅可比矩阵接近奇异系统可观测性变差。这对应着现实中的一个重要结论布站几何直接影响定位精度应尽可能让无人机与两个信标构成的张角即观测角 ( \theta )远离0度或180度。正则化系数 ( \lambda ) 的影响调整平滑项的权重。观察 ( \lambda ) 过小和过大时估计轨迹的特点。过小轨迹噪声大过大轨迹过于平滑无法跟踪真实机动。仿真编程技巧在 MATLAB 或 Python 中将整个仿真流程数据生成、算法求解、绘图分析封装成函数或脚本并参数化如噪声sigma、信标位置、轨迹类型、正则化系数等。这样可以快速进行批量实验和对比。使用subplot功能将误差曲线、轨迹对比图等放在一个画布上便于分析报告。6. 从理想模型到现实挑战误差源与改进方向通过仿真我们在一个受控的理想环境中验证了模型和算法的有效性。但现实要复杂得多。要让这个“夹角观测无源定位”系统真正工作我们必须考虑以下挑战和可能的改进方向6.1 主要的误差来源传感器误差我们只模拟了高斯白噪声。实际的测角传感器如视觉特征匹配、射频测向天线可能存在系统误差如标定误差、零点漂移、非高斯噪声以及视场角/测角范围的限制。时钟同步误差题目隐含假设了无人机内部时钟与观测数据的时间戳是完美同步的。实际上如果观测数据来自外部记录设备可能存在微小的时间同步误差这在高动态场景下会引入显著的位置误差。信标位置误差我们假设信标位置精确已知。现实中信标的部署位置也存在测量误差。这个误差会直接传递到最终的定位结果中。无人机姿态的影响题目模型假设夹角测量是在无人机机体坐标系下直接得到的。但很多传感器如固定在机身上的相机的测量值是在机体坐标系下的。要得到无人机到信标向量在世界坐标系下的夹角必须知道无人机当前的姿态滚转、俯仰、偏航角。如果姿态未知或由低精度IMU提供这将成为另一个巨大的误差源。一个更完整的模型需要将姿态角也作为状态变量进行估计或者使用已标定的稳定云台来隔离姿态影响。6.2 模型与算法的扩展方向融合多源观测单一的夹角观测信息量有限。可以融合其他传感器数据如惯性测量单元即使是最廉价的IMU在短时间内也能提供相对准确的速度和角度变化信息。将IMU的积分结果作为运动模型即前面提到的滤波方法中的状态转移模型与夹角观测进行融合可以极大地提高系统的鲁棒性和精度尤其是在观测几何较差或短暂丢失信标时。高度计如果知道无人机的大致高度如通过气压计可以将这个信息作为一个软约束或观测值加入优化问题有效降低垂直方向Z轴的估计不确定性因为纯角度观测对高度通常不敏感。考虑更复杂的轨迹先验我们使用了简单的速度/加速度平滑先验。如果知道无人机的任务类型如巡检、测绘可以引入更符合其运动规律的先验例如样条曲线、B样条等参数化轨迹模型直接优化曲线参数从而减少待优化变量的数量。鲁棒损失函数我们使用了最小二乘L2范数它对离群值Outliers非常敏感。在实际中传感器可能出现短暂的野值。可以考虑使用Huber损失、Cauchy损失等鲁棒核函数降低离群值对整体解的影响。滑动窗口优化对于在线实时定位无法进行全局批量优化。可以采用滑动窗口优化只优化最近一段时间内的状态保持计算量恒定适用于嵌入式平台。6.3 工程实现中的注意事项初始值敏感性非线性优化对初始值敏感。在实际系统中需要有一个可靠的初始化阶段。例如在启动时可以让无人机短暂悬停或缓慢移动用最初几帧数据单独解算一个粗略的初始位置可能需要假设一个初始高度或者利用其他辅助信息如GPS、UWB进行冷启动。数值稳定性在计算向量夹角余弦时分母是两个向量的模的乘积。当无人机非常接近某个信标时该向量的模会非常小导致计算不稳定。在代码中需要对分母做保护避免除零错误。计算效率对于长时间、高频率的定位算法效率很重要。利用雅可比矩阵的稀疏结构是关键。此外可以考虑使用更高效的求解器如基于Cholesky分解的稀疏求解器或者使用C/CUDA进行加速。7. 总结与个人体会复盘2022年高教社杯B题远不止是重温一次数学建模竞赛。它系统地训练了我们如何将一个抽象的、基于几何约束的定位问题转化为一个可建模、可求解、可分析的完整技术链路。从建立坐标系和观测方程到构建包含平滑先验的优化模型再到实现和调试LM算法最后通过系统的仿真实验来验证性能和探索边界这一过程本身就是解决许多工程问题的标准范式。我个人在实现和调试过程中最深的一点体会是模型的“魔鬼”藏在细节里。推导雅可比矩阵时的一个正负号错误就足以让LM算法在迭代中震荡发散正则化系数lambda相差一个数量级估计出的轨迹就可能从“毛刺丛生”变成“僵化死板”信标布局的一个微小改变可能导致整个系统的可观测性发生质变。这些都不是纸上谈兵能轻易发现的必须动手编程、运行仿真、观察结果、反复调试。这道题也清晰地揭示了纯角度定位的固有局限性对观测几何极度敏感在信标连线方向上的定位精度天生较差。这提醒我们在实际系统设计中单一技术路径往往风险很高。融合IMU、气压计甚至偶然的地图匹配信息构建一个多传感器融合的定位系统才是走向实用的必然选择。最后对于想深入无人机、机器人定位领域的朋友我建议以这道题为起点但不限于此题。可以尝试将模型扩展到三维、更多信标、考虑姿态或者用EKF/UKF重新实现一遍进行对比。也可以寻找开源的真实数据集如视觉SLAM数据集进行测试。只有将理论、算法和实际数据或高保真仿真反复碰撞才能真正掌握状态估计这门艺术。