SLAM+AI Agent 机器人自主导航最小原型实现

一、核心逻辑

• SLAM = 机器人的眼睛 + 定位：回答'我在哪？周围环境什么样？'
• AI Agent = 机器人的大脑 + 决策：回答'我该去哪？怎么走？避障怎么搞？'
• SLAM+AI Agent = 能自主走路的智能机器人：眼睛感知→大脑决策→身体执行，形成闭环。

二、Demo 目标（10×10 网格场景）

机器人从 (0,0) 出发，SLAM 实时输出位置 + 栅格地图，AI Agent 自动规划路径、避开障碍，最终到达目标点 (5,5)。全程仅 3 个核心模块，无冗余逻辑：

1. Map 模块：模拟栅格地图（10×10）
2. SLAM 模块：维护机器人坐标，移动后更新位置
3. AI Agent 模块：读地图→做决策→下发移动指令

三、核心代码

以下是核心代码实现，注释详细，可直接运行：

import numpy as np

# ===================== 1. 极简 SLAM（模拟定位 + 建图） =====================
class SimpleSLAM:
    def __init__(self):
        self.x, self.y = 0, 0  # 机器人初始位置
        # 构建 10×10 栅格地图，0=可通行，1=障碍物
        self.map = np.zeros((10,10))
        self.map[2,3], self.map[4,5], self.map[7,8] = 1, 1, 1  # 手动加障碍

    def update_pose(self, dx, dy):  # 移动后更新位置（模拟 SLAM 定位更新），避障逻辑
        new_x, new_y = self.x+dx, self.y+dy
        if 0<=new_x<10 and 0<=new_y<10 and self.map[new_y, new_x]==0:
            self.x, self.y = new_x, new_y

    def get_state(self):
        return self.x, self.y, self.map  # 给 Agent 输出：位置 + 地图

# ===================== 2. 极简 AI Agent（决策核心） =====================
class SimpleAgent:
    def __init__(self, target_x=5, target_y=5):
        self.target_x, self.target_y = target_x, target_y

    def think(self, x, y, map_data):  # 核心决策：朝目标走，避障（极简版）
        if x == self.target_x and y == self.target_y:
            return "ARRIVED"
        # 优先调 x 坐标，再调 y 坐标，避开障碍
        if x < self.target_x and map_data[y, x+1]==0:
            return "RIGHT"
        elif x > self.target_x and map_data[y, x-1]==0:
            return "LEFT"
        elif y < self.target_y and map_data[y+1, x]==0:
            return "UP"
        elif y > self.target_y and map_data[y-1, x]==0:
            return "DOWN"
        return "STOP"  # 遇障暂停

    def act(self, action):  # 决策转移动指令（dx, dy）
        action_map = {"RIGHT":(1,0), "LEFT":(-1,0), "UP":(0,1), "DOWN":(0,-1)}
        return action_map.get(action, (0,0))

# ===================== 3. 主循环（SLAM+Agent 闭环） =====================
if __name__ == "__main__":
    slam = SimpleSLAM()
    agent = SimpleAgent(target_x=5, target_y=5)
    print("机器人开始导航，初始位置：(0,0)，目标位置：(5,5)")
    step = 0
    while step < 100:  # 防止死循环
        x, y, map_data = slam.get_state()
        print(f"第{step}步：当前位置 ({x},{y})")
        action = agent.think(x, y, map_data)
        if action == "ARRIVED":
            print("✅ 到达目标点！导航结束")
            break
        dx, dy = agent.act(action)
        slam.update_pose(dx, dy)
        step += 1

四、运行效果

执行代码后，控制台将输出每一步的机器人位置及动作，直至到达目标点。