非线性最小二乘与无源定位：从夹角观测到无人机轨迹估计

1. 项目概述从一道赛题到一套方法论去年带学生打数模B题“夹角观测条件下无人机无源定位”这道题给我留下了挺深的印象。它不像一些纯理论推导题那么“飘”也不像一些数据处理题那么“糙”而是把一个非常典型的工程问题——如何仅凭角度信息给一个移动目标“画”出它的运动轨迹——抽象成了一个精巧的数学模型。说白了就是假设我们手头有几个只能测角度的观测站比如被动雷达、声学阵列或者视觉传感器它们动不了但天上有个无人机在飞。我们不知道无人机在哪只知道从每个观测站看过去无人机和自己的连线与某个参考方向比如正北之间的夹角。题目就给了这么一堆随时间变化的夹角数据要求我们反推出无人机每一刻的位置甚至预测它未来的路径。这问题听起来就很有“实战”味儿。在现实里主动发射信号的雷达容易暴露自己而无源定位靠“听”或者“看”别人的信号来定位隐蔽性好在电子侦察、野生动物追踪、甚至某些民用导航领域都有应用。题目给的“夹角观测”条件算是无源定位里比较经典但也颇具挑战性的一种。挑战在哪呢第一信息量少。只有一个角度没有距离光靠一个站是定不出位置的必须多个站协同。第二非线性。角度和位置坐标之间的关系不是简单的加减乘除是三角函数这就导致了后续的求解方程是个非线性系统处理起来比线性问题麻烦得多。第三有误差。实测数据不可能完美观测站自身的位置标定有误差测角也有误差这些误差在非线性模型里会被放大怎么抗干扰、提精度才是真正考验模型和算法的地方。所以复盘这道题绝不仅仅是把答案再算一遍。我更想聊的是面对这样一个有明确工程背景的数学建模问题我们怎么一步步拆解从问题分析、模型建立、算法选择到结果分析形成一套可复用的解题框架。尤其是“非线性最小二乘”这个核心工具很多人知道用它但未必清楚为什么在这里必须用它以及怎么把它用好、调稳。接下来我就结合当时的解题思路和后续的一些思考把这道题的“里子”和“面子”都摊开来聊聊希望能给以后遇到类似问题的朋友一些实在的参考。2. 核心思路拆解如何把“测角”问题转化为数学问题拿到题目第一步不是急着建模型而是要把题目描述的场景用数学语言清晰地定义出来。这一步梳理清楚了后面的工作就顺了。2.1 问题场景的数学抽象我们先建立一个二维平面直角坐标系题目通常是二维的三维原理类似但更复杂。假设有N个观测站它们的坐标是已知的记为(x_i, y_i)其中i 1, 2, ..., N。有一个无人机目标在运动它在t时刻的位置是未知的记为(X(t), Y(t))。“夹角观测”是什么意思通常这个夹角指的是目标相对于观测站的方位角Bearing Angle即从观测站指向目标的连线与某个固定参考方向比如正北方向或坐标系的x轴正方向之间的夹角记为θ_i(t)。这个角度是我们可以从观测站测量得到的数据。根据几何关系这个角度满足以下公式θ_i(t) arctan2( Y(t) - y_i, X(t) - x_i )这里arctan2是四象限反正切函数它比普通的arctan更能准确反映角度所在的象限直接输出一个介于-π到π或0到2π之间的角度。这个公式就是最基本的观测方程它建立了未知量目标位置与观测量夹角之间的数学关系。一个关键点这个方程对于单个观测站和单个时刻是成立的但一个方程含有两个未知数X,Y显然无法求解。所以无源定位必须依靠多个观测站N ≥ 2或者单个观测站的多时刻观测结合目标运动模型。题目通常提供多个观测站同时刻的观测数据这正是利用空间信息进行定位。2.2 从几何关系到误差函数的构建有了多个观测站的方程理论上我们可以列出方程组θ_1(t) arctan2( Y(t) - y_1, X(t) - x_1 ) θ_2(t) arctan2( Y(t) - y_2, X(t) - x_2 ) ... θ_N(t) arctan2( Y(t) - y_N, X(t) - x_N )对于每个时刻t我们想求解出(X(t), Y(t))。但由于arctan2函数的存在这是一个非线性方程组直接求解解析解非常困难尤其是当N 2时通常没有简单的封闭解。更实际且强大的思路是将其转化为一个优化问题。核心思想是我们寻找一个目标位置(X, Y)使得根据这个位置计算出来的理论夹角与实际观测到的夹角之间的总体误差最小。这就引出了误差函数也叫残差函数的定义。对于第i个观测站在t时刻的观测残差为r_i(X, Y) θ_i^{obs}(t) - arctan2( Y - y_i, X - x_i )这里θ_i^{obs}(t)是实际观测值。需要注意的是角度差的计算因为角度是周期性的比如 359° 和 1° 实际上只差 2°所以在计算残差时通常需要将其归一化到[-π, π)区间避免跨周期带来的巨大误差。一个稳健的做法是r_i mod( (θ_obs - θ_calc) π, 2π ) - π其中mod是取模运算θ_calc是计算值。我们的目标是让所有观测站的残差尽可能小。最常用的准则就是最小二乘准则即最小化所有残差的平方和。因此定义目标函数代价函数为F(X, Y) Σ_{i1}^{N} [ r_i(X, Y) ]^2这里假设各个观测站的权重相同。如果知道某些观测站精度更高可以引入权重系数w_i目标函数变为Σ w_i * r_i^2。至此我们成功地将“根据夹角求位置”的定位问题转化为了一个数学上的非线性最小二乘优化问题寻找(X, Y)使得目标函数F(X, Y)的值最小。2.3 为什么是非线性最小二乘这是一个值得深入理解的节点。有人可能会问能不能用解析几何的方法比如利用两条射线的交点来定位对于两个观测站理论上可以。两条方位线相交即可确定目标位置这就是经典的“三角定位”或“交叉定位”。但这里有几个大问题误差放大当目标距离观测站很远或者两个观测站与目标的夹角很小时两条方位线的交点对角度误差极其敏感一个微小的测角误差会导致交点位置定位结果发生巨大的偏移这在几何上称为“定位模糊区”或“几何稀释精度GDOP恶化”。无法处理多站数据当观测站数量多于两个时由于误差的存在多条方位线不会交于一点形成一个“误差三角形”或更复杂的多边形。解析的交叉定位法无法直接利用所有信息。无法融入统计特性最小二乘法的本质是一种最大似然估计在观测噪声为高斯白噪声的假设下。它不仅能给出一个最优估计值其求解过程如高斯-牛顿法的副产品海森矩阵或近似海森矩阵的逆还可以用于评估估计值的协方差矩阵从而定量分析定位精度、误差椭圆等这是纯几何方法做不到的。因此采用非线性最小二乘框架是处理多站、带噪、非线性观测数据的标准且强有力的方法。它以一种统一的方式优雅地解决了多信息融合和抗误差干扰的问题。3. 模型求解核心非线性最小二乘算法实战问题转化成了优化问题接下来就是怎么求解。对于非线性最小二乘我们有多种武器需要根据问题特点选择。3.1 算法选型高斯-牛顿法 vs. 列文伯格-马夸尔特法目标函数F(X, Y)是关于(X, Y)的非线性函数。我们需要迭代算法来寻找它的最小值点。高斯-牛顿法是专门为最小二乘问题设计的牛顿法变种。它的思想是对残差函数r_i(X, Y)进行一阶泰勒展开将非线性问题在当前迭代点附近局部线性化。设当前迭代点为β_k [X_k, Y_k]^T残差向量为r(β) [r_1, r_2, ..., r_N]^T目标函数F(β) r(β)^T r(β)。对r(β)在β_k处泰勒展开r(β) ≈ r(β_k) J(β_k) * (β - β_k)其中J是残差向量r的雅可比矩阵大小为N × 2。它的每一行就是对应残差r_i关于参数[X, Y]的梯度J_i [ ∂r_i/∂X, ∂r_i/∂Y ]而∂r_i/∂X (Y - y_i) / d_i^2,∂r_i/∂Y -(X - x_i) / d_i^2这里d_i sqrt((X-x_i)^2 (Y-y_i)^2)是目标到观测站的距离。注意这里求导时考虑了arctan2的导数形式并忽略了常数因子因为会在后续计算中约去最终得到简洁的几何形式。将线性化后的r代入目标函数得到一个关于步长p β - β_k的二次函数。最小化这个二次函数得到高斯-牛顿法的迭代步长公式(J^T J) p_gn -J^T r求解这个线性方程组得到p_gn然后更新参数β_{k1} β_k p_gn。高斯-牛顿法在残差很小、且当前估计接近真值时收敛速度非常快二阶收敛速率。但它有个致命缺点当残差较大或者初始值离真值太远时近似海森矩阵J^T J可能不是正定的导致算法不稳定步长可能过大而发散。列文伯格-马夸尔特法正是为了解决这个问题而生的。它被认为是高斯-牛顿法和最速下降法的混合体。LM算法引入了一个阻尼因子λ将迭代方程修改为(J^T J λ I) p_lm -J^T r其中I是单位矩阵。当λ很大时J^T J项可忽略方程变为λ I p ≈ -J^T r即p ≈ (-1/λ) J^T r这近似于最速下降法的步长负梯度方向。最速下降法步长小稳定性好但收敛慢。当λ很小时方程退化为高斯-牛顿方程收敛快。LM算法在每次迭代中动态调整λ如果本次迭代使得目标函数下降就减小λ更信任高斯-牛顿方向加快收敛如果目标函数没有下降就增大λ更接近最速下降方向缩小步长保证稳定性。在实际解题中的选择对于这道无人机定位题观测数据通常含有噪声且我们给的初始猜测值可能偏离较远比如简单地取所有观测站坐标的平均值作为初始值因此列文伯格-马夸尔特法LM算法的鲁棒性远高于高斯-牛顿法。在MATLAB中lsqnonlin函数默认使用LM算法在Python的SciPy库中scipy.optimize.least_squares方法默认使用的也是LM算法的变种称为‘trf’或‘lm’。所以无脑选LM算法作为求解器是稳妥且高效的做法。3.2 关键步骤与代码实现要点下面以Python (SciPy) 为例说明实现的关键步骤。步骤一定义残差函数这是最核心的一步。函数输入是待估参数当前猜测的目标位置params [X, Y]输出是所有观测站在该位置下的残差向量。import numpy as np from scipy.optimize import least_squares def residual_function(params, stations, observed_angles): 计算残差向量。 params: 列表或数组 [X, Y]当前估计的目标位置。 stations: 二维数组形状为 (N, 2)每一行是一个观测站的 [x_i, y_i]。 observed_angles: 一维数组长度为 N对应每个观测站的实测夹角弧度制。 X, Y params residuals [] for (x_i, y_i), theta_obs in zip(stations, observed_angles): # 计算理论夹角 theta_calc np.arctan2(Y - y_i, X - x_i) # 计算角度差并归一化到 [-pi, pi) angle_diff theta_obs - theta_calc angle_diff (angle_diff np.pi) % (2 * np.pi) - np.pi residuals.append(angle_diff) return np.array(residuals)步骤二提供初始猜测值一个好的初始值能大大加快收敛速度避免陷入局部极小。一个简单有效的策略是使用最小二乘交点法的粗略解。对于两两观测站可以计算其方位线的交点然后对所有交点取平均或中位数。更简单的方法是取所有观测站坐标的几何中心或者根据角度大致画一个范围取其中点。在比赛中如果时间紧直接用观测站坐标的均值[np.mean(stations[:,0]), np.mean(stations[:,1])]作为初值LM算法通常也能收敛。步骤三调用优化器求解# 假设已有数据 stations np.array([[0, 0], [10, 0], [5, 10]]) # 三个观测站坐标 observed_angles np.array([0.5, 1.2, 2.0]) # 对应夹角单位弧度 # 初始猜测 initial_guess np.mean(stations, axis0) # 观测站中心 # 使用最小二乘求解默认使用LM算法的变种‘trf’ result least_squares(residual_function, initial_guess, args(stations, observed_angles)) # 或者显式指定方法为 ‘lm’ (Levenberg-Marquardt) # result least_squares(residual_function, initial_guess, args(stations, observed_angles), methodlm) if result.success: estimated_position result.x print(f估计的目标位置: ({estimated_position[0]:.2f}, {estimated_position[1]:.2f})) print(f残差平方和: {result.cost:.6f}) # result.jac 是最后一次迭代的雅可比矩阵可用于近似计算协方差 # 协方差矩阵 Cov ≈ (J^T J)^{-1} * (残差方差) residuals_at_solution result.fun # 计算残差的标准差假设无偏估计自由度为 N-2 sigma2 np.sum(residuals_at_solution**2) / (len(observed_angles) - 2) # 近似海森矩阵的逆 J result.jac hess_inv np.linalg.inv(J.T J) covariance_matrix sigma2 * hess_inv print(f估计位置的近似协方差矩阵:\n{covariance_matrix}) else: print(优化失败:, result.message)步骤四结果分析与可视化求解出每个时刻的位置后就得到了无人机的轨迹。一定要做可视化画出观测站位置。画出估计出的无人机轨迹点。可以画出每个时刻的“误差椭圆”。误差椭圆由该时刻位置估计的协方差矩阵决定它直观地反映了定位精度在各个方向上的分布。椭圆的长轴方向表示不确定性最大的方向通常对应于观测站几何构型最差的方向。注意上面计算协方差矩阵(J^T J)^{-1} * sigma2是在观测噪声相互独立且同方差、线性化近似有效的假设下进行的。它给出了Cramer-Rao下界的一个近似对于评估精度趋势很有用但并非严格的误差界。4. 模型进阶与鲁棒性考量基本的非线性最小二乘模型能解决大部分问题但要拿高分必须考虑得更深。题目数据往往包含“陷阱”考验模型的鲁棒性。4.1 观测站几何布局的影响GDOP分析观测站怎么摆对定位精度有决定性影响。这用几何稀释精度来描述。GDOP本质上描述了测距或测角误差被放大为位置误差的程度。在我们的模型中近似的位置误差协方差矩阵P σ_θ^2 * (J^T J)^{-1}其中σ_θ^2是测角误差的方差。那么位置误差的方差在x和y方向上的和可以表示为σ_p^2 trace(P) σ_θ^2 * trace((J^T J)^{-1})定义GDOP sqrt( trace((J^T J)^{-1}) )。那么σ_p σ_θ * GDOP。GDOP的物理意义它是一个无量纲的放大因子。GDOP越大相同的测角误差会导致越大的定位误差。GDOP完全由观测站与目标的相对几何关系决定。好的几何构型观测站围绕目标分布且各站与目标的连线夹角较大接近90度。此时J^T J矩阵的条件数小其逆也小GDOP值小定位精度高。差的几何构型观测站集中在一边或者所有观测站与目标几乎共线。此时方位线交角很小J^T J接近奇异病态其逆巨大GDOP值非常大定位结果极不可靠。在解题报告中如果能够对关键时刻或整个任务区域的GDOP进行计算和分析并指出哪些时段/区域定位精度可能较差会是一个很大的亮点。这体现了你对问题本质的深刻理解。4.2 处理异常观测值与加权最小二乘实际数据中可能存在“野值”即严重偏离正常范围的错误观测角。经典的最小二乘以平方和为损失对野值非常敏感因为残差平方会放大大误差的影响导致估计结果被拉偏。解决方案一鲁棒损失函数SciPy的least_squares函数提供了loss参数。默认是‘linear’即标准最小二乘。可以将其改为‘soft_l1’、‘huber’或‘cauchy’等鲁棒损失函数。这些函数对于大的残差不进行平方惩罚而是进行某种饱和惩罚从而抑制野值的影响。result least_squares(residual_function, initial_guess, args(stations, observed_angles), losssoft_l1, f_scale0.1)f_scale参数控制损失函数从二次型向线性型转变的阈值需要根据数据尺度进行调整。使用鲁棒损失函数是一种简单有效的抗野值方法。解决方案二迭代重加权最小二乘这是一种更主动的方法。先进行一次标准最小二乘估计根据残差大小给每个观测值分配一个权重残差大的权重小然后用加权最小二乘再次求解迭代数次。 权重函数可以用Huber权重、Tukey双权重函数等。例如Huber权重w_i 1 / max(1, |r_i| / c)其中c是一个调优常数通常取残差中位数的1.345倍等。 在SciPy中可以手动实现循环或者在least_squares中结合loss和jac选项进行更精细的控制。解决方案三基于统计的离群点剔除先求解计算标准化残差残差除以其估计的标准差将那些标准化残差超过某个阈值如2.5或3的观测值标记为离群点剔除后重新拟合。这种方法比较直接但需要注意剔除后自由度变化对协方差估计的影响。在比赛中如果数据质量尚可使用loss‘soft_l1’通常是省心且有效的选择。如果明确知道某些观测站精度更高可以在初始残差函数中就引入固定权重。4.3 融合时序信息扩展卡尔曼滤波题目如果要求“实时定位”或“预测”或者给出的观测数据是时间序列那么单纯对每个时刻独立进行静态定位就浪费了信息。无人机运动是有惯性的上一时刻的位置和速度信息可以用来约束当前时刻的估计。这时就需要引入动力学模型最经典的框架就是扩展卡尔曼滤波。EKF将状态估计位置、速度和观测夹角在非线性框架下结合起来。它分为预测和更新两步预测步根据上一时刻的状态估计和运动模型如匀速模型CV、匀加速模型CA预测当前时刻的状态和误差协方差。x_{k|k-1} f(x_{k-1|k-1})P_{k|k-1} F_k P_{k-1|k-1} F_k^T Q_k其中f是状态转移函数F_k是其雅可比矩阵Q_k是过程噪声协方差表示模型的不确定性。更新步利用当前时刻的观测值来修正预测。计算观测残差y_k z_k - h(x_{k|k-1})其中h是我们的夹角观测函数。计算观测矩阵H_k即前面残差函数的雅可比矩阵J在预测状态处计算。计算卡尔曼增益K_k P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T R_k)^{-1}其中R_k是观测噪声协方差。更新状态估计x_{k|k} x_{k|k-1} K_k y_k更新误差协方差P_{k|k} (I - K_k H_k) P_{k|k-1}EKF的优势在于平滑轨迹利用运动模型即使某时刻观测质量差也能通过预测给出一个合理的位置估计。提供速度信息状态向量中可以包含速度从而估计出无人机的速度。实时性计算是递推的适合在线实时处理。不确定性传播协方差矩阵P始终伴随着状态估计定量描述了估计的不确定性。在解题中如果数据是时间序列采用EKF框架会比独立时刻定位得到更平滑、更合理的轨迹尤其是在观测数据有间断或噪声大的情况下。实现EKF需要定义运动模型和观测模型并合理设置过程噪声Q和观测噪声R这部分需要一些调参经验。5. 常见问题、调试技巧与结果分析在实际编程求解和撰写报告时会遇到不少坑。这里总结几个典型问题和处理技巧。5.1 算法不收敛或收敛到错误解这是最常见的问题。症状迭代次数爆满残差始终很大或者最终位置明显不合理比如跑到无穷远或观测站包围圈之外。可能原因与对策初始值太差LM算法虽然鲁棒但初始值也不能离谱。尝试不同的初始值策略观测站中心、通过两个观测站粗略交点、在可能的区域网格搜索一个使初始残差较小的点。角度单位错误务必确保所有角度计算使用弧度制。np.arctan2、np.sin、np.cos等函数默认接受弧度。如果数据给的是角度一定要np.radians()转换。角度周期性未处理残差计算中必须进行(差值 π) % 2π - π操作。忘记这一步会导致当理论角从359度跳到0度时残差出现一个接近360度的巨大错误值导致优化失败。观测站共线或几何构型极差如果所有观测站几乎在一条直线上且目标也在这条线附近那么问题本身是病态的任何算法都难以得到稳定解。此时GDOP会极大。在报告中应指出这种情况并说明定位结果不可信。数据存在系统性偏差比如所有观测角都有一个固定的偏移。这需要检查观测站的方向基准是否统一都指北吗。可以在模型中加入一个共同的偏差角作为待估参数但会增大问题复杂度。调试技巧在迭代开始时打印初始猜测位置和对应的残差向量、目标函数值。画图将初始猜测点、观测站、以及从观测站画出的方位线都显示出来。肉眼就能看出初始点是否合理理想情况下初始点应该靠近所有方位线的交汇区域。5.2 结果精度评估与可视化不能只给出一个坐标点就完事。定量评估残差分析求解后观察最终残差向量。它们应该近似服从均值为0的正态分布如果噪声是高斯的话。可以画残差的直方图或Q-Q图检验。协方差与误差椭圆如前所述利用雅可比矩阵计算近似协方差并绘制误差椭圆。椭圆的大小和形状直观反映了精度。与参考轨迹对比如果题目提供了部分真实位置用于验证计算估计位置与真实位置的均方根误差或平均绝对误差。定性可视化轨迹图在二维平面上画出观测站用不同形状/颜色的点画出估计出的无人机轨迹点用连线连成路径。方位线交汇图对于某个关键时刻从每个观测站出发沿观测角方向画一条射线。这些射线应该大致交汇于估计的目标位置附近。这能非常直观地展示定位原理和几何构型。误差演变图如果有多时刻数据可以画出RMSE或位置误差协方差的行列式代表误差椭圆面积随时间变化的曲线分析哪个阶段定位精度高/低。5.3 对题目数据的针对性处理思路竞赛题目数据往往有“特色”需要灵活调整模型。数据分段如果无人机轨迹运动模式发生变化比如从直线飞行变为盘旋可以考虑分段建模对不同段使用不同的运动模型EKF中或不同的过程噪声参数。观测站数量变化如果题目中某些时刻部分观测站失效那么在构建该时刻的残差函数时只使用有效的观测站即可。在EKF中对应时刻的观测矩阵H_k的行数也会减少。非同步观测如果各观测站数据时间戳不完全同步则需要先进行时间对齐插值到统一的时间网格上再进行处理。这本身就是一个预处理难点。三维扩展如果题目是三维空间原理完全一样。观测角可能包含方位角azimuth和俯仰角elevation。残差函数变为两个角度差参数变为[X, Y, Z]雅可比矩阵变为N×3。GDOP分析也扩展到三维。5.4 一份优秀解题报告应包含的要点基于以上所有内容一份能拿高分的数模论文或报告其“模型求解”部分应该层次分明地展现模型建立清晰定义坐标系、变量给出观测方程推导出非线性最小二乘的目标函数。算法描述说明为什么选用LM算法简述其原理和相对于高斯-牛顿法的优势。给出迭代公式。实现细节说明如何处理角度周期性、如何设置初始值、是否使用鲁棒损失函数。结果展示提供关键时刻的定位结果表格、整个轨迹图、误差分析如RMSE。深入分析进行GDOP分析指出几何构型好的区域和差的区域。讨论模型对测角误差的敏感性可以做一个简单的蒙特卡洛仿真给观测角加随机噪声多次求解统计位置误差的分布。模型扩展提及或实现EKF方法对比其与静态定位的结果展示平滑效果和速度估计。讨论加权最小二乘、异常值处理等鲁棒性措施。优缺点评价客观评价所建模型的优点如原理清晰、抗噪能力强并指出其局限性如依赖观测站几何、对初始值敏感、假设噪声分布等以及可能的改进方向。这道“夹角观测条件下无人机无源定位”题就像一座连接理论数学和工程实践的桥梁。它考察的绝不仅仅是套公式编程而是从具体问题中抽象数学模型、选择合适的数值方法、处理实际数据中的不完美、并对结果进行严谨分析和解释的完整能力链。通过这样的复盘我们把一个赛题变成了一套可迁移的解决问题的方法论这才是参加数模竞赛最大的收获。在实际科研或工程中遇到类似的参数估计、传感器融合问题这套以非线性最小二乘为核心、以优化算法为工具、以统计评估为保障的思路依然会非常有用。