无人机组队编队与相对定位原理详解

优质文章学习记录

09 Apr 2026 — 15 min read

前言

随着无人机技术的快速发展，单一无人机的应用已经无法满足日益复杂的任务需求。无人机集群编队飞行技术应运而生，在军事侦察、灾害救援、农业植保、物流配送、灯光表演等领域展现出巨大潜力。本文将深入探讨无人机编队飞行中的核心技术——相对定位原理，并提供完整的实现代码。

一、无人机编队飞行概述

1.1 基本概念

无人机编队飞行是指多架无人机按照预定的队形和轨迹进行协同飞行的技术。这种技术需要解决以下核心问题：

位置感知：每架无人机需要知道自己和其他无人机的位置
通信协调：无人机之间需要实时交换信息
队形控制：保持预定的几何队形
避障避撞：防止无人机之间的碰撞
容错机制：单机故障时的队形重构

1.2 编队架构分类

集中式架构

由地面站或领导者无人机统一控制
优点：全局优化、控制精确
缺点：通信压力大、单点故障风险高

分布式架构

每架无人机自主决策
优点：鲁棒性强、可扩展性好
缺点：协调复杂、可能产生局部最优

混合式架构

结合集中式和分布式的优点
分层控制：高层集中决策、底层分布执行

二、相对定位技术原理

2.1 定位技术分类

绝对定位

GPS/北斗定位：精度5-10米（民用）
RTK差分定位：精度厘米级
视觉SLAM：通过视觉特征构建地图

相对定位

基于通信的定位：利用信号强度、到达时间差等
基于视觉的定位：识别其他无人机的视觉标记
基于UWB的定位：超宽带技术，精度10-30厘米
基于激光雷达的定位：高精度但成本较高

2.2 相对定位数学模型

2.2.1 坐标系定义

全局坐标系：G = {Og, Xg, Yg, Zg} 机体坐标系：B = {Ob, Xb, Yb, Zb} 相对坐标系：R = {Or, Xr, Yr, Zr}

2.2.2 相对位置计算

设无人机i和j的全局位置分别为Pi和Pj，则相对位置向量为：

Rij = Pj - Pi = [Δx, Δy, Δz]T

相对距离：

dij = ||Rij|| = √(Δx² + Δy² + Δz²)

相对方位角：

θij = atan2(Δy, Δx)

2.3 基于UWB的相对定位

UWB（Ultra-Wideband）技术因其高精度、低功耗、抗干扰能力强等特点，成为无人机编队相对定位的主流选择。

2.3.1 TOA定位原理

通过测量信号传播时间计算距离：

d = c × t

其中c为光速，t为信号传播时间。

2.3.2 TDOA定位原理

利用信号到达不同接收器的时间差进行定位，避免了时钟同步问题。

三、编队控制算法

3.1 领航-跟随法（Leader-Follower）

最经典的编队控制方法，设定一架领航机，其他无人机作为跟随者保持相对位置。

import numpy as np import matplotlib.pyplot as plt from matplotlib.animation import FuncAnimation classDroneFormation:def__init__(self, num_drones=5): self.num_drones = num_drones self.positions = np.zeros((num_drones,3))# x, y, z坐标 self.velocities = np.zeros((num_drones,3)) self.formation_pattern = self.generate_v_formation()defgenerate_v_formation(self):"""生成V字形编队""" pattern =[]for i inrange(self.num_drones):if i ==0:# 领航者在原点 pattern.append([0,0,0])else:# 跟随者按V字排列 side =1if i %2==0else-1 row =(i +1)//2 pattern.append([-row *2, side * row *2,0])return np.array(pattern)defleader_follower_control(self, leader_pos, leader_vel, dt=0.1):"""领航-跟随控制算法"""# 更新领航者位置 self.positions[0]= leader_pos self.velocities[0]= leader_vel # 跟随者控制for i inrange(1, self.num_drones):# 计算期望位置（相对于领航者） desired_pos = leader_pos + self.formation_pattern[i]# PID控制器参数 Kp =1.0# 比例增益 Kd =0.5# 微分增益# 位置误差 pos_error = desired_pos - self.positions[i]# 速度误差 vel_error = leader_vel - self.velocities[i]# 控制输入 control = Kp * pos_error + Kd * vel_error # 更新速度和位置 self.velocities[i]+= control * dt self.positions[i]+= self.velocities[i]* dt return self.positions.copy()# 使用示例 formation = DroneFormation(num_drones=5)# 模拟领航者轨迹 t = np.linspace(0,10,100) leader_trajectory = np.column_stack([10* np.cos(0.5* t),10* np.sin(0.5* t),5+2* np.sin(t)])# 执行编队飞行 positions_history =[]for i inrange(len(t)):if i >0: leader_vel =(leader_trajectory[i]- leader_trajectory[i-1])/0.1else: leader_vel = np.zeros(3) positions = formation.leader_follower_control( leader_trajectory[i], leader_vel, dt=0.1) positions_history.append(positions.copy())

