纯方位无源定位与无人机编队MATLAB建模避坑

简介这份PDF文档是2022年全国大学生数学建模竞赛本科组B题的完整获奖论文曾获河北省一等奖聚焦无人机纯方位无源定位问题适合备战数学建模竞赛的本科生、指导教师以及关注编队控制建模的读者研读。全文以二维直角坐标系与余弦定理为工具围绕圆形编队和锥形编队两条主线展开先建立被动接收信号无人机的位置模型论证当两定点夹角为定值时动点轨迹呈“眼睛”或“葫芦”形封闭曲线再借助三圆共点思想确定需要额外增加的发射信号无人机数量随后以0.01米步长做全方位遍历完成对偏差位置的迭代调整。问题二进一步讨论锥面约束下的调整策略。论文中的MATLAB实现涵盖符号运算、数值求解与轨迹可视化。压缩包仅含1个PDF文件约2.63MB篇幅紧凑便于通读。目前已有8041人学习下载。1. 三个方位角锁死一个点纯方位无源定位这个词听着唬人拆开看很朴素无人机不主动发信号、不测距只被动接收几组方位角反推自己在编队里的坐标。这道数学建模赛题里最反直觉的结论是只要三架位置已知的无人机在发信号剩下那架哪怕完全不知道自己偏了多少也能把坐标算出来而只需再多一架发射机整个圆形编队就能从“个别可定位”变成“逐架可定位”。整套推导没超出余弦定理落到 MATLAB 里不过是几个solve加一个while。真正值得琢磨的不是公式而是“只有角度”这种看起来信息量极少的观测凭什么能撑起一个可收敛的编队闭环。2. 圆周角定理为什么夹角恒定就画出一只“眼睛”2.1 一条方向信息一条封闭曲线先把场景收窄。圆心停着 0 号机圆周上停着 1 号机两架都在发信号位置精确已知第三架机在附近某处被动接收它测到的是“两条视线之间的夹角”不是绝对方位也不是距离。这一条观测只给出一个标量而平面上的位置有两个自由度所以它不可能定出一个点只能定出一条曲线。这条曲线是什么形状靠的是圆周角定理平面上两个定点 A、B动点 P 满足 ∠APB α 恒定则 P 落在以 AB 为公共弦的一对圆上。弦长记 d则这对圆的半径恒为R d / (2·sin α)两个圆心关于弦 AB 对称各自到弦中点的距离是 h (d/2)·|cot α|。注意半径只跟 sin α 有关所以 α 和 180°−α 给出的是同一对圆区别只在于 P 取的是优弧还是劣弧。这一点后面会变成一个大坑先记着。用代码把这个几何关系写出来比背公式直观得多function [C1, C2, R] locus_circles(A, B, alpha_deg) % A, B : 两架发射机的平面坐标1x2 向量 % alpha_deg : 接收机测到的方位角单位度 % C1, C2 : 一对等半径圆的圆心 % R : 半径仅由弦长和 sin(alpha) 决定 d norm(B - A); % 弦长 |AB| a d / 2; al deg2rad(alpha_deg); R a / abs(sin(al)); % 圆周角定理R |AB| / (2 sin α) h a * abs(cot(al)); % 圆心到弦中点的垂距 mid (A B) / 2; u (B - A) / d; % 弦方向单位向量 n [-u(2), u(1)]; % 法向单位向量 C1 mid h * n; % 两个圆心关于弦对称 C2 mid - h * n; end三个参数各司其职alpha_deg是观测量d由两架发射机的已知位置算出R和h是派生量。α 90° 时h 0两个圆心重合曲线退化成一个以 AB 为直径的圆——这就是泰勒斯定理也是全套模型最好用的一个直觉参照点。α 趋近 0° 或 180° 时R发散曲线被拉平逐渐向直线 AB 靠拢退化风险就在这里埋下。2.2 眼睛与葫芦优弧劣弧决定形状原文给了一句很形象的描述方位角是钝角时轨迹像一只“眼睛”是锐角时轨迹像一个“葫芦”。这句话不是修辞用上面的R和h就能解释清楚。锐角 α 较小P 必须落在优弧上才能张出这么小的视角两段鼓得厉害的优弧上下拼接中间宽、两端收形似葫芦。钝角 α 较大P 只能落在靠近弦的劣弧上两段扁弧拼起来细长形似眼睛。两种形状的半径公式完全一样|cot α|也对称唯一变的是“取哪段弧”。这也解释了为什么单靠一个角度永远定不了点一段弧上每一点给出的视角都是 α曲线上的点彼此不可区分。换成代数语言设 A(−a, 0)、B(a, 0)P(x, y)两条视线的斜率分别是 y/(xa) 和 y/(x−a)用夹角公式展开tan α | 2ay / (x² y² − a²) |整理后得到x² y² − a² ± 2a·y·cot α配方即 (x² (y ∓ a·cot α)²) a²/sin²α正是上面那对圆。所谓“配平”配的就是这一步。钝角和锐角在方程上的差别只有一个符号落到几何上就是圆心落在弦的哪一侧、P 落在哪段弧上。原文用“眼睛”和“葫芦”两个词把这件事讲完其实等价于这条轨迹方程的两次分类讨论。2.3 三圆共点三条曲线为什么必然交于一点一个角度给出一条曲线两个角度给出两条曲线的交点。两条封闭曲线一般交于两点而且这两点关于某条轴镜像对称——这正是“只有两架发射机时会出现位置歧义”的几何来源。想消掉歧义要么换一对发射机再测一次要么加第三个方位角。原文的叫法是“三圆共点”三个方位角对应三对等半径圆每个方位角各挑出“正确的那一个”三个圆交于同一点那一点就是接收机的真实位置。用根轴的语言能讲得更透两个圆的根轴是一条直线三个圆两两的根轴交于一点根心只要这三个圆不同心、不共轴公共交点唯一。换成线性代数的口味每个方位角是一个非线性约束三个约束把解集从二维曲面压成一个孤立点这是冗余观测带来的“过定”效果。这里有个容易忽略的前提三个方位角对应的发射机对不能退化。如果接收机恰好落在某两架发射机的连线上视角变成 0° 或 180°sin α 0半径发散曲线塌成一条射线交点就没了。这个退化不是数学上的边角料它在问题二里直接把一半无人机挡在门外后面第 4 章会正面撞上它。2.4 轨迹法的边界什么时候它不 work轨迹法优雅但它对输入极其挑剔。第一它假设三个方位角是同一时刻、同一位置的观测接收机在测量期间不能移动否则三条曲线根本不属于同一个点。第二它假设发射机位置绝对精确而实际编队里“位置无偏差”本身就是个建模假设误差会直接放大到半径R上。第三也是最要命的一条它只能回答“点在哪儿”回答不了“点偏了多少、该往哪走”。后者需要另一套机制也就是第 4 章的步长遍历和极坐标增量。提示把locus_circles当成一个诊断工具用比当成求解器用更值。给定任意一组观测先算三个半径和三个圆心如果某个半径大得离谱说明这一组发射机选得不好直接换组别硬解。3. 余弦定理反解三个方位角怎么变成一组坐标3.1 坐标系约定与九个理想位置轨迹法给的是几何直觉真正落到数值上论文换了一条路建二维直角坐标系用余弦定理把三条边解出来再由边反推角度和坐标。坐标系定得很干脆0 号机在原点1 号机在 (r, 0)其余八架按 40° 等间隔逆时针铺在半径 r 的圆周上。于是第 k 架的理想位置有闭式表达x_k r·cos(40°·(k−1)), y_k r·sin(40°·(k−1))用 MATLAB 一行就能生成九个点省得手抄表 1 抄错一个小数r 100; % 圆形编队半径题给 100 m k 0:8; % 0 号在圆心1~9 号在圆周 theta 40 * k; % 相邻间隔 40° P [0, 0; r*cosd(theta(:)), r*sind(theta(:))]; % 第 1 行是 0 号机这段代码没有任何技巧但它是后面所有校验的基准。任何时候算出一个坐标先跟P比一下比任何推导都可靠。相邻两架机的弦长也顺手可得2*r*sind(20)因为弦对应的圆心角是 40°半角 20°。3.2 链式求解三次余弦定理拆开三边三角设两架发射机编号为 i、j接收机编号为 k观测量是三个角α₁ ∠(0, k, i)、α₂ ∠(0, k, j)、α₃ ∠(i, k, j)。已知量是 0 号到 i 号、0 号到 j 号的距离都等于 r以及 i 号到 j 号的弦长 a₁。未知量有三个k0 |0k|、ki |ik|、kj |jk|。三个三角形各列一次余弦定理正好三个方程三角形 0-k-icos α₁ (k0² ki² − r²) / (2·k0·ki)三角形 0-k-jcos α₂ (k0² kj² − r²) / (2·k0·kj)三角形 i-k-jcos α₃ (ki² kj² − a₁²) / (2·ki·kj)三个未知、三个方程解出来就拿到了三条边。再用一次余弦定理把 k 相对 0i 连线的张角求出来减去 0i 相对 01 的已知夹角40°·|i−1|就得到 k 的极角乘上 k0 就是坐标。整套流程是链式的边→角→坐标每一步只依赖上一步。原文附录一里那几行solve就是这件事的直译syms k0 ki kj a1 2*r*sind(20*abs(i-j)); % i、j 的弦长 eq1 (k0^2 ki^2 - r^2)/(2*k0*ki) - cosd(alpha1); % 三角形 0-k-i eq2 (k0^2 kj^2 - r^2)/(2*k0*kj) - cosd(alpha2); % 三角形 0-k-j eq3 (ki^2 kj^2 - a1^2)/(2*ki*kj) - cosd(alpha3); % 三角形 i-k-j S vpasolve([eq1, eq2, eq3], [k0, ki, kj], [r, r, r]); k0v double(S.k0);syms声明符号变量vpasolve给一组初值做数值寻根double转成浮点。注意这里写的是cosd而不是cos题面所有角度都是度一旦混用弧度解出来的边长能差出两个数量级——这是第一个必须记住的细节。3.3 优角陷阱cos(2π−θ) 为什么不破坏模型论文专门用一小节讨论“优角”的情况当接收机落在某个特殊方位某个内角会变成大于 180° 的优角图上看是“凹进去”的一块。直觉上这会毁掉余弦定理因为余弦定理的返回值域是 [0, π]。但推导只用到一条恒等式cos(2π − θ) cos θ也就是说优角和它的劣角补角在余弦值上完全等价三条边的长度关系不变模型算出来的边长依然正确。真正会错的是“角度值”本身如果你拿acosd反解角度去判断方向会把优角误判成劣角坐标就镜像到另一侧去了。解决的办法不是修公式而是别用角度判断象限改用边长关系和已知点的相对位置判断符号。这条结论的适用范围值得说清楚它保证的是“边长可解”不保证“坐标唯一”。方程组的多个根里只有落在正确弧段上的那个才是物理解剩下的要么是镜像点要么是纯数学上的外解。3.4 符号解里的复数与取实部附录一里有一句x real(K0*cosd(k01))取实部。为什么会有复数因为三个余弦方程联立之后是三元高次方程组solve会把所有复根一并返回其中一部分是共轭成对出现的伪解。工程上取实部是一种“粗暴但有效”的过滤物理坐标必然是实数虚部非零的根直接丢掉。更稳妥的写法是先筛abs(imag(z)) 1e-9再取实部避免把一个虚部 0.3 的伪解当成真解。这个细节在论文里只是一行real()但换一组观测角就可能翻车第 5 章会把它列成一条具体的坑。4. 额外发射机与步长调整从“能不能定位”到“怎么走过去”4.1 一架不够两架有歧义三架刚好回到问题一第二小问0 号和 1 号已经在发信号还需要额外增加几架论文的答案是“一架就够”推理链条是信息量。只发两架时接收机测到一个角落在一段弧上位置不可定加一架之后接收机测到三个角三条曲线交于一点位置可定再加更多架当然更可定但冗余没有额外收益。这里容易被误读成“三架是必要条件”。严格说两个方位角在一般情况下也能定出两个候选点只是存在镜像歧义必须靠先验知识比如“无人机不会跑到编队外侧”去筛。第三个角度干的事是把这个歧义直接消掉让解唯一。从工程角度看这比加先验判据更稳因为先验判据一旦遇到异常机动就会失效。4.2 平方和最小判据56 种组合怎么匹配实际接收到的三个角 β₁、β₂、β₃ 是带偏差的因为接收机自己就站偏了。于是问题变成已知三个带噪角度反推“哪一架在额外发信号”。论文的做法是建表加枚举发射机对 (i, j) 与接收机 k 的所有组合去掉重复和自指共 56 种每种组合按理想几何算出一组角度三元组拿实测三元组去表里找欧氏距离平方和最小的那一行。cost (β₁ − α₁)² (β₂ − α₂)² (β₃ − α₃)²取最小值的那个组合对应的就是需要额外加入的发射机。这一步本质上是最近邻分类不是精确反解所以它有一个隐含前提位置偏差要小小到真实组合的残差一定低于其他组合。偏差一大最近邻就会跳到错误的行上这是模型的软肋也是第 5 章要展开的一条坑。代码骨架很短短到容易被低估best inf; pick -1; for j 2:9 if j k, continue; end % 自己不能给自己发信号 a1 2*r*sind(20*abs(j-1)); % 0 号与 j 号的弦长 % 按 3.2 节公式算出该组合的理想三元组 A1, A2, A3 cost (beta1-A1)^2 (beta2-A2)^2 (beta3-A3)^2; if cost best best cost; pick j; % 记录最优发射机编号 end endcontinue那行不能省否则 k 号机会把自己算进候选残差恒为 0永远选中自己。这种“自指 bug”在枚举类代码里非常常见而且因为结果看起来“收敛了”很难被发现。4.3 0.01 米步长遍历无人机不知道自己在哪一侧定位解决的是“我在哪”调整解决的是“我怎么走”。论文给的方案很朴素设步长 0.01 m向四周全方位遍历每走一步就用新位置重新算一遍观测角跟理想角比残差下降就继续直到与准确位置相差 0.01 m 以内。这套做法在数学上叫坐标下降在工程上叫“盲搜索”。它成立的前提是残差函数在真实位置附近是单峰的而这一点由几何保证越靠近真实位置三个观测角越接近理想值残差单调下降。缺点是步长固定时收敛慢半径 100 m 的编队偏差几米就要走几百步步长放大又会在最优点附近来回震荡。注意原文假设“调整时间不计”所以可以慢慢走。真实系统里每走一步都要时间步长和调整周期必须联合设计否则编队一边收敛一边漂移。4.4 极坐标增量更新走一步之后 r 和 θ 怎么变无人机控制量是“朝哪个方向、走多远”天然是极坐标所以每走一步都要更新 (ρ, θ)。设当前位置 B 的极径为 ρ朝与 OB 夹角 φ 的方向走 s 到 C三角形 OBC 里已知两边一夹角余弦定理直接给新极径ρ √(ρ² s² − 2ρs·cos φ)新极角的变化量用正弦定理或余弦定理都能求取 ∠BOC acos((ρ² ρ² − s²) / (2ρρ))符号由旋转方向决定rho_new sqrt(rho^2 s^2 - 2*rho*s*cosd(phi)); % 新极径 dtheta acosd((rho^2 rho_new^2 - s^2)/(2*rho*rho_new)); % 角增量 theta_new theta sign * dtheta; % 逆时针为正s是步长0.01phi是试探方向sign由“往北还是往南”决定。这套更新是纯几何的不依赖任何滤波所以每步误差都会累积步数一多累积误差可能超过步长本身。实际做的时候每几十步要用一次绝对定位重锚把漂移拉回来。4.5 锥形编队两架发射机加信息共享问题二换成锥形编队几何从圆周变成等边三角形网格方法却可以复用。论文先假设 1 号和 5 号位置无偏差、间距固定然后定位 2 号让 1、2 发、5 收得到一个角让 2、5 发、1 收得到另一个角三角形 1-2-5 里两角已知、一边1-5已知第三角可解2 号的位置就定了。这里藏着一个题面没明说、但建模必须加的假设信息共享。1 号收到的角要能传给 2 号否则两个角分散在不同无人机上凑不成一个三角形。论文在问题分析里点了一句“我们认为无人机之间得到的信息可以共享”这句话的分量比它看起来重得多——它把一个分布式观测问题变成了集中式解算问题。多轮接力的动机也在这第二轮用 1、2、5 去定位 3、6、8、10、12、15剩下的 4、7、9、11、13、14 因为落在三角形边的延长线上视角退化定不了第三轮换 2、6、8 当发射机剩下那批才全部可定。轮次和可定位编号可以排成一张表轮次发射机组合可定位编号不可定位原因第一轮1、2、52初始基准第二轮1、2、53、6、8、10、12、15其余落在边延长线上第三轮2、6、84、7、9、11、13、14已换组退化消除这张表说明的不是“多试几次总能成”而是“发射机组的几何构型决定了可观测性”。选组的原则很硬待定位点不能落在任意两架发射机的连线上。5. 避坑排查这几个地方几乎一定会算错5.1 角度单位混用边长直接崩现象解出来的k0是负数或者比半径大出一个量级坐标画出来飞到图外。原因MATLAB 的cos、acos吃弧度cosd、acosd吃度。题面所有角度都是度一旦在某个环节写成cos(alpha)等于把 60° 当成 60 弧度余弦值在正负之间乱跳。解决全篇统一用带d后缀的函数输入输出都标度在函数入口处加一句断言assert(alpha 0 alpha 180)把非法值挡在外面。5.2 余弦定理的双解镜像点冒充真解现象算出来的坐标关于某条轴对称跟真实位置差一个符号残差却很小。原因余弦定理只返回 [0, π]优角 2π−θ 与劣角余弦相同再加上两段圆弧天然关于弦对称方程组必然给出镜像解。解决不要用角度判象限用第三个方位角和已知点的相对位置定符号或者干脆把两个候选都代回三个余弦方程取残差小的那个。5.3 符号求解出复数根real()掩盖了问题现象double(S.k0)里混着复数取实部之后坐标偶尔跳变换一组输入又正常。原因三元高次方程组有共轭复根real()只是把虚部砍掉并没有验证这个根是否满足原方程。解决先筛abs(imag(z)) 1e-9再取实部更稳的做法是改用fsolve加多组初值按残差排序取最优。5.4 步长遍历假收敛现象while循环退了判据也满足但编队整体还是歪的。原因判据用的是“与准确位置差 0.01 m”而准确位置本身是从带噪观测反解的观测有偏时收敛到的是错误点另外八个方向遍历容易卡在局部极小。解决改成“残差不再下降”与“步长小于阈值”双判据步长先粗后细每轮结束用一次绝对定位重锚。5.5 延长线上的不可观测怎么调都调不准现象某几架无人机的残差始终降不下去换步长、换方向都没用。原因它落在两架发射机的连线上视角为 0° 或 180°sin α 0轨迹退化成射线三个约束实际只剩两个。解决换发射机组避开共线这正是问题二三段接力的设计动机。判断方法很简单算一下待定位点到发射机连线的距离小于阈值就换组。6. 设计取舍解析解漂亮盲搜索能活这套模型最值得复盘的地方不是它用了多少公式而是它在“精确”和“可用”之间反复横跳。第 3 章的链式余弦定理是解析路线给定三个角闭式解出坐标快、准、可解释但它对观测噪声零容忍一个角偏 0.5°坐标就能飘出几米。第 4 章的步长遍历是数值路线慢、糙、还要假设单峰但它对噪声鲁棒观测有偏也能慢慢磨过去。论文两头都用先用解析解算初值再用遍历精修这个组合在工程上有个更常见的名字——粗定位加精调。第二个取舍是无源本身。不发信号换来的是隐蔽性和抗干扰代价是观测维度低必须靠多机协同补信息量。这也解释了为什么“额外增加一架发射机”这道小题会被单独拎出来问它问的不是数量而是可观测性的临界点。第三个取舍在假设上。“调整时间不计”“信息共享”“两架无偏差”三条假设每条都在给模型减负每一条在真实系统里都要用通信链路、时钟同步和滤波去补。建模时把它们写成显式假设是对的但不能当成免费午餐——论文自己在模型缺点里也承认了“个别模型可能有点过多简化”。要验证这套东西到底站不站得住最省事的办法是闭环自测拿理想位置算出理论观测角加不同强度的高斯噪声喂回定位模型看坐标残差的分布再让调整算法从随机偏移出发统计收敛步数和最终误差。跑一百次蒙特卡洛收敛率低于九成就说明某条假设在你的场景里不成立别急着调参数先回去看几何。我自己的习惯是每次写完这种“几何反解 数值迭代”的模型都强制走一遍闭环自测哪怕数据是编的。有一回省了这一步直接把带镜像歧义的解喂进调整循环编队收敛出一个漂亮的对称图形所有人都在正确位置的镜像点上——图上完全看不出来血泪经验。希望帮到你。本文还有配套的精品资源点击获取