🤖 第04课：电梯交互

📌 电梯交互概述

电梯交互是服务机器人跨楼层运行的关键。不同于普通导航，乘电梯需要与电梯控制系统通信，处理门禁安全、空间检测、调度决策等复杂问题。

📌 电梯控制协议模拟

import json, time

class ElevatorController:
    def __init__(self, floors=10, elevs=2):
        self.floors = floors
        self.elevators = [{"id":i,"floor":1,"target":None,"dir":0,"door":"closed","state":"idle"} for i in range(elevs)]

    def call(self, floor, direction=1):
        best = min(self.elevators, key=lambda e: abs(e["floor"]-floor) if e["state"]=="idle" else 999)
        if best["state"] == "idle":
            best["target"] = floor
            best["dir"] = 1 if floor > best["floor"] else -1
            best["state"] = "moving"
        return best["id"]

    def step(self):
        for e in self.elevators:
            if e["state"] == "moving":
                e["floor"] += e["dir"]
                if e["floor"] == e["target"]:
                    e["state"] = "open"; e["dir"] = 0
            elif e["state"] == "open":
                e["state"] = "idle"

class RobotElevatorInterface:
    PROTOCOL = ["呼叫电梯", "等待门开", "进入电梯", "选择楼层", "等待到达", "离开电梯"]

    def __init__(self, ctrl):
        self.ctrl = ctrl; self.log = []

    def interact(self, from_f, to_f):
        steps = [
            ("call", f"呼叫电梯到{from_f}楼"),
            ("wait", f"等待到达{from_f}楼, 门打开"),
            ("enter", "激光检测门宽>0.6m, 安全进入"),
            ("select", f"选择{to_f}楼"),
            ("wait_arrive", f"等待到达{to_f}楼"),
            ("exit", "语音播报'到达', 安全驶出"),
        ]
        for action, desc in steps:
            self.log.append(f"[{action}] {desc}")
            if action == "call":
                eid = self.ctrl.call(from_f)
                for _ in range(abs(from_f-1)+2): self.ctrl.step()
            elif action == "select":
                self.ctrl.call(to_f)
                for _ in range(abs(to_f-from_f)+3): self.ctrl.step()
        return self.log

ctrl = ElevatorController(8, 2)
iface = RobotElevatorInterface(ctrl)

print("机器人-电梯交互协议模拟")
print("=" * 55)
print("\n📋 场景1: 1楼→5楼")
for log in iface.interact(1, 5): print(f"  {log}")

ctrl2 = ElevatorController(8, 2)
iface2 = RobotElevatorInterface(ctrl2)
print("\n📋 场景2: 5楼→3楼")
for log in iface2.interact(5, 3): print(f"  {log}")

print("\n✅ 电梯交互协议验证通过")

📌 门安全检测与进入决策

import math

class DoorSafetyChecker:
    def __init__(self):
        self.min_width = 0.6; self.robot_w = 0.5; self.margin = 0.1

    def check(self, laser_pts, frame_w=0.9):
        left = right = None
        for angle_deg, dist in laser_pts:
            angle = math.radians(angle_deg)
            x = dist*math.cos(angle)
            if 0.5 < dist < 2.0:
                if left is None or x < left: left = x
                if right is None or x > right: right = x
        if left is not None and right is not None:
            w = right - left
            ratio = w / frame_w
            return {"width": w, "ratio": ratio,
                    "safe": w >= (self.robot_w + 2*self.margin),
                    "can_enter": ratio >= 0.9}
        return {"width":0,"ratio":0,"safe":False,"can_enter":False}

    def risk(self, door, people=0, fullness=0):
        score = 0
        if not door["safe"]: score += 30
        elif door["width"] < self.robot_w + 0.3: score += 10
        score += 15*(1-door["ratio"])
        score += people*5
        score += fullness*20
        level = "LOW" if score < 20 else ("MEDIUM" if score < 50 else "HIGH")
        return {"score": score, "level": level, "proceed": level != "HIGH"}

checker = DoorSafetyChecker()
scenarios = [
    ("正常开门", [(a, 1.5+0.2*abs(a)/90) for a in range(-80,81,5)], 0, 0.2),
    ("门半开", [(a, 1.5+0.4*abs(a)/90) for a in range(-50,51,5)], 1, 0.5),
    ("门窄+拥挤", [(a, 1.5+0.6*abs(a)/90) for a in range(-30,31,5)], 3, 0.8),
]

