无人机编队纯方位无源定位：数学建模与非线性最小二乘求解-平芜编程栈

1. 项目概述与核心价值

最近几年，无人机编队飞行从技术演示逐渐走向了实际应用，无论是表演灯光秀还是执行协同作业任务，都离不开一个核心问题：如何让一群无人机在没有GPS或者GPS信号不佳的环境下，还能精确地知道彼此的位置，并保持队形？这正是“纯方位无源定位”技术要解决的难题。简单来说，就是每架无人机只通过“听”或“看”队友发出的信号（比如无线电、声音或视觉特征），来判断队友的方向，然后像解一道复杂的几何谜题一样，推算出整个编队中所有无人机的相对位置。这听起来有点像我们蒙上眼睛，只靠听周围朋友说话的声音方向，在脑海里画出一张座位图。

2022年高教社杯全国大学生数学建模竞赛的B题，正是将这个前沿又极具挑战性的问题抛给了广大参赛者。题目要求参赛队伍建立数学模型，仅利用无人机观测到的队友方位角信息，来解算所有无人机的相对位置，并设计队形调整策略。这个题目不仅考察数学建模能力，更直接对接了无人机集群协同、自主导航等领域的实际技术瓶颈。我当年作为指导老师，带着学生啃下了这块硬骨头，过程中积累了大量实战经验和代码技巧。今天，我就把当时解题的核心思路、模型构建的详细过程、算法实现的参考代码，以及那些在论文里不会写的“踩坑”心得，毫无保留地分享出来。无论你是正在备战数模竞赛的学生，还是对无人机集群技术感兴趣的工程师，这篇文章都能为你提供一条从理论到实践的清晰路径。

2. 问题拆解与核心思路解析

面对“纯方位无源定位”这个问题，我们首先要把它拆解成几个可以逐步攻克的子问题。不能一上来就想用一个公式解决所有事情，那样只会陷入混乱。

2.1 问题本质：一个几何约束下的优化问题

题目的核心输入是什么？是每架无人机观测到的其他无人机的方位角。注意，这里只有角度信息，没有距离信息。这就像你站在一个广场上，只能看到几个朋友分别在你的“2点钟方向”、“5点钟方向”，但不知道他们离你有多远。我们的目标是根据所有这些两两之间的方向观测，反推出每个人的具体坐标。

这本质上是一个基于图论的几何约束满足问题。我们可以把无人机看作图中的节点，把一次方位观测看作一条有向边，边上携带的信息就是观测角度。整个编队构成一个观测图。我们的任务是为每个节点赋予二维坐标（x, y），使得图中任意一条边所连接的两个节点，其连线方向与观测到的方位角尽可能一致。

因此，解题思路的主干非常清晰：

数据抽象与图模型构建：将观测数据转化为一个数学图。
相对位置初始化：为图中所有节点赋予一个初始的坐标猜测。这个初始值不能太随意，否则优化算法可能陷入局部最优或无法收敛。
建立优化模型：定义一个“损失函数”或“代价函数”，用来衡量当前估算坐标与所有观测角度之间的吻合程度。
迭代求解：使用数值优化算法，不断调整各节点的坐标，使损失函数最小化，从而得到最优的相对位置估计。
队形调整策略：在已知相对位置的基础上，设计控制指令，让无人机从当前位置移动到目标队形位置。

2.2 模型选择：为什么是“最小二乘法”？

在众多优化模型中，非线性最小二乘法是解决此类问题的首选。原因如下：

直观性：损失函数直接定义为所有观测角度预测值与实际值之差的平方和。我们的目标就是让这个“总误差”最小。
普适性：它不要求观测图是完全连通的（即每架无人机都能看到所有其他无人机），只要能形成一个连通图，理论上就可解。
成熟工具：有大量成熟、高效的数值优化库（如 MATLAB 的lsqnonlin, Python 的scipy.optimize.least_squares）可以直接调用，让我们专注于模型本身而非算法细节。

假设我们有N架无人机。对于第i架无人机观测第j架无人机，得到的方位角为θ_ij(以弧度为单位)。如果我们估计的i和j的坐标分别为(x_i, y_i)和(x_j, y_j)，那么根据坐标计算出的理论方位角为：atan2(y_j - y_i, x_j - x_i)这里atan2是四象限反正切函数，能给出-π到π范围内的正确角度。

