简介针对2022年全国大学生数学建模竞赛本科组B题这份原创获奖论文完整呈现了无人机纯方位无源定位问题的建模与求解过程并配有MATLAB实现思路。内容覆盖圆形编队中被动接收信号无人机的定位模型建立、额外发射信号无人机数量如何判定以及初始位置出现偏差时的迭代调整策略同时延伸到锥形编队的定位方案展示数学方法应对不同队形的通用性。论文为河北省一等奖作品适合参加数学建模竞赛的本科生、指导教师及无人机编队控制研究者参考。资源共1个PDF文件压缩包大小约2.63MB文档结构涵盖摘要、问题重述、问题分析、模型建立与求解、符号说明等可作为论文写作规范和建模表达范本。目前已有8039人浏览学习可帮助读者快速掌握国赛B题的解题框架与MATLAB实践技巧。1. 纯方位无源定位为什么只靠角度就能算出无人机坐标2022年全国大学生数学建模竞赛本科组B题的核心就是把“无人机纯方位无源定位”这件事落到一个可计算的几何模型上编队里的无人机不依赖GPS也不主动发射测距信号只测量其他无人机相对自己的方位角再用这些角度反推坐标。这个场景在工程里非常真实——GPS被干扰、雷达信号不能开、光电设备只能给出“目标在哪个方向”位置信息就得靠三角交会和最小二乘硬算出来。对参赛者来说这道题考察的不是某个高深算法而是三件事能不能把角度观测写成干净的数学方程能不能把几何退化条件分析清楚以及能不能在参考点不足时用编队结构调整恢复可观性。下文按这个顺序把整条定位链路拆开每一步都给出可以直接复现的代码和分析方法。2. 观测方程与能观性先弄清哪些几何条件会让定位失稳做纯方位定位第一件要做的事不是写代码而是统一“方位角”的定义。同一个角度在不同坐标系里可以差出90度甚至完全相反。这一章先把观测方程写清楚再讨论什么时候方程有唯一解什么时候方程本身就没法解。2.1 坐标约定方位角的三种定义为什么必须统一工程中常见的方向角定义有三种。第一种是地理方位角从正北方向顺时针旋转范围是0到360度这是飞行控制系统里最常见的约定第二种是数学方位角从x轴正方向逆时针旋转范围通常是负180度到正180度这也是大多数数值计算库的默认约定第三种是机体坐标系下的相对方位角它以无人机机头方向为0度基准机头偏右为正。三种定义之间没有本质区别但在同一套方程里混用就会直接导致定位结果整体旋转或者镜像翻转。我在做这类题目时会先把所有输入角度统一成数学方位角也就是弧度制下的atan2输出。如果题目给的是地理方位角先做一次转换import numpy as np def geo_to_math_rad(geo_deg): 地理方位角正北顺时针度转数学方位角x轴逆时针弧度。 例正东方向 geo_deg90 - math_rad0 deg (90.0 - geo_deg) % 360.0 return np.deg2rad(deg)这里的逻辑是正东在地理方位角里是90度在数学坐标系里是0弧度正北是0度对应数学坐标系的90度。所以用“90减原角度”就能完成映射。模360度是为了把结果压到标准区间。实际题目里如果角度定义比较隐晦我会先拿一个已知位置的参考点做回代验证确认定义没错再继续这一步可以省掉后面大量排错时间。2.2 从方位角到观测等式一条视线变成一个线性约束假设待定位无人机的位置是 (p(x,y))第 (i) 个参考无人机的位置是 (a_i(x_i,y_i))。参考机测得的方位角记为 (\theta_i)那么视线方向满足[ \tan \theta_i \frac{y - y_i}{x - x_i} ]这个方程是非线性的直接求解不方便。把等式两边交叉相乘并整理可以得到一个线性形式[ -\sin\theta_i \cdot x \cos\theta_i \cdot y -\sin\theta_i \cdot x_i \cos\theta_i \cdot y_i ]写成矩阵形式就是 (A p b)其中每一行来自一条视线观测。这个变换叫伪线性化不是近似而是对atan2关系的代数重写只是它在噪声环境下会给误差加权所以后面还要接迭代优化。为什么这个形式很有用因为它把定位问题变成了最小二乘问题。多架参考机给多条视线多条直线在理想情况下交于一点有测量误差时就退化成求一个使所有视线残差最小的点。这是整道题最核心的数学模型也是后面所有代码的公共骨架。2.3 几何退化共线、共圆和镜面对称什么时候破坏唯一性有两条不平行视线就一定能交出唯一点这是直觉结论。但纯方位无源定位里真正容易出问题的是三类退化情形。第一类是参考点与目标共线。所有视线都落在同一条直线上时方程组的秩降为1矩阵条件数变成无穷大最小二乘解在垂直方向完全没有约束随便哪个点都能满足残差要求。第二类是参考点共圆。当所有参考点和待定位点恰好落在同一个圆上并且只能观测相对方位差时整个编队绕圆心旋转不会改变任何观测角度定位结果存在一个整体旋转自由度。第三类是镜面对称。如果所有无人机只通过两两相对方位关联把整个编队沿某条参考点连线做镜像翻转所有相对角度仍然保持不变。这三类问题在圆形编队题目里尤其典型因为圆周上的无人机天然满足“共圆”这个条件。解决思路通常是引入额外约束比如固定半径、固定圆心、已知编号顺序用这些先验信息去打破对称性。后面第四章会专门讲这些约束怎么落到方程里。3. 最小二乘定位解算器把多元观测变成可复现的 Python 代码很多参赛者卡在“模型建出来了但代码算不对”。这一章直接给出一个能跑通的最小实现先用伪线性最小二乘得到初始解再用高斯牛顿迭代消除非线性误差最后给出参数说明和调试建议。3.1 伪线性最小二乘三步把非线性问题压成 A p b把 2.2 节里的线性化结果写成代码只需要三步遍历所有参考点构造系数矩阵A和右端向量b然后用numpy.linalg.lstsq求解。下面是最小实现import numpy as np def solve_linear_least_squares(refs, azis): 伪线性最小二乘定位。 refs: (n, 2) 参考点坐标单位是米 azis: (n,) 方位角弧度数学坐标系x轴逆时针 返回: (x, y) 待定位点的最小二乘估计 A [] b [] for (xr, yr), theta in zip(refs, azis): s np.sin(theta) c np.cos(theta) A.append([-s, c]) b.append(-s * xr c * yr) A np.array(A) b np.array(b) p, _, rank, _ np.linalg.lstsq(A, b, rcondNone) if rank 2: raise RuntimeError(rank2观测退化最小二乘解不唯一) return p这段代码的逻辑很直接每个角度约束对应方程的一个系数行。-s和c就是2.2节推导出的系数右端项来自参考点坐标。lstsq返回的最小二乘解在观测相互矛盾时仍然给一个在平方误差意义下最优的点但不会告诉你这个点靠不靠谱所以要用rank判断方程的秩。参数上需要注意三件事。第一azis必须和refs一一对应顺序错位会直接解出错误坐标第二角度单位必须是弧度如果题目给的是角度制要先统一转换第三rcondNone是让 NumPy 自动选择截断奇异值的阈值但在参考点近似共线时这个默认值可能不够敏感建议后续配合条件数一起检查。3.2 高斯牛顿迭代角度残差的跳变怎么处理伪线性最小二乘在噪声较小时已经能用但它的误差分配不均衡远离参考点的方向会被放大。更常见的做法是在伪线性解的基础上做一步高斯牛顿迭代目标是最小化真实角度残差[ \min_p \sum_i \left[ \mathrm{wrap}(\arctan2(p_y!-!y_i,\ p_x!-!x_i) - \theta_i) \right]^2 ]这里wrap是关键它把角度差折到负180度到正180度避免“350度与10度实际差20度却被算成340度”这种跳变。角度残差对位置参数的偏导数也可以直接写出来目标点相对参考点的距离为 (d)方向为 (\phi)则残差对x的偏导是 (-\sin\phi / d)对y的偏导是 (\cos\phi / d)也就是视线方向旋转90度再除以距离。这个雅可比矩阵的计算在工程实现里非常常用。下面给出带高斯牛顿迭代的完整求解器def solve_gn(refs, azis, p0None, max_iter30, tol1e-9): 先伪线性求初值再高斯牛顿迭代优化角度残差。 返回: 最终位置估计 (x, y) if p0 is None: p0 solve_linear_least_squares(refs, azis) p np.array(p0, dtypefloat) for _ in range(max_iter): r [] J [] for (xr, yr), theta in zip(refs, azis): dx p[0] - xr dy p[1] - yr obs np.arctan2(dy, dx) # 角度残差折叠到 [-pi, pi] diff (obs - theta np.pi) % (2.0 * np.pi) - np.pi r.append(diff) d2 dx * dx dy * dy if d2 1e-12: d2 1e-12 # 防止除零 J.append([-dy / d2, dx / d2]) r np.array(r) J np.array(J) delta np.linalg.lstsq(J, -r, rcondNone)[0] p p delta if np.linalg.norm(delta) tol: break return p这个函数把伪线性解当作初始值然后迭代修正直到位置增量小于tol。为什么不用随机初始化因为纯方位定位的误差曲面存在局部极小点用伪线性解做初值能保证绝大多数情况下落在正确收敛域内。max_iter一般设20到30就够高斯牛顿在近线性条件下收敛非常快tol设1e-9是为了让解的精度远高于实际测量误差。3.3 参数表一个可以照抄的调试配置参数建议值作用和调整方式方位角单位弧度与atan2一致避免角度弧度混用参考点数量3到8个大于3个时冗余观测能压制测量噪声距离尺度和R同量纲参考点坐标用米不要混用公里和米max_iter30收敛慢时可提到50但通常用不到tol1e-9位置迭代终止阈值单位与坐标一致角度残差上界实测噪声3倍如果残差超过这个值当成粗差剔除这里最容易被忽略的是“参考点数量”。有的题目里只给两个参考点需要先用几何条件判断能否定位再决定是直接计算还是换算法。两个参考点确实可以交会出唯一位置但对噪声极其敏感任何一个角度误差都会被基线长度放大实务中至少要三个参考点才稳定。4. 圆形编队里的核心用结构约束减少定位歧义B题一般给的不是任意分布的无人机而是带强几何结构的圆形编队。这种结构既是约束也是解题线索这一章专门讨论圆阵下哪些量可观、哪些量不可观以及如何用圆周约束排除错误解。4.1 均匀圆形编队的几何关系与编号顺序常见的编队描述是0号无人机位于圆心O其余1到N号无人机均匀分布在半径为R的圆周上编号按圆周方向递增。两架相邻圆周无人机对应的圆心角固定为[ \Delta\theta \frac{2\pi}{N} ]如果编号 (k) 的无人机与某一已知基准之间的圆心角序号差是 (m)那么它在圆上的理论方位是 (\theta_k \theta_1 (k-1)\Delta\theta)。这个关系本身不直接给出坐标因为整个圆阵可能还有一个整体旋转角 (\delta) 未知。但只要题目给出任意一架圆周无人机的绝对坐标(\delta) 就被固定了其余圆周无人机可以立刻用极坐标得到[ x_k x_0 R\cos(\theta_1 (k-1)\Delta\theta), \quad y_k y_0 R\sin(\theta_1 (k-1)\Delta\theta) ]这个结论很重要它意味着圆形编队的定位本质上不是“21个独立未知数”而是“一个整体旋转角加少数几个待定值”的降维问题。很多参赛者一上来就把所有无人机坐标都当作未知变量方程规模大而且容易陷入局部解这属于把简单问题复杂化了。4.2 全局平移、旋转和缩放纯方位测量的三个不可观方向纯方位观测天然丢失三类信息整体平移、整体旋转、整体缩放。平移由参考点的绝对坐标决定缩放通常由半径R固定住真正棘手的是旋转。举一个最典型的例子如果只有一架圆周无人机1号已知那么整个圆周编队绕圆心旋转任意角度后所有相邻无人机之间的相对方位角完全不变。也就是说就算你有无穷多条角度观测也无法确定这个编队在绝对空间里朝哪个方向。这个问题不是算法能解决的是信息本身不够。解法只有两种要么题目已经给出0号圆心坐标和某圆周无人机的绝对方位要么通过控制指令让编队建立绝对方向基准。在建模时需要做的就是检查题目给出的已知量到底锁住了几个自由度圆心坐标锁平动半径锁缩放一个圆周参考点锁旋转。对应关系明确后就可以判断哪些无人机可以直接算、哪些还需要额外条件。4.3 圆周约束如何破除镜像歧义当目标无人机在圆周上且参考点信息不足以唯一确定它的位置时角度观测通常给出两个关于参考点连线对称的候选解这两个解的角度残差是完全一样的。单纯靠最小二乘法分不出谁对谁错。这时圆周半径R就是最强的先验约束。把两个候选解分别代回圆周方程 ((x-x_0)^2 (y-y_0)^2 R^2)通常只有一个是满足的。更稳妥的做法是定义一个带约束的代价函数[ \min_{p} \sum_i \mathrm{wrap}^2(\cdot) \lambda \left( (x-x_0)^2 (y-y_0)^2 - R^2 \right)^2 ]其中 (\lambda) 是约束权重。实现时不需要真的写这个式子我的习惯是先用无约束最小二乘得到所有候选点再用圆周方程筛选。这个方法简单、可解释性强答辩时也容易讲清楚。4.4 从二维圆阵到三维定位增加俯仰角后发生了什么竞赛题原文虽以二维为主但很多派生问题会问“如果无人机不在同一高度怎么办”。三维纯方位定位需要两类角度水平方位角 (\alpha) 和俯仰角 (\beta)。视线方向单位向量为[ u_i (\cos\beta_i\cos\alpha_i,\ \cos\beta_i\sin\alpha_i,\ \sin\beta_i) ]定位方程依然是“目标点减去参考点后与视线方向共线”[ u_i \times (p - a_i) 0 ]展开后每个参考点提供两个独立的伪线性方程因此三维最少需要三个非共线参考点才能锁定目标。实现上完全可以把2.2节的A矩阵直接升级为6行以上的形式求解方法仍然是最小二乘加高斯牛顿。如果只测水平方位角而没有俯仰角三维问题会退化成一条射线位置完全不可观这一点在建模时务必写清楚。5. 避坑纯方位无源定位最常翻车的五个问题这一章全部来自实际操作里踩过的坑。每一条都按“现象、原因、解决”的结构写方便你在比赛现场直接对着排查。5.1 角度基准不统一结果整体旋转或镜像翻转现象算出来的无人机位置相对真实位置不是绕圆心转了一个固定角度就是沿某条轴镜像对称距离看起来差不多但方向完全不对。 原因有的参考机输出的是地理方位角有的输出的是数学方位角代码里没有统一或者题目给的方位角以机头方向为基准而你用了地面坐标系。 解决建一个角度约定校验函数用一架已知位置无人机回代。把它的真实坐标代入观测方程算理论方位角和题目给的方位角对比差值应当为0或接近0。如果不为0检查是不是差了90度、180度或者符号相反。5.2 参考点近似共线方程秩亏却不报错现象lstsq返回了一个坐标残差也很小但结果离真实位置差出上百米而且换一组初始值结果完全不同。 原因参考点几乎共线时矩阵奇异值很小但还没有小到让rank直接减一最小二乘解被噪声主导。 解决在求解器里显式检查矩阵条件数用np.linalg.cond(A)提前判断。条件数大于1e6时别用普通最小二乘改用带Tikhonov正则化的版本或者在代价函数里加位置先验约束。5.3 高斯牛顿收敛到错误局部解现象理论上3个参考点就能定位但迭代结果有时正确、有时跳到另一个明显不合理的点。 原因初始值落在误差曲面错误谷底对纯方位问题来说最常见的就是镜像解区域。 解决用多个初始值做并行计算伪线性解作为第一个初始值再加入两个镜像候选作为备用初始值最后比较各解的角度残差平方和选最小的那个。这个方法会增加一点计算量但能几乎彻底消除收敛到镜像解的问题。5.4 编队绕圆心旋转相对角度全部不变却被当成新信息现象角度观测数量很多矩阵方程也满秩但所有无人机坐标对应多个不同的整体朝向模型无法分辨。 原因把“相对方位角”误当成了“绝对方位角”。圆形编队整体旋转时每两架无人机之间的相对角度保持不变所以观测方程根本不包含绝对旋转信息。 解决把旋转角 (\delta) 显式建模成未知参数然后检查已知参考点能否约束它。如果参考点只给了一个圆心坐标那旋转自由度必定无法从角度观测中恢复必须在题目条件里找额外的绝对方向信息。5.5 粗差没有剔除最小二乘被一个错误角度拖偏现象大部分参考点视线交汇在合理区域但最终解偏了好几公里画图时发现只有一条视线方向完全不对。 原因题目数据里可能存在个别无人机的方位角标号错误或粗大误差最小二乘没有鲁棒性一个粗差就能把结果拖走。 解决先做一次预定位计算每个参考点的角度残差把残差大于3倍中位绝对偏差的观测标记为粗差并剔除再重新解算。实现时用中位数而不是均值来估计残差尺度因为均值本身会被粗差污染。6. 用残差自检把结果做扎实留一角交叉验证法算出来的坐标不是终点还要用一套能说服评委和自己的验证方法。我的习惯是每个定位结果都必须过三个自检指标再去写进论文。第一个指标是角度残差平方和把求得的坐标代回观测方程反推每个参考点应有的方位角与实测方位角的差折叠到负180度到正180度再算平方和。残差平方和应当和测量噪声水平一致如果大出数量级基本说明解不正确。第二个指标是最大单角残差它用来抓粗差——单个残差明显大于其他残差时大概率是某条观测标号对错了。第三个指标是位置后验误差用残差协方差传播近似估计虽然粗糙但能给出一个“定位精度大概在几十米”的直观量级。留一角交叉验证的做法是把参考点分为两组先用其中 (n-1) 个点定位再用剩下的那一个点做独立验证比较它实测方位角与预测方位角的差值。遍历每个参考点执行一遍得到一组角度残差。这比单纯看拟合残差更诚实因为被验证的点没有参与解算角度差完全反映了模型泛化能力。比赛时把这个过程写成一张小表列为最终成果的一部分比单纯堆公式更有说服力。这段代码可以直接嵌进论文附录def leave_one_out_residual(refs, azis, solversolve_gn): residuals [] for i in range(len(refs)): refs_tr np.delete(refs, i, axis0) azis_tr np.delete(azis, i, axis0) p solver(refs_tr, azis_tr) xr, yr refs[i] pred np.arctan2(p[1] - yr, p[0] - xr) diff (pred - azis[i] np.pi) % (2 * np.pi) - np.pi residuals.append(diff) return np.array(residuals)这里的逻辑是逐个留出参考点做验证最后得到一组与定位解算无关的独立残差。比赛中如果题目给的参考点数不多可以只留一个点做验证这本质上是一种高效的模型检查手段也适合作为论文第三问的精度分析材料。算完这些指标我一般还会做一件很朴素的事把参考点、实测视线、预测位置画在同一张图上用肉眼确认所有视线确实都交汇在结果附近。这个方法看起来很土但真的能发现各种数字检查不出来的坐标代换错误和编号错位。定位问题最终是几何问题几何直觉永远是最后一道防线。希望这篇拆解能帮到你。本文还有配套的精品资源点击获取 
长河君 · 2026/10/29 8:25:25 · 阅读 1.2k 无人机纯方位无源定位：从角度观测到最小二乘坐标解算的完整指南
简介针对2022年全国大学生数学建模竞赛本科组B题这份原创获奖论文完整呈现了无人机纯方位无源定位问题的建模与求解过程并配有MATLAB实现思路。内容覆盖圆形编队中被动接收信号无人机的定位模型建立、额外发射信号无人机数量如何判定以及初始位置出现偏差时的迭代调整策略同时延伸到锥形编队的定位方案展示数学方法应对不同队形的通用性。论文为河北省一等奖作品适合参加数学建模竞赛的本科生、指导教师及无人机编队控制研究者参考。资源共1个PDF文件压缩包大小约2.63MB文档结构涵盖摘要、问题重述、问题分析、模型建立与求解、符号说明等可作为论文写作规范和建模表达范本。目前已有8039人浏览学习可帮助读者快速掌握国赛B题的解题框架与MATLAB实践技巧。1. 纯方位无源定位为什么只靠角度就能算出无人机坐标2022年全国大学生数学建模竞赛本科组B题的核心就是把“无人机纯方位无源定位”这件事落到一个可计算的几何模型上编队里的无人机不依赖GPS也不主动发射测距信号只测量其他无人机相对自己的方位角再用这些角度反推坐标。这个场景在工程里非常真实——GPS被干扰、雷达信号不能开、光电设备只能给出“目标在哪个方向”位置信息就得靠三角交会和最小二乘硬算出来。对参赛者来说这道题考察的不是某个高深算法而是三件事能不能把角度观测写成干净的数学方程能不能把几何退化条件分析清楚以及能不能在参考点不足时用编队结构调整恢复可观性。下文按这个顺序把整条定位链路拆开每一步都给出可以直接复现的代码和分析方法。2. 观测方程与能观性先弄清哪些几何条件会让定位失稳做纯方位定位第一件要做的事不是写代码而是统一“方位角”的定义。同一个角度在不同坐标系里可以差出90度甚至完全相反。这一章先把观测方程写清楚再讨论什么时候方程有唯一解什么时候方程本身就没法解。2.1 坐标约定方位角的三种定义为什么必须统一工程中常见的方向角定义有三种。第一种是地理方位角从正北方向顺时针旋转范围是0到360度这是飞行控制系统里最常见的约定第二种是数学方位角从x轴正方向逆时针旋转范围通常是负180度到正180度这也是大多数数值计算库的默认约定第三种是机体坐标系下的相对方位角它以无人机机头方向为0度基准机头偏右为正。三种定义之间没有本质区别但在同一套方程里混用就会直接导致定位结果整体旋转或者镜像翻转。我在做这类题目时会先把所有输入角度统一成数学方位角也就是弧度制下的atan2输出。如果题目给的是地理方位角先做一次转换import numpy as np def geo_to_math_rad(geo_deg): 地理方位角正北顺时针度转数学方位角x轴逆时针弧度。 例正东方向 geo_deg90 - math_rad0 deg (90.0 - geo_deg) % 360.0 return np.deg2rad(deg)这里的逻辑是正东在地理方位角里是90度在数学坐标系里是0弧度正北是0度对应数学坐标系的90度。所以用“90减原角度”就能完成映射。模360度是为了把结果压到标准区间。实际题目里如果角度定义比较隐晦我会先拿一个已知位置的参考点做回代验证确认定义没错再继续这一步可以省掉后面大量排错时间。2.2 从方位角到观测等式一条视线变成一个线性约束假设待定位无人机的位置是 (p(x,y))第 (i) 个参考无人机的位置是 (a_i(x_i,y_i))。参考机测得的方位角记为 (\theta_i)那么视线方向满足[ \tan \theta_i \frac{y - y_i}{x - x_i} ]这个方程是非线性的直接求解不方便。把等式两边交叉相乘并整理可以得到一个线性形式[ -\sin\theta_i \cdot x \cos\theta_i \cdot y -\sin\theta_i \cdot x_i \cos\theta_i \cdot y_i ]写成矩阵形式就是 (A p b)其中每一行来自一条视线观测。这个变换叫伪线性化不是近似而是对atan2关系的代数重写只是它在噪声环境下会给误差加权所以后面还要接迭代优化。为什么这个形式很有用因为它把定位问题变成了最小二乘问题。多架参考机给多条视线多条直线在理想情况下交于一点有测量误差时就退化成求一个使所有视线残差最小的点。这是整道题最核心的数学模型也是后面所有代码的公共骨架。2.3 几何退化共线、共圆和镜面对称什么时候破坏唯一性有两条不平行视线就一定能交出唯一点这是直觉结论。但纯方位无源定位里真正容易出问题的是三类退化情形。第一类是参考点与目标共线。所有视线都落在同一条直线上时方程组的秩降为1矩阵条件数变成无穷大最小二乘解在垂直方向完全没有约束随便哪个点都能满足残差要求。第二类是参考点共圆。当所有参考点和待定位点恰好落在同一个圆上并且只能观测相对方位差时整个编队绕圆心旋转不会改变任何观测角度定位结果存在一个整体旋转自由度。第三类是镜面对称。如果所有无人机只通过两两相对方位关联把整个编队沿某条参考点连线做镜像翻转所有相对角度仍然保持不变。这三类问题在圆形编队题目里尤其典型因为圆周上的无人机天然满足“共圆”这个条件。解决思路通常是引入额外约束比如固定半径、固定圆心、已知编号顺序用这些先验信息去打破对称性。后面第四章会专门讲这些约束怎么落到方程里。3. 最小二乘定位解算器把多元观测变成可复现的 Python 代码很多参赛者卡在“模型建出来了但代码算不对”。这一章直接给出一个能跑通的最小实现先用伪线性最小二乘得到初始解再用高斯牛顿迭代消除非线性误差最后给出参数说明和调试建议。3.1 伪线性最小二乘三步把非线性问题压成 A p b把 2.2 节里的线性化结果写成代码只需要三步遍历所有参考点构造系数矩阵A和右端向量b然后用numpy.linalg.lstsq求解。下面是最小实现import numpy as np def solve_linear_least_squares(refs, azis): 伪线性最小二乘定位。 refs: (n, 2) 参考点坐标单位是米 azis: (n,) 方位角弧度数学坐标系x轴逆时针 返回: (x, y) 待定位点的最小二乘估计 A [] b [] for (xr, yr), theta in zip(refs, azis): s np.sin(theta) c np.cos(theta) A.append([-s, c]) b.append(-s * xr c * yr) A np.array(A) b np.array(b) p, _, rank, _ np.linalg.lstsq(A, b, rcondNone) if rank 2: raise RuntimeError(rank2观测退化最小二乘解不唯一) return p这段代码的逻辑很直接每个角度约束对应方程的一个系数行。-s和c就是2.2节推导出的系数右端项来自参考点坐标。lstsq返回的最小二乘解在观测相互矛盾时仍然给一个在平方误差意义下最优的点但不会告诉你这个点靠不靠谱所以要用rank判断方程的秩。参数上需要注意三件事。第一azis必须和refs一一对应顺序错位会直接解出错误坐标第二角度单位必须是弧度如果题目给的是角度制要先统一转换第三rcondNone是让 NumPy 自动选择截断奇异值的阈值但在参考点近似共线时这个默认值可能不够敏感建议后续配合条件数一起检查。3.2 高斯牛顿迭代角度残差的跳变怎么处理伪线性最小二乘在噪声较小时已经能用但它的误差分配不均衡远离参考点的方向会被放大。更常见的做法是在伪线性解的基础上做一步高斯牛顿迭代目标是最小化真实角度残差[ \min_p \sum_i \left[ \mathrm{wrap}(\arctan2(p_y!-!y_i,\ p_x!-!x_i) - \theta_i) \right]^2 ]这里wrap是关键它把角度差折到负180度到正180度避免“350度与10度实际差20度却被算成340度”这种跳变。角度残差对位置参数的偏导数也可以直接写出来目标点相对参考点的距离为 (d)方向为 (\phi)则残差对x的偏导是 (-\sin\phi / d)对y的偏导是 (\cos\phi / d)也就是视线方向旋转90度再除以距离。这个雅可比矩阵的计算在工程实现里非常常用。下面给出带高斯牛顿迭代的完整求解器def solve_gn(refs, azis, p0None, max_iter30, tol1e-9): 先伪线性求初值再高斯牛顿迭代优化角度残差。 返回: 最终位置估计 (x, y) if p0 is None: p0 solve_linear_least_squares(refs, azis) p np.array(p0, dtypefloat) for _ in range(max_iter): r [] J [] for (xr, yr), theta in zip(refs, azis): dx p[0] - xr dy p[1] - yr obs np.arctan2(dy, dx) # 角度残差折叠到 [-pi, pi] diff (obs - theta np.pi) % (2.0 * np.pi) - np.pi r.append(diff) d2 dx * dx dy * dy if d2 1e-12: d2 1e-12 # 防止除零 J.append([-dy / d2, dx / d2]) r np.array(r) J np.array(J) delta np.linalg.lstsq(J, -r, rcondNone)[0] p p delta if np.linalg.norm(delta) tol: break return p这个函数把伪线性解当作初始值然后迭代修正直到位置增量小于tol。为什么不用随机初始化因为纯方位定位的误差曲面存在局部极小点用伪线性解做初值能保证绝大多数情况下落在正确收敛域内。max_iter一般设20到30就够高斯牛顿在近线性条件下收敛非常快tol设1e-9是为了让解的精度远高于实际测量误差。3.3 参数表一个可以照抄的调试配置参数建议值作用和调整方式方位角单位弧度与atan2一致避免角度弧度混用参考点数量3到8个大于3个时冗余观测能压制测量噪声距离尺度和R同量纲参考点坐标用米不要混用公里和米max_iter30收敛慢时可提到50但通常用不到tol1e-9位置迭代终止阈值单位与坐标一致角度残差上界实测噪声3倍如果残差超过这个值当成粗差剔除这里最容易被忽略的是“参考点数量”。有的题目里只给两个参考点需要先用几何条件判断能否定位再决定是直接计算还是换算法。两个参考点确实可以交会出唯一位置但对噪声极其敏感任何一个角度误差都会被基线长度放大实务中至少要三个参考点才稳定。4. 圆形编队里的核心用结构约束减少定位歧义B题一般给的不是任意分布的无人机而是带强几何结构的圆形编队。这种结构既是约束也是解题线索这一章专门讨论圆阵下哪些量可观、哪些量不可观以及如何用圆周约束排除错误解。4.1 均匀圆形编队的几何关系与编号顺序常见的编队描述是0号无人机位于圆心O其余1到N号无人机均匀分布在半径为R的圆周上编号按圆周方向递增。两架相邻圆周无人机对应的圆心角固定为[ \Delta\theta \frac{2\pi}{N} ]如果编号 (k) 的无人机与某一已知基准之间的圆心角序号差是 (m)那么它在圆上的理论方位是 (\theta_k \theta_1 (k-1)\Delta\theta)。这个关系本身不直接给出坐标因为整个圆阵可能还有一个整体旋转角 (\delta) 未知。但只要题目给出任意一架圆周无人机的绝对坐标(\delta) 就被固定了其余圆周无人机可以立刻用极坐标得到[ x_k x_0 R\cos(\theta_1 (k-1)\Delta\theta), \quad y_k y_0 R\sin(\theta_1 (k-1)\Delta\theta) ]这个结论很重要它意味着圆形编队的定位本质上不是“21个独立未知数”而是“一个整体旋转角加少数几个待定值”的降维问题。很多参赛者一上来就把所有无人机坐标都当作未知变量方程规模大而且容易陷入局部解这属于把简单问题复杂化了。4.2 全局平移、旋转和缩放纯方位测量的三个不可观方向纯方位观测天然丢失三类信息整体平移、整体旋转、整体缩放。平移由参考点的绝对坐标决定缩放通常由半径R固定住真正棘手的是旋转。举一个最典型的例子如果只有一架圆周无人机1号已知那么整个圆周编队绕圆心旋转任意角度后所有相邻无人机之间的相对方位角完全不变。也就是说就算你有无穷多条角度观测也无法确定这个编队在绝对空间里朝哪个方向。这个问题不是算法能解决的是信息本身不够。解法只有两种要么题目已经给出0号圆心坐标和某圆周无人机的绝对方位要么通过控制指令让编队建立绝对方向基准。在建模时需要做的就是检查题目给出的已知量到底锁住了几个自由度圆心坐标锁平动半径锁缩放一个圆周参考点锁旋转。对应关系明确后就可以判断哪些无人机可以直接算、哪些还需要额外条件。4.3 圆周约束如何破除镜像歧义当目标无人机在圆周上且参考点信息不足以唯一确定它的位置时角度观测通常给出两个关于参考点连线对称的候选解这两个解的角度残差是完全一样的。单纯靠最小二乘法分不出谁对谁错。这时圆周半径R就是最强的先验约束。把两个候选解分别代回圆周方程 ((x-x_0)^2 (y-y_0)^2 R^2)通常只有一个是满足的。更稳妥的做法是定义一个带约束的代价函数[ \min_{p} \sum_i \mathrm{wrap}^2(\cdot) \lambda \left( (x-x_0)^2 (y-y_0)^2 - R^2 \right)^2 ]其中 (\lambda) 是约束权重。实现时不需要真的写这个式子我的习惯是先用无约束最小二乘得到所有候选点再用圆周方程筛选。这个方法简单、可解释性强答辩时也容易讲清楚。4.4 从二维圆阵到三维定位增加俯仰角后发生了什么竞赛题原文虽以二维为主但很多派生问题会问“如果无人机不在同一高度怎么办”。三维纯方位定位需要两类角度水平方位角 (\alpha) 和俯仰角 (\beta)。视线方向单位向量为[ u_i (\cos\beta_i\cos\alpha_i,\ \cos\beta_i\sin\alpha_i,\ \sin\beta_i) ]定位方程依然是“目标点减去参考点后与视线方向共线”[ u_i \times (p - a_i) 0 ]展开后每个参考点提供两个独立的伪线性方程因此三维最少需要三个非共线参考点才能锁定目标。实现上完全可以把2.2节的A矩阵直接升级为6行以上的形式求解方法仍然是最小二乘加高斯牛顿。如果只测水平方位角而没有俯仰角三维问题会退化成一条射线位置完全不可观这一点在建模时务必写清楚。5. 避坑纯方位无源定位最常翻车的五个问题这一章全部来自实际操作里踩过的坑。每一条都按“现象、原因、解决”的结构写方便你在比赛现场直接对着排查。5.1 角度基准不统一结果整体旋转或镜像翻转现象算出来的无人机位置相对真实位置不是绕圆心转了一个固定角度就是沿某条轴镜像对称距离看起来差不多但方向完全不对。 原因有的参考机输出的是地理方位角有的输出的是数学方位角代码里没有统一或者题目给的方位角以机头方向为基准而你用了地面坐标系。 解决建一个角度约定校验函数用一架已知位置无人机回代。把它的真实坐标代入观测方程算理论方位角和题目给的方位角对比差值应当为0或接近0。如果不为0检查是不是差了90度、180度或者符号相反。5.2 参考点近似共线方程秩亏却不报错现象lstsq返回了一个坐标残差也很小但结果离真实位置差出上百米而且换一组初始值结果完全不同。 原因参考点几乎共线时矩阵奇异值很小但还没有小到让rank直接减一最小二乘解被噪声主导。 解决在求解器里显式检查矩阵条件数用np.linalg.cond(A)提前判断。条件数大于1e6时别用普通最小二乘改用带Tikhonov正则化的版本或者在代价函数里加位置先验约束。5.3 高斯牛顿收敛到错误局部解现象理论上3个参考点就能定位但迭代结果有时正确、有时跳到另一个明显不合理的点。 原因初始值落在误差曲面错误谷底对纯方位问题来说最常见的就是镜像解区域。 解决用多个初始值做并行计算伪线性解作为第一个初始值再加入两个镜像候选作为备用初始值最后比较各解的角度残差平方和选最小的那个。这个方法会增加一点计算量但能几乎彻底消除收敛到镜像解的问题。5.4 编队绕圆心旋转相对角度全部不变却被当成新信息现象角度观测数量很多矩阵方程也满秩但所有无人机坐标对应多个不同的整体朝向模型无法分辨。 原因把“相对方位角”误当成了“绝对方位角”。圆形编队整体旋转时每两架无人机之间的相对角度保持不变所以观测方程根本不包含绝对旋转信息。 解决把旋转角 (\delta) 显式建模成未知参数然后检查已知参考点能否约束它。如果参考点只给了一个圆心坐标那旋转自由度必定无法从角度观测中恢复必须在题目条件里找额外的绝对方向信息。5.5 粗差没有剔除最小二乘被一个错误角度拖偏现象大部分参考点视线交汇在合理区域但最终解偏了好几公里画图时发现只有一条视线方向完全不对。 原因题目数据里可能存在个别无人机的方位角标号错误或粗大误差最小二乘没有鲁棒性一个粗差就能把结果拖走。 解决先做一次预定位计算每个参考点的角度残差把残差大于3倍中位绝对偏差的观测标记为粗差并剔除再重新解算。实现时用中位数而不是均值来估计残差尺度因为均值本身会被粗差污染。6. 用残差自检把结果做扎实留一角交叉验证法算出来的坐标不是终点还要用一套能说服评委和自己的验证方法。我的习惯是每个定位结果都必须过三个自检指标再去写进论文。第一个指标是角度残差平方和把求得的坐标代回观测方程反推每个参考点应有的方位角与实测方位角的差折叠到负180度到正180度再算平方和。残差平方和应当和测量噪声水平一致如果大出数量级基本说明解不正确。第二个指标是最大单角残差它用来抓粗差——单个残差明显大于其他残差时大概率是某条观测标号对错了。第三个指标是位置后验误差用残差协方差传播近似估计虽然粗糙但能给出一个“定位精度大概在几十米”的直观量级。留一角交叉验证的做法是把参考点分为两组先用其中 (n-1) 个点定位再用剩下的那一个点做独立验证比较它实测方位角与预测方位角的差值。遍历每个参考点执行一遍得到一组角度残差。这比单纯看拟合残差更诚实因为被验证的点没有参与解算角度差完全反映了模型泛化能力。比赛时把这个过程写成一张小表列为最终成果的一部分比单纯堆公式更有说服力。这段代码可以直接嵌进论文附录def leave_one_out_residual(refs, azis, solversolve_gn): residuals [] for i in range(len(refs)): refs_tr np.delete(refs, i, axis0) azis_tr np.delete(azis, i, axis0) p solver(refs_tr, azis_tr) xr, yr refs[i] pred np.arctan2(p[1] - yr, p[0] - xr) diff (pred - azis[i] np.pi) % (2 * np.pi) - np.pi residuals.append(diff) return np.array(residuals)这里的逻辑是逐个留出参考点做验证最后得到一组与定位解算无关的独立残差。比赛中如果题目给的参考点数不多可以只留一个点做验证这本质上是一种高效的模型检查手段也适合作为论文第三问的精度分析材料。算完这些指标我一般还会做一件很朴素的事把参考点、实测视线、预测位置画在同一张图上用肉眼确认所有视线确实都交汇在结果附近。这个方法看起来很土但真的能发现各种数字检查不出来的坐标代换错误和编号错位。定位问题最终是几何问题几何直觉永远是最后一道防线。希望这篇拆解能帮到你。本文还有配套的精品资源点击获取