🤖 第14课：引导与跟随

📌 引导与跟随概述

引导和跟随是服务机器人最常见的人机物理交互——带路、跟随、并行行走：

📌 人员追踪

import math

class PersonTracker:
    """人员追踪器"""
    def __init__(self):
        self.target = None
        self.history = []
        self.follow_distance = 1.5  # 目标跟随距离
        self.max_distance = 5.0     # 丢失距离
        self.kf_state = [0, 0, 0, 0]  # x, y, vx, vy

    def update(self, detection):
        """更新目标位置"""
        if detection:
            x, y, confidence = detection
            # 简单卡尔曼更新
            self.kf_state[0] = x; self.kf_state[1] = y
            self.target = (x, y, confidence)
            self.history.append((x, y))
            if len(self.history) > 100: self.history.pop(0)
        else:
            # 预测
            self.kf_state[0] += self.kf_state[2] * 0.1
            self.kf_state[1] += self.kf_state[3] * 0.1
            self.target = (self.kf_state[0], self.kf_state[1], 0.3)

    def get_follow_command(self, robot_pos):
        """计算跟随控制命令"""
        if not self.target:
            return {"action": "stop", "reason": "目标丢失"}
        
        tx, ty, conf = self.target
        rx, ry = robot_pos
        dx, dy = tx - rx, ty - ry
        dist = math.sqrt(dx*dx + dy*dy)
        
        if dist < self.follow_distance - 0.3:
            return {"action": "stop", "distance": dist, "reason": "太近"}
        elif dist > self.max_distance:
            return {"action": "stop", "distance": dist, "reason": "太远，丢失"}
        elif dist > self.follow_distance + 0.5:
            speed = min(1.0, dist / 3.0)
            angle = math.atan2(dy, dx)
            return {"action": "follow", "speed": speed, "angle": angle, "distance": dist}
        else:
            return {"action": "follow_slow", "speed": 0.3, "angle": math.atan2(dy, dx), "distance": dist}

    def predict_trajectory(self, steps=20):
        """预测目标轨迹"""
        if len(self.history) < 2: return []
        recent = self.history[-10:]
        if len(recent) < 2: return []
        vx = (recent[-1][0] - recent[0][0]) / max(len(recent)-1, 1)
        vy = (recent[-1][1] - recent[0][1]) / max(len(recent)-1, 1)
        return [(recent[-1][0]+vx*i, recent[-1][1]+vy*i) for i in range(1, steps+1)]

tracker = PersonTracker()
print("人员追踪与跟随")
print("=" * 55)

# 模拟人员移动和机器人跟随
robot_pos = [0, 0]
person_path = [(2+i*0.2, 1+0.1*math.sin(i*0.3)) for i in range(20)]

for step, (px, py) in enumerate(person_path):
    tracker.update((px, py, 0.9))
    cmd = tracker.get_follow_command(robot_pos)
    if cmd["action"] in ["follow", "follow_slow"]:
        robot_pos[0] += cmd["speed"] * math.cos(cmd["angle"]) * 0.2
        robot_pos[1] += cmd["speed"] * math.sin(cmd["angle"]) * 0.2
    if step % 5 == 0:
        print(f"  步骤{step}: 人员({px:.2f},{py:.2f}) 机器人({robot_pos[0]:.2f},{robot_pos[1]:.2f}) {cmd['action']} 距离{cmd['distance']:.2f}m")

print(f"\n最终: 人员({person_path[-1][0]:.2f},{person_path[-1][1]:.2f}) 机器人({robot_pos[0]:.2f},{robot_pos[1]:.2f})")
print("✅ 人员追踪验证通过")

📌 引导导航

import math

class GuideNavigator:
    """引导导航器"""
    def __init__(self):
        self.waypoints = []
        self.current_wp = 0
        self.guide_distance = 2.0
        self.wait_timeout = 15  # 等待超时(秒)

    def set_route(self, waypoints):
        self.waypoints = waypoints; self.current_wp = 0

    def get_guide_action(self, robot_pos, person_pos):
        """获取引导动作"""
        if self.current_wp >= len(self.waypoints):
            return {"action": "arrived", "message": "已到达目的地！"}
        
        wp = self.waypoints[self.current_wp]
        dx = wp[0] - robot_pos[0]
        dy = wp[1] - robot_pos[1]
        dist_to_wp = math.sqrt(dx*dx + dy*dy)
        
        if dist_to_wp < 0.5:
            self.current_wp += 1
            if self.current_wp >= len(self.waypoints):
                return {"action": "arrived", "message": "已到达目的地！"}
            return {"action": "next_waypoint", "wp": self.current_wp, "message": f"请继续跟随，向第{self.current_wp+1}个路点前进"}
        
        # 检查人员是否跟上
        pdx = person_pos[0] - robot_pos[0]
        pdy = person_pos[1] - robot_pos[1]
        person_dist = math.sqrt(pdx*pdx + pdy*pdy)
        
        if person_dist > self.guide_distance * 2:
            return {"action": "wait", "message": "请跟上我，我在前方等待", "person_dist": person_dist}
        
        angle = math.atan2(dy, dx)
        speed = min(0.8, 0.3 + 0.1 * person_dist)
        
        return {"action": "guide", "speed": speed, "angle": angle,
                "waypoint": self.current_wp+1, "person_dist": person_dist,
                "dist_to_wp": dist_to_wp}

