4. 动态障碍物环境下的航迹重规划
任务说明
本案例面向城市低空环境中的动态障碍物（如其他无人机、直升机）场景。给定一条预先规划的参考航迹，当飞行器沿航迹运动时，若检测到前方动态障碍物进入安全距离，则触发局部重规划，生成一条绕行航迹并平滑衔接原航迹。核心目标是验证“检测—重规划—切换”闭环逻辑的正确性。
实现方案
采用速度障碍法（Velocity Obstacle, VO）进行碰撞风险检测，使用Dubins路径生成绕行航迹。整体流程为：飞行器沿参考航迹匀速前进，每个控制周期检查与动态障碍物的相对位置和速度；若相对速度落入速度障碍圆锥且距离小于安全阈值，则触发重规划——以当前点为起点、原航迹前方某点为终点，生成Dubins绕行路径；绕行完成后切回原参考航迹。代码实现完整的检测与重规划逻辑，并输出关键航迹点供可视化验证。
代码实现
import numpy as np
import math
# ---------- 动态障碍物模型 ----------
class DynamicObstacle:
def __init__(self, pos, vel, radius=5.0):
self.pos = np.array(pos, dtype=float) # 位置 [x, y]
self.vel = np.array(vel, dtype=float) # 速度 [vx, vy]
self.radius = radius # 障碍物半径
# ---------- 飞行器模型 ----------
class UAV:
def __init__(self, pos, speed):
self.pos = np.array(pos, dtype=float)
self.speed = speed # 巡航速度（标量）
self.heading = 0.0 # 当前航向角（弧度）
# ---------- 速度障碍碰撞检测 ----------
def velocity_obstacle_detection(uav, obstacle, safety_margin=10.0):
"""
基于速度障碍法检测碰撞风险。
返回 True 表示存在碰撞风险，需要重规划。
"""
rel_pos = obstacle.pos - uav.pos
rel_vel = obstacle.vel - uav.speed * np.array([math.cos(uav.heading),
math.sin(uav.heading)])
dist = np.linalg.norm(rel_pos)
combined_r = obstacle.radius + safety_margin
# 距离过近直接判定为风险
if dist < combined_r:
return True
# 相对速度方向与相对位置方向夹角判断
if dist > 1e-6 and np.linalg.norm(rel_vel) > 1e-6:
angle = math.acos(np.dot(rel_pos, rel_vel) / (dist * np.linalg.norm(rel_vel)))
vo_half_angle = math.asin(combined_r / dist) if dist > combined_r else math.pi/2
if angle < vo_half_angle:
return True
return False
# ---------- Dubins路径生成（简化版：圆弧+直线） ----------
def generate_dubins_bypass(start_pos, start_heading, target_pos, turn_radius=15.0):
"""
生成从当前点绕过障碍的Dubins路径。
返回路径点列表（numpy数组），路径由左转圆弧+直线+右转圆弧组成。
"""
# 目标方向
dx = target_pos[0] - start_pos[0]
dy = target_pos[1] - start_pos[1]
target_heading = math.atan2(dy, dx)
# 计算转弯方向（统一先左转再直行再右转，简化处理）
# 实际Dubins需根据几何关系选择CLC或CLCL，此处为教学演示采用固定模式
arc1_angle = 0.6 # 第一段圆弧角度（弧度）
arc2_angle = 0.6 # 第二段圆弧角度
# 第一段：左转圆弧
cx1 = start_pos[0] + turn_radius * math.cos(start_heading + math.pi/2)
cy1 = start_pos[1] + turn_radius * math.sin(start_heading + math.pi/2)
theta_start = start_heading - math.pi/2
theta_end = theta_start + arc1_angle
arc1_points = []
steps = 10
for i in range(steps+1):
theta = theta_start + (theta_end - theta_start) * i / steps
px = cx1 + turn_radius * math.cos(theta)
py = cy1 + turn_radius * math.sin(theta)
arc1_points.append([px, py])
# 直线段：从圆弧末端指向目标方向
end1 = np.array(arc1_points[-1])
straight_len = max(np.linalg.norm(target_pos - end1) * 0.5, 10.0)
straight_dir = np.array([math.cos(target_heading), math.sin(target_heading)])
straight_end = end1 + straight_dir * straight_len
straight_points = np.linspace(end1, straight_end, 10)
# 第二段：右转圆弧到目标点
cx2 = straight_end[0] - turn_radius * math.cos(target_heading - math.pi/2)
cy2 = straight_end[1] - turn_radius * math.sin(target_heading - math.pi/2)
theta_start2 = target_heading + math.pi/2
theta_end2 = theta_start2 - arc2_angle
arc2_points = []
for i in range(steps+1):
theta = theta_start2 + (theta_end2 - theta_start2) * i / steps
px = cx2 + turn_radius * math.cos(theta)
py = cy2 + turn_radius * math.sin(theta)
arc2_points.append([px, py])
# 拼接所有路径点
bypass_path = np.vstack([arc1_points, straight_points, arc2_points])
return bypass_path
# ---------- 主仿真流程 ----------
def run_replanning_simulation():
# 初始条件
uav = UAV(pos=[0.0, 0.0], speed=10.0)
uav.heading = 0.0 # 沿x轴正方向飞行
# 参考航迹：直线飞行
reference_path = np.array([[i*5.0, 0.0] for i in range(40)]) # 200米直线
# 动态障碍物：从侧向接近
obstacle = DynamicObstacle(pos=[80.0, -20.0], vel=[0.0, 5.0], radius=5.0)
# 仿真参数
dt = 0.1
sim_time = 20.0
t = 0.0
replan_triggered = False
bypass_path = None
bypass_index = 0
current_path = reference_path
path_index = 0
# 记录飞行轨迹
trajectory = [uav.pos.copy()]
while t < sim_time and path_index < len(current_path)-1:
# 更新障碍物位置
obstacle.pos += obstacle.vel * dt
# 碰撞检测
risk = velocity_obstacle_detection(uav, obstacle, safety_margin=8.0)
# 触发重规划
if risk and not replan_triggered:
# 以当前点为起点，参考航迹前方30米处为终点
target_idx = min(path_index + 6, len(reference_path)-1)
target_pos = reference_path[target_idx]
bypass_path = generate_dubins_bypass(uav.pos, uav.heading, target_pos)
replan_triggered = True
current_path = bypass_path
bypass_index = 0
print(f"t={t:.1f}s: 碰撞风险触发，开始重规划")
# 沿当前路径运动
if replan_triggered:
# 沿绕行路径运动
if bypass_index < len(bypass_path)-1:
next_pt = bypass_path[bypass_index+1]
uav.pos = next_pt.copy()
bypass_index += 1
# 更新航向
if bypass_index < len(bypass_path)-1:
dx = bypass_path[bypass_index+1][0] - uav.pos[0]
dy = bypass_path[bypass_index+1][1] - uav.pos[1]
uav.heading = math.atan2(dy, dx)
# 绕行结束，切回参考航迹
if bypass_index >= len(bypass_path)-1:
replan_triggered = False
# 找到参考航迹上最近点
dists = np.linalg.norm(reference_path - uav.pos, axis=1)
path_index = np.argmin(dists)
current_path = reference_path
print(f"t={t:.1f}s: 绕行完成，切回参考航迹")
else:
replan_triggered = False
current_path = reference_path
else:
# 沿参考航迹运动
if path_index < len(reference_path)-1:
path_index += 1
uav.pos = reference_path[path_index].copy()
if path_index < len(reference_path)-1:
dx = reference_path[path_index+1][0] - uav.pos[0]
dy = reference_path[path_index+1][1] - uav.pos[1]
uav.heading = math.atan2(dy, dx)
trajectory.append(uav.pos.copy())
t += dt
return np.array(trajectory), replan_triggered
# ---------- 执行仿真 ----------
if __name__ == "__main__":
traj, triggered = run_replanning_simulation()
print(f"仿真完成，轨迹点数: {len(traj)}")
print(f"是否触发重规划: {triggered}")
# 输出关键航迹点（每10个点采样）
print("采样轨迹点（每10个点）:")
for i in range(0, len(traj), 10):
print(f" t_step={i}: ({traj[i][0]:.1f}, {traj[i][1]:.1f})")运行与验证
运行方式：将上述代码保存为 replanning_demo.py，在装有NumPy的Python环境中执行 python replanning_demo.py。
预期输出：程序应打印“碰撞风险触发”和“绕行完成”两条状态信息，随后输出采样轨迹点。轨迹点应显示飞行器在x≈80m附近产生y方向偏移（绕行），之后回到y=0继续沿参考航迹飞行。
判定标准：
- 触发重规划时，输出信息中的时间戳应出现在障碍物接近飞行器的时刻（约t=8~12s区间）；
- 绕行轨迹点中y坐标出现明显非零值（偏移量大于障碍物半径），表明成功避开动态障碍物；
- 绕行结束后轨迹点y坐标回归0附近，说明成功切回原参考航迹。
关键逻辑说明
本案例的碰撞检测采用速度障碍法：将飞行器视为质点，障碍物膨胀为半径等于其物理半径与安全裕度之和的圆。通过判断相对速度方向是否落入障碍物相对位置形成的圆锥区域，确定是否存在碰撞风险。重规划采用简化的Dubins路径生成——先左转圆弧、再直线、最后右转圆弧，确保绕行路径满足最小转弯半径约束。代码中绕行终点取参考航迹前方30米处，绕行完成后通过最近点匹配切回原航迹，实现平滑过渡。