移动机器人电池能耗预测与顺路充电调度 —— 基于EKF与事件驱动的OOP仿真

 

“那次夜班，巡检机器人跑到一半没电了，瘫在管廊中间，叫天天不应。后来我们在上位机里加了能耗预测模块，让机器人像老司机一样‘算着油钱开车’——电量低于阈值就自动规划顺路充电，充到80%立刻回头继续任务，全程不用中断工艺流程。”

—— 哈尔滨工程大学《工业过程控制》课程核心思想延伸

 

一、实际应用场景描述

 

在石化、煤化工等大型流程工厂，巡检机器人需要沿着长距离管廊执行周期性巡检任务。然而电池容量有限，而生产不能停：

 

┌──────────────────────────────────────────────┐

│ 移动机器人能源管理系统 │

│ │

│ [上位机调度中枢] │

│ │ 能耗预测 / 充电决策 / 任务编排 │

│ ▼ │

│ ┌────────────────────────────┐ │

│ │ 能耗预测器 (EKF/PF) │ │

│ │ ┌──────────────────────┐ │ │

│ │ │ 1. 里程能耗模型 │ │ │

│ │ │ (SOC - 里程映射) │ │ │

│ │ └──────────────────────┘ │ │

│ │ ┌──────────────────────┐ │ │

│ │ │ 2. 坡度/负载补偿 │ │ │

│ │ │ (重力势能+电机损耗) │ │ │

│ │ └──────────────────────┘ │ │

│ │ ┌──────────────────────┐ │ │

│ │ │ 3. 剩余里程预测 │ │ │

│ │ │ (SOC/能耗率) │ │ │

│ │ └──────────────────────┘ │ │

│ └────────────┬───────────────┘ │

│ │ 预测剩余里程 │

│ ┌───────┴───────┐ │

│ ▼ ▼ │

│ ┌─────────┐ ┌─────────┐ │

│ │ 决策引擎 │ │ 路径规划器 │ │

│ │ (Rule+ML) │ │ (Dijkstra) │ │

│ │ • SOC阈值 │ │ • 充电桩位置 │ │

│ │ • 任务节点 │ │ • 顺路代价 │ │

│ │ • 工艺约束 │ │ • 返回路径 │ │

│ └────┬────┘ └────┬────┘ │

│ │ 充电指令 │ 新路径 │

│ ▼ ▼ │

│ ┌────────────────────────────┐ │

│ │ 机器人执行层 │ │

│ │ ┌───┐ ┌───┐ ┌───┐ │ │

│ │ │导航├──►运动控制├──►充电管理│ │ │

│ │ │ │ │ │ │ │ │

│ │ └─┬─┘ └─┬─┘ └─┬─┘ │ │

│ │ └──►工艺巡检任务◄──┘ │ │

│ │ ▲ (不中断) │ │

│ └────────────┬───────────────┘ │

│ │ 状态反馈 (SOC, pos) │

│ ▼ │

│ ┌────────────────────────────┐ │

│ │ 充电桩网络 (Charging Spots) │ │

│ │ • 桩A: 管廊中部 (50m) │ │

│ │ • 桩B: 罐区入口 (150m) │ │

│ │ • 桩C: 装置区 (300m) │ │

│ └───────────────────────────┘ │

│ │

│ 核心: 能耗预测 + 顺路充电 + 任务连续性保障 │

└──────────────────────────────────────────────┘

 

传统固定返航 vs 顺路充电调度

 

维度 固定返航（电量阈值） 顺路充电调度

任务连续性 ❌ 中断返回 ✅ 无缝衔接

充电时机 ❌ 慌忙返航 ✅ 提前预判

路程损耗 ❌ 往返空跑 ✅ 顺路补给

产能影响 ❌ 任务中断 ✅ 零感知

 

二、引入痛点

 

2.1 现场的真实困境

 

场景 现场发生了什么 根因

“瘫在半路” “机器人没电停在危险区域” 无能耗预测

“任务中断” “巡检做到一半被迫回家充电” 充电决策粗暴

“满电也跑” “明明够电却跑去充电” 无剩余里程评估

“充电排队” “三个机器人抢一个桩” 无多机调度

