第14章：具身智能与自动驾驶

xt+1 = xt + v × cos(θt) × Δt
yt+1 = yt + v × sin(θt) × Δt
θt+1 = θt + (v/L) × tan(δ) × Δt
vt+1 = vt + a × Δt

Value(move) = P(reveal) × Importance - Risk(move)

道路状况 → 前车减速
         ↘
行人出现 → 视觉反应 → 前车减速
         ↘
信号灯 → 前车减速
障碍物 → 前车减速

P(原因|减速) ∝ P(减速|原因) × P(原因)

# 训练M个模型
models = [train_model(data, seed=i) for i in range(M)]

# 预测
predictions = [model(x) for model in models]
mean = np.mean(predictions)
epistemic_var = np.var(predictions)  # 认知不确定性

# 每个模型输出均值和方差
aleatoric_var = np.mean([m.variance for m in models])  # 偶然不确定性

# 使用MC Dropout
def predict_with_uncertainty(x, n_samples=100):
    predictions = []
    for _ in range(n_samples):
        # Dropout在测试时开启
        pred = model(x, training=True)
        predictions.append(pred)

    mean = np.mean(predictions)
    uncertainty = np.var(predictions)
    return mean, uncertainty

# 输出分布参数
μ, σ² = world_model(st, at)

# 采样多个未来
futures = [sample(μ, σ²) for _ in range(N)]

# 分解不确定性
total_var = np.var(futures)
noise_var = np.mean(σ²)  # 偶然
model_var = total_var - noise_var  # 认知

if epistemic_uncertainty > threshold:
    # 模型不确定区域，保守驾驶
    action = conservative_policy(state)
elif aleatoric_uncertainty > threshold:
    # 环境随机性高，增加安全边际
    action = robust_policy(state)
else:
    # 低不确定性，正常驾驶
    action = nominal_policy(state)

# 选择信息量最大的行动
info_gain = compute_information_gain(action)
if info_gain > threshold:
    explore_action = action

# 计算碰撞概率
p_collision = integrate_over_uncertainty(collision_check)
if p_collision > safety_threshold:
    request_human_takeover()

class EmbodiedIntelligenceEvaluator:
    def __init__(self):
        self.metrics = {
            'generalization': 0,
            'causal_understanding': 0,
            'active_perception': 0,
            'physical_reasoning': 0,
            'adaptation': 0
        }

    def evaluate_generalization(self, agent, novel_scenarios):
        # 测试未见场景的表现
        performance = []
        for scenario in novel_scenarios:
            success = agent.navigate(scenario)
            performance.append(success)
        return np.mean(performance)

    def evaluate_causal_understanding(self, agent, interventions):
        # 测试因果推理能力
        correct_inferences = 0
        for intervention in interventions:
            prediction = agent.predict_effect(intervention)
            actual = simulate_intervention(intervention)
            if matches(prediction, actual):
                correct_inferences += 1
        return correct_inferences / len(interventions)

    def evaluate_active_perception(self, agent, occlusion_scenarios):
        # 测试主动信息获取
        info_efficiency = []
        for scenario in occlusion_scenarios:
            actions = agent.explore(scenario)
            efficiency = compute_info_gain(actions) / len(actions)
            info_efficiency.append(efficiency)
        return np.mean(info_efficiency)

class KnowledgeLayer:
    def __init__(self, name, risk_level, verification_needed):
        self.name = name
        self.risk_level = risk_level  # 1-5
        self.verification_needed = verification_needed

layers = [
    KnowledgeLayer("视觉特征提取", risk=2, verify="统计验证"),
    KnowledgeLayer("物体检测", risk=3, verify="标注验证"),
    KnowledgeLayer("轨迹预测", risk=4, verify="实车测试"),
    KnowledgeLayer("控制策略", risk=5, verify="封闭场地验证")
]

