【花雕学编程】Arduino BLDC 之感知-规划-平滑：BLDC工业巡检机器人的轻量级避障闭环
·
“Arduino BLDC 之感知-规划-平滑：BLDC 工业巡检机器人的轻量级避障闭环”可以理解为：上层以低成本传感器持续感知前方环境，中层用轻量局部规划或规则化避障策略生成可达的速度/角速度或航点指令，下层由 BLDC 差速底盘的 FOC 电流环、速度环和航向环柔顺执行；同时通过里程计、IMU 和传感器反馈不断修正轨迹，使机器人在走廊、车间、管廊等工业场景中能够低延迟地绕行障碍、平滑回归路径，并避免急停、抖动和跑偏。
一、主要特点
- 闭环链路短，适合资源受限平台
完整链路是：传感器感知障碍物距离/角度 → 局部代价评估或规则判断 → 生成线速度/角速度或绕行航点 → 差速解算为左右轮目标 → BLDC FOC 执行 → 编码器/IMU 反馈修正。 在 Arduino 级别平台上，通常不运行完整 SLAM，而是采用“全局航点 + 局部避障”的轻量结构。 - 感知层强调“远-中-近”覆盖和盲区管理
常用组合包括前向 ToF、超声波、红外、低成本 2D 激光雷达、IMU 和编码器。 超声波和 ToF 负责近距快速响应，激光雷达提供更高角度分辨率，IMU 补偿差速底盘的航向漂移。需要注意玻璃、金属反光、低矮台阶和管道壁等环境可能造成漏检或误检。 - 规划层以轻量局部避障为主
工业巡检机器人常用 A*、DWA、VFH、人工势场法或滚动窗口重规划。 在 Arduino 上更现实的做法是：
使用有限状态机切换“巡航、探测、绕障、回归”；
用滚动窗口或局部代价评估选择左转/右转/减速/等待；
把复杂全局规划交给上位机，Arduino 只执行局部指令。 - 平滑层是工业巡检体验的关键
避障不能只追求“躲开”，还要避免急转、急停和路径振荡。速度、角速度和加速度应设置斜坡限制；避障结束回归原路径时，航向和速度应渐变衔接；差速底盘的左右轮指令也要限幅，防止单侧突然加速。 - BLDC FOC 提供柔顺执行能力
相比开环 ESC 或有刷电机，BLDC FOC 能在低速巡检、频繁加减速和小角度转向时保持较低转矩脉动。 这对走廊巡检、狭窄通道绕行、贴边检测和频繁启停场景非常重要。电流环稳定后，速度环和航向环才能准确跟踪避障指令。 - 定位与航向补偿不能缺失
差速底盘容易因轮径误差、地面摩擦不均和轻微打滑产生航向漂移。轻量方案通常用编码器里程计 + IMU 互补滤波来维持短时航向一致性；若长时间无外部校正，避障后的路径回归会逐渐变差。
二、应用场景 - 车间/仓库巡检 AGV
沿固定通道巡检设备、货架或安全标识，遇到临时堆放物、叉车或人员时减速绕行，随后平滑回归巡检线。 - 配电房/变电站巡检机器人
在机柜、电缆沟和狭窄通道间运行，需要低噪声、低抖动和稳定航向。轻量避障闭环可应对临时障碍物和通道变化。 - 管廊/管道巡检平台
空间狭窄、光照弱、反射面多，适合用超声波/ToF/磁导航补充激光雷达。BLDC 低速平稳性有利于携带气体、温度或超声检测设备。 - 园区/厂区安防巡逻机器人
在走廊、装卸区、停车场等半动态环境中运行，需要持续检测行人、车辆和临时障碍。 - 教育与科研原型平台
适合验证 DWA/VFH/APF 简化版、滚动窗口避障、差速航向控制、FOC 速度跟踪和传感器融合。 - 轻量物流/送物机器人
在酒店、医院或办公楼场景中，避障平滑性直接影响乘坐感、货物稳定性和行人安全感。
三、需要注意的事项 - 先稳电流环，再谈避障平滑
FOC 电流环若未校准，速度指令会被执行误差放大，表现为绕障抖动、回归路径偏航或左右轮响应不一致。 - 避障决策要有滞回和时间窗口
单次传感器跳变不应立即触发大幅转向。应结合滑动窗口、距离阈值、角度置信度和持续确认，避免在障碍边缘来回“犹豫”。 - 速度/角速度/加速度必须限幅
避障时最容易出现指令突变。应对线速度、角速度、差速差值和加速度设置上限，并在模式切换时加入斜坡过渡。 - 回归路径要平滑衔接
绕过障碍后不要直接跳回原航点，而应根据当前位姿重新计算切入角，使用渐进航向修正，避免 S 形振荡。 - IMU 航向反馈几乎必不可少
纯编码器里程计在多次绕行后容易累积角度误差。引入 IMU 后，可用互补滤波或轻量卡尔曼滤波修正航向，提高避障后回归精度。 - 传感器盲区要专门处理
正前方通常容易覆盖，但侧前方、低矮台阶、玻璃门、黑色吸光面和金属反射面容易误判。可通过多传感器融合、降低运行速度、增加探测频率或加入底部跌落检测来缓解。 - Arduino 算力边界要清晰
ATmega328P 适合做单/双轮 FOC 执行、轻量状态机、简单规则避障和串口通信；若运行 DWA、滚动窗口代价地图、IMU 融合和多传感器滤波，建议迁移到 ESP32-S3、Teensy 4.1、STM32 或采用“上位机规划 + Arduino 执行”的分层架构。 - 控制周期要固定，避免阻塞
电流环和速度环应放在定时器中断或固定周期任务中；串口打印、传感器初始化、复杂日志不应阻塞主控制循环。 - 电源与 EMC 需要隔离
BLDC 加减速会引起母线波动，可能干扰 IMU、编码器和 MCU。控制电源应独立稳压，动力线与控制线分开布线，并在驱动器附近布置退耦电容。 - 必须设置安全降级策略
包括最大避障尝试次数、障碍物持续存在时的等待/报警、传感器失联时的低速停车、通信丢失停机、急停按钮和看门狗保护。
整体来看，这套系统的核心链路是：传感器感知障碍 → 轻量局部规划生成 v/ω 或航点 → 差速解算左右轮目标 → BLDC FOC 柔顺执行 → 里程计/IMU 闭环修正航向 → 平滑回归巡检路径。做原型时，建议按“单轮 FOC → 差速速度闭环 → IMU 航向补偿 → 规则避障 → 轻量 DWA/VFH → 路径平滑回归 → 安全降级”逐步推进。
1、超声波感知 + 增量栅格局部重规划 + 贝塞尔曲率平滑衔接
适用场景：工业厂区巡检中，超声波检测到临时堆放的设备或人员后，在原有路径基础上做局部增量修正，绕开障碍后用贝塞尔曲线平滑衔接，实现“避障不中断、路径不突变”。
#include <SimpleFOC.h>
#include <NewPing.h>
// 双轮 BLDC 差速底盘
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11, 8);
BLDCDriver3PWM driverR(3, 5, 6, 7);
Encoder encL(18, 19, 2048), encR(20, 21, 2048);
// 超声波
#define TRIG_F 22
#define ECHO_F 23
NewPing sonarF(TRIG_F, ECHO_F, 200);
// 栅格地图
#define GRID_SIZE 30
byte gridMap[GRID_SIZE][GRID_SIZE];
// 路径点
struct Point { float x, y; };
#define MAX_PATH 20
Point originalPath[MAX_PATH];
Point smoothedPath[MAX_PATH];
int pathCount = 0;
int smoothCount = 0;
// 障碍记录
int obstacles[5][2];
int obsCount = 0;
// 贝塞尔平滑参数
const float MAX_CURVATURE = 0.5;
const int BEZIER_POINT_NUM = 8;
void initGridAndPath() {
for (int x = 0; x < GRID_SIZE; x++)
for (int y = 0; y < GRID_SIZE; y++)
gridMap[x][y] = 0;
// 预设巡检路径（栅格坐标）
originalPath[0] = {2, 5};
originalPath[1] = {5, 5};
originalPath[2] = {8, 4};
originalPath[3] = {12, 6};
originalPath[4] = {15, 5};
originalPath[5] = {18, 8};
pathCount = 6;
}
// 增量障碍更新：仅标记新障碍周围区域
void dynamicObstacleUpdate(int obsX, int obsY) {
int radius = 2;
for (int dx = -radius; dx <= radius; dx++) {
for (int dy = -radius; dy <= radius; dy++) {
int x = obsX + dx, y = obsY + dy;
if (x >= 0 && x < GRID_SIZE && y >= 0 && y < GRID_SIZE) {
if (dx * dx + dy * dy <= radius * radius)
gridMap[x][y] = 1;
}
}
}
}
// 增量重规划：仅修正被障碍覆盖的路径点
int incrementalReplan(Point* origin, int cnt, Point* out) {
int newCnt = 0;
for (int i = 0; i < cnt; i++) {
int gx = (int)origin[i].x;
int gy = (int)origin[i].y;
if (gx >= 0 && gx < GRID_SIZE && gy >= 0 && gy < GRID_SIZE && gridMap[gx][gy] == 0) {
out[newCnt++] = origin[i]; // 无障碍，保留原点
} else {
// 被障碍覆盖：向侧方偏移
out[newCnt++] = {origin[i].x, origin[i].y + 2.0};
}
}
return newCnt;
}
// 三次贝塞尔插值
float cubicBezier(float t, float p0, float p1, float p2, float p3) {
float mt = 1 - t;
return p0*mt*mt*mt + 3*p1*mt*mt*t + 3*p2*mt*t*t + p3*t*t*t;
}
// 贝塞尔平滑
void smoothPathWithBezier(Point* raw, int cnt, Point* smooth) {
smoothCount = 0;
for (int i = 0; i < cnt - 3; i++) {
Point p0 = raw[i], p1 = raw[i+1], p2 = raw[i+2], p3 = raw[i+3];
// 简化曲率修正：辅助点向路径内侧收缩
float cross = (p1.x-p0.x)*(p2.y-p1.y) - (p1.y-p0.y)*(p2.x-p1.x);
if (fabs(cross) > MAX_CURVATURE * 10) {
p1.y -= 0.5; p2.y += 0.5;
}
for (int t = 0; t < BEZIER_POINT_NUM; t++) {
float tt = (float)t / (BEZIER_POINT_NUM - 1);
smooth[smoothCount].x = cubicBezier(tt, p0.x, p1.x, p2.x, p3.x);
smooth[smoothCount].y = cubicBezier(tt, p0.y, p1.y, p2.y, p3.y);
smoothCount++;
}
}
}
void setup() {
Serial.begin(115200);
encL.init(); encR.init();
motorL.linkSensor(&encL); motorR.linkSensor(&encR);
driverL.voltage_power_supply = 24; driverR.voltage_power_supply = 24;
driverL.init(); driverR.init();
motorL.linkDriver(&driverL); motorR.linkDriver(&driverR);
motorL.controller = MotionControlType::velocity;
motorR.controller = MotionControlType::velocity;
motorL.init(); motorL.initFOC();
motorR.init(); motorR.initFOC();
initGridAndPath();
}
void loop() {
motorL.loopFOC(); motorR.loopFOC();
// ===== 感知 =====
float distF = sonarF.ping_cm();
if (distF > 0 && distF < 40) {
dynamicObstacleUpdate(10, 5); // 模拟：标记前方障碍
}
// ===== 规划：增量重规划 =====
Point replanned[MAX_PATH];
int newCount = incrementalReplan(originalPath, pathCount, replanned);
// ===== 平滑：贝塞尔衔接 =====
smoothPathWithBezier(replanned, newCount, smoothedPath);
// ===== 执行：沿平滑路径行驶（简化：取第一个平滑点方向）=====
if (smoothCount > 0) {
float dx = smoothedPath[0].x - 5.0;
float dy = smoothedPath[0].y - 5.0;
float heading = atan2(dy, dx);
float baseSpeed = (distF < 30) ? 0.3 : 0.8;
float correction = constrain(heading * 0.5, -1.0, 1.0);
motorL.move((baseSpeed - correction) / 0.05);
motorR.move((baseSpeed + correction) / 0.05);
}
delay(50);
}
解读：该案例直接采用了搜索结果中“增量栅格重规划 + 贝塞尔曲率平滑”的框架。incrementalReplan() 只修正被障碍覆盖的路径点，而非全局重算，降低了 Arduino 的算力负担；smoothPathWithBezier() 将重规划后的折线路径插值为连续曲线，避免机器人在避障衔接处急停急转。搜索结果中强调“贝塞尔曲线天生具有 C² 连续性（加速度连续），适合工业巡检的平稳性要求”，这正是“平滑”层存在的意义。
2、路径节点压缩（Douglas-Peucker）+ S型速度平滑执行
适用场景：A* 或人工预设的巡检路径点过密，直接跟踪会导致 BLDC 频繁微调、数据采集抖动。先用 Douglas-Peucker 剔除冗余航点，再用 S 型加减速执行，适合长时间低速巡检。
#include <SimpleFOC.h>
#include <math.h>
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11, 8);
BLDCDriver3PWM driverR(3, 5, 6, 7);
Encoder encL(18, 19, 2048), encR(20, 21, 2048);
struct Point { float x, y; };
// 原始密集路径（A* 输出示例）
Point rawPath[12] = {
{0,0}, {1,0}, {2,0}, {3,0}, {4,0.2}, {5,0.5},
{6,1.0}, {7,1.8}, {8,2.5}, {9,2.8}, {10,3.0}, {11,3.0}
};
int rawCount = 12;
Point compressedPath[8];
int compressedCount = 0;
// 当前跟踪状态
float robotX = 0, robotY = 0, robotHeading = 0;
int targetIdx = 0;
float currentSpeed = 0;
// S型平滑参数
const float MAX_ACCEL = 0.8; // m/s²
const float MAX_SPEED = 0.6; // m/s
const float JERK = 0.4; // 加加速度
// Douglas-Peucker 递归压缩
float perpendicularDist(Point p, Point a, Point b) {
float dx = b.x - a.x, dy = b.y - a.y;
float len = sqrt(dx*dx + dy*dy);
if (len < 0.001) return sqrt(pow(p.x-a.x,2) + pow(p.y-a.y,2));
return fabs(dy*p.x - dx*p.y + b.x*a.y - b.y*a.x) / len;
}
void douglasPeucker(Point* pts, int start, int end, float epsilon, Point* out, int& outCnt) {
float maxDist = 0;
int maxIdx = start;
for (int i = start + 1; i < end; i++) {
float d = perpendicularDist(pts[i], pts[start], pts[end]);
if (d > maxDist) { maxDist = d; maxIdx = i; }
}
if (maxDist > epsilon) {
douglasPeucker(pts, start, maxIdx, epsilon, out, outCnt);
out[outCnt++] = pts[maxIdx];
douglasPeucker(pts, maxIdx, end, epsilon, out, outCnt);
}
}
void compressPath(float epsilon) {
compressedCount = 0;
compressedPath[compressedCount++] = rawPath[0];
douglasPeucker(rawPath, 0, rawCount - 1, epsilon, compressedPath, compressedCount);
compressedPath[compressedCount++] = rawPath[rawCount - 1];
}
// S型速度更新
float sCurveSpeed(float current, float target, float dt) {
float diff = target - current;
if (fabs(diff) < 0.01) return target;
float step = MAX_ACCEL * dt;
if (diff > 0) return min(current + step, target);
else return max(current - step, target);
}
void setup() {
Serial.begin(115200);
encL.init(); encR.init();
motorL.linkSensor(&encL); motorR.linkSensor(&encR);
driverL.voltage_power_supply = 24; driverR.voltage_power_supply = 24;
driverL.init(); driverR.init();
motorL.linkDriver(&driverL); motorR.linkDriver(&driverR);
motorL.controller = MotionControlType::velocity;
motorR.controller = MotionControlType::velocity;
motorL.init(); motorL.initFOC();
motorR.init(); motorR.initFOC();
// 压缩路径：容差 0.3m
compressPath(0.3);
Serial.print("压缩后航点数: "); Serial.println(compressedCount);
}
void loop() {
motorL.loopFOC(); motorR.loopFOC();
if (targetIdx >= compressedCount) {
motorL.move(0); motorR.move(0);
delay(10); return;
}
// 目标点
float dx = compressedPath[targetIdx].x - robotX;
float dy = compressedPath[targetIdx].y - robotY;
float dist = sqrt(dx*dx + dy*dy);
if (dist < 0.2) {
targetIdx++;
return;
}
// 朝向目标
float desiredHeading = atan2(dy, dx);
float headingError = desiredHeading - robotHeading;
while (headingError > PI) headingError -= 2*PI;
while (headingError < -PI) headingError += 2*PI;
// 距离越近，目标速度越低（S型减速）
float targetSpeed = min(MAX_SPEED, dist * 0.8);
currentSpeed = sCurveSpeed(currentSpeed, targetSpeed, 0.05);
// 差速执行
float turn = constrain(headingError * 1.2, -1.0, 1.0);
float vL = currentSpeed - turn * 0.15;
float vR = currentSpeed + turn * 0.15;
motorL.move(vL / 0.05);
motorR.move(vR / 0.05);
// 更新位姿
robotX += currentSpeed * cos(robotHeading) * 0.05;
robotY += currentSpeed * sin(robotHeading) * 0.05;
robotHeading += turn * 0.5 * 0.05;
delay(50);
}
解读：该案例的“规划”层体现在 Douglas-Peucker 压缩——将 12 个密集路径点压缩到仅保留关键转折点，减少 BLDC 在平直段的无效微调。sCurveSpeed() 提供速度平滑：接近目标点时目标速度自然降低，但实际速度受 MAX_ACCEL 限制逐步逼近，避免急刹。搜索结果指出“节点压缩不仅减少点数，也提升轨迹平滑性”，压缩后的路径配合 S 型速度规划可“避免机器人在每个航点处急停急转”。
3、感知-规划-平滑三级状态机（避障抢占 + 平滑恢复）
适用场景：巡检机器人正常沿平滑路径巡航时，超声波突然检测到近距离障碍。避障逻辑临时抢占路径跟踪，避障完成后不直接切回原路径，而是经过一段贝塞尔过渡段平滑汇入，避免“避障后猛打方向回归”的冲击。
#include <SimpleFOC.h>
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11, 8);
BLDCDriver3PWM driverR(3, 5, 6, 7);
Encoder encL(18, 19, 2048), encR(20, 21, 2048);
#define TRIG 22
#define ECHO 23
// 状态机
enum NavState { CRUISE, AVOID, BLEND };
NavState state = CRUISE;
// 路径（简化：直线 + 转弯）
struct Point { float x, y; };
Point path[4] = {{0,0}, {5,0}, {8,3}, {10,5}};
int pathIdx = 0;
// 避障参数
float obstacleDist = 999;
unsigned long avoidStart = 0;
float lastHeading = 0;
// 平滑过渡
float blendHeading = 0;
float blendProgress = 0;
float readDist() {
digitalWrite(TRIG, LOW); delayMicroseconds(2);
digitalWrite(TRIG, HIGH); delayMicroseconds(10);
digitalWrite(TRIG, LOW);
long dur = pulseIn(ECHO, HIGH, 20000);
return (dur == 0) ? 999 : dur * 0.0343 / 2.0;
}
void setup() {
Serial.begin(115200);
pinMode(TRIG, OUTPUT); pinMode(ECHO, INPUT);
encL.init(); encR.init();
motorL.linkSensor(&encL); motorR.linkSensor(&encR);
driverL.voltage_power_supply = 24; driverR.voltage_power_supply = 24;
driverL.init(); driverR.init();
motorL.linkDriver(&driverL); motorR.linkDriver(&driverR);
motorL.controller = MotionControlType::velocity;
motorR.controller = MotionControlType::velocity;
motorL.init(); motorL.initFOC();
motorR.init(); motorR.initFOC();
}
void loop() {
motorL.loopFOC(); motorR.loopFOC();
obstacleDist = readDist();
// ===== 状态转移 =====
if (state == CRUISE && obstacleDist > 0 && obstacleDist < 25) {
state = AVOID;
avoidStart = millis();
Serial.println("→ 避障抢占");
} else if (state == AVOID && millis() - avoidStart > 800 && obstacleDist > 35) {
state = BLEND;
blendProgress = 0;
Serial.println("→ 贝塞尔平滑回归");
}
// ===== 执行 =====
float baseSpeed = 0, turn = 0;
switch (state) {
case CRUISE:
baseSpeed = 0.8;
// 简化路径跟踪：朝向下一个点
if (pathIdx < 4) {
float dx = path[pathIdx].x - 0; // 假设当前位置为 0
float dy = path[pathIdx].y - 0;
float targetHeading = atan2(dy, dx);
turn = constrain(targetHeading * 0.3, -0.8, 0.8);
}
break;
case AVOID:
// 后退转向
baseSpeed = -0.3;
turn = 1.0;
break;
case BLEND:
// 贝塞尔过渡：转向从避障方向平滑回归到路径方向
blendProgress += 0.05;
if (blendProgress >= 1.0) {
state = CRUISE;
blendProgress = 0;
} else {
// 三次贝塞尔：0→1 平滑过渡
float t = blendProgress;
float smoothT = t * t * (3 - 2 * t); // smoothstep
turn = (1.0 - smoothT) * 1.0; // 从避障转向逐渐归零
baseSpeed = 0.4;
}
break;
}
float vL = baseSpeed - turn * 0.15;
float vR = baseSpeed + turn * 0.15;
motorL.move(vL / 0.05);
motorR.move(vR / 0.05);
delay(20);
}
解读：该案例将“平滑”从路径层延伸到了状态层。BLEND 状态使用 smoothstep 函数（tt(3-2t)）对转向量做插值，使避障结束后的回归过程不是阶跃式的“猛打方向”，而是约 0.4 秒的渐进过渡。搜索结果中贝塞尔平滑解决的是“路径连续性”，而状态层的平滑解决的是“控制连续性”——两者结合才能让巡检机器人在频繁避障中保持数据采集的稳定性。
要点解读
-
“感知-规划-平滑”三层中，平滑层的价值在工业巡检场景被放大
普通移动机器人避障后即使有轻微顿挫也可接受，但工业巡检机器人在经过传感器点位时需要稳定姿态采集数据（相机抖动、气体采样波动、IMU 漂移都会影响数据质量）。搜索结果明确指出，节点压缩与平滑“能减少相机抖动、气体采样波动、IMU 漂移和机械磨损”。案例二的 S 型速度规划和案例三的 smoothstep 状态过渡，都是针对这一需求的工程回应。 -
增量重规划比全局重规划更适合 Arduino 算力边界
案例一的 incrementalReplan() 只修正被障碍覆盖的路径点，而非每次避障都重新跑 A。搜索结果中该框架的注释明确写着“D Lite 思想局部修正”。全局 A* 在 30×30 栅格上的计算量对 Arduino 已接近极限，而增量修正的复杂度与障碍数量线性相关，更适合实时避障循环。 -
Douglas-Peucker 压缩与贝塞尔平滑解决的是不同问题，不能互相替代
压缩解决的是航点数量过多（减少 BLDC 微调频率、节省算力），平滑解决的是转折点不够连续（消除急停急转）。案例二展示了压缩后的路径仍有折角，需要 S 型速度规划来柔化执行；案例一展示了平滑需要先有一个可用的路径序列。搜索结果将两者并列为“路径节点压缩 + 智能规划”的组合，正是因为它们在流水线中处于不同位置。 -
BLDC FOC 的平滑执行能力是上层平滑算法的物理前提
如果底层使用开环 ESC 或有刷电机 PWM，上层计算出的 S 型速度曲线或贝塞尔路径在低速段会因死区、齿槽效应而无法被精确复现。搜索结果中强调 BLDC 配合 FOC 后“电流环可抑制转矩脉动，速度环可平滑跟踪期望轮速，差速底盘也能更自然地执行弧线转向”。这意味着上层平滑算法的收益只有在 FOC 执行层才能兑现。 -
工业巡检的避障闭环必须区分“安全抢占”与“平滑恢复”两个阶段
案例三的 AVOID 状态是硬抢占（后退转向，不追求平滑），因为安全优先级最高；BLEND 状态才引入平滑过渡。如果避障本身也追求平滑，响应延迟会增大碰撞风险。搜索结果中电力巡检的分层架构同样将超声波避障定义为“硬优先级”，近距障碍时“直接中断路径跟踪指令”。平滑不是在所有阶段都适用——安全层要快，恢复层才要柔。
4、极坐标扇区感知 + 间隙选择 + 转角-速度耦合平滑
适用硬件：ESP32 + SimpleFOC 差速底盘（2×BLDC）+ 编码器 + 8~12 路超声/红外阵列（或单线激光的扇区化输出）。
核心思路：不建栅格、不跑搜索。感知层把一圈测距压成 9 个扇区的最近距离（约 100 字节），规划层做“从正前方向两侧扩展找最宽可行间隙”，平滑层把间隙中心偏角经速率限幅变为角速度，并按偏角降速。整个闭环的算力在几百次浮点运算量级，1 ms 内可跑完。
/*
* 案例4：扇区感知 -> 间隙选择 -> 转角/速度耦合平滑
* 扇区：9 个，覆盖 -80°..+80°，索引 0..8，中心索引 4 = 正前
*/
#include <SimpleFOC.h>
BLDCMotor wheelL(7), wheelR(7);
BLDCDriver3PWM drvL(9,10,11,8), drvR(22,23,24,25);
Encoder encL(2,3,2048), encR(32,33,2048);
void onL(){ encL.handleA(); } void onR(){ encR.handleA(); }
const float R_W = 0.075f, TRACK = 0.38f;
// ---------------- 感知层 ----------------
#define NSEC 9
const float SEC_ANG[NSEC] = {-1.396f,-1.047f,-0.698f,-0.349f,0.0f,
0.349f, 0.698f, 1.047f, 1.396f}; // rad
float secDist[NSEC]; // 各扇区最近距离 m
unsigned long secSeen[NSEC]; // 最近一次有效测距时间
const float MAX_RANGE = 4.0f; // 有效量程
const float OBS_DECAY_MS = 1500; // 1.5s 未再测到则视为已清除
// 简化：由外部测距阵列喂入（(角度,距离) 对），此处用 3 路示意 + 插值
const int TRIG[3] = {6,8,10}, ECHO[3] = {7,9,11};
const float INST_ANG[3] = {-0.55f, 0.0f, 0.55f};
float readUS(int i) {
digitalWrite(TRIG[i], LOW); delayMicroseconds(2);
digitalWrite(TRIG[i], HIGH); delayMicroseconds(10); digitalWrite(TRIG[i], LOW);
long d = pulseIn(ECHO[i], HIGH, 20000);
return (d > 0) ? d * 0.034f / 2.0f / 100.0f : MAX_RANGE;
}
void senseSectors() {
for (int k = 0; k < 3; k++) {
float d = readUS(k);
if (d >= MAX_RANGE) continue;
// 落点投射到最近的两个扇区（保持最近值）
int best = 0; float bd = 9.9f;
for (int s = 0; s < NSEC; s++) {
float dd = fabsf(SEC_ANG[s] - INST_ANG[k]);
if (dd < bd) { bd = dd; best = s; }
}
if (d < secDist[best]) { secDist[best] = d; secSeen[best] = millis(); }
}
// 时间衰减：久未测到 -> 恢复为自由
for (int s = 0; s < NSEC; s++) {
if (millis() - secSeen[s] > OBS_DECAY_MS) secDist[s] = MAX_RANGE;
}
}
// ---------------- 规划层：间隙选择 ----------------
const float D_SAFE = 0.45f; // 该方向余量足够才算可行
const float D_BLOCK = 0.28f; // 低于此距离：硬禁止
const int MIN_GAP = 2; // 最小可行间隙扇区数
struct Gap { int start, end, center; float width; bool valid; };
// 找最宽的连续可行扇区段，优先靠近正前方
Gap findBestGap() {
bool ok[NSEC];
for (int s = 0; s < NSEC; s++)
ok[s] = (secDist[s] > D_SAFE) && (secDist[s] > D_BLOCK);
Gap best = {4,4,4,0,false};
int s = 0;
while (s < NSEC) {
if (!ok[s]) { s++; continue; }
int e = s;
while (e + 1 < NSEC && ok[e+1]) e++;
int w = e - s + 1;
if (w >= MIN_GAP) {
int c = (s + e) / 2;
// 评分：宽度优先，其次靠近正前
float score = w * 1.0f - 0.35f * fabsf((float)(c - 4));
float bestScore = best.valid
? (best.width - 0.35f * fabsf((float)(best.center - 4))) : -99.0f;
if (score > bestScore) best = {s, e, c, (float)w, true};
}
s = e + 1;
}
return best;
}
// ---------------- 平滑层：速率限幅 + jerk 约束 ----------------
float wCmd = 0, vCmd = 0; // 实际下发
float wTgt = 0, vTgt = 0; // 规划给出的期望
const float W_LIM = 1.4f; // 角速度上限 rad/s
const float W_ACC = 4.0f; // 角加速度上限 rad/s²
const float W_JERK = 20.0f; // 角加加速度上限 rad/s³
float wAcc = 0;
void smooth(float dt) {
// 1) 一阶规划：目标角速度
float dw = wTgt - wCmd;
float dwMax = W_ACC * dt;
float dStep = constrain(dw, -dwMax, dwMax);
// 2) jerk 约束：限制加速度的变化率
float accWant = dStep / dt;
float accMax = W_JERK * dt;
wAcc += constrain(accWant - wAcc, -accMax, accMax);
wCmd += wAcc * dt;
wCmd = constrain(wCmd, -W_LIM, W_LIM);
// 3) 速度平滑
vCmd += constrain(vTgt - vCmd, -1.2f*dt, 0.8f*dt); // 减快加慢
}
// ---------------- 主循环 ----------------
const float V_CRUISE = 0.55f;
void setup() {
encL.init(); encL.enableInterrupts(onL);
encR.init(); encR.enableInterrupts(onR);
wheelL.linkSensor(&encL); wheelR.linkSensor(&encR);
wheelL.controller = wheelR.controller = MotionControlType::velocity;
wheelL.PID_velocity.P = 0.4f; wheelL.PID_velocity.I = 4.0f;
wheelR.PID_velocity.P = 0.4f; wheelR.PID_velocity.I = 4.0f;
wheelL.PID_velocity.output_ramp = 400.0f;
wheelR.PID_velocity.output_ramp = 400.0f;
wheelL.LPF_velocity.Tf = 0.01f; wheelR.LPF_velocity.Tf = 0.01f;
wheelL.voltage_limit = wheelR.voltage_limit = 12.0f;
wheelL.current_limit = wheelR.current_limit = 5.0f;
wheelL.init(); wheelR.init();
drvL.voltage_power_supply = drvR.voltage_power_supply = 12.0f;
drvL.init(); drvR.init();
wheelL.linkDriver(&drvL); wheelR.linkDriver(&drvR);
wheelL.initFOC(); wheelR.initFOC();
for (int i = 0; i < 3; i++) { pinMode(TRIG[i], OUTPUT); pinMode(ECHO[i], INPUT); }
for (int s = 0; s < NSEC; s++) { secDist[s] = MAX_RANGE; secSeen[s] = 0; }
Serial.begin(115200);
}
void loop() {
static unsigned long tPrev = millis();
unsigned long now = millis();
float dt = (now - tPrev) / 1000.0f; tPrev = now;
if (dt <= 0 || dt > 0.15f) dt = 0.02f;
// ---- 1. 感知 ----
senseSectors();
// ---- 2. 规划：间隙 + 速度降额 ----
Gap g = findBestGap();
float front = secDist[4];
if (!g.valid) {
// 无可行间隙：停机（绝不"猜一个方向"冲出去）
vTgt = 0; wTgt = 0;
} else {
float bearing = SEC_ANG[g.center]; // 期望朝向偏角
wTgt = constrain(2.0f * bearing, -W_LIM, W_LIM);
// 转角-速度耦合：偏角越大越慢；前方越近越慢
float kTurn = cosf(constrain(bearing, -1.2f, 1.2f)); // 1 -> 0.36
float kClear = constrain((front - 0.30f) / 0.60f, 0.15f, 1.0f);
vTgt = V_CRUISE * kTurn * kClear;
vTgt = max(vTgt, 0.06f); // 保底蠕动
}
// ---- 3. 平滑 ----
smooth(dt);
// ---- 4. 执行 ----
float vL = vCmd - wCmd * TRACK * 0.5f;
float vR = vCmd + wCmd * TRACK * 0.5f;
wheelL.loopFOC(); wheelR.loopFOC();
wheelL.move(vL / R_W); wheelR.move(vR / R_W);
static unsigned long tp = 0;
if (millis() - tp > 200) {
tp = millis();
Serial.print("gap="); Serial.print(g.valid ? g.width : 0);
Serial.print(" c="); Serial.print(g.center);
Serial.print(" v="); Serial.print(vCmd, 2);
Serial.print(" w="); Serial.println(wCmd, 2);
}
delay(20); // 50Hz 闭环；轮速环建议移入 1kHz 定时器
}
与 DWA 相比，扇区法的全部计算是“9 次比较 + 一次游程扫描”，在 8 位 MCU 上都能跑；代价是它不做前向仿真，因此必须靠平滑层把粗糙的扇区决策“磨”成可执行指令——这正是三层结构里“平滑”存在的意义。
5、FreeRTOS 三层 + 稀疏路点 + 弹性带式平滑 + 曲率限速
适用硬件：ESP32 双核 + SimpleFOC 差速底盘 + 单线激光/ToF 阵列 + 编码器。
核心思路：规划层不做高频决策，只在“障碍构型变化”时输出一段稀疏路点（3~5 个）；平滑层以 100 Hz 追踪这些路点，做两件事——曲率限速（弯道自动降速）与速率限幅（转角与速度的一阶、二阶连续）。这种“低频规划 + 高频平滑”的分工，是被工业 AGV 反复验证过的轻量方案。
/*
* 案例5：规划(5Hz/事件驱动) - 平滑(100Hz) - 执行(1kHz)
* 平滑：短前瞻追踪 + 曲率限速 + 速率限幅
*/
#include <SimpleFOC.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
BLDCMotor wheelL(7), wheelR(7);
// 驱动/编码器初始化同案例一（略）
SemaphoreHandle_t mux;
const float R_W = 0.075f, TRACK = 0.38f;
const float V_MAX = 0.70f, A_MAX = 0.55f;
const float W_MAX = 1.5f, W_ACC = 4.5f;
// ---- 路点缓冲（环形，无动态分配）----
#define NP_MAX 8
struct Pt { float x, y; };
Pt path[NP_MAX]; int nPath = 0, idxPath = 0;
volatile bool pathNew = false;
// ---- 位姿 ----
float px = 0, py = 0, pth = 0;
// ---- 平滑输出 ----
volatile float c_v = 0, c_w = 0;
volatile bool c_stop = false;
float angDiff(float a, float b) {
float d = a - b;
while (d > PI) d -= 2*PI;
while (d < -PI) d += 2*PI;
return d;
}
// ---------------- 规划层：事件驱动，只出路点 ----------------
// 触发条件：前方障碍构型变化（简化为"每 200ms 检查一次前向余量"）
void taskPlan(void*) {
TickType_t last = xTaskGetTickCount();
for (;;) {
float dF = readFront();
if (dF < 0.60f) {
// 生成绕行路点：向更空一侧偏 0.5m，前进 0.8m，再回原线
int side = (readLeft() > readRight()) ? 1 : -1;
float ox = px, oy = py;
float th = pth;
xSemaphoreTake(mux, portMAX_DELAY);
nPath = 0; idxPath = 0;
// 3 个路点：侧移 -> 越过 -> 回线
path[nPath++] = {ox + 0.8f*cosf(th) - side*0.50f*sinf(th),
oy + 0.8f*sinf(th) + side*0.50f*cosf(th)};
path[nPath++] = {ox + 1.6f*cosf(th) - side*0.50f*sinf(th),
oy + 1.6f*sinf(th) + side*0.50f*cosf(th)};
path[nPath++] = {ox + 2.2f*cosf(th),
oy + 2.2f*sinf(th)};
pathNew = true;
xSemaphoreGive(mux);
}
vTaskDelayUntil(&last, pdMS_TO_TICKS(200)); // 5Hz 检查即可
}
}
// ---------------- 平滑层：100Hz ----------------
void taskSmooth(void*) {
TickType_t last = xTaskGetTickCount();
float vPrev = 0, wPrev = 0, wAccP = 0;
float tx = 0, ty = 0; bool haveTgt = false;
for (;;) {
float dt = 0.01f;
// 路点更新
bool np = false;
xSemaphoreTake(mux, portMAX_DELAY);
np = pathNew;
if (np && nPath > 0) { tx = path[0].x; ty = path[0].y; haveTgt = true; pathNew = false; }
int npth = nPath;
xSemaphoreGive(mux);
float vTgt = 0, wTgt = 0;
if (haveTgt) {
float dx = tx - px, dy = ty - py;
float dist = hypotf(dx, dy);
// 到点：推进下一个路点，全部走完则清空
if (dist < 0.12f) {
xSemaphoreTake(mux, portMAX_DELAY);
if (idxPath + 1 < nPath) {
idxPath++; tx = path[idxPath].x; ty = path[idxPath].y;
} else {
nPath = 0; idxPath = 0; haveTgt = false;
}
xSemaphoreGive(mux);
if (!haveTgt) { dx = 0; dy = 0; dist = 1e-3f; }
}
if (haveTgt) {
// 短前瞻：不足 0.25m 时直接瞄准该点，否则取该点方向
float look = min(dist, 0.25f);
float headToTgt = atan2f(dy, dx);
float alpha = angDiff(headToTgt, pth);
// 曲率估计（简化：用偏角/前瞻距离）
float kappa = 2.0f * sinf(alpha) / max(look, 0.05f);
// 曲率限速：v <= sqrt(a_lat_max / |kappa|)
const float A_LAT = 0.8f; // 侧向加速度上限 m/s²
float vCurve = (fabsf(kappa) > 1e-3f)
? sqrtf(A_LAT / fabsf(kappa)) : V_MAX;
vTgt = min(V_MAX, vCurve);
vTgt *= cosf(constrain(alpha, -1.2f, 1.2f)); // 大偏角降速
wTgt = vTgt * kappa;
wTgt = constrain(wTgt, -W_MAX, W_MAX);
}
}
// ---- 速率限幅（加速度 + 角加速度 + jerk）----
vTgt = vPrev + constrain(vTgt - vPrev, -A_MAX*dt*1.5f, A_MAX*dt);
float dwMax = W_ACC * dt;
float dw = constrain(wTgt - wPrev, -dwMax, dwMax);
wAccP += constrain(dw/dt - wAccP, -25.0f*dt, 25.0f*dt); // jerk 限
wPrev += wAccP * dt;
if (fabsf(wPrev) > W_MAX) wPrev = copysignf(W_MAX, wPrev);
vPrev = vTgt;
xSemaphoreTake(mux, portMAX_DELAY);
c_v = vPrev; c_w = wPrev;
c_stop = (nPath == 0 && !haveTgt) ? false : false; // 到点后按速度自然停止
xSemaphoreGive(mux);
vTaskDelayUntil(&last, pdMS_TO_TICKS(10));
}
}
// ---------------- 执行层 1kHz ----------------
void taskExec(void*) {
TickType_t last = xTaskGetTickCount();
for (;;) {
float v, w;
xSemaphoreTake(mux, portMAX_DELAY);
v = c_v; w = c_w;
xSemaphoreGive(mux);
float vL = v - w*TRACK*0.5f, vR = v + w*TRACK*0.5f;
wheelL.loopFOC(); wheelR.loopFOC();
wheelL.move(vL/R_W); wheelR.move(vR/R_W);
// 里程积分（供规划层使用）
float vl = wheelL.shaft_velocity*R_W, vr = wheelR.shaft_velocity*R_W;
float vv = 0.5f*(vl+vr), ww = (vr-vl)/TRACK;
px += vv*cosf(pth)*0.001f; py += vv*sinf(pth)*0.001f; pth += ww*0.001f;
vTaskDelayUntil(&last, pdMS_TO_TICKS(1));
}
}
void setup() {
// 电机初始化同案例一
mux = xSemaphoreCreateMutex();
xTaskCreatePinnedToCore(taskPlan, "plan", 4096, NULL, 1, NULL, 0);
xTaskCreatePinnedToCore(taskSmooth, "smooth", 4096, NULL, 2, NULL, 1);
xTaskCreatePinnedToCore(taskExec, "exec", 4096, NULL, 3, NULL, 1);
Serial.begin(115200);
}
void loop() { vTaskDelay(portMAX_DELAY); }
曲率限速 v ≤ sqrt(a_lat/|κ|) 是平滑层的核心公式：它保证转弯时的离心加速度不超限，也就是“弯道自动减速”的物理来源，而不是靠 if (转角大) 降速 这种经验规则。配合 jerk 限幅，转向会呈现为“缓入—稳定—缓出”的连续过程。
6、巡检停点闭环 —— 接近段速度包络 + 朝向对齐 + 停稳判定
适用硬件：ESP32 + SimpleFOC 差速底盘 + 编码器 + 前向 ToF + IMU + 巡检点标记（编码器里程或地标）。
核心思路：工业巡检的闭环不是“一直在走”，而是“走到点、停稳、朝向对准、执行、再出发”。避障在这条链里必须让位于任务：绕行后要回到巡检点，而不是绕过就算了。平滑层在这里承担三件具体工作——接近段的减速包络、末端朝向对齐、停稳判定（速度与角速度双阈值 + 持续时间）。
/*
* 案例6：巡检点队列 + 局部避障 + 平滑靠停
* 状态：CRUISE -> DETOUR -> APPROACH -> ALIGN -> SETTLE -> DWELL -> NEXT
*/
#include <SimpleFOC.h>
#include <Wire.h>
#include <MPU6050.h>
BLDCMotor wheelL(7), wheelR(7);
// 驱动/编码器初始化同案例一（略）
MPU6050 imu;
const float R_W = 0.075f, TRACK = 0.38f;
const float V_CRUISE = 0.55f, A_DEC = 0.45f; // 减速包络用减速度
const float V_MIN_APP = 0.06f; // 接近段最低速
// ---- 巡检点队列 ----
#define N_STOP 6
struct Stop { float x, y; float yaw; uint8_t dwellS; }; // yaw: 期望朝向
const Stop STOPS[N_STOP] = {
{2.0f, 0.0f, 0.0f, 3},
{4.0f, 1.2f, 1.57f, 4},
{6.0f, 1.2f, -1.57f, 3},
{8.0f, 0.0f, 3.14f, 5},
{8.0f,-2.0f, 1.57f, 4},
{4.0f,-2.0f, 0.0f, 3},
};
int stopIdx = 0;
// ---- 状态 ----
enum St { S_CRUISE, S_DETOUR, S_APPROACH, S_ALIGN, S_SETTLE, S_DWELL, S_DONE };
St st = S_CRUISE;
unsigned long stT0 = 0;
float px = 0, py = 0, pth = 0;
float vCmd = 0, wCmd = 0;
float angDiff(float a, float b) {
float d = a - b;
while (d > PI) d -= 2*PI;
while (d < -PI) d += 2*PI;
return d;
}
// ---- 避障感知（前 + 左 + 右）----
float dF, dL, dR;
void sense() {
dF = readUS(1); dL = readUS(0); dR = readUS(2);
}
// ---- 接近段速度包络：v = sqrt(2·a·d)，末端自动趋零 ----
float approachEnvelope(float dist) {
float v = sqrtf(2.0f * A_DEC * max(dist, 0.0f));
return constrain(v, 0.0f, V_CRUISE);
}
// ---- 平滑：速率与 jerk 限幅 ----
float vSm = 0, wSm = 0, wAcc = 0;
void smoothStep(float vTgt, float wTgt, float dt) {
float aUp = 0.8f, aDn = 1.6f;
float dV = vTgt - vSm;
vSm += constrain(dV, -aDn*dt, aUp*dt);
vSm = max(vSm, 0.0f);
float dw = constrain(wTgt - wSm, -5.0f*dt, 5.0f*dt);
wAcc += constrain(dw/dt - wAcc, -30.0f*dt, 30.0f*dt);
wSm += wAcc * dt;
wSm = constrain(wSm, -1.4f, 1.4f);
}
void setup() {
// 电机初始化同案例一
Wire.begin(); imu.initialize();
stT0 = millis();
Serial.begin(115200);
}
void loop() {
static unsigned long tPrev = millis();
unsigned long now = millis();
float dt = (now - tPrev) / 1000.0f; tPrev = now;
if (dt <= 0 || dt > 0.15f) dt = 0.02f;
sense();
imu.update();
const Stop &s = STOPS[stopIdx];
float dx = s.x - px, dy = s.y - py;
float dist = hypotf(dx, dy);
float bearToStop = atan2f(dy, dx);
float alpha = angDiff(bearToStop, pth);
float tIn = (millis() - stT0) / 1000.0f;
float vTgt = 0, wTgt = 0;
bool obsAhead = (dF < 0.50f);
switch (st) {
// ---------------- 巡航：朝巡检点走，遇障绕行 ----------------
case S_CRUISE: {
vTgt = min(V_CRUISE, approachEnvelope(dist));
wTgt = constrain(1.8f * alpha, -1.4f, 1.4f);
if (obsAhead) { st = S_DETOUR; stT0 = millis(); }
else if (dist < 0.80f) { st = S_APPROACH; stT0 = millis(); }
break;
}
// ---------------- 绕行：侧向偏出，不丢目标 ----------------
case S_DETOUR: {
float side = (dL > dR) ? -1.0f : 1.0f; // 往更空一侧
wTgt = 0.55f * side; // 侧向转出
vTgt = 0.22f;
// 前方恢复通畅 或 超时 -> 回到朝目标
if (!obsAhead && tIn > 0.4f) { st = S_CRUISE; stT0 = millis(); }
else if (tIn > 4.0f) { st = S_DONE; } // 绕不出去：停机上报
break;
}
// ---------------- 接近：减速包络主导 ----------------
case S_APPROACH: {
vTgt = max(approachEnvelope(dist), V_MIN_APP);
wTgt = constrain(2.0f * alpha, -1.2f, 1.2f);
// 到达接近窗口 + 基本对准 -> 转 ALIGN
if (dist < 0.18f && fabsf(alpha) < 0.35f) { st = S_ALIGN; stT0 = millis(); }
if (dist < 0.10f) { st = S_ALIGN; stT0 = millis(); }
break;
}
// ---------------- 对准：原地微调朝向 ----------------
case S_ALIGN: {
float yawErr = angDiff(s.yaw, pth);
vTgt = 0;
wTgt = constrain(1.5f * yawErr, -0.6f, 0.6f);
if (fabsf(yawErr) < 0.05f || tIn > 3.0f) { st = S_SETTLE; stT0 = millis(); }
break;
}
// ---------------- 停稳：双阈值 + 持续时间 ----------------
case S_SETTLE: {
float vl = wheelL.shaft_velocity * R_W;
float vr = wheelR.shaft_velocity * R_W;
float vNow = fabsf(0.5f*(vl+vr));
float wNow = fabsf((vr-vl)/TRACK);
vTgt = 0; wTgt = 0;
// 速度 < 0.02 m/s 且角速度 < 0.05 rad/s 保持 400ms -> 判定停稳
if (vNow < 0.02f && wNow < 0.05f && fabsf(vSm) < 0.02f) {
if (tIn > 0.4f) { st = S_DWELL; stT0 = millis();
Serial.print("SETTLED at stop "); Serial.println(stopIdx); }
} else {
stT0 = millis(); // 未停稳：重置计时
}
break;
}
// ---------------- 驻留：执行巡检动作（拍照/测温）----------------
case S_DWELL: {
vTgt = 0; wTgt = 0;
if (tIn > s.dwellS) {
stopIdx++;
if (stopIdx >= N_STOP) { st = S_DONE; }
else { st = S_CRUISE; }
stT0 = millis();
}
break;
}
case S_DONE:
vTgt = 0; wTgt = 0;
break;
}
// ---- 平滑 + 执行 ----
smoothStep(vTgt, wTgt, dt);
float vL = vSm - wSm * TRACK * 0.5f;
float vR = vSm + wSm * TRACK * 0.5f;
wheelL.loopFOC(); wheelR.loopFOC();
wheelL.move(vL / R_W); wheelR.move(vR / R_W);
// ---- 里程 ----
px += vSm * cosf(pth) * dt;
py += vSm * sinf(pth) * dt;
pth += wSm * dt;
Serial.print("st="); Serial.print(st);
Serial.print(" stop="); Serial.print(stopIdx);
Serial.print(" d="); Serial.print(dist, 2);
Serial.print(" v="); Serial.println(vSm, 2);
delay(20);
}
S_SETTLE 是工业巡检最容易被漏掉的一环：命令归零不等于车停稳，轮胎弹性、地面坡度都会让车继续蠕动，而蠕动 2 cm 就足以让测温或拍照失焦。判据必须是“实测速度 + 实测角速度同时低于阈值，并持续 400 ms”，任何一次超限都重置计时。
要点解读
一、轻量级的正确做法是“信息压缩”，不是“算法降级”
把 12 路测距压成 9 个扇区（案例一），把 30 个栅格压成 3~5 个路点（案例二），把连续轨迹压成 6 个巡检点（案例三）——这三步都在做同一件事：在进入规划层之前把数据量降两个数量级。这样规划层的计算量从“搜索”降到“扫描”，在 MCU 上就能跑。反过来做（先上完整栅格 A*，再想办法优化）在 ESP32 上勉强能跑，但在 1 kHz 执行环旁边会不断抢 CPU，最终表现为电机抖动——轻量化的第一刀应该切在数据入口，而不是算法本身。
二、三层结构的本质是“粗糙的决策 + 精细的过渡”
感知层给出的必然是粗糙的（扇区只有 9 个、路点只有 3 个）；如果直接把它当指令下发，机器人就会画折线、猛打方向。平滑层的职责就是把离散决策变成连续运动，具体是三件事：① 速率限幅（一阶连续）；② 加加速度限幅（二阶连续，案例一的 W_JERK、案例三的 wAcc）；③ 与运动学耦合（转角大则降速、曲率大则限速）。少了这一层，规划再准，车也是“一顿一顿”的——这就是为什么工业 AGV 里“平滑”往往比“规划”更值钱。
三、转向必须与速度耦合，否则平滑只是把抖动变慢
最典型的问题：转角大时速度不变，车就会“甩尾式过弯”，离心加速度超限，轮胎侧滑、里程失真，甚至侧翻。两个必须耦合的公式：① 大偏角降速 v = V·cos(α)（案例一、二）；② 曲率限速 v ≤ sqrt(a_lat/|κ|)（案例二）。前者是几何约束（转向前先减速），后者是动力学约束（侧向加速度上限）。二者叠加后，转弯会自动呈现“减速—转向—加速”的连续过程，这才是真正的平滑，而不是简单滤波。
四、工业巡检的优先级是“任务 > 通行”，避障不能把巡检点弄丢
消费级机器人绕开障碍就算成功，工业巡检不是：绕过障碍但没到巡检点，等于这次巡检白做。所以案例三的结构是绕行后必须回归目标（S_DETOUR → S_CRUISE，航向始终指向巡检点），而不是“躲开就往前开”。更进一步，接近段用速度包络（v = sqrt(2·a·d)）而不是固定低速：既保证末端速度趋零（精停精度），又不牺牲中段效率。这条“任务优先”的原则还体现在失效处理上——绕行超时（4 s）宁可停机上报，也不能带着未完成的巡检点继续跑。
五、闭环要“停在物理意义上”，而不是停在指令意义上
move(0) 只是让电机不再出力，车未必停住。工业巡检的停点判定必须是物理量 + 时间双重确认：实测线速度 < 0.02 m/s、角速度 < 0.05 rad/s，并持续 400 ms（案例三 S_SETTLE）。同时要有明确的三段收束：接近（包络减速）→ 对准（原地校朝向）→ 停稳（双阈值确认），缺任何一段都会出现“到了但没对准”或“对准了但还在漂”。这条判据是所有巡检动作（拍照、测温、测振、贴标）能重复一致的前提，也是轻量闭环里最不可省的一环。
请注意：以上案例仅作为思路拓展的参考示例，不保证完全正确、适配所有场景或可直接编译运行。由于硬件平台、实际使用场景、Arduino 版本的差异，均可能影响代码的适配性与使用方法的选择。在实际编程开发时，请务必根据自身硬件配置、使用场景及具体功能需求进行针对性调整，并通过多次实测验证效果；同时需确保硬件接线正确，充分了解所用传感器、执行器等设备的技术规范与核心特性。对于涉及硬件操作的代码，使用前务必核对引脚定义、电平参数等关键信息的准确性与安全性，避免因参数错误导致硬件损坏或运行异常。
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
0
0- 0
已为社区贡献477条内容
所有评论(0)