“坡上趴窝” “上坡电量跳水” 无动态补偿

 

2.2 核心矛盾

 

机器人的电池是“标量”，而巡检任务是“矢量”（有方向、有工艺节点、有截止时间）。 单纯监控 SOC（State of Charge）百分比是不够的——必须预测“按当前路径跑下去，电够不够撑到下一个安全停靠点”。这需要建立里程-能耗模型，并结合坡度、负载进行动态补偿，最终通过事件驱动触发顺路充电。

2.3 我们要解决什么

 

用一段精简的 Python 程序，构建一个移动机器人能耗预测与顺路充电调度仿真系统，实现：

 

1. 里程-能耗模型 —— SOC 与行驶距离的映射

2. 动态补偿 —— 坡度、负载、速度对能耗的影响

3. 剩余里程预测 —— 基于当前能耗率的 EKF 滤波

4. 顺路充电决策 —— 不中断现有任务节点的充电规划

5. 可视化 —— 电量、位置、决策事件的时间线

 

三、核心逻辑讲解

 

3.1 理论基础：里程-能耗模型与 EKF

 

本工具基于哈工程《工业过程控制》第五章“状态估计”和第十三章“最优控制”：

 

① 基础能耗模型

 

假设电压平稳，能耗与行驶距离呈近似线性关系：

 

SOC(s) = SOC_0 - k_{base} \cdot s

 

其中 k_{base} 为单位距离能耗系数（%/m）。

 

② 动态补偿模型

 

考虑坡度和负载：

 

k_{actual} = k_{base} \cdot (1 + \alpha \cdot \sin\theta) \cdot (1 + \beta \cdot \Delta m) \cdot (1 + \gamma \cdot \Delta v)

 

其中 \theta 为坡度， \Delta m 为负载变化， \Delta v 为速度变化。

 

③ 扩展卡尔曼滤波（EKF）预测

 

状态向量： x = [SOC, k_{actual}]^T 

 

预测步：

 

\hat{x}_k^- = f(\hat{x}_{k-1})

 

P_k^- = F_k P_{k-1} F_k^T + Q

 

更新步：

 

K_k = P_k^- H_k^T (H_k P_k^- H_k^T + R)^{-1}

 

\hat{x}_k = \hat{x}_k^- + K_k(z_k - h(\hat{x}_k^-))

 

通过 EKF，我们可以实时估计真实的能耗率，并预测剩余可行驶里程：

 

s_{remaining} = \frac{SOC_k}{k_{estimated}}

 

3.2 顺路充电决策树

 

        ┌──────────────────┐

        │ 当前 SOC < 阈值? │ ──NO──▶ 继续巡检

        └────────┬─────────┘

                 YES

                 ▼

        ┌──────────────────┐

        │ 预测能否撑到 │

        │ 下一个任务节点? │

        └────────┬─────────┘

           NO────────┼────────YES

           ▼ ▼ ▼

    ┌───────┐ ┌───────┐ ┌───────┐

    │ 查找 │ │ 评估 │ │ 继续 │

    │ 顺路桩 │─▶│ 充电量 │─▶│ 任务 │

    └───────┘ └───────┘ └───────┘

           │ │

           ▼ ▼

    ┌──────────────────────┐

    │ 规划: 当前→桩→断点续传 │

    └──────────────────────┘

 

关键规则：

 

- 不中断原则：如果机器人正在执行关键工艺节点（如读取表盘），即使低电量也不打断，而是等节点完成后再决策。

- 最小充电原则：充到 SOC_{safe} 即停止，而非充满，最大化在线时间。

 

四、代码讲解（面向对象设计）

 

4.1 类结构总览

 

类名 职责 设计模式

 

"ChargeSpot" 充电桩（dataclass） 值对象

 

"MissionNode" 任务节点（dataclass） 值对象

 

"BatteryState" 电池状态（dataclass） 观察者模式

 

"EnergyModel" 里程-能耗模型 策略模式

 

"EKFEenergyEstimator" 能耗卡尔曼滤波器 策略模式

 

"ChargingPlanner" 顺路充电规划器 模板方法

 

"MissionExecutor" 任务执行器 状态模式

 