那么，这一项观测的误差就是：e_ij = atan2(y_j - y_i, x_j - x_i) - θ_ij由于角度是周期性的，我们需要处理角度差跨越±π边界的情况，通常的做法是将误差e_ij规整到(-π, π]区间，例如：e_ij = mod(e_ij + π, 2π) - π。

最终，我们的损失函数F(X)就是所有有效观测误差的平方和：F(X) = Σ_{(i,j) in Observations} e_ij^2其中X是一个包含所有2N个坐标参数（x1, y1, x2, y2, ..., xN, yN）的向量。

注意：角度处理的坑。这是第一个容易出错的地方。如果直接用减法而不做周期规整，当真实角度差接近±π时，优化算法会认为误差突然变得很大（比如从3.14跳到-3.14），导致收敛困难甚至失败。务必在计算误差项之前，先处理好角度差。

2.3 未知数与约束：解决尺度、旋转和平移模糊

仔细观察我们的模型，你会发现一个关键问题：损失函数F(X)对于坐标X的整体平移、旋转和缩放是不变的。

平移：所有无人机坐标同时加上一个固定向量(dx, dy)，任意两机之间的方向角不变。
旋转：所有无人机坐标同时绕原点旋转一个角度α，任意两机之间的方向角都增加α，但如果我们观测到的是绝对方向（例如相对于正北），这就引入了系统误差。在纯相对定位中，我们通常没有绝对方向基准，所以旋转自由度无法确定。
缩放：所有无人机坐标同时乘以一个正系数s，方向角同样不变。

这意味着，我们的优化问题有无数个解！为了得到一个唯一确定的解，我们必须引入额外的约束来“锚定”这个解。这是建模中最关键的一步。

常用的约束策略：

固定两架无人机：这是最直接的方法。例如，固定无人机0在原点(0,0)，固定无人机1在(d, 0)轴上（d是一个设定的正数，比如1或10）。这相当于固定了坐标系的平移、一个轴的方向和尺度。
固定重心和主方向：在优化过程中或优化后，对解进行标准化。例如，令所有无人机坐标的均值为(0,0)（消除平移），然后通过奇异值分解（SVD）或类似方法，将点云的主轴对齐到坐标轴上（消除旋转），并缩放到一个固定大小（消除尺度）。这种方法更优雅，但实现稍复杂。

在竞赛的有限时间内，策略1（固定两架无人机）是更稳妥的选择。它简单有效，直接将3个自由度（平移x, y，旋转）固定下来，尺度由你设定的d值决定。d可以设为1，那么得到的就是一个“单位尺度”下的相对坐标；你也可以根据对无人机间典型距离的估计来设定d，让坐标值更符合物理直觉。

3. 核心算法实现与代码详解

理论清晰后，我们进入实战环节。这里我以Python语言为例，结合NumPy和SciPy库，展示完整的实现流程。MATLAB的实现思路类似，函数调用不同而已。

3.1 数据准备与图结构构建

首先，我们需要组织观测数据。假设我们从题目附件或自己生成的数据中，得到了一个观测列表。每一条观测记录可以表示为(observer_id, target_id, measured_angle)。

import numpy as np from scipy.optimize import least_squares # 示例：5架无人机的观测数据 (observer, target, angle_in_radians) # 注意：这里角度应以弧度为单位。如果题目给的是角度，需转换为弧度：rad = deg * np.pi / 180 observations = [ (0, 1, 0.0), # 无人机0看无人机1，方向为0度（正东） (0, 2, 0.6435), # 约36.87度 (1, 0, 3.1416), # 约180度 (1, 2, -0.6435), # 约-36.87度 (2, 0, -2.4981), # 约-143.13度 (2, 1, 2.4981), # 约143.13度 # ... 更多观测数据 ] N = 5 # 无人机数量 # 我们可以将观测数据存储为字典或列表，方便后续计算 obs_dict = {} for obs in observations: i, j, theta = obs if i not in obs_dict: obs_dict[i] = {} obs_dict[i][j] = theta

3.2 定义损失函数（残差函数）

这是整个代码的核心。我们需要定义一个函数，它接收一个包含所有2N个坐标的参数向量params，计算并返回所有观测误差组成的数组。