3.2 虚拟结构法（Virtual Structure）

将整个编队视为一个刚体结构，每架无人机维持在虚拟结构中的相对位置。

classVirtualStructureFormation:def__init__(self, num_drones=6): self.num_drones = num_drones self.positions = np.zeros((num_drones,3)) self.virtual_center = np.array([0,0,5])# 虚拟结构中心 self.virtual_orientation =0# 虚拟结构朝向# 定义虚拟结构中的相对位置（正六边形） self.structure_pattern = self.generate_hexagon_pattern()defgenerate_hexagon_pattern(self):"""生成正六边形编队""" pattern =[] radius =3.0for i inrange(self.num_drones): angle =2* np.pi * i / self.num_drones x = radius * np.cos(angle) y = radius * np.sin(angle) pattern.append([x, y,0])return np.array(pattern)defupdate_virtual_structure(self, center_trajectory, orientation):"""更新虚拟结构的位置和朝向""" self.virtual_center = center_trajectory self.virtual_orientation = orientation # 旋转矩阵 R = np.array([[np.cos(orientation),-np.sin(orientation),0],[np.sin(orientation), np.cos(orientation),0],[0,0,1]])# 计算每架无人机的目标位置for i inrange(self.num_drones): rotated_pattern = R @ self.structure_pattern[i] self.positions[i]= self.virtual_center + rotated_pattern return self.positions.copy()defmaintain_formation(self, dt=0.1):"""维持编队队形的控制算法""" control_commands =[]for i inrange(self.num_drones):# 计算到目标位置的向量 target_pos = self.positions[i]# 简化的控制律（实际应用中需要更复杂的控制器） Kp =2.0 control = Kp *(target_pos - self.positions[i]) control_commands.append(control)return np.array(control_commands)

3.3 基于势场的编队控制

利用人工势场理论，通过吸引势场维持编队，排斥势场避免碰撞。