def measure_domain_gap(sim_data, real_data):
    # 特征分布差异
    mmd = maximum_mean_discrepancy(sim_data, real_data)

    # 预测性能差异
    sim_perf = evaluate_on(model, sim_data)
    real_perf = evaluate_on(model, real_data)
    perf_gap = abs(sim_perf - real_perf)

    # 对抗鉴别器
    domain_classifier = train_discriminator(sim_data, real_data)
    distinguishability = domain_classifier.accuracy

    return {
        'mmd': mmd,
        'performance_gap': perf_gap,
        'distinguishability': distinguishability
    }

class ProgressiveVerification:
    def __init__(self):
        self.stages = [
            "仿真回放测试",
            "硬件在环测试",
            "封闭场地测试",
            "限定区域路测",
            "扩展路测"
        ]

    def verify_component(self, component, stage):
        if stage == "仿真回放测试":
            # 使用真实数据在仿真中回放
            return self.sim_replay_test(component)
        elif stage == "硬件在环测试":
            # 真实传感器+仿真环境
            return self.hil_test(component)
        elif stage == "封闭场地测试":
            # 受控真实环境
            return self.closed_course_test(component)
        # ...

class SafetyBoundary:
    def __init__(self):
        self.safe_to_transfer = []
        self.needs_adaptation = []
        self.must_retrain = []

    def analyze_component(self, component):
        # 分析组件特性
        invariance = compute_domain_invariance(component)
        criticality = assess_safety_criticality(component)
        uncertainty = measure_transfer_uncertainty(component)

        if invariance > 0.9 and criticality < 0.3:
            self.safe_to_transfer.append(component)
        elif invariance > 0.7 or criticality < 0.6:
            self.needs_adaptation.append(component)
        else:
            self.must_retrain.append(component)

class RuntimeMonitor:
    def __init__(self, sim_model, real_model):
        self.sim_model = sim_model
        self.real_model = real_model
        self.divergence_threshold = 0.1

    def monitor(self, state):
        sim_pred = self.sim_model(state)
        real_pred = self.real_model(state)
        divergence = compute_divergence(sim_pred, real_pred)

        if divergence > self.divergence_threshold:
            # 触发安全机制
            return "SAFETY_FALLBACK"
        return "NORMAL"

class NeuroSymbolicDrivingSystem:
    def __init__(self):
        # 神经组件
        self.perception = PerceptionNetwork()
        self.scene_graph_generator = SceneGraphNet()
        self.trajectory_predictor = TrajectoryNet()

        # 符号组件
        self.knowledge_base = KnowledgeBase()
        self.rule_engine = RuleEngine()
        self.planner = SymbolicPlanner()

        # 接口层
        self.neural_to_symbolic = NeuralSymbolicInterface()
        self.symbolic_to_neural = SymbolicNeuralInterface()

    def forward(self, sensor_data):
        # 阶段1：神经感知
        features = self.perception(sensor_data)
        objects = self.perception.detect_objects(features)

        # 阶段2：符号提取
        scene_graph = self.scene_graph_generator(objects)
        symbols = self.neural_to_symbolic.extract_symbols(scene_graph)

        # 阶段3：符号推理
        facts = self.parse_facts(symbols)
        self.knowledge_base.update(facts)
        inferences = self.rule_engine.reason(self.knowledge_base)
        plan = self.planner.plan(inferences)

        # 阶段4：神经执行
        neural_constraints = self.symbolic_to_neural.convert(plan)
        trajectory = self.trajectory_predictor(features, neural_constraints)

        return trajectory, inferences  # 返回轨迹和解释

