🤖 第03课：动态避障

📌 动态避障概述

服务机器人运行在动态环境中——行人走动、推车移动、门开合——静态路径规划不够。动态避障需要感知-预测-规避完整能力链。

感知 → 追踪 → 预测 → 规划 → 执行 → 反馈
 │      │      │      │      │      │
激光雷达 数据关联 轨迹预测 速度选择 运动控制 安全监测
摄像头  卡尔曼滤波 线性/社交 VO/DWA 底层驱动 紧急停止

📌 动态障碍物追踪与预测

import math, random

class DynamicObstacle:
    def __init__(self, oid, x, y, vx, vy, radius=0.3):
        self.id = oid; self.x = x; self.y = y
        self.vx = vx; self.vy = vy; self.radius = radius
        self.history = [(x, y)]

    def update(self, dt=0.1):
        self.x += self.vx*dt; self.y += self.vy*dt
        self.history.append((self.x, self.y))
        if len(self.history) > 50: self.history.pop(0)

    def predict(self, steps=10, dt=0.1):
        px, py = self.x, self.y
        return [(px + self.vx*dt*i, py + self.vy*dt*i) for i in range(1, steps+1)]

class ObstacleTracker:
    def __init__(self):
        self.obstacles = {}
        self.next_id = 0

    def update(self, detections):
        for det_x, det_y, det_vx, det_vy, det_r in detections:
            best_id, best_dist = None, 1.0
            for oid, obs in self.obstacles.items():
                d = math.sqrt((obs.x-det_x)**2 + (obs.y-det_y)**2)
                if d < best_dist: best_dist = d; best_id = oid
            if best_id is not None:
                o = self.obstacles[best_id]
                o.x, o.y, o.vx, o.vy = det_x, det_y, det_vx, det_vy
            else:
                oid = self.next_id; self.next_id += 1
                self.obstacles[oid] = DynamicObstacle(oid, det_x, det_y, det_vx, det_vy, det_r)

tracker = ObstacleTracker()
obstacles = [[3.0,2.0,0.5,0.3,0.3],[7.0,4.0,-0.3,0.4,0.3],[5.0,1.0,0.0,0.6,0.4]]

print("动态障碍物追踪与预测")
print("=" * 50)
for step in range(20):
    for obs in obstacles:
        obs[0] += obs[2]*0.1; obs[1] += obs[3]*0.1
    tracker.update([tuple(d) for d in obstacles])
    if step % 5 == 0:
        print(f"\n步骤{step}:")
        for oid, obs in tracker.obstacles.items():
            pred = obs.predict(5)
            ps = ", ".join(f"({p[0]:.1f},{p[1]:.1f})" for p in pred[:3])
            print(f"  障碍物{oid}: ({obs.x:.2f},{obs.y:.2f}) 速度({obs.vx:.2f},{obs.vy:.2f}) 预测:{ps}")

print(f"\n✅ 追踪{len(tracker.obstacles)}个动态障碍物")

📌 速度障碍物法(VO)

速度障碍物法将动态障碍物映射为速度空间中的禁止锥体：

import math

class VOAvoidance:
    def __init__(self, robot_r=0.3, max_speed=1.0):
        self.robot_r = robot_r
        self.max_speed = max_speed

    def compute_safe_vel(self, rpos, rvel, goal, obstacles):
        best_vel = (0, 0); best_score = -1e9
        for vx in [i*0.1-1.0 for i in range(21)]:
            for vy in [i*0.1-1.0 for i in range(21)]:
                speed = math.sqrt(vx*vx+vy*vy)
                if speed > self.max_speed: continue
                safe = True
                for obs in obstacles:
                    dx = obs["pos"][0]-rpos[0]; dy = obs["pos"][1]-rpos[1]
                    dist = math.sqrt(dx*dx+dy*dy)
                    if dist < 0.01: continue
                    combined_r = self.robot_r + obs["radius"]
                    angle = math.atan2(dy, dx)
                    half = math.asin(min(combined_r/dist, 1.0))
                    rel_vx = obs["vel"][0]-vx; rel_vy = obs["vel"][1]-vy
                    rel_angle = math.atan2(rel_vy, rel_vx)
                    diff = abs(rel_angle - angle)
                    if diff > math.pi: diff = 2*math.pi - diff
                    if diff < half: safe = False; break
                if safe:
                    ga = math.atan2(goal[1]-rpos[1], goal[0]-rpos[0])
                    va = math.atan2(vy, vx)
                    ad = abs(ga-va)
                    if ad > math.pi: ad = 2*math.pi - ad
                    score = speed - ad*2
                    if score > best_score: best_score = score; best_vel = (vx, vy)
        return best_vel

vo = VOAvoidance()
rpos = (0.0, 0.0); goal = (10.0, 8.0)
obstacles = [
    {"pos":(3.0,2.5),"vel":(0.3,0.2),"radius":0.4},
    {"pos":(6.0,5.0),"vel":(-0.2,0.3),"radius":0.3},
    {"pos":(8.0,3.0),"vel":(0.1,-0.1),"radius":0.35},
]