def residual_function(params, observations, N): """ 计算非线性最小二乘的残差向量。 params: 一维数组，形状为 (2*N,)，按 [x0, y0, x1, y1, ..., x_{N-1}, y_{N-1}] 排列。 observations: 观测数据列表，每个元素为 (i, j, theta_ij)。 N: 无人机数量。 返回: 残差向量，每个元素对应一个观测的误差。 """ # 将参数向量重构为Nx2的坐标矩阵 coords = params.reshape((N, 2)) # 每一行是 (x_i, y_i) residuals = [] for i, j, theta_ij in observations: xi, yi = coords[i] xj, yj = coords[j] # 计算理论方位角 computed_angle = np.arctan2(yj - yi, xj - xi) # 计算角度误差，并规整到 [-pi, pi) 区间 error = computed_angle - theta_ij error = (error + np.pi) % (2 * np.pi) - np.pi residuals.append(error) return np.array(residuals)

3.3 设置初始值与约束

初始值的好坏直接影响优化速度和结果。一个不错的策略是利用观测图的部分信息来初始化。

初始化策略：

将无人机0固定在原点(0, 0)。
对于能被无人机0观测到的无人机j（比如无人机1），我们可以将其初始位置放在一个以原点为圆心、半径为r_init（例如r_init=10）的圆上，方向角就是观测角θ_0j。即：(r_init * cos(θ_0j), r_init * sin(θ_0j))。
对于其他无人机，如果它们能被已初始化的无人机观测到，可以用类似三角定位的方法估算初始位置；如果信息不足，可以简单地放在原点附近的一个随机位置。

添加约束：如前所述，我们通过固定参数值来添加约束。在调用优化器之前，我们直接修改参数向量和损失函数。

def project_params_for_optimization(full_params, fixed_indices, fixed_values): """ 将完整的参数向量投影为优化器使用的自由参数向量。 固定某些参数的值。 full_params: 完整的初始参数向量 (2N)。 fixed_indices: 需要固定的参数在full_params中的索引列表。 fixed_values: 对应的固定值列表。 返回: 自由参数初始值 (用于优化)，以及一个函数用于将自由参数映射回完整参数。 """ free_mask = np.ones(len(full_params), dtype=bool) free_mask[fixed_indices] = False free_init = full_params[free_mask] def expand_params(free_params): """将自由参数扩展回完整参数向量""" full = np.zeros_like(full_params) full[free_mask] = free_params full[fixed_indices] = fixed_values return full return free_init, expand_params # 设置初始坐标 initial_coords = np.zeros((N, 2)) initial_coords[0] = [0.0, 0.0] # 固定无人机0在原点 # 假设观测数据中，无人机0能看到1和2 r_init = 10.0 for obs in observations: i, j, theta = obs if i == 0: initial_coords[j] = [r_init * np.cos(theta), r_init * np.sin(theta)] # 对于没有通过无人机0初始化的无人机，给一个小的随机初始值 for idx in range(N): if np.all(initial_coords[idx] == 0): initial_coords[idx] = np.random.uniform(-1, 1, size=2) # 将坐标矩阵展平为参数向量 initial_params = initial_coords.flatten() # 定义约束：固定无人机0在(0,0)，固定无人机1在(d, 0) fixed_distance = 10.0 # 这是设定的尺度 fixed_indices = [0, 1, 2, 3] # 对应 x0, y0, x1, y1 在参数向量中的索引 fixed_values = [0.0, 0.0, fixed_distance, 0.0] # (x0,y0)=(0,0), (x1,y1)=(d,0) # 获取自由参数的初始值，并创建映射函数 free_init, expand_func = project_params_for_optimization(initial_params, fixed_indices, fixed_values) # 重新包装残差函数，使其接受自由参数 def wrapped_residuals(free_params): full_params = expand_func(free_params) return residual_function(full_params, observations, N)

3.4 调用优化器求解

现在，我们可以使用SciPy的least_squares优化器来求解。

