用Python+NumPy手把手复现数学建模国赛题：无人机编队纯方位定位（附完整代码）
用Python+NumPy手把手实现无人机编队纯方位定位算法
在无人机集群协同飞行的场景中，保持编队队形是核心技术挑战之一。当无人机需要避免电磁干扰而减少主动信号发射时，如何仅通过方位信息实现精确定位就成为了关键问题。本文将带你用Python和NumPy从零实现2022年全国大学生数学建模竞赛B题的解决方案，通过代码拆解数学原理，最终完成无人机编队的纯方位无源定位系统。
1. 问题背景与数学模型构建
无人机编队飞行时，通常由部分无人机发射信号，其余无人机被动接收信号。通过提取信号中的方位信息，接收无人机可以调整自身位置。假设：
- 编队由10架无人机组成，其中9架均匀分布在半径为100m的圆周上
- 1架（FY00）位于圆心位置
- 所有无人机保持相同高度飞行
我们需要建立的数学模型是：当圆心无人机(FY00)和编队中另外2架无人机发射信号时，其他位置略有偏差的无人机如何通过接收到的方位信息调整自身位置。
核心数学原理：利用正弦定理建立角度与距离的关系。对于圆周上的任意三架无人机FY00、FY01和FY02，接收无人机FY0X的位置可以通过解三角形确定：
a/sin(A) = b/sin(B) = c/sin(C) = 2R其中R为三角形的外接圆半径。通过测量多个角度关系，我们可以建立方程组求解接收无人机的位置坐标。
2. Python环境准备与基础代码框架
我们使用Python的科学计算栈来实现这个定位系统：
import numpy as np import matplotlib.pyplot as plt from scipy.optimize import least_squares class DroneFormation: def __init__(self, radius=100): self.radius = radius # 编队半径 self.drones = {} # 无人机位置字典 self.initialize_formation() def initialize_formation(self): """初始化理想编队位置""" # 圆心无人机 self.drones['FY00'] = (0, 0) # 圆周上的9架无人机 angles = np.linspace(0, 2*np.pi, 10)[:-1] # 0到2π均分10份，取前9个 for i, angle in enumerate(angles, 1): x = self.radius * np.cos(angle) y = self.radius * np.sin(angle) self.drones[f'FY{i:02d}'] = (x, y)这个基础类建立了理想编队位置，后续我们将在此基础上添加定位算法。
3. 纯方位定位的核心算法实现
定位算法的核心是根据接收到的角度信息求解无人机位置。我们采用最小二乘法来优化位置估计：
def calculate_position(self, angle_measurements): """ 根据角度测量计算无人机位置 :param angle_measurements: 字典，格式为{'发射无人机编号': 测量角度} :return: 估计的(x,y)位置 """ def residuals(params, anchors, angles): """最小二乘法的残差函数""" x, y = params residuals = [] for (ax, ay), measured_angle in zip(anchors, angles): # 计算理论角度 theoretical_angle = np.arctan2(y - ay, x - ax) # 角度差（考虑周期） angle_diff = (theoretical_angle - measured_angle + np.pi) % (2*np.pi) - np.pi residuals.append(angle_diff) return residuals # 准备锚点（发射信号的无人机）位置和测量角度 anchors = [] measured_angles = [] for drone_id, angle in angle_measurements.items(): anchors.append(self.drones[drone_id]) measured_angles.append(np.radians(angle)) # 初始猜测位置（可以改进为更智能的初始猜测） initial_guess = (self.radius * 0.8, self.radius * 0.8) # 使用最小二乘法求解 result = least_squares(residuals, initial_guess, args=(anchors, measured_angles)) return result.x[0], result.x[1]这个算法可以处理来自多个发射无人机的角度测量信息，通过优化位置估计使得理论角度与测量角度的差异最小。
4. 编队调整与可视化实现
有了定位算法后，我们需要实现编队调整策略：
- 每架接收无人机根据定位算法计算当前位置
- 与理想位置比较，计算调整向量
- 分步实施调整，避免过冲
def adjust_formation(self, actual_positions, max_steps=10): """ 调整编队到理想位置 :param actual_positions: 当前实际位置字典 :param max_steps: 最大调整步数 """ # 记录调整过程用于可视化 adjustment_history = {drone_id: [pos] for drone_id, pos in actual_positions.items()} for step in range(max_steps): for drone_id in actual_positions: if drone_id == 'FY00': continue # 圆心无人机不调整 current_pos = actual_positions[drone_id] target_pos = self.drones[drone_id] # 计算调整向量（只移动1/4距离避免过冲） adjustment = 0.25 * (np.array(target_pos) - np.array(current_pos)) new_pos = np.array(current_pos) + adjustment actual_positions[drone_id] = tuple(new_pos) adjustment_history[drone_id].append(tuple(new_pos)) return adjustment_history可视化代码使用Matplotlib展示调整过程：
def plot_adjustment(self, adjustment_history): """绘制编队调整过程""" plt.figure(figsize=(10, 8)) # 绘制理想位置 for drone_id, pos in self.drones.items(): if drone_id == 'FY00': plt.scatter(pos[0], pos[1], c='red', marker='*', s=200, label='圆心无人机(FY00)') else: plt.scatter(pos[0], pos[1], c='blue', marker='o', s=100, alpha=0.3, label='理想位置') # 绘制调整路径 for drone_id, positions in adjustment_history.items(): x = [p[0] for p in positions] y = [p[1] for p in positions] plt.plot(x, y, '--', linewidth=1) plt.scatter(x[-1], y[-1], c='green', marker='s', s=80, label='最终位置') plt.xlabel('X坐标 (m)') plt.ylabel('Y坐标 (m)') plt.title('无人机编队位置调整过程') plt.axis('equal') plt.grid(True) plt.legend() plt.show()5. 完整系统集成与测试
现在我们将所有组件集成到一个完整的系统中：
def simulate_complete_system(): # 初始化编队 formation = DroneFormation() # 模拟实际位置（加入随机偏差） actual_positions = {} for drone_id, pos in formation.drones.items(): if drone_id == 'FY00': actual_positions[drone_id] = pos else: # 添加随机位置偏差 deviation = np.random.uniform(-20, 20, size=2) actual_positions[drone_id] = (pos[0] + deviation[0], pos[1] + deviation[1]) # 模拟角度测量（实际应用中来自传感器） angle_measurements = {} for drone_id in ['FY01', 'FY02', 'FY00']: # 假设这三架发射信号 angle_measurements[drone_id] = np.random.uniform(0, 360) # 定位并调整 for drone_id in actual_positions: if drone_id not in angle_measurements: # 接收无人机 estimated_pos = formation.calculate_position(angle_measurements) actual_positions[drone_id] = estimated_pos # 调整编队 history = formation.adjust_formation(actual_positions) # 可视化 formation.plot_adjustment(history)这个完整实现展示了从定位到调整的全过程，读者可以在此基础上进一步优化算法或添加更多功能。
6. 算法优化与性能提升
基础实现虽然能工作，但在实际应用中还需要考虑以下优化方向：
- 多源信息融合：结合多个时间步的测量信息提高定位精度
- 运动模型：考虑无人机运动特性进行预测
- 抗干扰处理：对异常测量值进行滤波
改进后的定位算法可以加入卡尔曼滤波：
from filterpy.kalman import KalmanFilter class EnhancedPositionEstimator: def __init__(self): self.kf = KalmanFilter(dim_x=4, dim_z=2) # 状态(x,y,vx,vy)，观测(x,y) # 初始化状态转移矩阵（匀速模型） self.kf.F = np.array([[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]]) # 初始化观测矩阵 self.kf.H = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) # 初始化协方差矩阵 self.kf.P *= 100 self.kf.R = np.eye(2) * 5 # 观测噪声 def update(self, measured_position): """用新测量值更新估计""" self.kf.predict() self.kf.update(measured_position) return self.kf.x[:2] # 返回位置估计这种优化可以显著提高在噪声环境下的定位稳定性。
7. 实际应用中的挑战与解决方案
在实际部署这类系统时，会遇到一些理论模拟中不明显的挑战：
挑战1：测量误差累积
- 角度传感器的精度直接影响定位结果
- 解决方案：使用高精度IMU，并定期用GPS校正
挑战2：通信延迟
- 无人机间的信息传递存在延迟
- 解决方案：在状态估计中显式考虑延迟补偿
挑战3：动态环境适应
- 风扰等环境因素影响编队保持
- 解决方案：加入自适应控制算法
以下代码展示了如何加入简单的环境适应：
def adaptive_adjustment(self, current_pos, target_pos, wind_estimate): """ 考虑环境因素的调整算法 :param wind_estimate: 估计的风速向量(x,y) """ # 基本调整向量 adjustment = 0.25 * (np.array(target_pos) - np.array(current_pos)) # 风扰补偿（简单模型） wind_compensation = -0.1 * np.array(wind_estimate) # 综合调整 total_adjustment = adjustment + wind_compensation return tuple(np.array(current_pos) + total_adjustment)8. 扩展应用：不同编队队形的实现
除了圆形编队，这套算法也可以应用于其他队形，如直线形、V字形等。关键在于：
- 定义新的理想队形位置
- 调整定位算法中的几何关系
- 可能增加更多的发射无人机以保证定位精度
例如，对于直线编队：
def initialize_line_formation(self, spacing=50): """初始化直线编队""" self.drones.clear() for i in range(10): self.drones[f'FY{i:02d}'] = (i * spacing, 0)相应的定位算法需要调整几何关系计算，但核心思路保持不变。