"MobileRobot" 机器人（聚合根） 聚合根

 

"VisualizationEngine" 可视化引擎 封装

 

4.2 数据模型层

 

from dataclasses import dataclass, field

from typing import List, Dict, Optional, Tuple, Deque

from collections import deque

import numpy as np

import matplotlib.pyplot as plt

from datetime import datetime

import math

 

@dataclass(frozen=True)

class ChargeSpot:

    """充电桩 —— 值对象"""

    id: str

    position: float # 在路径上的位置 (m)

    power: float = 50.0 # 充电功率 (W)

    efficiency: float = 0.9 # 充电效率

 

@dataclass(frozen=True)

class MissionNode:

    """任务节点 —— 值对象"""

    id: str

    position: float # 路径位置 (m)

    is_critical: bool = False # 关键节点（不可打断）

    process_data: Dict = field(default_factory=dict)

 

@dataclass

class BatteryState:

    """电池状态 —— 观察者模式"""

    soc: float = 100.0 # 剩余电量 (%)

    voltage: float = 24.0 # 电压 (V)

    current: float = 0.0 # 电流 (A)

    temperature: float = 25.0 # 温度 (°C)

    capacity: float = 20.0 # 额定容量 (Ah)

    cycle_count: int = 0 # 循环次数

    observers: List['BatteryObserver'] = field(default_factory=list)

 

    def attach(self, observer: 'BatteryObserver'):

        self.observers.append(observer)

 

    def notify(self):

        for obs in self.observers:

            obs.update(self)

 

@dataclass

class EnergyConsumption:

    """能耗记录"""

    distance: float

    soc_used: float

    time_elapsed: float

    avg_speed: float

    grade: float = 0.0 # 坡度

 

4.3 能耗模型与卡尔曼滤波（策略模式）

 

class EnergyModel:

    """

    里程-能耗模型 —— 策略模式

 

    建立 SOC 与行驶距离的关系，含动态补偿

    """

 

    def __init__(self, base_consumption: float = 0.1):

        self.base_consumption = base_consumption # %/m

        self.alpha = 0.05 # 坡度影响系数

        self.beta = 0.02 # 负载影响系数

        self.gamma = 0.01 # 速度影响系数

 

    def calculate_consumption_rate(self, speed: float = 1.0,

                                  grade: float = 0.0,

                                  load_factor: float = 1.0) -> float:

        """

        计算当前能耗率 (%/m)

 

        Args:

            speed: 当前速度 (m/s)

            grade: 坡度 (弧度)

            load_factor: 负载因子 (1.0 为基准)

        """

        # 基础能耗

        rate = self.base_consumption

 

        # 坡度补偿（上坡多耗电）

        grade_factor = 1.0 + self.alpha * abs(math.sin(grade))

 

        # 负载补偿

        load_comp = 1.0 + self.beta * (load_factor - 1.0)

 

        # 速度补偿（高速风阻/电机损耗）

        speed_factor = 1.0 + self.gamma * abs(speed - 1.0)

 

        return rate * grade_factor * load_comp * speed_factor

 

    def estimate_range(self, soc: float, consumption_rate: float) -> float:

        """估算剩余续航里程"""

        if consumption_rate <= 0:

            return float('inf')

        return soc / consumption_rate

 

 