classPotentialFieldFormation:def__init__(self, num_drones=4): self.num_drones = num_drones self.positions = np.random.randn(num_drones,3)*5 self.velocities = np.zeros((num_drones,3)) self.desired_distances = self.compute_desired_distances()defcompute_desired_distances(self):"""计算理想的无人机间距离（正方形编队）""" distances = np.ones((self.num_drones, self.num_drones))*3.0 np.fill_diagonal(distances,0)return distances defattractive_potential(self, pos_i, pos_j, desired_dist):"""吸引势场：维持期望距离""" distance = np.linalg.norm(pos_i - pos_j)if distance <0.1:return np.zeros(3) Ka =0.5# 吸引增益 error = distance - desired_dist force = Ka * error *(pos_j - pos_i)/ distance return force defrepulsive_potential(self, pos_i, pos_j, safe_dist=1.0):"""排斥势场：避免碰撞""" distance = np.linalg.norm(pos_i - pos_j)if distance > safe_dist:return np.zeros(3) Kr =2.0# 排斥增益if distance <0.1: distance =0.1 force = Kr *(1/distance -1/safe_dist)*(pos_i - pos_j)/(distance**3)return force defcompute_control(self):"""计算每架无人机的控制输入""" controls = np.zeros((self.num_drones,3))for i inrange(self.num_drones): force = np.zeros(3)for j inrange(self.num_drones):if i != j:# 吸引力 force += self.attractive_potential( self.positions[i], self.positions[j], self.desired_distances[i, j])# 排斥力 force += self.repulsive_potential( self.positions[i], self.positions[j]) controls[i]= force return controls defupdate(self, dt=0.1):"""更新无人机位置""" controls = self.compute_control()# 限制最大加速度 max_acc =5.0 controls = np.clip(controls,-max_acc, max_acc)# 更新速度和位置 self.velocities += controls * dt # 限制最大速度 max_vel =10.0for i inrange(self.num_drones): vel_norm = np.linalg.norm(self.velocities[i])if vel_norm > max_vel: self.velocities[i]= self.velocities[i]/ vel_norm * max_vel self.positions += self.velocities * dt return self.positions.copy()

四、通信与协调机制

4.1 通信拓扑结构

import networkx as nx classCommunicationNetwork:def__init__(self, num_drones): self.num_drones = num_drones self.graph = nx.Graph() self.graph.add_nodes_from(range(num_drones))defcreate_full_mesh(self):"""全连接拓扑"""for i inrange(self.num_drones):for j inrange(i+1, self.num_drones): self.graph.add_edge(i, j)defcreate_star_topology(self, center=0):"""星形拓扑"""for i inrange(self.num_drones):if i != center: self.graph.add_edge(center, i)defcreate_ring_topology(self):"""环形拓扑"""for i inrange(self.num_drones): next_drone =(i +1)% self.num_drones self.graph.add_edge(i, next_drone)defget_neighbors(self, drone_id):"""获取邻居节点"""returnlist(self.graph.neighbors(drone_id))defbroadcast_message(self, sender_id, message):"""广播消息给所有邻居""" neighbors = self.get_neighbors(sender_id) received ={}for neighbor in neighbors: received[neighbor]= message return received

4.2 分布式一致性算法

classConsensusAlgorithm:def__init__(self, num_drones, comm_network): self.num_drones = num_drones self.comm_network = comm_network self.states = np.random.randn(num_drones,3)# 初始状态defaverage_consensus(self, iterations=100, epsilon=0.1):"""平均一致性算法""" history =[self.states.copy()]for _ inrange(iterations): new_states = self.states.copy()for i inrange(self.num_drones): neighbors = self.comm_network.get_neighbors(i)if neighbors:# 计算邻居的平均状态 neighbor_sum = np.zeros(3)for j in neighbors: neighbor_sum += self.states[j]- self.states[i]# 更新规则 new_states[i]+= epsilon * neighbor_sum self.states = new_states history.append(self.states.copy())# 检查收敛if np.max(np.abs(new_states - self.states))<1e-6:breakreturn history

五、避障与安全机制

5.1 碰撞检测

classCollisionAvoidance:def__init__(self, safe_distance=1.0): self.safe_distance = safe_distance defcheck_collision_risk(self, pos_i, vel_i, pos_j, vel_j, time_horizon=2.0):"""检测碰撞风险"""# 相对位置和速度 rel_pos = pos_j - pos_i rel_vel = vel_j - vel_i # 计算最近接近时间if np.dot(rel_vel, rel_vel)<1e-6:# 相对静止 tcpa =0else: tcpa =-np.dot(rel_pos, rel_vel)/ np.dot(rel_vel, rel_vel) tcpa =max(0,min(tcpa, time_horizon))# 计算最近接近距离 closest_pos = rel_pos + rel_vel * tcpa min_distance = np.linalg.norm(closest_pos)return min_distance < self.safe_distance, min_distance, tcpa defcompute_avoidance_velocity(self, pos_i, vel_i, obstacles):"""计算避障速度""" avoidance_vel = np.zeros(3)for obs_pos, obs_vel in obstacles: is_risk, distance, tcpa = self.check_collision_risk( pos_i, vel_i, obs_pos, obs_vel )if is_risk:# 计算避障方向（垂直于相对速度） rel_pos = obs_pos - pos_i avoid_direction = rel_pos / np.linalg.norm(rel_pos)# 避障力与距离成反比 avoid_force =(self.safe_distance - distance)*5.0 avoidance_vel -= avoid_direction * avoid_force return avoidance_vel

