【花雕学编程】Arduino BLDC 之BLDC差速底盘机器人扫描运动 + FOC闭环执行
·
“Arduino BLDC 之 BLDC 差速底盘机器人扫描运动 + FOC 闭环执行”可以理解为：上层规划扫描路径并输出线速度/角速度或目标位姿，中层把运动指令解算为左右轮独立的速度/扭矩指令，下层由 BLDC FOC 电流环、速度环和位置环精确执行；同时通过编码器里程计、IMU 航向和电流反馈持续修正左右轮输出，保证扫描轨迹平滑、转向稳定、抗扰动能力强。
一、主要特点
- 扫描运动依赖连续、低抖动的差速控制
扫描通常要求机器人沿直线、弧线、螺旋线或栅格路径运动，并在转向时保持传感器视场稳定。差速底盘的运动学关系是：线速度由左右轮平均速度决定，角速度由左右轮速度差决定。如果左右轮响应不一致，扫描轨迹会出现蛇形、偏航、转角过冲，直接影响激光、视觉或超声扫描质量。 - FOC 闭环是低速平稳执行的基础
BLDC 配合 FOC 后，三相电流被解耦为 d 轴励磁分量和 q 轴转矩分量，电流环通过调节电压间接控制 Iq，从而实现连续、可预测的扭矩输出。相比六步方波驱动，FOC 能显著降低低速抖动和换相冲击，使机器人在慢速扫描、原地旋转、贴墙行驶和微小转向时更平稳。 - 典型采用“位置/航向环 + 速度环 + 电流环”分层结构
最内环：FOC 电流环，负责快速响应负载变化；
中间环：速度环，保证左右轮准确跟踪目标转速；
最外环：位置或航向环，根据里程计、IMU 或外部定位修正轨迹。
整定顺序应先电流环、再速度环、最后位置/航向环，避免外层误差被内环不稳定放大。 - 差速同步不能只靠左右轮各自闭环
两轮独立速度闭环仍然可能因轮胎磨损、地面摩擦差异、负载偏置、电池电压跌落而产生航向漂移。工程上常引入交叉耦合、主从跟随或 IMU 航向纠偏：当一侧轮速异常或航向偏离时，同时修正左右轮输出，而不是只单独提高某一侧速度。 - 扫描路径需要前馈补偿
纯 PID 反馈在加减速和转弯时容易滞后。加入速度前馈、角速度前馈或加速度前馈后，系统能提前补偿惯性和摩擦，使直线段更直、圆弧段更顺滑、转向响应更及时。 - Arduino 平台更适合做轻量原型或单轮执行层
ATmega328P 可以运行简化版速度闭环、差速解算和基础 FOC，但若同时承担双轮 FOC、里程计积分、IMU 融合、扫描路径规划和串口通信，实时性会明显吃紧。更稳妥的是采用 ESP32-S3、STM32F4/G4、Teensy 4.1 或上位机负责上层规划，底层 MCU 专注电机闭环。
二、应用场景 - 室内扫描/清洁机器人
扫地机、清洁机器人或室内建图小车常采用双轮差速结构。FOC 闭环能保证低速贴边、原地旋转和跨越地毯边缘时不抖动，同时提升里程计积分稳定性。 - 激光/视觉 SLAM 移动底盘
SLAM 对轨迹平滑性和航向一致性要求较高。差速同步和 IMU 纠偏能减少“走直线却缓慢偏航”的问题，使建图和定位更可靠。 - 园区/厂区巡检机器人
在走廊、厂房、配电房等场景中，机器人需要沿固定路线扫描设备、仪表或热源。FOC 差速控制可提升低速巡航稳定性，并在门槛、接缝、坡道等扰动下保持轨迹。 - 教育与竞赛平台
适合验证差速运动学、编码器测速、FOC 三环整定、IMU 航向补偿、交叉耦合同步、前馈控制和路径跟踪。 - 轻量 AGV/送物机器人
对停位精度、直线跟踪和启停平滑性要求较高。双轮差速配合 FOC 闭环可减少货物晃动、轮端打滑和定位误差。 - 小型移动扫描云台底盘
若机器人搭载单线激光、深度相机或气体传感器，底盘需要以稳定角速度旋转或沿规则栅格移动。FOC 闭环可降低转向抖动，提高扫描数据一致性。
三、需要注意的事项 - 轮径、轮距和编码器方向必须标定
差速运动学高度依赖轮径 R 和轮距 D。若参数不准，机器人会出现直线跑偏、转弯半径偏差和里程计漂移。编码器方向错误会导致左右轮反馈符号相反，闭环直接发散。 - 编码器测速要兼顾低速精度和高速响应
低速时纯频率法误差大，高速时纯周期法分辨率不足，可采用 M/T 法、滑动窗口平均或互补滤波。编码器线缆应屏蔽并远离动力线，避免速度噪声被微分项放大。 - 电流环稳定是前提，不能跳过直接调速度环
FOC 的底层是电流环，若电流采样、母线电压、PWM 频率、死区时间或编码器角度未校准，速度环和位置环都会不稳定。应先确保 Iq 跟踪准确，再加入速度前馈和航向纠偏。 - 控制周期要固定，避免使用阻塞延时
电流环通常需要较高频率，速度环常在百赫兹量级，航向/位置环可更低。底层闭环应放在定时器中断或实时任务中，避免在串口打印、传感器读取或路径计算中插入 delay。 - 必须加入 IMU 航向反馈
仅靠左右轮速度闭环无法完全消除跑偏。地面摩擦不均、轮胎磨损、重心偏移都会造成不对称。应把 IMU 横摆角速度或航向角误差反馈到左右轮分配中，形成“差速 + 航向”双纠正。 - 扫描路径需要限幅和斜率限制
速度、角速度和加速度都应设置上限。否则从静止直接跳到高速、或原地快速反转，会引起电流尖峰、轮端打滑和传感器数据跳变。 - 打滑和堵转要有检测与降级策略
当某轮编码器速度异常升高而电流下降，或电流持续饱和但速度不上升时，应判定打滑或堵转。此时应降低扭矩、暂停扫描路径跟踪，并进入安全等待，而不是继续累积位置误差。 - 电源与 EMC 需要隔离设计
双路 BLDC 同时加减速会造成母线电压波动，可能干扰 IMU、编码器和 MCU。控制电源建议使用独立 DC-DC，动力线与控制线分开布线，并在驱动器附近布置足够退耦电容。 - 扫描任务与电机闭环应分层运行
上层负责扫描路径生成、传感器触发和数据记录；下层负责左右轮 FOC 闭环、里程计更新和故障保护。两层之间通过固定周期交换数据，避免上层计算阻塞底层电机控制。 - 8 位 Arduino 不建议承担完整扫描 + 双轮 FOC
如果只做一个轮子的 FOC 执行、简单速度闭环或轻量差速解算，ATmega328P 仍可尝试；但双轮 FOC、IMU 融合、里程计积分、扫描路径规划和实时同步更建议交给 32 位 MCU 或上位机。
整体来看，这套系统的核心链路是：扫描路径给出期望运动 → 差速运动学分解为左右轮目标 → FOC 电流环保证扭矩柔顺输出 → 速度环保证轮速跟踪 → 里程计/IMU 闭环修正航向与轨迹。做原型时，建议按“单轮 FOC 电流环 → 单轮速度闭环 → 双轮差速同步 → IMU 航向纠偏 → 扫描路径跟踪 → 抗打滑降级”逐步推进。
1、原地旋转扫描 + FOC 速度闭环执行（基础扫描模式）
适用场景：机器人在未知区域暂停行进，通过左右轮反向等速旋转实现原地扫描，配合超声波传感器采集周围距离信息，扫描完成后恢复直行。核心是差速底盘的零半径转向能力与 FOC 速度环的精确执行。
#include <SimpleFOC.h>
// 双轮 BLDC 差速底盘
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11, 8);
BLDCDriver3PWM driverR(3, 5, 6, 7);
Encoder encL(2, 3, 2048), encR(18, 19, 2048);
// 超声波（扫描时采集距离）
#define TRIG 22
#define ECHO 23
// 扫描参数
float scanSpeed = 1.5; // 扫描旋转速度 rad/s
unsigned long scanDuration = 3000; // 扫描持续时间 ms
unsigned long scanStart = 0;
bool scanning = false;
float readDistance() {
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 startScan() {
scanning = true;
scanStart = millis();
Serial.println("扫描开始");
}
void loop() {
motorL.loopFOC(); motorR.loopFOC();
if (scanning) {
// 原地旋转：左轮正转，右轮反转
motorL.move(scanSpeed);
motorR.move(-scanSpeed);
// 扫描期间采集距离
float dist = readDistance();
static unsigned long lastPrint = 0;
if (millis() - lastPrint > 300) {
lastPrint = millis();
Serial.print("扫描距离: "); Serial.println(dist);
}
// 扫描结束
if (millis() - scanStart > scanDuration) {
scanning = false;
motorL.move(0);
motorR.move(0);
Serial.println("扫描结束，恢复直行");
}
} else {
// 正常直行
motorL.move(1.0);
motorR.move(1.0);
// 触发扫描条件（简化：每 5 秒扫描一次）
static unsigned long lastScan = 0;
if (millis() - lastScan > 5000) {
lastScan = millis();
startScan();
}
}
delay(10);
}
解读：该案例利用差速底盘的零半径转向特性实现“扫描运动”：motorL.move(scanSpeed) 与 motorR.move(-scanSpeed) 使底盘绕中心旋转。FOC 速度环确保两轮在低速反向旋转时转速精确对称，避免因轮速差异导致扫描轨迹偏移。搜索结果中差速驱动的基础框架直接支持这种左右轮独立速度控制，而 FOC 的速度闭环保证了 scanSpeed 指令的精确跟踪。
2、分段扫描 + 航向记录 + FOC 位置闭环回正
适用场景：机器人需要扫描多个固定方向（如左、前、右），每个方向停留采集数据，扫描完成后精确回到初始航向继续执行任务。核心是利用 FOC 位置环记录和恢复航向。
#include <SimpleFOC.h>
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11, 8);
BLDCDriver3PWM driverR(3, 5, 6, 7);
Encoder encL(2, 3, 2048), encR(18, 19, 2048);
// 扫描方向角度（相对于初始航向）
const float scanAngles[] = {-0.8, 0.0, 0.8}; // 左、前、右 (rad)
const int scanCount = 3;
int currentScanIdx = 0;
bool scanning = false;
// 轮距与轮半径（用于角度→轮位置换算）
const float wheelBase = 0.30; // m
const float wheelRadius = 0.05; // m
// 初始轮位置记录
float initPosL = 0, initPosR = 0;
// 角度转轮位移：原地旋转 θ 弧度，每轮移动 wheelBase/2 * θ
float angleToWheelPos(float angle) {
return (wheelBase / 2.0) * angle / wheelRadius; // 转换为 rad（电机轴）
}
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::angle; // 位置模式
motorR.controller = MotionControlType::angle;
motorL.P_angle.P = 15;
motorR.P_angle.P = 15;
motorL.init(); motorL.initFOC();
motorR.init(); motorR.initFOC();
// 记录初始位置
initPosL = motorL.shaftAngle();
initPosR = motorR.shaftAngle();
}
void startScan() {
scanning = true;
currentScanIdx = 0;
Serial.println("分段扫描开始");
}
void loop() {
motorL.loopFOC(); motorR.loopFOC();
if (scanning) {
float targetAngle = scanAngles[currentScanIdx];
float wheelOffset = angleToWheelPos(targetAngle);
// 原地旋转：左轮 +offset，右轮 -offset（差速旋转）
motorL.move(initPosL + wheelOffset);
motorR.move(initPosR - wheelOffset);
// 到达目标角度附近后停留采集
float actualOffset = motorL.shaftAngle() - initPosL;
if (abs(actualOffset - wheelOffset) < 0.05) {
// 采集数据（简化：打印）
Serial.print("扫描方向 "); Serial.print(currentScanIdx);
Serial.print(" 角度: "); Serial.println(targetAngle);
delay(500);
currentScanIdx++;
if (currentScanIdx >= scanCount) {
// 扫描完成，回到初始位置
motorL.move(initPosL);
motorR.move(initPosR);
if (abs(motorL.shaftAngle() - initPosL) < 0.05) {
scanning = false;
Serial.println("扫描完成，已回正");
}
}
}
} else {
// 正常任务
static unsigned long lastScan = 0;
if (millis() - lastScan > 8000) {
lastScan = millis();
startScan();
}
}
delay(10);
}
解读：该案例将扫描运动从“连续旋转”升级为“分段定位”。FOC 位置环在此承担双重角色：扫描时精确到达每个目标角度（angleToWheelPos 将底盘旋转角度换算为轮轴位置增量），扫描完成后精确回到初始位置。搜索结果中 SimpleFOC 的位置控制模式支持这种精确角度定位。与案例一的速度环扫描相比，位置环扫描消除了累积误差，适合需要多次扫描且每次扫描后必须回到同一基准的场景。
3、扫描-执行协同状态机 + FOC 双环切换
适用场景：机器人在扫描模式下用速度环快速旋转采集，检测到目标后立即切换到位置环锁定朝向，执行对准或追踪。核心是速度环与位置环的模式切换，以及状态机对“扫描中/锁定中/执行中”的管理。
#include <SimpleFOC.h>
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11, 8);
BLDCDriver3PWM driverR(3, 5, 6, 7);
Encoder encL(2, 3, 2048), encR(18, 19, 2048);
// 状态机
enum RobotState { IDLE, SCANNING, LOCKING, EXECUTING };
RobotState state = IDLE;
unsigned long stateTimer = 0;
// 扫描参数
float scanSpeed = 2.0;
// 锁定参数
float lockAngle = 0; // 目标锁定角度
// 模拟目标检测（实际由传感器触发）
bool targetDetected = false;
float targetAngle = 0;
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.init(); motorL.initFOC();
motorR.init(); motorR.initFOC();
}
void enterState(RobotState s) {
state = s;
stateTimer = millis();
switch (s) {
case SCANNING:
motorL.controller = MotionControlType::velocity;
motorR.controller = MotionControlType::velocity;
Serial.println("进入扫描模式");
break;
case LOCKING:
motorL.controller = MotionControlType::angle;
motorR.controller = MotionControlType::angle;
motorL.P_angle.P = 20;
motorR.P_angle.P = 20;
// 记录当前角度作为锁定基准
lockAngle = motorL.shaftAngle();
Serial.println("进入锁定模式");
break;
case EXECUTING:
motorL.controller = MotionControlType::velocity;
motorR.controller = MotionControlType::velocity;
Serial.println("进入执行模式");
break;
default:
break;
}
}
void loop() {
motorL.loopFOC(); motorR.loopFOC();
// 模拟目标检测（每 8 秒触发一次）
static unsigned long lastTrigger = 0;
if (millis() - lastTrigger > 8000 && state == SCANNING) {
lastTrigger = millis();
targetDetected = true;
targetAngle = motorL.shaftAngle(); // 记录检测时的角度
}
switch (state) {
case IDLE:
motorL.move(0); motorR.move(0);
if (millis() - stateTimer > 1000) enterState(SCANNING);
break;
case SCANNING:
motorL.move(scanSpeed);
motorR.move(-scanSpeed);
if (targetDetected) {
targetDetected = false;
enterState(LOCKING);
}
break;
case LOCKING:
// 位置环保持在检测时的角度
motorL.move(lockAngle);
motorR.move(-lockAngle);
if (millis() - stateTimer > 1000) {
enterState(EXECUTING);
}
break;
case EXECUTING:
// 朝向锁定角度直行
motorL.move(1.2);
motorR.move(1.2);
if (millis() - stateTimer > 2000) {
enterState(IDLE);
}
break;
}
delay(10);
}
解读：该案例的核心是控制模式随状态机动态切换。扫描阶段使用速度环（快速旋转、功耗低），检测到目标后切换到位置环（精确保持朝向、抵抗扰动），执行阶段再切回速度环。SimpleFOC 支持运行时修改 motor.controller 并保持 FOC 底层运行，这使得模式切换无需重新初始化电机。搜索结果中差速驱动的位置/速度模式切换示例提供了这一能力的代码基础。FOC 闭环在此的作用是：无论速度环还是位置环，底层电流/电压的 FOC 解算持续运行，保证模式切换瞬间的力矩连续性。
要点解读
-
“扫描运动”在差速底盘上的实现本质是零半径旋转，而非额外扫描机构
与云台扫描不同，差速底盘的扫描运动通过左右轮反向等速旋转实现车体整体旋转。案例一的 motorL.move(scanSpeed) + motorR.move(-scanSpeed) 是这一运动的最简形式。这种方式的优势是不需要额外的舵机或云台，整车的传感器（超声波、摄像头、ToF）随车体一起扫描；代价是扫描过程中底盘占用空间较大，不适合狭窄通道内的原地扫描。 -
FOC 速度环保证扫描的匀速性，位置环保证扫描的可重复性
案例一依赖速度环维持恒定 scanSpeed，如果左右轮速因地面摩擦差异而不对称，扫描轨迹会偏离圆形。FOC 的闭环速度控制将两轮实际转速反馈与目标值比较，自动补偿负载差异。案例二则使用位置环，其优势是扫描角度不随时间累积漂移——每次扫描都基于编码器绝对位置，适合需要多次重复扫描且每次结果可比较的场景。 -
扫描模式与执行模式的切换必须包含“软过渡”，避免力矩突变
案例三在 LOCKING 状态切换时重新设置 P_angle.P 并记录当前角度作为目标。如果直接从速度环切到位置环而不做过渡，位置环会以当前角度与目标角度的巨大误差计算力矩，导致电机猛冲。SimpleFOC 的 motor.controller 切换本身是瞬时的，但实际应用中应在切换前先将速度指令降至零附近，或使用低通滤波平滑目标值。搜索结果中差速驱动的位置/速度协同案例同样隐含了这一过渡需求。 -
扫描期间的传感器数据必须与角度关联，否则扫描失去意义
案例一和案例三在扫描时仅打印距离或记录角度，但完整的扫描运动应建立“角度-数据”映射表：每旋转一个固定角度间隔（如 15°），记录该方向上的传感器读数。差速底盘的扫描角度可通过编码器精确推算（wheelOffset = (wheelBase/2) * angle / wheelRadius），而非依赖时间估算。案例二的 angleToWheelPos 函数展示了这一换算关系。没有角度关联的扫描只能得到一维距离序列，无法用于构建环境地图或目标方位判断。 -
Arduino 主循环的时序分配：FOC 高频、扫描逻辑低频
三个案例的 delay(10) 对应约 100Hz 的主循环频率，但 loopFOC() 实际应以更高频率运行。在 Arduino 上，loopFOC() 的执行时间取决于 MCU 主频和电机数量，100Hz 的循环意味着 FOC 更新率仅 100Hz，远低于推荐的 1kHz+。合理的做法是：将 loopFOC() 放在定时器中断中（或主循环中去掉 delay 以最大速度运行），扫描状态机的逻辑每 10-20ms 执行一次。搜索结果中 SimpleFOC 在 Arduino 上的基础示例通常不包含 delay，而是让 loopFOC() 尽可能频繁地执行。若使用 ESP32 等双核平台，可将 FOC 放在一个核心、扫描逻辑放在另一个核心。
4、阿基米德螺旋扫描 + 纯追踪 + FOC 速度闭环
适用硬件：ESP32 + SimpleFOC 差速底盘（2×BLDC）+ 双编码器 + IMU（航向辅助）。
核心思路：螺旋扫描不做成固定时间节拍，而做成几何参数序列：r = k·θ，每转一圈半径增加一个“扫描间距”。运行时用纯追踪（pure pursuit）朝螺旋上的前视点走，输出 (v, ω)，再解算成左右轮速，交给 FOC 速度环执行。覆盖率由 k 决定，跟踪误差由里程闭环吸收。
/*
* 案例4：阿基米德螺旋扫描 + 纯追踪 + FOC 速度环
* 螺旋：r = PITCH * θ/(2π)，PITCH=扫描间距（略小于传感器幅宽）
*/
#include <SimpleFOC.h>
#include <Wire.h>
#include <MPU6050.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(); }
MPU6050 imu;
// ---- 车体参数 ----
const float R_W = 0.065f; // 轮半径 m
const float TRACK = 0.34f; // 轮距 m
const float V_SCAN = 0.28f; // 扫描线速度 m/s
const float PITCH = 0.26f; // 螺旋间距 m（≈传感器幅宽×0.85）
const float LOOKAHEAD = 0.22f; // 前视距离 m
const float ACC_MAX = 0.5f; // 加速度上限 m/s²
// ---- 位姿与里程 ----
float px = 0, py = 0, pth = 0;
float vCur = 0, wCur = 0;
// ---- 螺旋参数（极坐标）----
float thetaSpiral = 0; // 当前螺旋参数角
float rSpiral = 0;
// 由 theta 求笛卡尔点（世界系，起点为原点）
void spiralPoint(float th, float &x, float &y) {
float r = PITCH * th / (2.0f * PI);
x = r * cosf(th);
y = r * sinf(th);
}
// 沿螺旋前进 ds 弧长对应的 dθ（弧长≈sqrt(r²+(dr/dθ)²)·dθ）
float thetaStep(float th, float ds) {
float r = PITCH * th / (2.0f * PI);
float drdth = PITCH / (2.0f * PI);
float jac = sqrtf(r*r + drdth*drdth);
return ds / max(jac, 0.02f);
}
float angDiff(float a, float b) {
float d = a - b;
while (d > PI) d -= 2*PI;
while (d < -PI) d += 2*PI;
return d;
}
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.35f; wheelL.PID_velocity.I = 4.0f;
wheelL.PID_velocity.output_ramp = 400.0f;
wheelR.PID_velocity.P = 0.35f; wheelR.PID_velocity.I = 4.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();
Wire.begin(); imu.initialize();
thetaSpiral = 0.0f; rSpiral = 0.0f;
Serial.begin(115200);
}
void loop() {
static unsigned long tPrev = millis();
// ---------- 1. 里程计：轮速 -> (v,ω) -> 位姿 ----------
float vl = wheelL.shaft_velocity * R_W;
float vr = wheelR.shaft_velocity * R_W;
vCur = 0.5f * (vl + vr);
wCur = (vr - vl) / TRACK;
unsigned long now = millis();
float dt = (now - tPrev) / 1000.0f;
tPrev = now;
if (dt <= 0 || dt > 0.2f) dt = 0.02f;
px += vCur * cosf(pth) * dt;
py += vCur * sinf(pth) * dt;
pth += wCur * dt;
// ---------- 2. 螺旋推进：按已走弧长更新 theta ----------
float ds = vCur * dt;
thetaSpiral += thetaStep(thetaSpiral, ds);
float sx, sy;
spiralPoint(thetaSpiral, sx, sy);
// ---------- 3. 纯追踪：找前视点（沿螺旋再往前一段弧长）----
float thLook = thetaSpiral + thetaStep(thetaSpiral, LOOKAHEAD);
float lx, ly;
spiralPoint(thLook, lx, ly);
float dx = lx - px, dy = ly - py;
float ld = hypotf(dx, dy);
if (ld < 0.01f) ld = 0.01f;
// 纯追踪曲率：kappa = 2*sin(alpha)/ld，alpha = 前视点方向 - 当前航向
float alpha = angDiff(atan2f(dy, dx), pth);
float kappa = 2.0f * sinf(alpha) / ld;
// ---------- 4. 输出 (v,ω)，含加速度限幅 ----------
static float vCmdPrev = 0;
float vTgt = V_SCAN * cosf(constrain(alpha, -1.2f, 1.2f)); // 大偏差时降速
vTgt = max(vTgt, 0.06f); // 保底蠕动
float vCmd = vCmdPrev + constrain(vTgt - vCmdPrev, -ACC_MAX*dt, ACC_MAX*dt);
vCmdPrev = vCmd;
float wCmd = vCmd * kappa;
wCmd = constrain(wCmd, -1.6f, 1.6f);
// ---------- 5. FOC 执行：差速解算 -> 轮速 ----------
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);
Serial.print("r="); Serial.print(PITCH*thetaSpiral/(2*PI), 2);
Serial.print(" v="); Serial.print(vCmd, 2);
Serial.print(" w="); Serial.print(wCmd, 2);
Serial.print(" e="); Serial.println(ld, 3);
delay(20); // 50Hz 扫描层；轮速环建议移入 1kHz 定时器
}
螺旋扫描的间距 PITCH 要与传感器有效幅宽挂钩（通常取 0.8~0.9 倍，留重叠冗余）。vTgt *= cos(alpha) 是必要的降速逻辑：航向偏差大时先转向再加度，否则会在螺旋外侧“甩出去”。
案例5、弓字形栅格扫描 + 定点扇形扫描（搜索）+ 位置/航向双闭环
适用硬件：ESP32 + SimpleFOC 差速底盘 + 编码器 + IMU + 云台传感器（可选）。
核心思路：把扫描分成两类动作：覆盖型（弓字形逐行推进）与搜索型（到点后原地自转，做扇形扫描）。两者都用同一套“到点 + 转向”的位置闭环：走直线用航向 PID、原地转用角速度积分判断是否到位，执行仍由 FOC 速度环承担。
/*
* 案例5：弓字形覆盖 + 定点扇形扫描
* 状态：LINE(直行) / TURN(换行) / SECTOR(原地扇扫) / DONE
*/
#include <SimpleFOC.h>
#include <Wire.h>
#include <MPU6050.h>
BLDCMotor wheelL(7), wheelR(7);
// 驱动/编码器初始化同案例4（略）
MPU6050 imu;
const float R_W = 0.065f, TRACK = 0.34f;
// ---- 覆盖区域参数 ----
const float X_LEN = 4.0f; // 行长度 m
const float ROW_GAP = 0.30f; // 行距 m
const int N_ROW = 8;
const float V_ROW = 0.30f; // 行内速度
const float V_TURN = 0.12f;
// ---- 扇形扫描参数 ----
const int N_SECTOR = 12; // 一圈 12 个扇区（每 30° 停一次）
const float SECTOR_HOLD_MS = 400;
enum St { S_LINE, S_TURN, S_SECTOR, S_DONE };
St st = S_LINE;
float px = 0, py = 0, pth = 0, vCur = 0, wCur = 0;
int rowIdx = 0;
bool dirFwd = true;
int sectorIdx = 0;
unsigned long sectorT0 = 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;
}
// ---- 航向 PID（输出角速度）----
const float KP_H = 1.8f, KD_H = 0.12f;
float hPrevErr = 0;
float headingCtrl(float target, float dt) {
float e = angDiff(target, pth);
float d = (e - hPrevErr) / dt;
hPrevErr = e;
return constrain(KP_H*e + KD_H*d, -1.3f, 1.3f);
}
// ---- 直行：横向偏差 + 航向双修正 ----
float rowYRef = 0;
float lineCtrl(float dt) {
float eLat = rowYRef - py; // 横向偏差（沿 Y 方向偏移）
float yawCorr = constrain(1.2f * eLat, -0.35f, 0.35f); // 偏差转航向修正
float targetYaw = dirFwd ? 0.0f : PI;
targetYaw += yawCorr;
return headingCtrl(targetYaw, dt);
}
void setup() {
// 电机初始化同案例4（速度环模式）
Wire.begin(); imu.initialize();
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.2f) dt = 0.02f;
// ---- 里程计 ----
float vl = wheelL.shaft_velocity * R_W;
float vr = wheelR.shaft_velocity * R_W;
vCur = 0.5f*(vl+vr); wCur = (vr-vl)/TRACK;
px += vCur*cosf(pth)*dt; py += vCur*sinf(pth)*dt; pth += wCur*dt;
pth = angDiff(pth, 0.0f); // 归一化到 [-π, π]
float vCmd = 0, wCmd = 0;
switch (st) {
case S_LINE: {
rowYRef = rowIdx * ROW_GAP;
wCmd = lineCtrl(dt);
vCmd = V_ROW;
// 到行末：前进到 X_LEN，或反向回到 0
bool done = dirFwd ? (px >= X_LEN) : (px <= 0.0f);
if (done) { st = S_TURN; hPrevErr = 0; }
break;
}
case S_TURN: {
// 原地转向 90°：先判断下一行在上方还是下方
float nextY = rowIdx * ROW_GAP + ((rowIdx < N_ROW-1) ? ROW_GAP : 0);
bool goUp = (nextY > rowYRef);
float targetYaw = goUp ? (PI/2) : (-PI/2);
// 转向同时小幅前进，避免原地打滑
vCmd = V_TURN * 0.4f;
wCmd = headingCtrl(targetYaw, dt);
if (fabsf(angDiff(targetYaw, pth)) < 0.08f) {
// 转向完成：横移一行（继续用直行控制，目标 Y 更新）
rowIdx++;
if (rowIdx >= N_ROW) { st = S_SECTOR; sectorIdx = 0; sectorT0 = millis(); }
else { dirFwd = !dirFwd; st = S_LINE; hPrevErr = 0; }
}
break;
}
case S_SECTOR: {
// 定点扇形扫描：每 30° 停 400ms（此处以停转 + 采样模拟）
vCmd = 0;
float targetYaw = angDiff(sectorIdx * (2*PI/N_SECTOR), 0.0f);
wCmd = headingCtrl(targetYaw, dt);
if (fabsf(angDiff(targetYaw, pth)) < 0.05f) {
wCmd = 0;
if (millis() - sectorT0 > SECTOR_HOLD_MS) {
sectorIdx++;
sectorT0 = millis();
// 此处触发一次传感器采样 / 拍照
if (sectorIdx >= N_SECTOR) st = S_DONE;
}
}
break;
}
case S_DONE:
vCmd = 0; wCmd = 0;
break;
}
// ---- 加速度 / 角加速度限幅 ----
static float vPrev = 0, wPrev = 0;
vCmd = vPrev + constrain(vCmd - vPrev, -0.6f*dt, 0.6f*dt);
wCmd = wPrev + constrain(wCmd - wPrev, -2.5f*dt, 2.5f*dt);
vPrev = vCmd; wPrev = wCmd;
// ---- FOC 执行 ----
float vL = vCmd - wCmd*TRACK*0.5f, vR = vCmd + wCmd*TRACK*0.5f;
wheelL.loopFOC(); wheelR.loopFOC();
wheelL.move(vL/R_W); wheelR.move(vR/R_W);
Serial.print("st="); Serial.print(st);
Serial.print(" row="); Serial.print(rowIdx);
Serial.print(" sec="); Serial.print(sectorIdx);
Serial.print(" yaw="); Serial.println(pth, 2);
delay(20);
}
关键细节：换行时让 vCmd 保留 40% 的前进分量，也就是“边走边转向”；纯原地转在差速底盘上会因两轮反向摩擦而打滑、里程失真。扇形扫描每 30° 停 400ms，是为了给传感器留出采样窗口（相机曝光、激光整圈），并非为了控制。
6、FreeRTOS 三层 + 扫描状态机 + 堵转/打滑保护（工程版）
适用硬件：ESP32 双核 + SimpleFOC 差速底盘 + 编码器 + IMU + 电流采样。
核心思路：扫描层（20Hz）负责生成目标和推进状态；运动学层（100Hz）负责解算 (v,ω) 并做里程与误差监控；执行层（1kHz）只做轮速环和限幅。堵转、打滑和里程漂移都在运动学层判定，并按等级降级（降速、重定位、中止）。
/*
* 案例6：扫描规划(20Hz) - 运动学(100Hz) - FOC执行(1kHz)
* 保护：堵转(电流高+轮速低) / 打滑(指令与轮速长期不符) / 里程漂移(IMU 航向偏差)
*/
#include <SimpleFOC.h>
#include <Wire.h>
#include <MPU6050.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
BLDCMotor wheelL(7), wheelR(7);
// 驱动/编码器初始化同案例一（略）
MPU6050 imu;
SemaphoreHandle_t mux;
const float R_W = 0.065f, TRACK = 0.34f;
// ---- 扫描目标（极简螺旋，可换成任意轨迹）----
volatile float t_x = 1.0f, t_y = 0.0f; // 当前目标点
volatile float t_vmax = 0.30f; // 该段限速
volatile bool t_done = false;
// ---- 执行输出 ----
volatile float c_v = 0, c_w = 0;
volatile bool c_stop = false;
// ---- 状态 ----
volatile float s_px=0, s_py=0, s_pth=0, s_v=0, s_w=0;
volatile float s_iL=0, s_iR=0, s_yawImu=0;
volatile int s_fault = 0; // 0 正常 1 打滑 2 堵转 3 航向漂移
// ---- 螺旋目标生成器（扫描层内部）----
float theta = 0.0f;
const float PITCH = 0.26f;
void nextSpiralTarget(float stepDs) {
float r = PITCH*theta/(2*PI);
float dr = PITCH/(2*PI);
float jac = sqrtf(r*r + dr*dr);
theta += stepDs / max(jac, 0.02f);
float rr = PITCH*theta/(2*PI);
t_x = rr*cosf(theta);
t_y = rr*sinf(theta);
}
// ---------------- 扫描规划层 20Hz ----------------
void taskScan(void*) {
TickType_t last = xTaskGetTickCount();
for (;;) {
float px, py;
xSemaphoreTake(mux, portMAX_DELAY);
px = s_px; py = s_py;
xSemaphoreGive(mux);
float d = hypotf(t_x - px, t_y - py);
if (d < 0.10f) nextSpiralTarget(0.10f); // 到点 -> 推进螺旋
t_vmax = (d < 0.15f) ? 0.10f : 0.30f; // 接近目标降速
vTaskDelayUntil(&last, pdMS_TO_TICKS(50));
}
}
// ---------------- 运动学层 100Hz ----------------
void taskKin(void*) {
TickType_t last = xTaskGetTickCount();
float vPrev = 0, wPrev = 0;
int stallCnt = 0, slipCnt = 0, driftCnt = 0;
for (;;) {
// 里程
float vl = wheelL.shaft_velocity * R_W;
float vr = wheelR.shaft_velocity * R_W;
float v = 0.5f*(vl+vr), w = (vr-vl)/TRACK;
float dt = 0.01f;
xSemaphoreTake(mux, portMAX_DELAY);
float px = s_px, py = s_py, th = s_pth;
xSemaphoreGive(mux);
px += v*cosf(th)*dt; py += v*sinf(th)*dt;
th += w*dt;
while (th > PI) th -= 2*PI;
while (th < -PI) th += 2*PI;
// 纯追踪 -> (v,ω)
float dx = t_x - px, dy = t_y - py;
float ld = max(hypotf(dx, dy), 0.05f);
float alpha = atan2f(dy, dx) - th;
while (alpha > PI) alpha -= 2*PI;
while (alpha < -PI) alpha += 2*PI;
float vTgt = min(t_vmax, 1.2f*ld);
vTgt *= cosf(constrain(alpha, -1.2f, 1.2f));
float wTgt = vTgt * (2.0f*sinf(alpha)/ld);
wTgt = constrain(wTgt, -1.6f, 1.6f);
// 加速度限幅
vTgt = vPrev + constrain(vTgt - vPrev, -0.6f*dt, 0.6f*dt);
wTgt = wPrev + constrain(wTgt - wPrev, -3.0f*dt, 3.0f*dt);
vPrev = vTgt; wPrev = wTgt;
// ---- 保护判据 ----
float iL = fabsf(wheelL.current), iR = fabsf(wheelR.current);
int fault = 0;
// 堵转：指令有明显速度但两轮转速远低于指令，且电流高
bool stall = (fabsf(vTgt) > 0.12f) && (fabsf(v) < fabsf(vTgt)*0.25f)
&& (iL > 3.0f || iR > 3.0f);
stallCnt = stall ? stallCnt+1 : 0;
if (stallCnt > 30) fault = 2; // 持续 0.3s
// 打滑：轮速明显高于指令（空转）或左右轮速差异常
bool slip = (fabsf(vTgt) > 0.12f) && (fabsf(v) > fabsf(vTgt)*1.6f);
slipCnt = slip ? slipCnt+1 : max(0, slipCnt-2);
if (slipCnt > 40 && fault == 0) fault = 1;
// 航向漂移：IMU 航向与里程航向长期不符（打滑累积）
imu.update();
float yawImu = imu.getYaw() * DEG_TO_RAD;
float dYaw = fabsf(atan2f(sinf(yawImu - th), cosf(yawImu - th)));
driftCnt = (dYaw > 0.25f) ? driftCnt+1 : max(0, driftCnt-1);
if (driftCnt > 100 && fault == 0) fault = 3; // 1s
xSemaphoreTake(mux, portMAX_DELAY);
s_px = px; s_py = py; s_pth = th;
s_v = v; s_w = w; s_iL = iL; s_iR = iR; s_yawImu = yawImu;
s_fault = fault;
if (fault == 2) { c_v = 0; c_w = 0; c_stop = true; }
else if (fault == 1) { c_v = vTgt*0.5f; c_w = wTgt; c_stop = false; } // 降速
else if (fault == 3) { c_v = 0; c_w = wTgt; c_stop = false; } // 停走原地校航向
else { c_v = vTgt; c_w = wTgt; c_stop = false; }
xSemaphoreGive(mux);
vTaskDelayUntil(&last, pdMS_TO_TICKS(10)); // 100Hz
}
}
// ---------------- FOC 执行层 1kHz ----------------
void taskExec(void*) {
TickType_t last = xTaskGetTickCount();
for (;;) {
float v, w; bool stop;
xSemaphoreTake(mux, portMAX_DELAY);
v = c_v; w = c_w; stop = c_stop;
xSemaphoreGive(mux);
if (stop) { v = 0; w = 0; }
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);
vTaskDelayUntil(&last, pdMS_TO_TICKS(1));
}
}
void setup() {
// 电机初始化同案例一
Wire.begin(); imu.initialize();
mux = xSemaphoreCreateMutex();
xTaskCreatePinnedToCore(taskScan, "scan", 4096, NULL, 2, NULL, 0);
xTaskCreatePinnedToCore(taskKin, "kin", 4096, NULL, 2, NULL, 0);
xTaskCreatePinnedToCore(taskExec, "exec", 4096, NULL, 3, NULL, 1);
Serial.begin(115200);
}
void loop() { vTaskDelay(portMAX_DELAY); }
三类保护的判据必须相互独立，且采用不同响应：堵转时立刻停机，否则会烧电机；打滑时降低速度，否则越滑越快；航向漂移时停止平移、原地校正，否则螺旋轨迹会变成偏心圆。打滑计数采用“快速增加、缓慢减少”的滞回（slipCnt+1 与 -2），以避免路面上小石子造成的瞬时误判。
要点解读
一、扫描运动本质是把几何曲线参数化成 (v,ω) 时间序列，不是给舵机打角度
差速底盘的自由度只有一个平面位姿，但能原地转、能零半径转弯，因此理论上可跟踪任意连续曲线。工程上的做法是选一条参数化曲线（螺旋 r=kθ、弓字折线、扇形射线），用“已走弧长”推进参数，再用纯追踪把曲线转成瞬时 (v,ω)。关键认知：ω = v·κ（κ 为曲率），所以同一个几何曲线在不同速度下角速度不同，把 ω 写死而让 v 变，轨迹立刻变形。这是扫描运动与固定节拍轮系最本质的区别。
二、扫描间距必须与传感器幅宽挂钩，覆盖率靠“重叠冗余”而非精度
螺旋的 PITCH、弓字的 ROW_GAP 都不是随手取的数，它们必须 ≈ 传感器有效幅宽的 0.8~0.9 倍。原因很直接：里程计有误差、跟踪有滞后、地面有打滑，实际轨迹一定偏离理想曲线，只有留出 10~20% 重叠，才能保证“不漏扫”。反过来说，间距取小了覆盖率上去了但效率骤降（重复扫），工程上是覆盖率与效率的折衷参数，通常按实测漏扫率回头调。
三、扫描必须有位置闭环，开环发速度一定会“螺旋变偏心圆”
纯开环（按时间发 ω）在理想地面能画出螺旋，但两次打滑、一次轮径误差，螺旋中心就漂了，越往外圈偏得越厉害。案例一用纯追踪 + 里程反馈、案例三用 100Hz 运动学层，都是在闭环空间位置而不是闭环时间。更要紧的是航向：差速底盘的航向误差会随行进距离累积，而扫描轨迹对航向极其敏感，所以必须有 IMU 辅助（案例三的“航向漂移”判据就是干这个的），必要时原地重新校准。
四、转向过渡比直线段更影响扫描质量，边走边转优于原地硬转
扫描轨迹的“拐角”和“换行”是最容易出问题的位置：原地 90° 转向时两轮反向、轮胎侧滑、里程严重失真，转向后位置就偏了。所以案例二让换行时保留 40% 的前进分量（边走边转），案例一把纯追踪的前视距离设成 0.22m（而不是 0.05m），都是为了让转向成为一段弧而不是一个点。前视距离太短会追得紧但抖，太长会切角漏扫，这是需要实车调的一个关键参数。
五、扫描系统必须能识别“扫不动了”，且保护动作要按故障类型分级
扫描是长时间连续运动，最怕两种失效：堵转（撞上障碍或卡住，电流高转速低）和打滑（地毯、湿地面、轮胎磨损，轮转车不走）。两者的保护完全相反：堵转要立即停机（否则烧电机、烧驱动），打滑要降速（继续给力矩只会更滑）。案例三还加了第三类：航向漂移（IMU 与里程长期不符），响应是停平移、原地校航向、再继续。三类判据必须独立、响应必须分级，且都落在运动学层而不是执行层——执行层只认一个 (v,ω) 和一个 stop 标志，保持纯净。
请注意：以上案例仅作为思路拓展的参考示例，不保证完全正确、适配所有场景或可直接编译运行。由于硬件平台、实际使用场景、Arduino 版本的差异，均可能影响代码的适配性与使用方法的选择。在实际编程开发时，请务必根据自身硬件配置、使用场景及具体功能需求进行针对性调整，并通过多次实测验证效果；同时需确保硬件接线正确，充分了解所用传感器、执行器等设备的技术规范与核心特性。对于涉及硬件操作的代码，使用前务必核对引脚定义、电平参数等关键信息的准确性与安全性，避免因参数错误导致硬件损坏或运行异常。
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
2
0- 0
已为社区贡献483条内容
所有评论(0)