# 设置优化选项 opt_options = { 'ftol': 1e-8, # 函数值变化的容忍度 'xtol': 1e-8, # 参数变化的容忍度 'gtol': 1e-8, # 梯度容忍度 'max_nfev': 2000, # 最大函数调用次数 'verbose': 2 # 输出详细过程，调试时有用，正式运行可设为0 } # 执行优化 result = least_squares(wrapped_residuals, free_init, method='trf', **opt_options) if not result.success: print(f"优化警告: {result.message}") # 即使不成功，也可能有可用结果，但需要谨慎对待 # 提取优化后的自由参数，并映射回完整坐标 optimized_free_params = result.x optimized_full_params = expand_func(optimized_free_params) optimized_coords = optimized_full_params.reshape((N, 2)) print("优化后的无人机相对坐标：") for i in range(N): print(f" 无人机 {i}: ({optimized_coords[i, 0]:.4f}, {optimized_coords[i, 1]:.4f})") # 计算最终残差，评估拟合优度 final_residuals = wrapped_residuals(optimized_free_params) rmse = np.sqrt(np.mean(final_residuals**2)) print(f"\n方位角拟合均方根误差 (RMSE): {rmse:.6f} rad (约 {rmse*180/np.pi:.4f} 度)")

3.5 队形调整策略实现

得到相对坐标后，第二部分任务是调整队形。假设目标队形由一组目标相对坐标target_coords定义（同样满足我们固定的坐标系，比如无人机0在原点，无人机1在X轴正向上）。

策略：分步调整法一种稳健的策略是让无人机依次移动到目标位置，每次只移动一架，其他无人机保持静止提供方位观测。这样可以最大限度地利用稳定的观测环境。

选择调整顺序：通常从位置确定性最高的无人机开始（比如被观测次数最多的，或者作为基准的0号和1号），或者从外围无人机开始向中心调整。
单机控制：对于当前要移动的无人机k，假设其他无人机都已就位或处于已知位置。无人机k根据其他多架无人机（至少2架）对其的方位观测，结合这些无人机已知的位置，可以实时解算自己的当前位置（这相当于一个小规模的实时定位问题）。然后，计算当前位置到目标位置的向量，转化为速度或位移指令。
迭代直至收敛：循环对所有无人机进行微调，直到所有无人机与目标位置的距离都小于一个阈值。

这里给出一个简化的、基于已知全局坐标的理想控制模拟代码（实际中需结合传感器反馈）：

def formation_adjustment(current_coords, target_coords, max_iter=100, tol=0.01): """ 一个简化的队形调整模拟。 假设：我们已知所有无人机的真实全局坐标（current_coords），并且可以精确控制其移动到任意位置。 实际中，current_coords需要通过持续的纯方位定位来更新。 """ adjusted_coords = current_coords.copy() n = len(current_coords) # 一个简单的顺序：按索引顺序调整 order = list(range(n)) # 或者更聪明的顺序：按到目标点的初始距离从大到小排序 # init_dist = np.linalg.norm(current_coords - target_coords, axis=1) # order = np.argsort(-init_dist) # 从最远的开始调整 for iteration in range(max_iter): max_error = 0 for idx in order: # 计算当前位置与目标位置的偏差 error_vector = target_coords[idx] - adjusted_coords[idx] error_dist = np.linalg.norm(error_vector) max_error = max(max_error, error_dist) # 如果误差较大，则向目标移动一步（这里用最简单的比例控制） if error_dist > tol: step_size = min(0.5, error_dist * 0.8) # 控制步长，避免振荡 adjusted_coords[idx] += step_size * (error_vector / error_dist) # 在实际系统中，移动后需要等待新的观测数据，并重新解算adjusted_coords # 这里为简化，假设移动后坐标立即更新 print(f"迭代 {iteration+1}: 最大位置误差 = {max_error:.4f}") if max_error < tol: print("队形调整完成！") break return adjusted_coords # 假设我们有一个目标队形（例如，一个正五边形） target_coords = np.array([ [0, 0], [10, 0], [5, 8.66], # 10 * sin(60°) [-5, 8.66], [-10, 0] ]) print("\n开始队形调整模拟...") final_coords = formation_adjustment(optimized_coords, target_coords) print("\n调整后的最终坐标：") print(final_coords)

4. 关键难点、技巧与避坑指南

在实际建模和编程过程中，你会遇到很多教科书上不会提的麻烦。下面是我总结的几个关键点和避坑经验。