class EKFEenergyEstimator:

    """

    能耗卡尔曼滤波器 —— 策略模式

 

    实时估计能耗率并预测剩余里程

    """

 

    def __init__(self, energy_model: EnergyModel):

        self.model = energy_model

 

        # 状态：[SOC, 能耗率(%/m)]

        self.state = np.array([[100.0], [0.1]])

        self.P = np.eye(2) * 0.1 # 协方差

        self.Q = np.eye(2) * 0.01 # 过程噪声

        self.R = np.array([[0.5]]) # 观测噪声

 

        self.history: Deque[Tuple[float, float, float]] = deque(maxlen=100)

 

    def predict(self, distance: float, speed: float = 1.0,

                grade: float = 0.0, load: float = 1.0) -> float:

        """

        预测 SOC（不更新状态）

        """

        rate = self.model.calculate_consumption_rate(speed, grade, load)

        predicted_soc = self.state[0, 0] - rate * distance

        return max(0.0, predicted_soc)

 

    def update(self, distance_traveled: float, soc_measured: float,

               speed: float = 1.0, grade: float = 0.0, load: float = 1.0):

        """

        EKF 更新步骤

        """

        dt = distance_traveled

 

        # 1. 预测步

        rate = self.model.calculate_consumption_rate(speed, grade, load)

        F = np.array([

            [1.0, -dt],

            [0.0, 1.0]

        ])

 

        self.state[0, 0] -= rate * dt # SOC 预测

        self.P = F @ self.P @ F.T + self.Q

 

        # 2. 更新步（观测 SOC）

        H = np.array([[1.0, 0.0]])

        z = np.array([[soc_measured]])

        y = z - H @ self.state

        S = H @ self.P @ H.T + self.R

        K = self.P @ H.T @ np.linalg.inv(S)

 

        self.state += K @ y

        self.P = (np.eye(2) - K @ H) @ self.P

 

        # 限制范围

        self.state[0, 0] = max(0.0, min(100.0, self.state[0, 0]))

        self.state[1, 0] = max(0.01, self.state[1, 0])

 

        # 记录历史

        self.history.append((

            datetime.now().timestamp(),

            self.state[0, 0],

            self.state[1, 0]

        ))

 

        return self.state[0, 0], self.state[1, 0]

 

    def get_estimated_range(self) -> float:

        """获取估计剩余里程"""

        soc = self.state[0, 0]

        rate = self.state[1, 0]

        return self.model.estimate_range(soc, rate)

 

    def reset(self):

        """复位滤波器"""

        self.state = np.array([[100.0], [0.1]])

        self.P = np.eye(2) * 0.1

        self.history.clear()

 

4.4 顺路充电规划器（模板方法）

 

class ChargingPlanner:

    """

    顺路充电规划器 —— 模板方法模式

 

    决定何时、何地、充多少电

    """

 

    def __init__(self, charge_spots: List[ChargeSpot]):

        self.charge_spots = sorted(charge_spots, key=lambda x: x.position)

        self.min_safe_soc = 20.0 # 最低安全电量

        self.target_soc = 80.0 # 顺路充电目标

        self.charge_power = 50.0 # W

        self.charge_efficiency = 0.9

 

    def find_nearest_spot(self, current_pos: float,

                          direction: int = 1) -> Optional[ChargeSpot]:

        """查找前进方向上的最近充电桩"""

        candidates = []

        for spot in self.charge_spots:

            if direction > 0: # 向前搜索

                if spot.position >= current_pos:

                    candidates.append(spot)

            else: # 向后搜索

                if spot.position <= current_pos:

                    candidates.append(spot)

 

        if not candidates:

            return None

 

        return min(candidates, key=lambda x: abs(x.position - current_pos))

 

    def plan_detour(self, current_pos: float,

                     target_node: MissionNode,

                     estimator: EKFEenergyEstimator) -> Optional[Dict]:

        """

        规划顺路充电路径

 

        Returns:

            {

                'charge_spot': ChargeSpot,

                'detour_distance': float,

                'charge_time': float,

                'soc_after_charge': float,

                'can_reach_target': bool

            }

        """

        # 1. 评估直接到达目标的可行性

        distance_to_target = abs(target_node.position - current_pos)

        predicted_soc = estimator.predict(distance_to_target)

 

        if predicted_soc >= self.min_safe_soc:

            return None # 可以直接到达，无需充电

 

        # 2. 查找顺路充电桩

        direction = 1 if target_node.position > current_pos else -1

        spot = self.find_nearest_spot(current_pos, direction)

 

        if not spot:

            return None # 无可用充电桩

 

        # 3. 计算绕行代价

        detour_to_spot = abs(spot.position - current_pos)

        detour_from_spot = abs(target_node.position - spot.position)

 

        # 4. 计算所需充电量

        # 充到能安全到达目标 + 余量

        required_soc_at_spot = self.min_safe_soc + \

            estimator.model.calculate_consumption_rate() * detour_from_spot

 

        charge_needed = max(0.0, required_soc_at_spot - estimator.state[0, 0])

        charge_needed = min(charge_needed, self.target_soc - estimator.state[0, 0])

 

        # 5. 估算充电时间 (简化: 假设恒功率)

        if charge_needed <= 0:

            charge_time = 0.0

        else:

            # Wh = Ah * V, 假设24V系统

            battery_wh = estimator.model.capacity * 24.0

            charge_rate = self.charge_power * self.charge_efficiency

            charge_time = (charge_needed / 100.0) * battery_wh / charge_rate

 

        return {

            'charge_spot': spot,

            'detour_distance': detour_to_spot + detour_from_spot - distance_to_target,

            'charge_time': charge_time,

            'soc_after_charge': estimator.state[0, 0] + charge_needed,

            'can_reach_target': True,

            'charge_needed': charge_needed

        }

 

    def execute_charge(self, robot: 'MobileRobot',

                        spot: ChargeSpot,

                        target_soc: float) -> float:

        """

        执行充电过程，返回实际充电时间

        """

        print(f"🔌 开始在 {spot.id} 充电 (当前SOC: {robot.battery.soc:.1f}%)")

 

        start_soc = robot.battery.soc

        charge_rate = 1.0 / 60.0 # 每分钟充1% (简化)

 

        charged = 0.0

        while robot.battery.soc < target_soc and charged < 30.0:

            robot.battery.soc += charge_rate

            robot.battery.soc = min(100.0, robot.battery.soc)

            charged += charge_rate

            yield charged # 允许外部控制循环

 

        actual_charged = robot.battery.soc - start_soc

        print(f"⚡ 充电完成: +{actual_charged:.1f}% (当前SOC: {robot.battery.soc:.1f}%)")

        return actual_charged

 