5.2 故障检测与恢复

classFaultTolerance:def__init__(self, num_drones): self.num_drones = num_drones self.drone_status =[True]* num_drones # True表示正常 self.heartbeat_timeout =1.0# 心跳超时时间 self.last_heartbeat =[0]* num_drones defsend_heartbeat(self, drone_id, current_time):"""发送心跳信号""" self.last_heartbeat[drone_id]= current_time defcheck_drone_health(self, current_time):"""检查无人机健康状态""" failed_drones =[]for i inrange(self.num_drones):if current_time - self.last_heartbeat[i]> self.heartbeat_timeout:if self.drone_status[i]: self.drone_status[i]=False failed_drones.append(i)return failed_drones defreconfigure_formation(self, failed_drone_id, formation_positions):"""重新配置编队"""# 移除故障无人机的位置 active_positions =[]for i, pos inenumerate(formation_positions):if i != failed_drone_id and self.drone_status[i]: active_positions.append(pos)# 重新分配位置（简化示例） num_active =len(active_positions)if num_active >0:# 均匀分布剩余无人机 angle_step =2* np.pi / num_active new_positions =[]for i inrange(num_active): angle = i * angle_step r =5.0# 编队半径 new_pos =[r * np.cos(angle), r * np.sin(angle),5.0] new_positions.append(new_pos)return np.array(new_positions)return np.array([])

六、仿真与可视化

classFormationSimulator:def__init__(self, formation_controller, num_drones=5): self.controller = formation_controller self.num_drones = num_drones self.fig = plt.figure(figsize=(12,8)) self.ax = self.fig.add_subplot(111, projection='3d')defsimulate(self, duration=20, dt=0.1):"""运行仿真""" steps =int(duration / dt) positions_history =[]for step inrange(steps):# 更新控制器 positions = self.controller.update(dt) positions_history.append(positions.copy())return np.array(positions_history)defanimate(self, positions_history):"""动画显示"""defupdate(frame): self.ax.clear() positions = positions_history[frame]# 绘制无人机for i inrange(self.num_drones): self.ax.scatter(positions[i,0], positions[i,1], positions[i,2], c='r'if i ==0else'b', s=100, marker='o')# 绘制连线for i inrange(self.num_drones -1): self.ax.plot([positions[i,0], positions[i+1,0]],[positions[i,1], positions[i+1,1]],[positions[i,2], positions[i+1,2]],'g--', alpha=0.3)# 设置坐标轴 self.ax.set_xlim([-10,10]) self.ax.set_ylim([-10,10]) self.ax.set_zlim([0,10]) self.ax.set_xlabel('X') self.ax.set_ylabel('Y') self.ax.set_zlabel('Z') self.ax.set_title(f'Drone Formation - Frame {frame}') anim = FuncAnimation(self.fig, update, frames=len(positions_history), interval=50, repeat=True) plt.show()return anim

七、实际应用案例

7.1 ROS2集成示例