4.1 观测图的连通性与可定位性

这是理论上的首要难点。不是随便给一些方位角就能定位的。

必要条件：观测图必须是刚性的。简单理解，就是它的几何形状是唯一确定的，不能像平行四边形那样可以“滑动”。对于二维平面，一个连通图至少需要2N - 3条边（观测）才可能具有刚性（在一般位置上）。2N-3是刚性的必要条件，但不是充分条件。
充分性检查：一个实用的（非严格的）方法是，在添加了固定两个节点的约束后，计算优化问题雅可比矩阵在初始点附近的秩。如果自由度（2N-4，因为固定了两个点，各消除2个自由度）等于雅可比矩阵的秩，那么局部可辨识的可能性就很大。
实操建议：在解题时，如果题目没有明确说明观测图是连通的，你需要在论文中讨论这一点。可以假设观测图是连通的，或者对数据进行预处理，只选取最大的连通分量进行定位。在代码中，可以先使用图论算法（如深度优先搜索）检查连通性。

4.2 初始值敏感性与局部最优

非线性最小二乘对初始值非常敏感。糟糕的初始值会导致算法收敛到错误的局部极小点，或者根本不收敛。

现象：优化后的坐标看起来乱七八糟，误差（RMSE）仍然很大。
解决方案：
1. 多初始点尝试：随机生成多组初始坐标（在固定基准点约束下），分别进行优化，选择最终残差最小的解作为最终结果。
2. 利用部分已知几何：如之前所述，利用基准无人机（如0号）的观测来初始化其邻居的位置，能极大提高成功率。
3. 分阶段优化：先使用一个更鲁棒但可能精度不高的方法（如直接线性变换的近似解）得到一个粗略解，再以其作为初始值进行精细的非线性优化。

代码实现多起点：

best_coords = None best_cost = np.inf num_trials = 20 for trial in range(num_trials): # 随机初始化（保持基准点固定） random_init = np.random.uniform(-10, 10, size=free_init.shape) result = least_squares(wrapped_residuals, random_init, method='trf', ftol=1e-9, max_nfev=1000) if result.cost < best_cost: best_cost = result.cost best_free_params = result.x if best_coords is not None: optimized_full_params = expand_func(best_free_params) optimized_coords = optimized_full_params.reshape((N, 2)) print(f"最佳成本函数值: {best_cost}")

4.3 角度周期性与误差函数设计

这是算法实现中最容易出bug的地方。

坑点：arctan2返回的角度在(-π, π]之间。如果真实角度差是179°（约3.12 rad），而一次迭代估计的角度差是-181°（约-3.16 rad），直接相减得到的误差是-6.28 rad，这会被优化器视为一个巨大的误差，从而错误地引导搜索方向。
正确做法：如前所述，必须对误差进行“归一化”或“规整”，确保其在(-π, π]范围内。error = ((computed - measured) + np.pi) % (2*np.pi) - np.pi这个公式是可靠的。
进阶技巧：对于某些优化器，你可能还需要提供损失函数关于参数的雅可比矩阵（导数）以加速收敛。计算雅可比矩阵时，也必须考虑角度规整对导数的影响，否则雅可比矩阵是错误的，可能导致收敛失败。如果使用SciPy.least_squares且不提供雅可比，它会用有限差分法数值近似，这时只要你的残差函数residual_function正确处理了角度，问题就不大。

4.4 数值稳定性与单位统一

尺度问题：如果你将固定距离fixed_distance设得太大（如1000）或太小（如0.001），而其他初始坐标在±1范围内，优化器可能会遇到数值问题（梯度尺度差异巨大）。建议将整个问题的尺度归一化到1附近。固定距离设为1或10都是不错的选择。
单位统一：确保所有角度单位一致（全部用弧度或全部用角度）。三角函数计算默认使用弧度，所以强烈建议在数据输入阶段就将角度转换为弧度。
处理异常观测：实际数据可能有噪声甚至粗差。可以在残差函数中加入鲁棒核函数（如 Huber loss），降低大残差项对总代价的影响，但这会增加模型复杂度。竞赛中，通常假设数据是“干净”的。

4.5 队形调整的实用考量

在第二部分队形调整中，模型假设往往比较理想。

