🤖 第02课：室内导航

📌 室内导航概述

室内导航是服务机器人的核心基础能力。与户外GPS导航不同，室内没有卫星信号，需要依靠激光雷达、视觉等传感器自主建图和定位。

📌 A*全局路径规划

A*是最经典的全局路径规划算法，结合了Dijkstra最优性和启发式搜索效率：

import heapq, math

class GridMap:
    def __init__(self, w, h):
        self.w, self.h = w, h
        self.obstacles = set()

    def add_rect(self, x1, y1, x2, y2):
        for x in range(x1, x2+1):
            for y in range(y1, y2+1):
                self.obstacles.add((x, y))

    def is_free(self, x, y):
        return 0<=x<self.w and 0<=y<self.h and (x,y) not in self.obstacles

    def neighbors(self, pos):
        x, y = pos
        for dx, dy in [(-1,0),(1,0),(0,-1),(0,1),(-1,-1),(-1,1),(1,-1),(1,1)]:
            if self.is_free(x+dx, y+dy):
                yield (x+dx, y+dy)

def heuristic(a, b):
    dx, dy = abs(a[0]-b[0]), abs(a[1]-b[1])
    return max(dx,dy) + (math.sqrt(2)-1)*min(dx,dy)

def astar(grid, start, goal):
    open_set = [(0, start)]
    came_from = {}
    g_score = {start: 0}
    visited = set()
    while open_set:
        _, cur = heapq.heappop(open_set)
        if cur in visited: continue
        visited.add(cur)
        if cur == goal:
            path = []
            while cur in came_from:
                path.append(cur)
                cur = came_from[cur]
            path.append(start)
            return path[::-1], visited
        for nb in grid.neighbors(cur):
            cost = math.sqrt(2) if abs(nb[0]-cur[0])+abs(nb[1]-cur[1])==2 else 1.0
            new_g = g_score[cur] + cost
            if new_g < g_score.get(nb, float('inf')):
                came_from[nb] = cur
                g_score[nb] = new_g
                heapq.heappush(open_set, (new_g + heuristic(nb, goal), nb))
    return None, visited

grid = GridMap(20, 15)
grid.add_rect(5, 0, 5, 5)
grid.add_rect(5, 7, 5, 12)
grid.add_rect(10, 3, 10, 10)
grid.add_rect(15, 0, 15, 8)
grid.add_rect(12, 12, 18, 12)

start, goal = (1, 7), (17, 3)
path, visited = astar(grid, start, goal)

if path:
    print(f"✅ 路径规划成功!")
    print(f"起点{start} → 终点{goal}, 路径{len(path)}步, 探索{len(visited)}节点")
    for y in range(grid.h):
        row = ""
        for x in range(grid.w):
            if (x,y)==start: row+="🟢"
            elif (x,y)==goal: row+="🔴"
            elif (x,y) in path: row+="⭐"
            elif (x,y) in grid.obstacles: row+="⬛"
            else: row+="⬜"
        print(row)

📌 DWA局部避障规划

动态窗口法(DWA)在速度空间中搜索最优控制输入，同时考虑动力学约束和障碍安全：

import math, random

class DWAPlanner:
    def __init__(self):
        self.max_speed = 1.0
        self.max_yaw = 1.0
        self.dt = 0.1
        self.predict_time = 3.0

    def predict(self, x, y, th, v, yw):
        traj = [(x, y, th)]
        t = 0
        while t <= self.predict_time:
            x += v*math.cos(th)*self.dt
            y += v*math.sin(th)*self.dt
            th += yw*self.dt
            traj.append((x, y, th))
            t += self.dt
        return traj

    def evaluate(self, traj, goal, obstacles):
        dx = goal[0]-traj[-1][0]
        dy = goal[1]-traj[-1][1]
        goal_cost = math.sqrt(dx*dx+dy*dy)
        obs_cost = 0
        for px, py, _ in traj:
            for ox, oy, r in obstacles:
                d = math.sqrt((px-ox)**2+(py-oy)**2) - r
                if d < 0.3: obs_cost += 1000
                elif d < 1.0: obs_cost += 10/(d+0.01)
        speed = math.sqrt((traj[-1][0]-traj[0][0])**2+(traj[-1][1]-traj[0][1])**2)
        return -goal_cost*2 - obs_cost + speed*0.5

    def plan(self, state, goal, obstacles):
        x, y, th, v, yw = state
        best_u, best_score, best_traj = (0,0), -1e9, None
        for vc in [i*0.1 for i in range(11)]:
            for yc in [i*0.2-1.0 for i in range(11)]:
                traj = self.predict(x, y, th, vc, yc)
                score = self.evaluate(traj, goal, obstacles)
                if score > best_score:
                    best_score = score
                    best_u = (vc, yc)
                    best_traj = traj
        return best_u, best_traj