nav = GuideNavigator()
nav.set_route([(2,0), (4,0), (4,3), (6,3), (6,6)])

print("引导导航模拟")
print("=" * 55)

robot = [0.0, 0.0]
person = [0.0, -0.5]
import random
random.seed(42)

for step in range(40):
    action = nav.get_guide_action(robot, person)
    if action["action"] == "arrived":
        print(f"  步骤{step}: ✅ {action['message']}")
        break
    elif action["action"] == "wait":
        print(f"  步骤{step}: ⏸️ {action['message']} (人员距离{action['person_dist']:.1f}m)")
        person[0] += (robot[0]-person[0]) * 0.3
        person[1] += (robot[1]-person[1]) * 0.3
    elif action["action"] == "guide":
        robot[0] += action["speed"] * math.cos(action["angle"]) * 0.3
        robot[1] += action["speed"] * math.sin(action["angle"]) * 0.3
        person[0] += (robot[0]-person[0]) * 0.2 + random.gauss(0, 0.05)
        person[1] += (robot[1]-person[1]) * 0.2 + random.gauss(0, 0.05)

print("✅ 引导导航验证通过")

📌 社交导航策略

在人机共存环境中，机器人需要遵循社交礼仪——保持个人空间、靠右行驶：

class SocialNavigation:
    """社交导航策略"""
    def __init__(self):
        self.personal_space = 1.2   # 个人空间半径
        self.social_space = 2.5     # 社交空间半径
        self.right_side_bias = 0.3  # 靠右偏移量
        self.passing_distance = 0.8 # 侧身通过距离

    def compute_social_cost(self, robot_pos, person_pos, person_facing=None):
        """计算社交代价"""
        dx = robot_pos[0] - person_pos[0]
        dy = robot_pos[1] - person_pos[1]
        dist = (dx*dx + dy*dy) ** 0.5
        
        if dist < self.personal_space:
            return 10.0  # 侵入个人空间
        elif dist < self.social_space:
            return 3.0 * (1 - (dist-self.personal_space)/(self.social_space-self.personal_space))
        
        return 0.0

    def plan_polite_path(self, robot_pos, goal, people):
        """规划社交礼仪路径"""
        path = [robot_pos]
        current = list(robot_pos)
        
        for step in range(100):
            dx = goal[0] - current[0]
            dy = goal[1] - current[1]
            dist = (dx*dx + dy*dy) ** 0.5
            
            if dist < 0.3:
                path.append(goal)
                break
            
            # 基础方向
            base_angle = math.atan2(dy, dx)
            angle = base_angle
            
            # 社交力调整
            for person in people:
                px, py = person["pos"]
                pdx = current[0] - px
                pdy = current[1] - py
                pdist = (pdx*pdx + pdy*pdy) ** 0.5
                
                if pdist < self.social_space and pdist > 0.01:
                    repel_angle = math.atan2(pdy, pdx)
                    strength = 0.5 * (1 - pdist/self.social_space)
                    angle += strength * (repel_angle - angle)
            
            # 靠右偏置
            angle += self.right_side_bias * math.sin(base_angle) * 0.1
            
            current[0] += 0.2 * math.cos(angle)
            current[1] += 0.2 * math.sin(angle)
            path.append(tuple(current))
        
        return path

sn = SocialNavigation()
print("社交导航策略")
print("=" * 55)

people = [
    {"pos": (3.0, 2.0), "facing": 0},
    {"pos": (5.0, 4.0), "facing": 1.57},
]
path = sn.plan_polite_path((0, 0), (8, 6), people)

print(f"路径点数: {len(path)}")
for i in range(0, len(path), max(1, len(path)//8)):
    print(f"  点{i}: ({path[i][0]:.2f}, {path[i][1]:.2f})")

# 计算社交代价
total_cost = 0
for p in path:
    for person in people:
        total_cost += sn.compute_social_cost(p, person["pos"])
print(f"\n总社交代价: {total_cost:.2f}")
print("✅ 社交导航验证通过")

📌 引导交互设计

开始: "请跟我来，我带您前往XX"
行进中: "请继续跟随，我们快到了"
转弯时: "前方左转"
等待时: "请跟上我"
到达时: "我们到了，这是XX"
确认: "还需要其他帮助吗？"

📌 练习

📌 成就