通信与同步：我们的策略假设无人机在移动时，其他无人机能提供精确的方位测量。实际上，这需要可靠的通信和时钟同步。在论文中，可以讨论采用“广播-接收”机制和时分复用策略来避免通信冲突。
控制律设计：上面给出的简单比例控制法可能产生振荡。更优的方法是使用PID控制或模型预测控制（MPC）。PID更容易实现：u = Kp * error + Ki * integral(error) + Kd * derivative(error)，其中u是控制量（如速度矢量）。
避障：在调整队形时，特别是密集编队，必须考虑防碰撞。可以在控制指令中增加排斥力项，当两机距离小于安全阈值时，产生一个相互排斥的速度。
仿真验证：在论文中，仅仅给出算法描述和最终坐标是不够的。最好能提供一段仿真动画或一系列位置演化图，展示无人机如何从初始散乱位置逐步收敛到目标队形。这能极大提升论文的说服力和表现力。可以用matplotlib.animation或更专业的机器人仿真工具如ROS/Gazebo（但竞赛时间有限，前者更实际）来实现。

5. 模型扩展与进阶思考

解决了基础问题后，我们可以思考一些更贴近实际或更具挑战性的扩展方向，这些内容如果能在竞赛论文中适当体现，会是重要的加分项。

5.1 考虑观测噪声的鲁棒定位

题目数据可能是无噪声的，但实际传感器（如视觉摄像头、射频测向设备）必然存在误差。我们可以模拟噪声，并评估模型的鲁棒性。

噪声模型：通常假设方位角观测噪声服从零均值的高斯分布，即θ_measured = θ_true + w,w ~ N(0, σ^2)。
影响分析：在仿真中，给观测数据添加不同强度（σ）的高斯噪声，然后运行定位算法。观察定位误差（估计坐标与真实坐标的欧氏距离）如何随噪声增大而增加。可以绘制“噪声标准差-定位误差”的关系曲线。
改进方法：为了提升抗噪能力，可以：
1. 增加观测数量：让每架无人机观测更多邻居，利用冗余信息平均掉噪声。
2. 使用鲁棒损失函数：将最小二乘的L2损失（平方和）换成Huber损失或L1损失（绝对值和），后者对 outliers（粗差）不敏感。
3. 引入滤波算法：将定位问题建模为状态估计问题，使用卡尔曼滤波（EKF）或粒子滤波进行递归估计，能有效平滑噪声。

5.2 动态编队与持续定位

之前的模型是“静态”的，即根据某一时刻的快照进行定位。在实际飞行中，编队是动态的，我们需要进行持续跟踪。

思路：将时间离散化为多个时刻t=0,1,2,...。每个时刻，无人机获得一组方位观测。我们可以利用上一时刻的位置估计作为当前时刻优化问题的初始值，这通常比随机初始化好得多，因为无人机位置不会突变。
模型整合：可以引入运动模型。假设无人机匀速运动，则状态向量可以包含位置和速度。观测模型仍然是方位角。这样就构成了一个典型的传感器融合问题，可以用扩展卡尔曼滤波（EKF）优雅地解决。EKF在预测步骤根据运动模型更新状态，在更新步骤根据方位观测修正状态。
计算复杂度：动态跟踪对计算实时性要求高。需要优化代码，可能采用固定滞后平滑或滑动窗口优化，而不是处理全部历史数据。

5.3 三维空间中的纯方位无源定位

如果无人机不是在二维平面，而是在三维空间中飞行（这才是更普遍的情况），问题会变得更加复杂。

观测量：从二维的方位角（azimuth）变为三维的方位角和俯仰角（azimuth and elevation）。
模型变化：坐标参数变为3N个（x, y, z）。理论方位角计算需要使用三维几何：azimuth = atan2(y_j - y_i, x_j - x_i),elevation = arcsin((z_j - z_i) / distance)。
约束：需要固定至少三个不共线的点的坐标来消除三维空间的6个自由度（3个平移，3个旋转）。尺度模糊依然存在，需要额外约束。
挑战：三维问题的非线性更强，对初始值和观测几何更为敏感。观测图需要更“丰富”才能保证刚性。

5.4 与其他传感器融合

纯方位定位虽然不依赖GPS，但在复杂环境中可能存在局限性（如遮挡导致观测缺失）。与其他廉价传感器融合是提升系统可靠性的方向。