4.5 任务执行器（状态模式）

 

class MissionExecutor:

    """

    任务执行器 —— 状态模式

 

    管理任务节点的执行与中断恢复

    """

 

    def __init__(self, nodes: List[MissionNode]):

        self.nodes = nodes

        self.current_index = 0

        self.is_interrupted = False

        self.interrupt_reason = None

        self.resume_context = None

 

    def get_current_node(self) -> Optional[MissionNode]:

        if 0 <= self.current_index < len(self.nodes):

            return self.nodes[self.current_index]

        return None

 

    def advance(self):

        """前进到下一个节点"""

        if not self.is_interrupted:

            self.current_index += 1

 

    def interrupt_for_charging(self, charge_plan: Dict):

        """中断任务进行充电"""

        self.is_interrupted = True

        self.interrupt_reason = "LOW_BATTERY"

        self.resume_context = {

            'node_index': self.current_index,

            'charge_spot': charge_plan['charge_spot'],

            'target_soc': charge_plan.get('soc_after_charge', 80.0)

        }

        print(f"⏸️ 任务中断: 节点 {self.current_index} -> 前往充电 {charge_plan['charge_spot'].id}")

 

    def resume(self) -> Optional[MissionNode]:

        """从中断点恢复任务"""

        if self.resume_context:

            self.current_index = self.resume_context['node_index']

            self.is_interrupted = False

            self.interrupt_reason = None

            resume_node = self.get_current_node()

            print(f"▶️ 任务恢复: 从节点 {self.current_index} 继续执行")

            return resume_node

        return None

 

    def is_complete(self) -> bool:

        return self.current_index >= len(self.nodes)

 

4.6 移动机器人（聚合根）

 

