自动驾驶系统的车辆动力学建模：自行车模型与汽车模型的混合策略及自动驾驶分层控制架构

    class HybridModelController:
    def __init__(self):
        self.bicycle = BicycleModel()
        self.double_track = DoubleTrackModel()  # 假设已实现双轨模型
    
    def select_model(self, speed):
        # 根据速度判断选择哪个模型
        return self.bicycle if speed < 15 else self.double_track
    
    # 典型应用流程示例
    def control_flow_example():
    # 初始化混合模型控制器
    controller = HybridModelController()
    
    # 设置当前速度和转向输入
    current_speed = 20.0  # m/s
    steering_input = 0.2  # 转向输入
    
    # 根据速度选择模型
    selected_model = controller.select_model(current_speed)
    
    # 获取当前车辆状态（示例状态）
    current_state = [0.0, 0.0, 0.0, current_speed]  # x, y, yaw, v
    
    # 更新车辆状态
    new_state = selected_model.update_state(current_state, steering_input, current_speed)
    
    # 输出新状态
    print("New vehicle state:", new_state)

    def simulate_model():
    # 参数设置
    T = 10.0  # 仿真时间 (s)
    dt = 0.1  # 时间步长
    N = int(T/dt)
    
    # 初始状态
    initial_state = [0.0, 0.0, 0.0, 10.0]  # x, y, yaw, v
    
    # 创建模型实例
    controller = HybridModelController()
    
    # 记录结果
    states_bicycle = [initial_state.copy()]
    states_double = [initial_state.copy()]
    
    # 模拟不同速度下的模型切换
    for i in range(N):
        current_time = i * dt
        
        # 生成速度曲线 (前5秒低速，后5秒高速)
        speed = 10.0 if current_time < 5 else 20.0
        
        # 计算转向输入（示例路径跟踪）
        desired_turn = np.sin(current_time) * 0.1  # 正弦转向指令
        
        # 自行车模型仿真
        model = controller.select_model(speed)
        new_state = model.update_state(states_bicycle[-1], desired_turn, speed, dt)
        states_bicycle.append(new_state)
        
        # 双轨模型仿真（假设已实现）
        # new_state_double = controller.double_track.update_state(...)
        # states_double.append(new_state_double)
    
    # 可视化结果
    plt.figure(figsize=(10,6))
    plt.plot([s[0] for s in states_bicycle], [s[1] for s in states_bicycle], label='Bicycle Model')
    # plt.plot([s[0] for s in states_double], [s[1] for s in states_double], label='Double Track Model')
    plt.xlabel('X Position (m)')
    plt.ylabel('Y Position (m)')
    plt.title('Vehicle Trajectory Comparison')
    plt.legend()
    plt.grid(True)
    plt.show()

    class HierarchicalController:
    def __init__(self):
        self.planner = PathPlanner()       # 上层规划模块
        self.controller = HybridController() # 底层控制模块
        
    def run(self, current_state, sensor_data):
        # 1. 上层规划生成参考轨迹
        ref_trajectory = self.planner.plan(current_state, sensor_data)
        
        # 2. 底层控制跟踪轨迹
        control_output = self.controller.track(ref_trajectory, current_state)
        
        # 3. 执行器控制
        self.actuators.apply(control_output)

    class PathPlanner:
    def plan(self, state, obstacles):
        # 10Hz规划频率
        dt_planning = 0.1
        horizon = 5  # 5秒前瞻
        steps = int(horizon/dt_planning)
        
        # 使用自行车模型快速生成路径
        path = []
        for _ in range(steps):
            # 简化的路径生成逻辑
            next_state = bicycle_model.predict(state)
            path.append(next_state)
            state = next_state
        return path

    class HybridController:
    def track(self, ref_path, current_state):
        speed = current_state[3]
        
        # 模型选择逻辑
        model = self.select_model(speed)
        
        # 控制器参数自适应
        if isinstance(model, BicycleModel):
            Kp = 0.5  # 低速控制器增益
        else:
            Kp = 0.8  # 高速控制器增益
        
        # 计算控制量
        error = self.calculate_error(ref_path, current_state)
        delta = Kp * error
        return delta

    class PathPlanner:
    def plan(self, state, obstacles):
        # 前馈路径生成
        nominal_path = self.generate_nominal_path(state)
        
        # 接收底层反馈
        if self.controller.has_feedback():
            correction = self.controller.get_feedback()
            nominal_path = self.adjust_path(nominal_path, correction)
        return nominal_path

    class HybridController:
    def track(self, ref_path, current_state):
        # 计算位置误差
        error = np.linalg.norm(current_state[:2] - ref_path[0][:2])
        
        # 反馈给规划层
        if error > 0.5:  # 超过阈值
            self.planner.update_error(error)
        return delta

    # 仿真测试配置
    def hmi_test():
    vehicle = CarHardwareInterface()
    controller = HierarchicalController()
    
    while running:
        state = vehicle.get_state()
        sensors = vehicle.get_sensors()
        
        # 分层控制
        ref_path = controller.planner.plan(state, sensors)
        control = controller.controller.track(ref_path, state)
        
        # 执行器控制
        vehicle.apply_control(control)
        
        # 数据记录
        log_data(state, control, ref_path)

    class PathPlanner:
    def generate_nominal_path(self, state):
        path = []
        for _ in range(horizon):
            # 添加车辆动力学约束
            if state[3] > 25:  # 超过速度阈值
                path.append(self.slow_down(state))
            else:
                path.append(self.normal_advance(state))
            state = path[-1]
        return path

    def safety_monitor():
    while True:
        if controller.model == DoubleTrack and speed < 10:
            log_warning("High-fidelity model used at low speed")
            controller.switch_to_bicycle()
        sleep(0.01)