# drone_formation_node.pyimport rclpy from rclpy.node import Node from geometry_msgs.msg import PoseStamped, TwistStamped from std_msgs.msg import Float32MultiArray import numpy as np classDroneFormationNode(Node):def__init__(self):super().__init__('drone_formation_controller')# 参数 self.declare_parameter('num_drones',5) self.declare_parameter('formation_type','v_shape') self.declare_parameter('control_rate',10.0) self.num_drones = self.get_parameter('num_drones').value self.formation_type = self.get_parameter('formation_type').value self.control_rate = self.get_parameter('control_rate').value # 订阅者：接收每架无人机的位置 self.pose_subscribers =[]for i inrange(self.num_drones): sub = self.create_subscription( PoseStamped,f'/drone_{i}/pose',lambda msg, drone_id=i: self.pose_callback(msg, drone_id),10) self.pose_subscribers.append(sub)# 发布者：发送控制命令 self.cmd_publishers =[]for i inrange(self.num_drones): pub = self.create_publisher( TwistStamped,f'/drone_{i}/cmd_vel',10) self.cmd_publishers.append(pub)# 定时器：控制循环 self.timer = self.create_timer(1.0/self.control_rate, self.control_loop)# 状态存储 self.drone_poses =[None]* self.num_drones self.formation_controller = self.create_formation_controller()defcreate_formation_controller(self):"""创建编队控制器"""if self.formation_type =='v_shape':return DroneFormation(self.num_drones)elif self.formation_type =='hexagon':return VirtualStructureFormation(self.num_drones)else:return PotentialFieldFormation(self.num_drones)defpose_callback(self, msg, drone_id):"""处理位置消息""" self.drone_poses[drone_id]= msg defcontrol_loop(self):"""主控制循环"""ifNonein self.drone_poses:return# 等待所有无人机位置# 提取当前位置 current_positions =[]for pose_msg in self.drone_poses: pos =[pose_msg.pose.position.x, pose_msg.pose.position.y, pose_msg.pose.position.z] current_positions.append(pos)# 计算控制命令 control_commands = self.compute_formation_control(current_positions)# 发布控制命令for i, cmd inenumerate(control_commands): twist_msg = TwistStamped() twist_msg.header.stamp = self.get_clock().now().to_msg() twist_msg.header.frame_id =f'drone_{i}' twist_msg.twist.linear.x = cmd[0] twist_msg.twist.linear.y = cmd[1] twist_msg.twist.linear.z = cmd[2] self.cmd_publishers[i].publish(twist_msg)defcompute_formation_control(self, positions):"""计算编队控制命令"""# 这里调用相应的编队控制算法return self.formation_controller.compute_control()defmain(args=None): rclpy.init(args=args) node = DroneFormationNode() rclpy.spin(node) node.destroy_node() rclpy.shutdown()if __name__ =='__main__': main()

7.2 MAVLink通信实现

# mavlink_formation.pyfrom pymavlink import mavutil import time import threading classMAVLinkFormation:def__init__(self, connection_strings):""" connection_strings: 列表，包含每架无人机的连接字符串 例如: ['udp:127.0.0.1:14550', 'udp:127.0.0.1:14551'] """ self.connections =[]for conn_str in connection_strings: conn = mavutil.mavlink_connection(conn_str) self.connections.append(conn) self.num_drones =len(self.connections) self.positions =[None]* self.num_drones self.running =True# 启动接收线程 self.receive_threads =[]for i, conn inenumerate(self.connections): thread = threading.Thread(target=self.receive_loop, args=(i, conn)) thread.start() self.receive_threads.append(thread)defreceive_loop(self, drone_id, connection):"""接收MAVLink消息"""while self.running: msg = connection.recv_match(type='GLOBAL_POSITION_INT', blocking=True)if msg: self.positions[drone_id]={'lat': msg.lat /1e7,'lon': msg.lon /1e7,'alt': msg.alt /1000.0,'relative_alt': msg.relative_alt /1000.0}defsend_position_target(self, drone_id, x, y, z, yaw=0):"""发送位置目标""" connection = self.connections[drone_id] connection.mav.set_position_target_local_ned_send(0,# time_boot_ms connection.target_system, connection.target_component, mavutil.mavlink.MAV_FRAME_LOCAL_NED,0b110111111000,# type_mask (位置控制) x, y, z,# 位置0,0,0,# 速度0,0,0,# 加速度 yaw,0# yaw, yaw_rate)defarm_and_takeoff(self, drone_id, target_altitude):"""解锁并起飞""" connection = self.connections[drone_id]# 等待自动驾驶仪初始化 connection.wait_heartbeat()# 切换到GUIDED模式 connection.mav.set_mode_send( connection.target_system, mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,4# GUIDED mode)# 解锁 connection.mav.command_long_send( connection.target_system, connection.target_component, mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,0,1,0,0,0,0,0,0)# 起飞 connection.mav.command_long_send( connection.target_system, connection.target_component, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,0,0,0,0,0,0,0, target_altitude )defexecute_formation(self, formation_type='line'):"""执行编队飞行"""# 起飞所有无人机 takeoff_altitude =10.0for i inrange(self.num_drones): self.arm_and_takeoff(i, takeoff_altitude) time.sleep(1)# 等待起飞完成 time.sleep(10)# 形成编队if formation_type =='line': spacing =5.0for i inrange(self.num_drones): x = i * spacing y =0 z =-takeoff_altitude # NED坐标系，向下为正 self.send_position_target(i, x, y, z)elif formation_type =='square': positions =[(0,0),(5,0),(5,5),(0,5)]for i inrange(min(4, self.num_drones)): x, y = positions[i] z =-takeoff_altitude self.send_position_target(i, x, y, z)defshutdown(self):"""关闭连接""" self.running =Falsefor thread in self.receive_threads: thread.join()for conn in self.connections: conn.close()