class NeuralSymbolicInterface:
    def extract_symbols(self, scene_graph):
        """将神经网络输出转换为符号表示"""
        symbols = []

        # 对象符号化
        for obj in scene_graph.objects:
            symbol = Symbol(
                type=self.classify_object(obj),
                attributes=self.extract_attributes(obj),
                confidence=obj.detection_confidence
            )
            symbols.append(symbol)

        # 关系符号化
        for rel in scene_graph.relations:
            relation = Relation(
                subject=symbols[rel.subject_id],
                predicate=self.classify_relation(rel),
                object=symbols[rel.object_id],
                confidence=rel.confidence
            )
            symbols.append(relation)

        return symbols

    def extract_attributes(self, obj):
        """提取对象属性"""
        return {
            'position': obj.bbox.center,
            'velocity': obj.motion.velocity,
            'heading': obj.motion.heading,
            'size': obj.bbox.size,
            'class': obj.class_label
        }

class SymbolicNeuralInterface:
    def convert(self, symbolic_plan):
        """将符号规划转换为神经网络约束"""
        constraints = []

        for action in symbolic_plan.actions:
            if action.type == "MAINTAIN_DISTANCE":
                constraint = DistanceConstraint(
                    target_id=action.target,
                    min_distance=action.params['min_dist']
                )
            elif action.type == "CHANGE_LANE":
                constraint = LaneChangeConstraint(
                    target_lane=action.params['lane'],
                    completion_time=action.params['time']
                )
            constraints.append(constraint)

        # 转换为可微分的损失项
        neural_constraints = self.make_differentiable(constraints)
        return neural_constraints

class KnowledgeBase:
    def __init__(self):
        self.facts = []
        self.rules = self.load_traffic_rules()
        self.ontology = self.load_driving_ontology()

    def load_traffic_rules(self):
        return [
            Rule("red_light(X) ∧ at_intersection(ego, X) → must_stop(ego)"),
            Rule("vehicle_ahead(V) ∧ distance(ego, V) < safe_dist → slow_down(ego)"),
            Rule("pedestrian_crossing(P) ∧ on_path(P) → yield(ego, P)"),
            # ... 更多规则
        ]

    def query(self, query_str):
        """Prolog风格的查询"""
        # 例如: "?- safe_to_change_lane(left)"
        return self.prolog_engine.query(query_str, self.facts)

class RuleEngine:
    def reason(self, kb):
        """前向链推理"""
        inferences = []
        changed = True

        while changed:
            changed = False
            for rule in kb.rules:
                bindings = self.match_rule(rule.antecedent, kb.facts)
                for binding in bindings:
                    consequence = self.apply_binding(rule.consequent, binding)
                    if consequence not in kb.facts:
                        kb.facts.append(consequence)
                        inferences.append(consequence)
                        changed = True

        return inferences

class HybridTraining:
    def __init__(self):
        self.neural_loss = nn.MSELoss()
        self.symbolic_loss = SymbolicConsistencyLoss()
        self.joint_loss_weight = 0.5

    def train_step(self, data):
        # 1. 神经网络前向传播
        neural_output = self.neural_model(data.sensors)

        # 2. 符号提取与推理
        symbols = self.extract_symbols(neural_output)
        reasoning_result = self.symbolic_reasoning(symbols)

        # 3. 计算损失
        # 神经网络损失（如检测、分割）
        neural_loss = self.neural_loss(neural_output, data.labels)

        # 符号一致性损失
        symbolic_loss = self.symbolic_loss(reasoning_result, data.rules)

        # 联合损失
        total_loss = neural_loss + self.joint_loss_weight * symbolic_loss

        # 4. 反向传播
        total_loss.backward()

        # 5. 符号知识蒸馏
        if reasoning_result.high_confidence:
            self.distill_to_neural(reasoning_result)

        return total_loss

class SymbolicConsistencyLoss:
    def __call__(self, reasoning_result, ground_truth_rules):
        """确保推理结果与已知规则一致"""
        violations = 0
        for rule in ground_truth_rules:
            if self.violates(reasoning_result, rule):
                violations += 1
        return violations / len(ground_truth_rules)

# 运行时推理示例
sensor_data = get_sensor_data()
trajectory, explanation = system.forward(sensor_data)

print(f"决策解释：{explanation}")
# 输出: "检测到红灯(confidence=0.95) ∧ 位于路口 → 必须停车"