planner = DWAPlanner()
state = (0.0, 0.0, 0.0, 0.0, 0.0)
goal = (8.0, 6.0)
obstacles = [(3.0,2.5,0.5),(5.0,4.0,0.6),(6.5,2.0,0.4),(4.0,5.5,0.5)]

print("DWA局部避障导航模拟")
print(f"起点(0,0) → 目标{goal}")
for step in range(60):
    (v, yw), _ = planner.plan(state, goal, obstacles)
    x, y, th, _, _ = state
    x += v*math.cos(th)*0.1
    y += v*math.sin(th)*0.1
    th += yw*0.1
    state = (x, y, th, v, yw)
    if math.sqrt((x-goal[0])**2+(y-goal[1])**2) < 0.5:
        print(f"✅ 第{step+1}步到达目标! 位置({x:.2f},{y:.2f})")
        break
else:
    print(f"⚠️ 未到达, 当前({state[0]:.2f},{state[1]:.2f})")

📌 SLAM建图模拟

SLAM解决"鸡和蛋"问题——不知道地图无法定位，不知道位置无法建图：

import math, random

class SimSLAM:
    def __init__(self, size=30):
        self.size = size
        self.occupancy = {}
        self.true_map = set()
        self.log_free = -0.5
        self.log_occ = 0.8

    def add_wall(self, x1, y1, x2, y2):
        dx = 1 if x2>x1 else (-1 if x2<x1 else 0)
        dy = 1 if y2>y1 else (-1 if y2<y1 else 0)
        x, y = x1, y1
        while True:
            self.true_map.add((x, y))
            if x==x2 and y==y2: break
            x += dx; y += dy

    def simulate_scan(self, rx, ry, num_beams=36, max_range=8):
        hits = []
        for i in range(num_beams):
            angle = 2*math.pi*i/num_beams
            for r in range(1, max_range+1):
                gx = int(rx + r*math.cos(angle))
                gy = int(ry + r*math.sin(angle))
                if (gx, gy) in self.true_map:
                    hits.append((gx, gy))
                    break
                self.occupancy[(gx,gy)] = self.occupancy.get((gx,gy),0) + self.log_free
        for gx, gy in hits:
            self.occupancy[(gx,gy)] = self.occupancy.get((gx,gy),0) + self.log_occ
        return hits

    def get_map(self):
        occ = set(); free = set()
        for (x,y), v in self.occupancy.items():
            (occ if v > 0 else free).add((x,y))
        return occ, free

slam = SimSLAM()
slam.add_wall(2,2,18,2); slam.add_wall(2,2,2,18)
slam.add_wall(18,2,18,18); slam.add_wall(2,18,18,18)
slam.add_wall(8,2,8,10); slam.add_wall(14,8,14,18)

waypoints = [(5,5),(5,12),(5,16),(11,5),(16,5),(16,16)]
for wx, wy in waypoints:
    slam.simulate_scan(wx, wy)

occ, free = slam.get_map()
tp = len(occ & slam.true_map)
fp = len(occ - slam.true_map)
fn = len(slam.true_map - occ)
prec = tp/(tp+fp) if tp+fp>0 else 0
rec = tp/(tp+fn) if tp+fn>0 else 0

print(f"✅ SLAM建图完成!")
print(f"真实障碍:{len(slam.true_map)} 检测障碍:{len(occ)} 空闲:{len(free)}")
print(f"精度:{prec:.1%} 召回:{rec:.1%}")

for y in range(20):
    row = ""
    for x in range(20):
        if (x,y) in slam.true_map: row += "█"
        elif (x,y) in occ: row += "▓"
        elif (x,y) in free: row += "·"
        else: row += " "
    print(row)
print("图例: █=真实墙 ▓=检测障碍 ·=空闲")

📌 Nav2导航栈架构

┌─────────────────────────────────────────┐
│  Planner Server  │  Controller Server  │  Recovery  │
│    (A*/NavFn)    │     (DWA/TEB)       │  (旋转等)  │
└───────┬──────────┴──────────┬──────────┴──────┬────┘
        │                    │                  │
   ┌────▼────────────────────▼──────────────────▼───┐
   │              BT Navigator (行为树)              │
   └────────────────────┬──────────────────────────┘
                        │
   ┌────────────────────▼──────────────────────────┐
   │           Costmap2D (代价地图)                  │
   │   静态层 │ 障碍层 │ 膨胀层 │ 语义层            │
   └───────────────────────────────────────────────┘

📌 练习

📌 成就