八、性能优化与实战经验

8.1 计算优化

并行计算：利用多线程/多进程处理多无人机数据
矩阵运算优化：使用NumPy向量化操作替代循环
预测控制：使用卡尔曼滤波预测未来状态
分级控制：高层路径规划 + 底层轨迹跟踪

8.2 通信优化

数据压缩：仅传输必要信息
自适应通信频率：根据任务需求调整
容错机制：数据包丢失处理
优先级队列：紧急信息优先传输

8.3 实际部署注意事项

GPS信号质量：室外环境、多路径效应
电池管理：编队飞行能耗优化
天气因素：风速、降雨对编队的影响
法规遵守：空域申请、飞行高度限制
应急预案：通信中断、GPS失锁等情况处理

九、未来发展趋势

9.1 技术发展方向

AI驱动的编队：深度强化学习自主决策
异构编队：不同类型无人机协同
群体智能：仿生物群体行为
边缘计算：机载计算能力提升
5G/6G通信：更低延迟、更高带宽

9.2 应用前景

城市空中交通：无人机物流网络
智慧农业：大规模植保作业
应急救援：灾区搜救与物资投送
军事应用：侦察、电子战、蜂群作战
娱乐表演：大型无人机灯光秀

总结

无人机编队飞行技术涉及多学科交叉，包括控制理论、通信技术、人工智能等。本文详细介绍了相对定位原理、主流编队控制算法及其实现。随着技术不断进步，无人机编队将在更多领域发挥重要作用。

关键成功因素：

可靠的相对定位技术
鲁棒的编队控制算法
高效的通信协调机制
完善的安全保障体系
充分的测试验证

希望本文能为从事无人机编队研究和开发的工程师提供参考和启发。建议读者结合具体应用场景，选择合适的技术方案，并在实践中不断优化完善。

参考资料

Reynolds, C. W. (1987). “Flocks, herds and schools: A distributed behavioral model”
Olfati-Saber, R. (2006). “Flocking for multi-agent dynamic systems”
Ren, W., & Beard, R. W. (2008). “Distributed consensus in multi-vehicle cooperative control”
Leonard, N. E., & Fiorelli, E. (2001). “Virtual leaders, artificial potentials and coordinated control of groups”
ArduPilot Documentation: https://ardupilot.org/
PX4 Autopilot: https://px4.io/
ROS2 Documentation: https://docs.ros.org/

作者信息
本文为技术分享文章，欢迎交流讨论。如有问题或建议，请在评论区留言。

版权声明
本文为原创文章，转载请注明出处。

前言