惯性测量单元（IMU）：提供加速度和角速度，通过积分可以推算短时间内的位移和姿态变化。虽然积分会漂移，但非常适合与纯方位定位互补：IMU提供高频、短时精确的相对运动，而方位定位提供低频、绝对或相对的位姿校正，抑制IMU的漂移。
气压计/高度计：提供高度信息，可以将三维定位问题简化为二维（如果高度已知）或提供高度约束。
融合框架：松耦合：分别用IMU和方位定位解算位置，然后进行加权平均。紧耦合：将IMU数据（如速度增量）和方位角观测一起构建到一个统一的优化问题或滤波器中，这是更优的方案。

6. 参赛论文写作要点与代码整合建议

最后，从竞赛实战角度，分享一些论文写作和代码提交的要点。

6.1 论文结构建议

一篇好的数模论文，逻辑清晰比文笔华丽更重要。

摘要：用精炼的语言概括问题、你的方法、主要模型、算法和结论。务必突出创新点和亮点结果（如定位精度、调整速度）。
问题重述与分析：用自己的话理解并拆分问题，明确要解决的两个子问题：定位和调整。
模型假设与符号说明：列出合理的假设（如观测无噪声、通信理想等），并给出所有用到符号的清晰定义。
模型建立与求解：这是核心。
- 定位模型：详细阐述图模型、非线性最小二乘损失函数、约束处理（固定基准点）、角度周期处理。给出损失函数的具体形式。
- 算法设计：说明如何使用scipy.optimize.least_squares或MATLAB lsqnonlin求解，包括初始值策略、参数设置。
- 队形调整模型：阐述你的控制策略（如分步调整、比例控制），给出控制律公式。
仿真实验与结果分析：
- 数据：说明你使用了题目数据，或生成了仿真数据。
- 定位结果：展示优化后的相对坐标表格，并可视化（画出示意图）。计算并分析方位角拟合残差。
- 调整过程：展示队形调整的动态过程（用一系列时序图或动画截图）。给出最终位置误差和收敛步数/时间。
- 灵敏度分析：讨论观测噪声、初始值、通信延迟等因素对结果的影响（可选，但加分）。
模型评价与推广：客观评价模型的优点（如原理清晰、实现简单）和缺点（如对初始值敏感、假设较强）。提出改进方向（如融合IMU、三维扩展）。
参考文献：规范引用。
附录：附上核心代码（关键函数，而非全部）。

6.2 代码提交与可复现性

评委可能会运行你的代码，因此代码质量很重要。

注释清晰：关键步骤，尤其是模型实现、约束处理、角度规整部分，必须有详细注释。
模块化：将数据读取、预处理、模型定义、优化求解、结果可视化等功能写成独立的函数或脚本。
依赖明确：在代码开头或单独的README中说明所需的Python/ MATLAB版本和第三方库（如numpy, scipy, matplotlib）。
一键运行：提供一个主脚本（如main.py或run.m），能够从头到尾执行整个流程，并生成论文中的关键结果和图。
数据接口：代码应能方便地读取题目提供的附件数据（如data.xlsx或data.txt）。

6.3 可视化技巧

“一图胜千言”，在论文中插入高质量的图表能极大提升表现力。

定位结果图：用散点图画出优化前后的无人机位置，用连线表示观测关系（或真实相对位置）。用不同颜色和标记区分。
误差分析图：绘制观测角残差的直方图，或绘制每个无人机最终定位误差的条形图。
队形调整动画/序列图：这是亮点。用matplotlib.animation.FuncAnimation生成GIF或MP4动画，展示无人机运动轨迹。如果时间紧张，至少提供几个关键时刻（初始、中间、最终）的队形快照。
收敛曲线：绘制优化过程中损失函数值下降的曲线，展示算法的收敛性。

记住，全国大学生数学建模竞赛不仅考察数学建模能力，也考察问题解决、编程实现和科技论文写作的综合能力。将清晰的思路、严谨的模型、稳健的代码和有力的表达结合起来，才能脱颖而出。希望这份超详细的解析和代码框架，能为你提供坚实的起点。在实际操作中，多调试、多思考、多尝试不同的参数和策略，祝你取得优异成绩！