print("电梯门安全检测与进入决策")
print("=" * 55)
for name, laser, ppl, full in scenarios:
    door = checker.check(laser)
    r = checker.risk(door, ppl, full)
    print(f"\n📋 {name}: 宽度{door['width']:.2f}m 开门比{door['ratio']:.0%}")
    print(f"  安全:{door['safe']} 风险:{r['level']}({r['score']:.0f}) 决策:{'✅进入' if r['proceed'] else '❌等待'}")

print("\n✅ 安全检测验证通过")

📌 多电梯调度优化

class RobotElevatorScheduler:
    def __init__(self, elevators):
        self.elevators = elevators

    def est_wait(self, e, floor):
        if e["state"] == "idle": return abs(e["floor"]-floor)*3
        return abs(e["floor"]-floor)*3 + 15

    def select(self, call_floor, target_floor):
        scores = []
        for e in self.elevators:
            trip = self.est_wait(e, call_floor) + abs(target_floor-call_floor)*3
            pax = len(e.get("passengers",[]))
            if pax >= e.get("capacity",8)-1: trip += 60
            scores.append((e["id"], trip + pax*5, trip))
        scores.sort(key=lambda x: x[1])
        return scores[0]

scheduler = RobotElevatorScheduler([
    {"id":0,"floor":3,"direction":1,"state":"moving","passengers":[1,2],"capacity":8},
    {"id":1,"floor":7,"direction":-1,"state":"moving","passengers":[3],"capacity":8},
    {"id":2,"floor":1,"direction":0,"state":"idle","passengers":[],"capacity":8},
])

print("多电梯调度优化")
print("=" * 55)
for e in scheduler.elevators:
    print(f"  电梯{e['id']}: {e['floor']}楼, 乘客{len(e['passengers'])}/{e['capacity']}")

for call, target in [(1,5),(3,8),(5,2),(8,1)]:
    best = scheduler.select(call, target)
    print(f"\n📋 {call}楼→{target}楼: 电梯{best[0]} (行程{best[2]}秒)")

print("\n✅ 多电梯调度验证通过")

📌 电梯交互状态机

IDLE → NAV_TO_ELEV → CALL_ELEV → WAIT_ELEV
  ↑        ↓timeout          ↓arrived    ↓
  │     CALL_ELEV          ENTER_ELEV   │
  │        ↓                 ↓          │
  │     WAIT_ELEV          SELECT_FLOOR │
  │        ↓                 ↓          │
  │     ERROR            IN_ELEV ←─────┘
  │                         ↓
  └── DONE ← EXITING ← NAV_TO_GOAL

📌 电梯协议适配层设计

不同品牌电梯使用不同的通信协议，需要设计统一适配层：

┌─────────────────────────────────────┐
│       统一电梯接口 (IElevator)       │
│  call() │ enter() │ select() │ exit()│
├──────────┬──────────┬────────────────┤
│ 三菱适配 │ 通力适配  │ 奥的斯适配     │
│ Modbus   │ CAN总线   │ RS485/OPC UA  │
├──────────┴──────────┴────────────────┤
│           硬件接口层                  │
│    继电器控制 │ 串口通信 │ 网络通信   │
└─────────────────────────────────────┘

📌 电梯交互时序与超时策略

电梯交互每个步骤都有明确的超时阈值，超时触发降级处理：

📌 练习

📌 成就

📌 电梯协议适配层设计

不同品牌电梯使用不同的通信协议，需要设计统一适配层：

┌─────────────────────────────────────┐
│       统一电梯接口 (IElevator)       │
│  call() │ enter() │ select() │ exit()│
├──────────┬──────────┬────────────────┤
│ 三菱适配 │ 通力适配  │ 奥的斯适配     │
│ Modbus   │ CAN总线   │ RS485/OPC UA  │
├──────────┴──────────┴────────────────┤
│           硬件接口层                  │
│    继电器控制 │ 串口通信 │ 网络通信   │
└─────────────────────────────────────┘

📌 电梯交互时序与超时策略

电梯交互每个步骤都有明确的超时阈值，超时触发降级处理：

📌 电梯内定位与通信

电梯是金属封闭空间，GPS和WiFi信号被屏蔽，需要特殊处理：