class MobileRobot:

    """

    移动机器人 —— 聚合根

 

    协调整个能耗预测与充电调度流程

    """

 

    def __init__(self, name: str = "ROBOT-01"):

        # 核心组件

        self.name = name

        self.battery = BatteryState()

        self.energy_model = EnergyModel(base_consumption=0.08)

        self.estimator = EKFEenergyEstimator(self.energy_model)

 

        # 路径与任务

        self.position = 0.0

        self.speed = 1.0 # m/s

        self.path_length = 500.0 # 总路径长度

 

        # 充电桩网络

        self.charge_spots = [

            ChargeSpot("CHARGE-A", 50.0, 50.0),

            ChargeSpot("CHARGE-B", 150.0, 100.0),

            ChargeSpot("CHARGE-C", 300.0, 50.0),

            ChargeSpot("CHARGE-D", 450.0, 50.0)

        ]

        self.planner = ChargingPlanner(self.charge_spots)

 

        # 任务节点

        self.mission_nodes = [

            MissionNode("START", 0.0, False),

            MissionNode("CHECK-VALVE-01", 80.0, True, {"type": "valve"}),

            MissionNode("CHECK-PRESSURE", 180.0, True, {"type": "pressure"}),

            MissionNode("CHECK-TEMP", 280.0, False, {"type": "temperature"}),

            MissionNode("END", 500.0, False)

        ]

        self.executor = MissionExecutor(self.mission_nodes)

 

        # 状态记录

        self.soc_history: List[Tuple[float, float]] = []

        self.position_history: List[Tuple[float, float]] = []

        self.event_log: List[Dict] = []

        self.charging_history: List[Dict] = []

 

        # 标志位

        self.is_charging = False

        self.charge_plan = None

 

    def drive_to(self, target_pos: float, dt: float = 1.0) -> bool:

        """

        驱动机器人到目标位置

 

        Returns:

            True if reached

        """

        if self.is_charging:

            return False

 

        direction = 1.0 if target_pos > self.position else -1.0

        distance = abs(target_pos - self.position)

        step = self.speed * dt

 

        if step >= distance:

            self.position = target_pos

            return True

 

        # 更新位置

        self.position += direction * step

 

        # 模拟坡度（基于位置的正弦波）

        grade = math.sin(self.position / 100.0) * 0.1

 

        # 更新能耗估计

        self.estimator.update(

            distance_traveled=step,

            soc_measured=self.battery.soc,

            speed=self.speed,

            grade=grade,

            load=1.0

        )

 

        # 更新电池 SOC（简化：直接从估计器同步）

        self.battery.soc = self.estimator.state[0, 0]

 

        # 记录历史

        self.soc_history.append((self.position, self.battery.soc))

        self.position_history.append((self.position, self.battery.soc))

 

        return False

 

    def check_and_plan_charging(self) -> Optional[Dict]:

        """

        检查是否需要充电并返回充电计划

        """

        current_node = self.executor.get_current_node()

        if not current_node:

            return None

 

        # 如果当前在关键节点执行中，不中断

        if current_node.is_critical and self.executor.is_interrupted == False:

            # 检查完成后是否还有足够电量离开

            remaining_after_task = self.estimator.predict(

                abs(self.position - current_node.position) + 10.0

            )

            if remaining_after_task < self.planner.min_safe_soc:

                print(f"⚠️ 关键节点 {current_node.id} 执行后电量不足，提前规划充电")

                # 不中断当前节点，但提前规划

                plan = self.planner.plan_detour(

                    self.position,

                    self.mission_nodes[self.executor.current_index + 1],

                    self.estimator

                )

                if plan:

                    self.charge_plan = plan

                    return plan

        else:

            # 非关键节点或已中断，正常检查

            next_node = self._get_next_node()

            if next_node:

                plan = self.planner.plan_detour(

                    self.position,

                    next_node,

                    self.estimator

                )

                if plan:

                    self.charge_plan = plan

                    return plan

 

        return None

 

    def _get_next_node(self) -> Optional[MissionNode]:

        idx = self.executor.current_index

        if idx + 1 < len(self.mission_nodes):

            return self.mission_nodes[idx + 1]

        return None

 

    def execute_mission(self, duration: float = 600.0, dt: float = 1.0) -> str:

        """

        执行完整任务流程

 

        Returns:

            执行结果摘要

        """

        print("=" * 60)

        print(f" 移动机器人能耗预测与顺路充电仿真 v1.0")

        print(f" 机器人: {self.name}, 初始SOC: {self.battery.soc:.1f}%")

        print(f" 路径总长: {self.path_length}m, 任务节点: {len(self.mission_nodes)}")

        print("=" * 60)

 

        steps = int(duration / dt)

        last_event_time = 0.0

 

        for 

利用AI解决实际问题，如果你觉得这个工具好用，欢迎关注长安牧笛！