print("速度障碍物法(VO)避障导航")
for step in range(80):
    for obs in obstacles:
        obs["pos"] = (obs["pos"][0]+obs["vel"][0]*0.1, obs["pos"][1]+obs["vel"][1]*0.1)
    vx, vy = vo.compute_safe_vel(rpos, (0,0), goal, obstacles)
    rpos = (rpos[0]+vx*0.1, rpos[1]+vy*0.1)
    d = math.sqrt((rpos[0]-goal[0])**2+(rpos[1]-goal[1])**2)
    if d < 0.5:
        print(f"✅ 第{step+1}步到达! ({rpos[0]:.2f},{rpos[1]:.2f})")
        break
else:
    print(f"⚠️ 80步未到达 ({rpos[0]:.2f},{rpos[1]:.2f})")

📌 社交力模型

社交力模型模拟行人运动行为，驱动力朝目标、排斥力远离障碍：

import math

class SocialForce:
    def __init__(self):
        self.A = 2.0; self.B = 0.3; self.desired_speed = 0.8; self.relax = 0.5

    def desired_force(self, pos, vel, goal):
        dx, dy = goal[0]-pos[0], goal[1]-pos[1]
        d = math.sqrt(dx*dx+dy*dy)
        if d < 0.01: return (0, 0)
        ex, ey = dx/d, dy/d
        return ((self.desired_speed*ex-vel[0])/self.relax, (self.desired_speed*ey-vel[1])/self.relax)

    def social_force(self, pi, pj, vj, ri=0.3, rj=0.3):
        dx, dy = pi[0]-pj[0], pi[1]-pj[1]
        d = math.sqrt(dx*dx+dy*dy)
        if d < 0.01: return (0, 0)
        rij = ri + rj
        nx, ny = dx/d, dy/d
        f = self.A * math.exp((rij-d)/self.B)
        if d < rij: f += 1.2*(rij-d)
        return (f*nx, f*ny)

sf = SocialForce()
robot = {"pos":[0.0,5.0], "vel":[0.0,0.0], "r":0.3}
goal = (10.0, 5.0)
peds = [{"pos":[4.0,4.5],"vel":[-0.3,0.1],"r":0.25},
        {"pos":[6.0,5.5],"vel":[0.2,-0.1],"r":0.25},
        {"pos":[8.0,4.0],"vel":[-0.1,0.3],"r":0.25}]

print("社交力模型避障")
for step in range(200):
    fd = sf.desired_force(tuple(robot["pos"]), tuple(robot["vel"]), goal)
    fsx, fsy = 0, 0
    for p in peds:
        fx, fy = sf.social_force(tuple(robot["pos"]), tuple(p["pos"]), tuple(p["vel"]), robot["r"], p["r"])
        fsx += fx; fsy += fy
    robot["vel"][0] += (fd[0]+fsx)*0.05
    robot["vel"][1] += (fd[1]+fsy)*0.05
    sp = math.sqrt(robot["vel"][0]**2+robot["vel"][1]**2)
    if sp > 1.2:
        robot["vel"][0] *= 1.2/sp; robot["vel"][1] *= 1.2/sp
    robot["pos"][0] += robot["vel"][0]*0.05
    robot["pos"][1] += robot["vel"][1]*0.05
    for p in peds:
        p["pos"][0] += p["vel"][0]*0.05; p["pos"][1] += p["vel"][1]*0.05
    if math.sqrt((robot["pos"][0]-goal[0])**2+(robot["pos"][1]-goal[1])**2) < 0.5:
        print(f"✅ 第{step+1}步到达! ({robot['pos'][0]:.2f},{robot['pos'][1]:.2f})")
        break
else:
    print(f"⚠️ 200步未到达 ({robot['pos'][0]:.2f},{robot['pos'][1]:.2f})")

📌 避障策略对比

📌 实时避障系统设计

完整的实时避障系统需要多传感器融合和分层决策：

┌────────────────────────────────────────┐
│  全局规划层 (1Hz)                       │
│  A*/NavFn → 全局最优路径               │
├────────────────────────────────────────┤
│  局部规划层 (10Hz)                      │
│  DWA/TEB → 局部轨迹优化+动态避障       │
├────────────────────────────────────────┤
│  安全防护层 (50Hz)                      │
│  碰撞检测 → 紧急制动                   │
└────────────────────────────────────────┘

📌 行人预测与意图识别

在人群密集场景中，仅靠当前状态不够，需要预测行人未来运动：

实际系统中通常采用CVM+社交力的轻量方案，在Jetson等边缘设备上实时运行：

📌 练习

📌 成就

📌 实时避障系统设计

完整的实时避障系统需要多传感器融合和分层决策：

┌────────────────────────────────────────┐
│  全局规划层 (1Hz)                       │
│  A*/NavFn → 全局最优路径               │
├────────────────────────────────────────┤
│  局部规划层 (10Hz)                      │
│  DWA/TEB → 局部轨迹优化+动态避障       │
├────────────────────────────────────────┤
│  安全防护层 (50Hz)                      │
│  碰撞检测 → 紧急制动                   │
└────────────────────────────────────────┘

📌 行人预测与意图识别

在人群密集场景中，仅靠当前状态不够，需要预测行人未来运动：