“Arduino BLDC 之送餐机器人：滚动窗口拥挤分级 + 动态重规划”可以理解为：送餐机器人在餐厅、酒店走廊等拥挤环境中，不依赖全局地图一次性决定整条路径，而是以机器人当前位置为中心维护一个局部感知窗口，把障碍物按距离、密度、运动趋势和通行代价进行拥挤分级，再据此动态调整速度、绕行方向、等待策略和局部目标点，最后由 BLDC 差速底盘平滑执行。它的典型链路是：传感器扫描 → 局部代价地图/滚动窗口更新 → 拥挤度评估与分级 → 行为/轨迹重规划 → BLDC 速度/力矩闭环 → 安全验证与恢复全局路径。
一、主要特点
全局 SLAM 地图适合判断“从厨房到包间怎么走”，但在行人频繁穿行的走廊里，全局地图更新慢、计算重。滚动窗口只维护机器人前方、侧方和后方一定范围内的局部环境信息，例如激光雷达扇区、ToF 阵列或多路超声/红外距离，计算量小，更适合嵌入式平台高频更新。
更合理的做法是把局部空间划分为多个方向扇区，对每个扇区计算拥挤度：近距离障碍权重高，中距离次之，远距离更低；同时考虑障碍密度、通行宽度、是否持续逼近、是否处于目标方向上。这样机器人能区分“旁边有人经过”和“正前方通道被堵死”，避免把轻微干扰误判为必须停车。
行为级重规划决定“继续前进、减速观察、左/右绕行、原地等待、退回安全点”；轨迹级重规划则在局部窗口内生成可执行的弧线或折线轨迹，例如 DWA 采样线速度和角速度，选择最安全、最贴近全局路径且最平滑的速度指令。行为级负责策略，轨迹级负责执行细节。
送餐机器人需要频繁启停、绕行和低速转弯，若电机响应差，容易出现顿挫、洒汤、托盘晃动。BLDC 配合编码器闭环、FOC 电流环和 S 曲线加减速，可以让速度指令变化更平滑，减少急刹和急转带来的冲击。
滚动窗口重规划只在局部有效，不能长期偏离全局目标。系统应在绕行后持续评估是否回到全局参考路径上；若局部拥堵持续存在，例如走廊长时间被人群占据，应触发更高层策略，如等待、更换通道或重新请求全局路径。
完整实现通常由上位机或高性能 MCU 负责 SLAM、全局路径、拥挤度评估和局部规划，底层 MCU 负责 BLDC 速度闭环、急停保护和传感器采样。8 位 Arduino 更适合做轻量原型或执行层，不建议同时承担激光 SLAM、DWA 采样和多路传感器滤波。
二、应用场景
走廊窄、桌椅多、服务员和顾客频繁穿行。机器人需要在门口、过道交汇处和餐桌间隙判断拥挤程度，选择减速绕行或短暂等待，避免碰撞托盘和顾客腿部。
电梯口、房门前和转角处容易出现短时拥堵。滚动窗口可判断电梯口是否有人排队，动态选择等待或绕行；进入客房走廊后可降低速度、增大安全裕度。
对安全性和安静性要求更高。BLDC 低噪声特性适合病房环境，拥挤分级可避免在病床旁或推车通道强行穿行。
高峰期走廊、楼梯口和茶水间附近拥堵明显。系统可根据局部拥挤度调整通行策略，例如在开阔区保持巡航速度，在狭窄区自动降速。
多台送餐机器人同时运行时，可通过局部拥挤度和对向距离判断是否需要让行，避免在窄通道中对向僵持。更高阶方案可加入优先级协商，例如载热餐机器人优先通行。
三、需要注意的事项
仅靠前方传感器无法判断侧向行人切入。建议前向使用激光雷达、深度相机或多路 ToF，侧向补充超声/红外，底盘下方可增加防撞条或跌落传感器。传感器安装高度应能同时覆盖行人腿部和托盘边缘。
不同餐厅通道宽度、桌椅密度和人流速度差异很大。近/中/远阈值、扇区权重、等待时间和绕行角度都不能照搬示例值，需要在真实环境中根据碰撞风险、通行效率和用户观感逐步调整。
局部规划更新过快会导致速度指令频繁跳变，过慢则避障滞后。建议先保证底层 BLDC 速度环稳定，再逐步提高滚动窗口更新频率。控制回路应使用定时器中断或非阻塞调度，避免 delay 阻塞电机闭环。
如果只按当前最近障碍决定左右绕行，机器人在狭窄通道中可能在两侧障碍之间来回切换方向。应加入方向保持、迟滞判断和最短绕行代价评估，使绕行方向在短时间内保持稳定。
局部避障成功后应尽快回归全局参考路径；若连续多次重规划仍无法接近目标，应判定为“局部被困”，触发等待、后退或请求全局重规划，而不是无限小幅度绕行。
送餐机器人载有液体和餐具，急加速和急转向会明显增加洒漏风险。应设置最大线速度、最大角速度、加速度上限和电流保护；在拥挤分级较高时自动降低速度上限和转弯角速度。
若上位机卡死、SLAM 丢失或串口通信中断，底层应进入安全模式：停止输出速度、保持刹车或低速原地等待，并上报故障状态。安全停机优先级高于路径跟踪。
轻量方案可以用多路距离传感器做扇区拥挤评估和简单绕行决策；如果引入 DWA、局部代价地图或 SLAM，建议把规划放在 ESP32-S3、STM32 或上位机，Arduino/底层 MCU 只接收目标线速度和角速度并执行闭环。
整体来看，这个方案的关键不是“避开每一个障碍”，而是让机器人具备局部拥挤感知能力、分级响应能力和平滑执行能力：轻拥堵时减速观察，中拥堵时择向绕行，重拥堵时等待或重规划，异常时安全降级。做原型时，建议先实现“多扇区距离扫描 → 拥挤度加权 → 四档行为输出 → BLDC 速度限幅执行”的最小闭环，再加入 DWA 轨迹采样和全局路径回归。
1、8方向滚动窗口拥挤分级 + 离散动作决策
此案例建立最核心的分级框架：8 方向传感器以 150ms 周期扫描，计算前方三方向拥堵比，超过阈值则触发转向或等待。
#include <SimpleFOC.h>
BLDCMotor motorL(5);
BLDCMotor motorR(6);
// 8方向传感器（模拟超声波/红外阵列）
const int SENSOR_PINS[8] = {A0, A1, A2, A3, A4, A5, A6, A7};
const int NUM_DIR = 8;
// 拥挤分级阈值：三级距离带
const uint16_t NEAR_DIST = 30; // 近距 0-30cm，权重3
const uint16_t MID_DIST = 80; // 中距 30-80cm，权重2
const uint16_t FAR_DIST = 150; // 远距 80-150cm，权重1
const float BLOCK_RATIO = 0.6; // 前方拥堵比阈值（60%）
struct DirState {
uint16_t dist;
int weight;
};
DirState dirs[8];
enum PathAction { GO_FORWARD, TURN_LEFT, TURN_RIGHT, WAIT };
PathAction action = GO_FORWARD;
unsigned long lastScan = 0;
const unsigned long SCAN_INTERVAL = 150; // 150ms 滚动周期
void setup() {
Serial.begin(115200);
for (int i = 0; i < 8; i++) pinMode(SENSOR_PINS[i], INPUT);
motorL.init(); motorR.init();
motorL.initFOC(); motorR.initFOC();
motorL.controller = MotionControlType::velocity;
motorR.controller = MotionControlType::velocity;
Serial.println("滚动窗口拥挤分级就绪");
}
void loop() {
motorL.loopFOC();
motorR.loopFOC();
if (millis() – lastScan > SCAN_INTERVAL) {
scanWindow();
computeCrowdLevel();
executeAction();
lastScan = millis();
}
delay(10);
}
uint16_t readDistance(int idx) {
return analogRead(SENSOR_PINS[idx]) / 4; // 简化映射
}
void scanWindow() {
for (int i = 0; i < NUM_DIR; i++) {
dirs[i].dist = readDistance(i);
if (dirs[i].dist < NEAR_DIST) dirs[i].weight = 3;
else if (dirs[i].dist < MID_DIST) dirs[i].weight = 2;
else if (dirs[i].dist < FAR_DIST) dirs[i].weight = 1;
else dirs[i].weight = 0;
}
}
// 前方三方向（索引 0,1,7 对应 0°/45°/315°）拥堵比计算
void computeCrowdLevel() {
int frontWeight = dirs[0].weight + dirs[1].weight + dirs[7].weight;
float frontRatio = (float)frontWeight / 9.0f; // 最大 3×3=9
if (frontRatio > BLOCK_RATIO) {
// 前方拥堵，检查左右侧净空
int leftWeight = dirs[5].weight + dirs[6].weight;
int rightWeight = dirs[2].weight + dirs[3].weight;
if (leftWeight < rightWeight && leftWeight <= 2) action = TURN_LEFT;
else if (rightWeight < leftWeight && rightWeight <= 2) action = TURN_RIGHT;
else action = WAIT; // 两侧均拥堵，等待
} else {
action = GO_FORWARD;
}
}
void executeAction() {
const float CRUISE = 1.5f;
const float TURN_SPEED = 0.8f;
switch (action) {
case GO_FORWARD:
motorL.move(CRUISE); motorR.move(CRUISE);
break;
case TURN_LEFT:
motorL.move(CRUISE * 0.3f); motorR.move(CRUISE);
break;
case TURN_RIGHT:
motorL.move(CRUISE); motorR.move(CRUISE * 0.3f);
break;
case WAIT:
motorL.move(0); motorR.move(0);
break;
}
}
要点：分级不是简单的“近中远”标签，而是加权求和——近距权重 3，中距 2，远距 1。前方拥堵比超过 0.6 时触发转向，而非一检测到障碍就停。这种“滚动窗口 + 分级决策”避免了连续避障的振荡问题。
2、拥挤分级 + 等待/绕行/直行的三态调度
此案例在案例一基础上引入时间维度的等待策略：当两侧均拥堵时，不立即永久等待，而是计时后重新评估；若持续拥堵超过阈值，则触发更激进的绕行。
#include <SimpleFOC.h>
BLDCMotor motorL(5), motorR(6);
// 传感器（前、左、右三方向简化）
const int PIN_FRONT = A0, PIN_LEFT = A1, PIN_RIGHT = A2;
// 分级阈值
const uint16_t NEAR = 30, MID = 80, FAR = 150;
// 状态机
enum TrafficState { CLEAR, MODERATE, CONGESTED, WAITING, DETOUR };
TrafficState state = CLEAR;
// 等待计时
unsigned long waitStart = 0;
const unsigned long MAX_WAIT = 3000; // 最长等待 3 秒
const unsigned long RE_EVAL_MS = 500; // 等待期间每 500ms 重评估
unsigned long lastEval = 0;
void setup() {
Serial.begin(115200);
pinMode(PIN_FRONT, INPUT); pinMode(PIN_LEFT, INPUT); pinMode(PIN_RIGHT, INPUT);
motorL.init(); motorR.init();
motorL.initFOC(); motorR.initFOC();
motorL.controller = MotionControlType::velocity;
motorR.controller = MotionControlType::velocity;
}
void loop() {
motorL.loopFOC(); motorR.loopFOC();
if (millis() – lastEval > 150) {
evaluateTraffic();
lastEval = millis();
}
executeTraffic();
delay(10);
}
void evaluateTraffic() {
uint16_t dF = analogRead(PIN_FRONT) / 4;
uint16_t dL = analogRead(PIN_LEFT) / 4;
uint16_t dR = analogRead(PIN_RIGHT) / 4;
// 前方分级
int frontLevel = (dF < NEAR) ? 3 : (dF < MID) ? 2 : (dF < FAR) ? 1 : 0;
int leftLevel = (dL < NEAR) ? 3 : (dL < MID) ? 2 : (dL < FAR) ? 1 : 0;
int rightLevel = (dR < NEAR) ? 3 : (dR < MID) ? 2 : (dR < FAR) ? 1 : 0;
if (frontLevel == 0) {
state = CLEAR;
waitStart = 0;
} else if (frontLevel == 1) {
state = MODERATE; // 减速通过
} else {
// 前方中距或近距有障碍
if (leftLevel <= 1 && leftLevel < rightLevel) {
state = DETOUR; // 左侧更空，绕行
} else if (rightLevel <= 1 && rightLevel < leftLevel) {
state = DETOUR; // 右侧更空，绕行（需区分方向，此处简化）
} else {
// 两侧均拥堵，进入等待
if (state != WAITING) {
state = WAITING;
waitStart = millis();
} else if (millis() – waitStart > MAX_WAIT) {
// 等待超时，尝试强制绕行
state = DETOUR;
}
}
}
}
void executeTraffic() {
const float CRUISE = 1.5f;
switch (state) {
case CLEAR:
motorL.move(CRUISE); motorR.move(CRUISE);
break;
case MODERATE:
motorL.move(CRUISE * 0.6f); motorR.move(CRUISE * 0.6f);
break;
case DETOUR:
// 简化：向左绕行
motorL.move(CRUISE * 0.3f); motorR.move(CRUISE);
break;
case WAITING:
motorL.move(0); motorR.move(0);
break;
}
}
要点：等待不是无限期的。MAX_WAIT 超时后强制绕行，避免机器人在拥堵点“卡死”。等待期间每 500ms 重新评估窗口，一旦前方清空立即恢复直行。这是时间维度上的动态重规划——决策不仅取决于当前空间状态，还取决于拥堵持续时间。
3、滚动窗口 + 全局目标引导的混合决策
此案例将滚动窗口的局部决策与全局目标方向融合：当局部窗口内无拥堵时，优先朝全局目标方向前进；当拥堵时，绕行方向的选择也参考全局目标，避免“绕反方向”。
#include <SimpleFOC.h>
BLDCMotor motorL(5), motorR(6);
// 8方向传感器
const int SENSOR_PINS[8] = {A0, A1, A2, A3, A4, A5, A6, A7};
// 全局目标方向（来自上位机或全局路径）
float globalTargetAngle = 0.0f; // 弧度，0 = 正前方
float currentHeading = 0.0f; // 当前航向（来自IMU或里程计）
// 分级参数
const uint16_t NEAR = 30, MID = 80, FAR = 150;
const float BLOCK_RATIO = 0.6;
struct DirInfo {
uint16_t dist;
int weight;
float angle; // 该方向相对机器人的角度
};
DirInfo dirs[8];
// 动作
enum Action { FWD, LEFT, RIGHT, WAIT };
Action action = FWD;
void setup() {
Serial.begin(115200);
for (int i = 0; i < 8; i++) pinMode(SENSOR_PINS[i], INPUT);
motorL.init(); motorR.init();
motorL.initFOC(); motorR.initFOC();
motorL.controller = MotionControlType::velocity;
motorR.controller = MotionControlType::velocity;
// 预计算方向角度（0°在前，顺时针）
for (int i = 0; i < 8; i++) {
dirs[i].angle = i * 45.0f * PI / 180.0f;
}
}
void loop() {
motorL.loopFOC(); motorR.loopFOC();
static unsigned long lastScan = 0;
if (millis() – lastScan > 150) {
scanAndDecide();
lastScan = millis();
}
delay(10);
}
void scanAndDecide() {
// 1. 扫描分级
for (int i = 0; i < 8; i++) {
uint16_t d = analogRead(SENSOR_PINS[i]) / 4;
dirs[i].dist = d;
if (d < NEAR) dirs[i].weight = 3;
else if (d < MID) dirs[i].weight = 2;
else if (d < FAR) dirs[i].weight = 1;
else dirs[i].weight = 0;
}
// 2. 前方拥堵比
int frontW = dirs[0].weight + dirs[1].weight + dirs[7].weight;
float frontRatio = frontW / 9.0f;
if (frontRatio <= BLOCK_RATIO) {
// 前方通畅：检查是否朝向全局目标
float targetDiff = globalTargetAngle – currentHeading;
while (targetDiff > PI) targetDiff -= 2*PI;
while (targetDiff < –PI) targetDiff += 2*PI;
if (fabs(targetDiff) < PI / 6) { // 偏差 < 30°，直行
action = FWD;
} else if (targetDiff > 0) {
action = RIGHT; // 目标在右，右转
} else {
action = LEFT;
}
} else {
// 前方拥堵：选择绕行方向，但需考虑全局目标
int leftW = dirs[5].weight + dirs[6].weight;
int rightW = dirs[2].weight + dirs[3].weight;
// 全局目标在左且左侧可行 → 左绕
float targetDir = globalTargetAngle – currentHeading;
bool targetLeft = (targetDir > 0 && targetDir < PI) || (targetDir < –PI);
if (leftW <= 2 && (leftW < rightW || targetLeft)) {
action = LEFT;
} else if (rightW <= 2) {
action = RIGHT;
} else {
action = WAIT;
}
}
execute();
}
void execute() {
const float CRUISE = 1.5f;
switch (action) {
case FWD:
motorL.move(CRUISE); motorR.move(CRUISE);
break;
case LEFT:
motorL.move(CRUISE * 0.3f); motorR.move(CRUISE);
break;
case RIGHT:
motorL.move(CRUISE); motorR.move(CRUISE * 0.3f);
break;
case WAIT:
motorL.move(0); motorR.move(0);
break;
}
}
要点：滚动窗口的局部决策必须受全局目标约束。否则机器人可能在拥堵中“绕反方向”，越绕越远。targetLeft 判断确保绕行方向与全局目标方向一致，避免局部避障破坏全局效率。这是“局部响应 + 全局引导”分层架构的核心。
要点解读
拥挤分级是“加权求和”，不是“取最危险值”
简单的“检测到近距障碍就停”会导致机器人频繁急停，餐品泼洒。分级加权（近距 3、中距 2、远距 1）后计算拥堵比，允许机器人在部分方向有障碍但整体仍可通过时继续前进。前方拥堵比 0.6 的阈值是经验值，需根据餐厅过道宽度和机器人尺寸标定。
滚动窗口的周期决定响应速度与计算负载的平衡
150ms 周期意味着每秒约 6~7 次决策更新。太短（如 50ms）会增加 CPU 负载且引入传感器噪声抖动；太长（如 500ms）会导致对行人移动响应滞后。150~200ms 是餐厅场景的合理区间。决策周期与 FOC 控制周期（1kHz）分离，是架构设计的必要前提。
等待策略必须有超时机制，否则机器人会“卡死”
纯反应式避障在两侧均拥堵时只能永久等待。案例二的 MAX_WAIT 超时后强制绕行，打破僵局。实际餐厅中，等待超过 3 秒意味着该通道可能被长时间占用，绕行或请求人工介入比继续等待更合理。
局部绕行方向必须与全局目标对齐
案例三的 targetLeft 判断是防止“绕反”的关键。如果全局目标在左侧，即使右侧局部净空更大，也应优先左绕或等待。局部避障不能以牺牲全局效率为代价，这是分层导航架构的核心原则。
BLDC 的 FOC 柔顺执行是“分级决策不洒汤”的物理保障
分级决策输出的是阶跃式的速度切换（直行→转向→等待），如果执行层用开环 ESC，速度突变会导致餐品惯性晃动。SimpleFOC 的电流环配合适当的加速度限制（案例中通过 CRUISE * 0.3f 的转向速度差实现），能让速度过渡“软着陆”。决策可以离散，执行必须连续。
4、滚动窗口拥挤分级 + 分级调速与侧向避让
适用硬件：ESP32 / Arduino + SimpleFOC BLDC（或 ESC）+ 前方超声波/红外阵列（左、前、右三路）。
核心思路：用"滑动窗口内近距采样比例"而非单次读数来定拥挤等级，抗瞬时抖动；等级映射到速度与避让策略，拥堵时按窗口内左右空档动态重选侧向偏移。
/*
* 案例4：滚动窗口拥挤分级 + 分级调速 / 侧向动态避让
* 窗口：10 个采样（每 50ms 一个，共 500ms）
*/
#include <SimpleFOC.h>
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 8);
Encoder encoder = Encoder(2, 3, 2048);
void onEncoder() { encoder.handleA(); }
// 三路超声波：(左, 前, 右) 的 trig/echo
const int usPin[3][2] = {{6,7},{8,9},{10,11}};
#define WIN 10
#define THRESH_NEAR 35.0f // 认为"有人挡路"的距离(cm)
float window[3][WIN]; // 每路一个滚动窗口
int wi = 0; // 当前写入位置
float readUS(int idx) {
digitalWrite(usPin[idx][0], LOW); delayMicroseconds(2);
digitalWrite(usPin[idx][0], HIGH); delayMicroseconds(10);
digitalWrite(usPin[idx][0], LOW);
long d = pulseIn(usPin[idx][1], HIGH);
return (d > 0) ? d * 0.034f / 2.0f : 200.0f;
}
// 拥挤分级：窗口内"近距采样占比" -> 0~3 级
int gradeLevel(int idx) {
int near = 0;
for (int i = 0; i < WIN; i++) if (window[idx][i] < THRESH_NEAR) near++;
float ratio = (float)near / WIN;
if (ratio > 0.7f) return 3; // 极高：持续拥堵
if (ratio > 0.4f) return 2; // 高
if (ratio > 0.15f) return 1; // 中
return 0; // 畅通
}
void setup() {
encoder.init(); encoder.enableInterrupts(onEncoder);
motor.linkSensor(&encoder);
motor.controller = MotionControlType::velocity;
motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 2.0f;
motor.voltage_limit = 12.0f;
motor.init();
driver.voltage_power_supply = 12.0f; driver.init();
motor.linkDriver(&driver);
for (int i = 0; i < 3; i++) pinMode(usPin[i][0], OUTPUT);
for (int i = 0; i < 3; i++) for (int j = 0; j < WIN; j++) window[i][j] = 200;
Serial.begin(115200);
}
void loop() {
// —- 感知：三路入窗口，只更新一个槽位 —-
for (int i = 0; i < 3; i++) window[i][wi] = readUS(i);
wi = (wi + 1) % WIN;
// —- 分级：左/前/右各取窗口等级 —-
int gF = gradeLevel(0), gL = gradeLevel(1), gR = gradeLevel(2);
// —- 决策1：拥挤等级 -> 目标速度（分级调速）—-
float speed = 3.0f; // 畅通全速
if (gF == 1) speed = 1.8f; // 中：明显减速
else if (gF == 2) speed = 0.8f; // 高：龟速试探
else if (gF == 3) speed = 0; // 极高：原地等待
// 左右也拥堵则直接停（走廊两侧都有人）
if (gL >= 2 && gR >= 2 && speed > 0) speed = 0;
// —- 决策2：动态重规划（侧向），用窗口均值比单次读数稳定 —-
float dL = 0, dR = 0;
for (int i = 0; i < WIN; i++) { dL += window[1][i]; dR += window[2][i]; }
dL /= WIN; dR /= WIN;
float steer = 0;
if (gF >= 2) { // 前向拥堵：往更空的一侧让
steer = (dL > dR) ? 0.6f : –0.6f;
if (fabs(dL – dR) < 10) steer = 0; // 两侧差不多：不偏，原地降速
}
// —- 执行：速度环，方向盘式映射到期望速度 —-
motor.move(speed); // 简化示例：真实差速车用左右轮差分
Serial.print("grade F/L/R="); Serial.print(gF); Serial.print("/");
Serial.print(gL); Serial.print("/"); Serial.print(gR);
Serial.print(" steer="); Serial.println(steer);
delay(50); // 窗口槽位周期 50ms
}
工程提示：pulseIn 最长可阻塞 30ms 以上，量产应将电流环挂 1kHz 定时器中断；本例为可读性保留主循环写法。
5、FreeRTOS 三层分离 + 窗口分级驱动的动态重规划（备选路线）
适用硬件：ESP32（双核）+ SimpleFOC BLDC 差速。
核心思路：感知 Task 维护滚动窗口；规划 Task 按"拥挤等级 + 等级持续时间"从路线表里动态切换（原路线→降速→备选路线→原地等待）；执行 Task 只跟目标速度。重规划是"分级 × 计时"驱动的，而不是每帧都重算——省算力且稳定。
/*
* 案例5：FreeRTOS 感知-规划-执行 + 滚动窗口分级 + 路线表动态重规划
*/
#include <SimpleFOC.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
BLDCMotor motorL = BLDCMotor(7), motorR = BLDCMotor(7);
BLDCDriver3PWM drvL(9,10,11,8), drvR(22,23,24,25);
Encoder encL(2,3,2048), encR(32,33,2048);
void onEncL() { encL.handleA(); } void onEncR() { encR.handleA(); }
SemaphoreHandle_t mux;
volatile int s_grade = 0; // 感知输出：0~3
volatile int s_gradeHold = 0; // 等级持续采样数（重规划判据）
volatile float c_speed = 0; // 规划输出：目标线速度
// 全局路线表：3 条送餐路线，各自 5 个路点(方向°, 时长ms) —— 示意
const int ROUTES[3][5][2] = {
{{0,3000},{45,2000},{0,4000},{–45,2000},{0,3000}}, // 路线0
{{0,2000},{90,1500},{0,3000},{–90,1500},{0,2500}}, // 路线1(备选)
{{0,1500},{0,6000},{–90,2000},{0,3000},{0,2000}}, // 路线2(备选)
};
int route = 0; // 当前路线索引
// —- 感知：滚动窗口分级（单任务内维护）—-
void taskSense(void*) {
TickType_t last = xTaskGetTickCount();
static float win[10]; static int i = 0;
for (;;) {
float d = readFrontUS(); // 前方人群距离
win[i] = d; i = (i + 1) % 10;
int near = 0;
for (int k = 0; k < 10; k++) if (win[k] < 35.0f) near++;
int g = (near > 7) ? 3 : (near > 4) ? 2 : (near > 1) ? 1 : 0;
xSemaphoreTake(mux, portMAX_DELAY);
s_grade = g;
s_gradeHold = (s_gradeHold == g) ? s_gradeHold + 1 : 1; // 等级持续时间
xSemaphoreGive(mux);
vTaskDelayUntil(&last, pdMS_TO_TICKS(100)); // 100ms/采样
}
}
// —- 规划：按等级 + 持续时间做动态重规划 —-
void taskPlanner(void*) {
TickType_t last = xTaskGetTickCount();
int prevRoute = 0;
for (;;) {
int g, hold;
xSemaphoreTake(mux, portMAX_DELAY);
g = s_grade; hold = s_gradeHold;
xSemaphoreGive(mux);
float sp = 3.0f;
if (g == 1) sp = 1.8f;
else if (g == 2) sp = 0.8f;
else if (g == 3) sp = 0;
// 动态重规划：高等级持续 >30 次采样(3s) -> 切备选路线
if (g >= 2 && hold > 30) {
route = (route + 1) % 3; // 绕行到备选路线
s_gradeHold = 0; // 防反复横跳（重置计时）
Serial.print("REPLAN -> route "); Serial.println(route);
}
xSemaphoreTake(mux, portMAX_DELAY);
c_speed = sp;
xSemaphoreGive(mux);
vTaskDelayUntil(&last, pdMS_TO_TICKS(50));
}
}
// —- 执行：BLDC 速度环（双轮差速）—-
void taskExec(void*) {
TickType_t last = xTaskGetTickCount();
for (;;) {
float sp;
xSemaphoreTake(mux, portMAX_DELAY);
sp = c_speed;
xSemaphoreGive(mux);
motorL.loopFOC(); motorR.loopFOC();
motorL.move(sp); motorR.move(sp); // 简化：转向由路点表另行处理
vTaskDelayUntil(&last, pdMS_TO_TICKS(1));
}
}
void setup() {
encL.init(); encL.enableInterrupts(onEncL);
encR.init(); encR.enableInterrupts(onEncR);
motorL.linkSensor(&encL); motorR.linkSensor(&encR);
motorL.controller = motorR.controller = MotionControlType::velocity;
motorL.PID_velocity.P = 0.2f; motorL.PID_velocity.I = 2.0f;
motorR.PID_velocity.P = 0.2f; motorR.PID_velocity.I = 2.0f;
motorL.voltage_limit = motorR.voltage_limit = 12.0f;
motorL.init(); motorR.init();
drvL.voltage_power_supply = drvR.voltage_power_supply = 12.0f;
drvL.init(); drvR.init();
motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
mux = xSemaphoreCreateMutex();
xTaskCreatePinnedToCore(taskSense, "sense", 4096, NULL, 3, NULL, 0);
xTaskCreatePinnedToCore(taskPlanner, "planner", 4096, NULL, 2, NULL, 1);
xTaskCreatePinnedToCore(taskExec, "exec", 4096, NULL, 3, NULL, 1);
}
void loop() { vTaskDelay(portMAX_DELAY); }
6、滚动窗口栅格代价地图 + 局部 A* 动态重规划
适用硬件：ESP32 + SimpleFOC BLDC 差速 + 前向激光/超声阵列（如 5 路）或轮式里程计。
核心思路：滚动窗口不只在时间轴上平滑，更把"每个方位格子的占用频次"累计成局部代价栅格（占用频次/窗口内采样数 = 拥挤概率，直接当通行代价）；以车体前方 3×5 栅格为地图，代价超过阈值即触发局部 A* 重规划，选出代价最小的下一步方向。
/*
* 案例6：滚动窗口拥挤概率栅格 + 局部A*动态重规划
* 栅格：3列 x 5行，车前方；每格存"窗口内被占用比例"作为通行代价
*/
#include <SimpleFOC.h>
#include <vector>
BLDCMotor motorL = BLDCMotor(7), motorR = BLDCMotor(7);
BLDCDriver3PWM drvL(9,10,11,8), drvR(22,23,24,25);
Encoder encL(2,3,2048), encR(32,33,2048);
void onEncL() { encL.handleA(); } void onEncR() { encR.handleA(); }
#define COLS 3
#define ROWS 5
#define WIN 12
float occ[COLS][ROWS]; // 拥挤概率(0~1)：占用采样数/窗口
int count[COLS][ROWS]; // 窗口内累计占用次数
int winIdx = 0;
// 5 路测距数组：模拟把前方扇区分成 3 列（左/中/右各取多路平均）
float readSector(int col) {
static const int pin[3][2] = {{6,7},{8,9},{10,11}}; // 左中右
digitalWrite(pin[col][0], LOW); delayMicroseconds(2);
digitalWrite(pin[col][0], HIGH); delayMicroseconds(10);
digitalWrite(pin[col][0], LOW);
long d = pulseIn(pin[col][1], HIGH);
return (d > 0) ? d * 0.034f / 2.0f : 200.0f;
}
// 每 100ms 滚动一个槽位：距离近(<40cm)则该格占用计数+1
void updateWindow() {
float front[3];
for (int c = 0; c < COLS; c++) front[c] = readSector(c);
// 新槽位写入第0行（最近处），旧数据向远处推一格后滑出窗口
for (int r = ROWS – 1; r > 0; r—)
for (int c = 0; c < COLS; c++) occ[c][r] = occ[c][r–1];
for (int c = 0; c < COLS; c++) {
float d = front[c];
// 按距离把占用落到不同行（越近行越靠前）
int r = (d < 20) ? 0 : (d < 35) ? 1 : (d < 60) ? 2 : 3;
occ[c][0] = (d < 60) ? 0.9f : 0.0f; // 简化：近距=高占用
}
winIdx = (winIdx + 1) % WIN;
}
// —- 局部A*：3×5 栅格，代价 = 拥挤概率 + 转向代价 + 距离 —-
struct Node { int x, y; float g, f; };
float astarNextDirection() {
// 起点(1,4)车头，终点(1,0)正前方远处；4方向移动
const float INF = 1e9;
float dist[COLS][ROWS]; bool closed[COLS][ROWS];
Node prev[COLS][ROWS];
for (int c = 0; c < COLS; c++) for (int r = 0; r < ROWS; r++) {
dist[c][r] = INF; closed[c][r] = false;
}
auto h = [](int x, int y) { return (float)(abs(x – 1) + y); }; // 曼哈顿启发
std::vector<Node> open = {{1, ROWS–1, 0, h(1, ROWS–1)}};
dist[1][ROWS–1] = 0;
while (!open.empty()) {
// 取 f 最小
int bi = 0;
for (int i = 1; i < (int)open.size(); i++) if (open[i].f < open[bi].f) bi = i;
Node n = open[bi]; open.erase(open.begin() + bi);
if (closed[n.x][n.y]) continue;
closed[n.x][n.y] = true;
if (n.x == 1 && n.y == 0) break; // 到达目标
int dx[4] = {0,0,–1,1
```cpp
int dx[4] = {0,0,–1,1}, dy[4] = {–1,1,0,0};
for (int k = 0; k < 4; k++) {
int nx = n.x + dx[k], ny = n.y + dy[k];
if (nx < 0 || nx >= COLS || ny < 0 || ny >= ROWS) continue;
if (closed[nx][ny]) continue;
// 代价 = 拥挤概率 + 转向惩罚（换列+0.3）+ 单位距离
float cost = occ[nx][ny] * 2.0f
+ (nx != n.x ? 0.3f : 0.0f)
+ 1.0f;
float nd = n.g + cost;
if (nd < dist[nx][ny]) {
dist[nx][ny] = nd;
prev[nx][ny] = n;
open.push_back({nx, ny, nd, nd + h(nx, ny)});
}
}
}
// 回溯第一步方向：从目标往前找起点后一格
Node cur{1, 0, 0, 0};
if (dist[1][0] == INF) return 0; // 无路可走：原地停
while (!(prev[cur.x][cur.y].x == 1 && prev[cur.x][cur.y].y == ROWS–1)) {
Node p = prev[cur.x][cur.y];
if (p.x == 0 && p.y == 0) break; // 防御
cur = p;
}
return (cur.x < 1) ? –0.6f : (cur.x > 1) ? 0.6f : 0.0f; // 转向方向
}
void setup() {
encL.init(); encL.enableInterrupts(onEncL);
encR.init(); encR.enableInterrupts(onEncR);
motorL.linkSensor(&encL); motorR.linkSensor(&encR);
motorL.controller = motorR.controller = MotionControlType::velocity;
motorL.PID_velocity.P = 0.2f; motorL.PID_velocity.I = 2.0f;
motorR.PID_velocity.P = 0.2f; motorR.PID_velocity.I = 2.0f;
motorL.voltage_limit = motorR.voltage_limit = 12.0f;
motorL.init(); motorR.init();
drvL.voltage_power_supply = drvR.voltage_power_supply = 12.0f;
drvL.init(); drvR.init();
motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
for (int c = 0; c < COLS; c++) for (int r = 0; r < ROWS; r++) occ[c][r] = 0;
Serial.begin(115200);
}
void loop() {
updateWindow(); // 滚动窗口：每 100ms 一槽
float steer = astarNextDirection();// 动态重规划：A* 找代价最小方向
float speed = 2.5f;
// 前方三列平均占用高 -> 减速；中列近距 -> 接近停
float occFront = (occ[0][0]+occ[1][0]+occ[2][0]) / 3.0f;
if (occFront > 0.6f) speed = 0.6f;
if (occ[1][0] > 0.8f && occ[0][0] > 0.5f && occ[2][0] > 0.5f) speed = 0;
motorL.move(speed + steer); // 差速转向：执行层只消费目标
motorR.move(speed – steer);
Serial.print("occF="); Serial.print(occFront, 2);
Serial.print(" steer="); Serial.println(steer);
delay(100);
}
要点解读
一、滚动窗口的本质是把"瞬时读数"变成"时间维度上的置信度"
单次超声波读到 30cm 可能是行人一闪而过，也可能是传感器噪声。滚动窗口（案例一 10 槽、案例二 10 槽、案例三 12 槽）统计的是"近距占比"，本质是一个低通滤波 + 占用频率统计：只有持续出现的障碍才升级拥挤等级。这直接消灭了抖动导致的频繁变速和原地抽搐，是送餐车在人群中"稳"的关键。
二、拥挤分级必须"非对称"：升级快、降级慢
人流是突发性的，拥堵出现往往一瞬间，散去却要好几秒。所以等级上升（1→2→3）应该用较短窗口/较低阈值快速响应，而等级下降要引入保持时间（案例二的 s_gradeHold 计数就是典型做法），防止"刚减速又立刻加速"的振荡。真正的工程里还会给降级加一个延时器（如持续畅通 2s 才升速），让乘客体感平顺。
三、动态重规划要"分级 × 计时"触发，而不是每帧重算
MCU 算力有限，A* 在 3×5 栅格上虽便宜，但每 100ms 重算一次仍属浪费，且可能来回横跳。更稳的触发条件是"等级 ≥2 且持续超过 N 秒"才切换路线（案例二）或"栅格代价超阈值"才跑 A*（案例三）。重规划输出的是"下一步方向"而非全局路径，这正是"局部滚动"的意义——全局路径由调度层维护（送哪桌、走哪条主路线），局部重规划只负责当前 1~2 秒的微调，两层都不越权。
四、执行层只消费"一个目标"，重规划的复杂逻辑绝不下沉
无论分级调速还是 A* 转向，最终落到电机上的只有一个量：目标速度/差速（案例一 speed、案例三 speed±steer）。实时控制环（电流环 1kHz、速度环数百 Hz）里不允许出现窗口统计、栅格计算、路线切换这些逻辑。这样规划层无论多复杂、甚至跑飞，执行层都能维持一个确定、可证的安全行为（限速、限力矩、急停）。这也是本主题与"实时控制分离"一脉相承的核心理念。
五、失效降级与"无路可走"要有明确出口
人群送餐场景必须回答两个问题：传感器全堵了怎么办？四面八方都拥堵怎么办？对应策略：单路超声超时 → 按窗口内其他槽位/其他方向补值（案例一 readUS 超时返回 200）；A* 找不到通路（dist==INF）→ 原地停车并上报调度层（案例三），而不是硬闯或反复横跳。送餐机器人可以迟到，但不能撞人——"安全停车"永远是重规划的最后兜底选项，优先级高于任何任务完成度。
请注意：以上案例仅作为思路拓展的参考示例，不保证完全正确、适配所有场景或可直接编译运行。由于硬件平台、实际使用场景、Arduino 版本的差异，均可能影响代码的适配性与使用方法的选择。在实际编程开发时，请务必根据自身硬件配置、使用场景及具体功能需求进行针对性调整，并通过多次实测验证效果；同时需确保硬件接线正确，充分了解所用传感器、执行器等设备的技术规范与核心特性。对于涉及硬件操作的代码，使用前务必核对引脚定义、电平参数等关键信息的准确性与安全性，避免因参数错误导致硬件损坏或运行异常。
