【花雕学编程】Arduino BLDC 之机器人超声波矩阵测距与卡尔曼滤波的单点目标稳定跟随
·
“Arduino BLDC 之机器人超声波矩阵测距与卡尔曼滤波的单点目标稳定跟随”可以理解为：在机器人底盘上布置多路超声波传感器形成空间感知矩阵，通过分时触发获取目标在不同方向上的距离信息；再利用卡尔曼滤波对目标位置、速度进行状态估计与预测，抑制超声波测距噪声和短暂遮挡带来的抖动；最后将滤波后的目标相对位置误差转化为线速度和角速度指令，由 BLDC 电机执行柔顺跟随。
一、主要特点
- 超声波矩阵提供方位感知能力
单个超声波传感器只能给出“前方有多远”，无法判断目标偏左还是偏右。通过在机器人前部或四周布置多个探头，例如左前、正前、右前，甚至增加后向传感器，可以比较不同探头的距离差，推算出目标相对于机器人中轴线的方位角。 - 分时触发避免声波串扰
多路超声波若同时发射，回波会互相干扰，导致测距异常。通常采用分时触发策略：依次触发每个探头，等待回波稳定后再触发下一个。这样虽然降低了单轮扫描频率，但能显著提升数据可靠性。 - 卡尔曼滤波抑制测距抖动与遮挡
超声波测距易受环境温度、多径反射、目标表面材质和气流影响，原始数据常出现跳变。卡尔曼滤波通过“预测-更新”两步，结合目标运动模型（如匀速假设）和实时测量值，输出平滑的位置与速度估计。当目标短暂被遮挡时，滤波器可依靠预测值维持短时间跟踪。 - 单点目标锁定与数据关联
系统通常选取距离最近、角度最稳定或符合预设特征的目标作为跟随对象。通过卡尔曼滤波维持目标状态，并在每次扫描后将新测量值与已有目标进行距离-角度关联，避免在多人或多障碍环境中频繁切换跟随对象。 - 跟随控制采用距离-角度双闭环
外环根据目标距离误差调节前进速度：距离过远则加速，过近则减速或后退。内环根据目标方位角误差调节转向：目标偏左则左转，偏右则右转。BLDC 电机配合 FOC 或编码器速度闭环，可使加减速和转向更平滑。 - 差速或全向底盘决定机动性
差速底盘结构简单，通过左右轮速差实现转向；全向底盘（如麦克纳姆轮）可横向平移，跟随路径更自然。Arduino 负责读取传感器、运行滤波和控制算法，并将线速度、角速度分解为各电机指令。
二、应用场景 - 智能行李车/购物车
在机场、超市等室内场景，机器人锁定用户腿部或随身信标，保持 1-2 米距离跟随，解放双手搬运物品。 - 工厂物料转运助手
跟随工人移动，承载工具、零件或检测设备，实现“人走到哪，物料送到哪”的柔性物流。 - 安防巡逻辅助
跟随安保人员移动，扩展监控范围或携带应急设备，在狭窄走廊、仓库通道中保持安全距离。 - 老年陪护与康复辅助
在室内跟随老人移动，提供随身物品携带、跌倒报警或紧急呼叫支持，超声波方案成本低且不受光线影响。 - 教育与科研平台
适合演示超声波阵列感知、卡尔曼滤波、PID 控制、差速运动学和 BLDC 速度闭环，是嵌入式与机器人控制综合实践的经典载体。 - 展会/餐厅服务跟随
在餐厅跟随服务员配送餐食，或在展会上跟随讲解员移动展示设备，超声波矩阵可在中短距离内提供稳定感知。
三、需要注意的事项 - 超声波测距存在盲区与波束角限制
HC-SR04 等常见模块近距盲区通常在 2-3 cm 左右，有效测距多在几十厘米到数米之间，且波束角较宽，容易受到侧面物体干扰。目标过小、过软或吸声材质会导致回波丢失。 - 多探头必须分时触发
若多个超声波同时工作，回波会互相干扰，产生异常大值或随机跳变。应设置触发间隔，并确保每次回波接收完成后再触发下一个探头。 - 卡尔曼滤波参数需要合理整定
过程噪声 Q 反映目标运动不确定性，Q 越大滤波器越信任新测量；测量噪声 R 反映传感器噪声水平，R 越大滤波器越依赖模型预测。Q、R 设置不当会导致滤波过迟或过度平滑。 - 目标丢失后不能盲目加速追赶
当超声波矩阵连续多帧无法检测到目标时，应进入搜索模式，例如原地小角度扫描、低速旋转或等待重新捕获，而不是全速前进，以免撞向障碍物或跟错目标。 - 跟随距离应设置安全上下限
过近容易发生碰撞或压迫感，过远则容易丢失目标。通常可设置最小安全距离、期望跟随距离和最大丢失距离，并结合速度限幅保证平稳性。 - BLDC 速度指令必须限幅和斜坡化
卡尔曼滤波虽然能平滑位置估计，但目标突然移动仍可能产生较大误差。应对线速度、角速度和加速度设置上限，避免电机急加速、打滑或电流尖峰。 - Arduino 算力与扫描频率需平衡
ATmega328P 可承担 3-6 路超声波分时扫描、一维或二维卡尔曼滤波和简单 PID 控制；但若同时运行多路 FOC、编码器闭环、IMU 融合和复杂状态机，建议迁移到 ESP32-S3、Teensy 4.1、STM32 或采用分层架构。 - 地面反射和多径干扰需处理
超声波可能在墙面、地面、桌腿之间多次反射，产生虚假远距离读数。应加入异常值剔除、滑动窗口校验和最大有效距离限制。 - 温度会影响声速
声速随温度变化，典型近似为 v ≈ 331.4 + 0.607 × T(℃)。若对精度要求较高，可加入温度补偿；一般跟随场景可通过卡尔曼滤波部分抵消该误差。 - 电源噪声与电机干扰要隔离
BLDC 加减速会引起母线波动，可能干扰超声波回波检测和 MCU 运行。控制电源与动力电源应合理隔离，传感器供电尽量稳定，串口打印不要阻塞主控制循环。
整体链路是：超声波矩阵分时扫描 → 剔除异常值并计算目标距离与方位 → 卡尔曼滤波估计目标位置/速度 → 生成距离误差与角度误差 → PID 输出线速度和角速度 → BLDC 差速/全向底盘执行 → 持续闭环跟踪并在目标丢失时进入搜索模式。做原型时，建议按“单路超声波测距 → 多路分时扫描 → 目标方位估计 → 一维卡尔曼滤波 → 距离-角度双环跟随 → BLDC 速度限幅与斜坡 → 目标丢失保护”逐步推进。
1、3路超声波矩阵 + 双卡尔曼滤波（位置/速度）+ 预测性 PID 跟随
适用场景：机器人跟随单个目标（人员或设备），3 路超声波（左/中/右）构成扇形探测矩阵。目标距离存在测量噪声，且目标可能突然加速或减速。核心逻辑直接来自搜索结果：kfPos 滤波距离，kfVel 滤波速度，用 filteredDist + kfVel.x * 0.5 做 0.5 秒前馈预测，PID 基于预测距离计算速度指令。
#include <SimpleFOC.h>
#include <NewPing.h>
// 双轮 BLDC 差速底盘
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11, 8), driverR(3, 5, 6, 7);
Encoder encL(18, 19, 2048), encR(20, 21, 2048);
// 3 路超声波矩阵（左/中/右）
#define TRIG_L 2
#define ECHO_L 3
#define TRIG_C 4
#define ECHO_C 5
#define TRIG_R 6
#define ECHO_R 7
NewPing sonarL(TRIG_L, ECHO_L, 200);
NewPing sonarC(TRIG_C, ECHO_C, 200);
NewPing sonarR(TRIG_R, ECHO_R, 200);
const int SONAR_NUM = 3;
float distances[SONAR_NUM];
// ===== 简化标量卡尔曼滤波器 =====
struct KalmanScalar {
float x; // 状态估计
float P; // 估计协方差
float Q; // 过程噪声
float R; // 测量噪声
void init(float initX, float initP, float q, float r) {
x = initX; P = initP; Q = q; R = r;
}
float update(float measurement) {
// 预测
P = P + Q;
// 更新
float K = P / (P + R);
x = x + K * (measurement - x);
P = (1 - K) * P;
return x;
}
};
KalmanScalar kfPos; // 距离滤波
KalmanScalar kfVel; // 速度滤波
// 跟随参数
const float TARGET_DIST = 1.0; // 目标距离 1m
const float MAX_SPEED = 0.8;
float lastFilteredDist = 0;
unsigned long lastTime = 0;
// 方位估计
float targetPos = 0; // -1=左, 0=中, 1=右
void setup() {
Serial.begin(115200);
kfPos.init(TARGET_DIST, 1.0, 0.01, 0.3); // 位置：低过程噪声，中测量噪声
kfVel.init(0, 1.0, 0.1, 0.5); // 速度：稍高过程噪声
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();
lastTime = millis();
}
void loop() {
motorL.loopFOC(); motorR.loopFOC();
unsigned long now = millis();
float dt = (now - lastTime) / 1000.0;
if (dt > 0.5) dt = 0.05; // 限制步长
lastTime = now;
// ===== 1. 多路超声波采样 =====
distances[0] = sonarL.ping_cm() / 100.0;
distances[1] = sonarC.ping_cm() / 100.0;
distances[2] = sonarR.ping_cm() / 100.0;
// 有效性过滤：无效值设为 1.5m
for (int i = 0; i < SONAR_NUM; i++) {
if (distances[i] <= 0 || distances[i] > 1.5) distances[i] = 1.5;
}
// ===== 2. 最值提取（取最小距离作为目标距离） =====
float minDist = distances[0];
for (int i = 1; i < SONAR_NUM; i++) {
if (distances[i] < minDist) minDist = distances[i];
}
// ===== 3. 卡尔曼滤波 =====
float filteredDist = kfPos.update(minDist);
// 速度估计：位置差分
if (dt > 0.01) {
float rawVel = (filteredDist - lastFilteredDist) / dt;
float filteredVel = kfVel.update(rawVel);
lastFilteredDist = filteredDist;
// ===== 4. 预测性 PID =====
// 预测 0.5 秒后的距离
float predictedDist = filteredDist + kfVel.x * 0.5;
// PID 仅用 P（简化）
float distError = TARGET_DIST - predictedDist;
float linearSpeed = constrain(distError * 1.5, -MAX_SPEED, MAX_SPEED);
// 预测过近时提前减速
if (predictedDist < 0.4) {
linearSpeed *= 0.3;
}
// ===== 5. 方位估计 =====
// 加权平均：距离越近的传感器权重越高
float pos = 0, totalW = 0;
for (int i = 0; i < SONAR_NUM; i++) {
float w = 1.0 / (distances[i] + 0.1);
pos += (i - 1) * w; // i=0→-1, i=1→0, i=2→+1
totalW += w;
}
targetPos = (totalW > 0.01) ? pos / totalW : 0;
float angularSpeed = constrain(targetPos * 1.5, -1.0, 1.0);
// ===== 6. 差速执行 =====
float wheelBase = 0.25;
float vL = linearSpeed - angularSpeed * wheelBase / 2;
float vR = linearSpeed + angularSpeed * wheelBase / 2;
motorL.move(vL / 0.05);
motorR.move(vR / 0.05);
// 调试
static unsigned long lastPrint = 0;
if (millis() - lastPrint > 300) {
lastPrint = millis();
Serial.print("Raw:"); Serial.print(minDist, 2);
Serial.print(" Filt:"); Serial.print(filteredDist, 2);
Serial.print(" Pred:"); Serial.print(predictedDist, 2);
Serial.print(" Lin:"); Serial.print(linearSpeed, 2);
Serial.print(" Ang:"); Serial.println(angularSpeed, 2);
}
}
delay(40);
}
解读：该案例直接映射了搜索结果中的“多路超声波最值提取 → 卡尔曼位置/速度双滤波 → 预测性前馈 → 差速执行”完整链路。kfPos 平滑距离噪声，kfVel 提供速度趋势，predictedDist 使 PID 基于“目标 0.5 秒后会在哪里”而非“现在在哪里”计算速度，从而补偿超声波采样与电机响应的固有延迟。方位估计通过距离加权实现——目标越近的传感器权重越高，避免远处噪声干扰方向判断。
2、5路环形超声波矩阵 + 自适应噪声卡尔曼 + 目标短暂丢失维持
适用场景：目标可能在机器人周围移动，5 路超声波（前左/前右/左/右/后）构成环形覆盖。超声波在目标短暂离开某路探测范围时输出无效值（0 或超量程），卡尔曼滤波器需要在测量缺失时依赖速度预测维持状态，而非直接跳变。核心参考搜索结果中 Kalman 库对“状态预测与测量更新分离”的设计。
#include <SimpleFOC.h>
#include <NewPing.h>
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11, 8), driverR(3, 5, 6, 7);
Encoder encL(18, 19, 2048), encR(20, 21, 2048);
// 5 路环形超声波
#define NUM_SONAR 5
NewPing sonar[NUM_SONAR] = {
NewPing(2, 3, 200), // 前左
NewPing(4, 5, 200), // 前右
NewPing(6, 7, 200), // 左
NewPing(8, 9, 200), // 右
NewPing(10, 11, 200) // 后
};
// 每路独立卡尔曼
KalmanScalar kf[NUM_SONAR];
float lastValidDist[NUM_SONAR];
bool sensorValid[NUM_SONAR];
// 目标跟踪参数
const float TARGET_DIST = 1.2;
const float MAX_SPEED = 0.7;
unsigned long lastUpdate = 0;
// 简化卡尔曼结构（同上案例）
struct KalmanScalar {
float x, P, Q, R;
void init(float initX, float q, float r) { x = initX; P = 1.0; Q = q; R = r; }
float predict() { P = P + Q; return x; }
float update(float meas) {
float K = P / (P + R);
x = x + K * (meas - x);
P = (1 - K) * P;
return x;
}
};
void setup() {
Serial.begin(115200);
for (int i = 0; i < NUM_SONAR; i++) {
kf[i].init(TARGET_DIST, 0.02, 0.5);
lastValidDist[i] = TARGET_DIST;
sensorValid[i] = false;
}
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();
lastUpdate = millis();
}
void loop() {
motorL.loopFOC(); motorR.loopFOC();
unsigned long now = millis();
float dt = (now - lastUpdate) / 1000.0;
if (dt < 0.02) { delay(10); return; }
lastUpdate = now;
// ===== 每路独立滤波 =====
float validSum = 0;
int validCount = 0;
float minDist = 10.0;
int minIdx = -1;
for (int i = 0; i < NUM_SONAR; i++) {
float raw = sonar[i].ping_cm() / 100.0;
// 有效性判定：0.15m ~ 2.5m
if (raw > 0.15 && raw < 2.5) {
sensorValid[i] = true;
kf[i].predict();
float filtered = kf[i].update(raw);
lastValidDist[i] = filtered;
validSum += filtered;
validCount++;
if (filtered < minDist) { minDist = filtered; minIdx = i; }
} else {
// 测量缺失：仅预测，不更新
sensorValid[i] = false;
kf[i].predict();
// 预测值保持但标记为不可靠
}
}
// ===== 目标距离决策 =====
float targetDist;
if (validCount >= 2) {
// 至少两路有效：取最可靠的最小值
targetDist = minDist;
} else if (validCount == 1) {
// 仅一路有效：使用该路滤波值
targetDist = validSum;
} else {
// 全部失效：依赖最近一次有效距离，减速
targetDist = 1.5; // 默认远距离，触发缓慢前进搜索
}
// ===== 速度控制 =====
float error = targetDist - TARGET_DIST;
float speed = constrain(error * 1.2, -0.3, MAX_SPEED);
if (validCount == 0) speed = 0.2; // 丢失时低速搜索
// ===== 方位控制：基于有效传感器索引 =====
float pos = 0, totalW = 0;
for (int i = 0; i < NUM_SONAR; i++) {
if (sensorValid[i] && lastValidDist[i] < 2.0) {
float w = 1.0 / (lastValidDist[i] + 0.1);
// 索引映射：0=前左(-0.5), 1=前右(0.5), 2=左(-1), 3=右(1), 4=后(0)
float angleMap[] = {-0.5, 0.5, -1.0, 1.0, 0.0};
pos += angleMap[i] * w;
totalW += w;
}
}
float turn = (totalW > 0.01) ? constrain(pos / totalW * 1.0, -0.8, 0.8) : 0;
// ===== 执行 =====
float vL = speed - turn * 0.125;
float vR = speed + turn * 0.125;
motorL.move(vL / 0.05);
motorR.move(vR / 0.05);
delay(30);
}
解读：该案例将卡尔曼滤波从“单路”扩展到“每路独立”，核心改进是测量缺失时的处理。当某路超声波输出无效值时，kf[i].predict() 仅执行预测步骤而不执行更新步骤，状态估计依靠速度趋势外推。这与搜索结果中 Kalman 库的 update() 与状态预测分离的设计一致。validCount 作为全局可靠性指标：全部失效时降速搜索而非停车或乱走，适合目标可能短暂离开探测范围的场景。
3、超声波矩阵 + 卡尔曼滤波 + 动态避障优先级仲裁
场景：目标跟随过程中，前方突然出现非目标障碍物（货架、行人）。超声波矩阵同时检测“目标”和“障碍”，卡尔曼滤波平滑目标距离，但避障具有硬优先级：障碍距离低于安全阈值时，跟随输出被旁路，执行后退/转向，障碍解除后卡尔曼状态从暂停处恢复。借鉴搜索结果中 FSM 避障硬优先级的设计。
#include <SimpleFOC.h>
#include <NewPing.h>
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11, 8), driverR(3, 5, 6, 7);
Encoder encL(18, 19, 2048), encR(20, 21, 2048);
// 前向超声波（目标+障碍共用探测）
NewPing sonarF(2, 3, 200);
// 侧向超声波（辅助判断障碍方位）
NewPing sonarL(4, 5, 200);
NewPing sonarR(6, 7, 200);
// 卡尔曼
struct KalmanScalar {
float x, P, Q, R;
void init(float x0) { x = x0; P = 1.0; Q = 0.03; R = 0.4; }
float update(float m) {
P += Q;
float K = P / (P + R);
x += K * (m - x);
P = (1 - K) * P;
return x;
}
};
KalmanScalar kfDist, kfVel;
// 状态机
enum Mode { FOLLOW, AVOID };
Mode mode = FOLLOW;
// 参数
const float TARGET_DIST = 1.0;
const float OBSTACLE_SAFE = 0.35; // 35cm 避障阈值
const float MAX_SPEED = 0.7;
float lastFiltered = 0;
unsigned long lastTime = 0;
unsigned long avoidStart = 0;
void setup() {
Serial.begin(115200);
kfDist.init(TARGET_DIST);
kfVel.init(0);
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();
lastTime = millis();
}
void loop() {
motorL.loopFOC(); motorR.loopFOC();
unsigned long now = millis();
float dt = (now - lastTime) / 1000.0;
if (dt > 0.5) dt = 0.05;
lastTime = now;
// ===== 传感器 =====
float dF = sonarF.ping_cm() / 100.0;
float dL = sonarL.ping_cm() / 100.0;
float dR = sonarR.ping_cm() / 100.0;
bool obstacle = (dF > 0 && dF < OBSTACLE_SAFE);
// ===== 状态仲裁 =====
if (obstacle && mode == FOLLOW) {
mode = AVOID;
avoidStart = millis();
Serial.println("→ 避障");
} else if (mode == AVOID && !obstacle && millis() - avoidStart > 600) {
mode = FOLLOW;
Serial.println("→ 恢复跟随");
}
// ===== 执行 =====
if (mode == AVOID) {
// 硬避障：后退转向
if (dL > dR) {
motorL.move(-0.5);
motorR.move(0.5);
} else {
motorL.move(0.5);
motorR.move(-0.5);
}
// 卡尔曼状态冻结（不更新）
} else {
// ===== 跟随模式 =====
if (dF > 0.15 && dF < 2.0) {
float filtered = kfDist.update(dF);
if (dt > 0.01) {
float rawVel = (filtered - lastFiltered) / dt;
float fVel = kfVel.update(rawVel);
lastFiltered = filtered;
float predicted = filtered + fVel * 0.4;
float error = TARGET_DIST - predicted;
float speed = constrain(error * 1.5, -0.4, MAX_SPEED);
// 方位
float turn = 0;
if (dL > 0.15 && dR > 0.15 && dL < 1.5 && dR < 1.5) {
turn = constrain((dL - dR) * 1.0, -0.8, 0.8);
}
float vL = speed - turn * 0.125;
float vR = speed + turn * 0.125;
motorL.move(vL / 0.05);
motorR.move(vR / 0.05);
}
} else {
// 目标丢失：停止
motorL.move(0);
motorR.move(0);
}
}
delay(30);
}
解读：该案例将卡尔曼滤波限定在“跟随模式”内生效，避障模式下卡尔曼状态冻结。搜索结果中 FSM 设计明确“避障检测具有硬优先级，触发时无条件挂起跟随/任务”。如果避障期间继续用超声波数据更新卡尔曼，障碍物距离会污染目标距离估计，导致避障结束后机器人朝错误方向运动。冻结状态使恢复跟随时从“避障前的目标位置”继续预测，而非被障碍物读数带偏。
要点解读
-
超声波矩阵的核心价值是方位感知，卡尔曼滤波的核心价值是距离平滑，两者解决不同问题
单路超声波只能得到“最近目标有多远”，无法判断目标在左还是在右。3 路或 5 路矩阵通过比较各路距离的差异（近距离传感器权重更高）解算方位角。卡尔曼滤波作用于“距离”标量，消除超声波在 1m 以外的测量抖动（HC-SR04 在 1m 处标准差可达数厘米）。两者串联时，矩阵负责“方向”，滤波负责“距离”，分工明确。 -
预测性前馈是补偿超声波采样延迟的必要手段，而非可选优化
超声波单次测量耗时约 10-30ms，加上滤波收敛和电机响应，从“目标移动”到“机器人动作”存在 100ms 以上的固有延迟。搜索结果中的 predictedDist = filteredDist + kfVel.x * 0.5 用速度估计外推 0.5 秒，使 PID 基于“目标将要去的位置”而非“目标刚才在的位置”计算速度。如果目标匀速运动，这消除大部分滞后；如果目标突然变速，预测会短暂偏差，但卡尔曼的速度估计会快速修正。 -
卡尔曼的测量缺失处理（仅预测不更新）是目标短暂遮挡场景的关键
案例二的 sensorValid 机制使某路超声波输出无效值时，滤波器仅执行 predict() 而不执行 update()。这意味着状态估计依靠上一时刻的速度趋势外推，协方差 P 增大（不确定性增加），但不会因测量值跳变（如从 0.5m 突然变为 0 或 2.5m）而剧烈波动。搜索结果中 Kalman 库的 update() 与状态预测分离的接口设计支持这一用法。 -
避障硬优先级要求卡尔曼状态在避障期间冻结，否则恢复时会被污染
案例三的 mode == AVOID 分支不调用 kfDist.update()。如果避障时超声波检测到的是障碍物（距离 0.3m）而非目标（距离 1.0m），继续更新滤波器会使 kfDist.x 从 1.0 被拉向 0.3。避障结束恢复跟随时，机器人会以为目标在 0.3m 处而急停或后退。冻结状态使恢复跟随时从“避障前最后确认的目标距离”继续，符合 FSM 避障硬优先级的语义。 -
Arduino 算力约束决定了卡尔曼滤波应使用标量实现而非矩阵库
搜索结果中的 Kalman 库基于矩阵运算（BLA::Matrix），适用于多状态耦合系统。但本场景中“距离”和“速度”可以解耦为两个独立的标量卡尔曼滤波器（案例一、二、三均如此），每个滤波器仅需 4 个 float 变量和几次乘除。矩阵库在 Arduino Uno 上的 SRAM 开销和浮点运算时间都不可忽视，而标量实现可以在 50Hz 下轻松运行。如果未来需要融合 IMU 加速度与超声波距离做联合估计，才需要引入矩阵卡尔曼。
4、扇形阵列 + 抛物线插值求方位 + 一维卡尔曼
适用硬件：ESP32 + 差速 BLDC 底盘 + 5×HC-SR04（-36°~+36° 扇形排布）+ 编码器。
核心思路：5 路超声分时轮询（同时触发会互相串扰，读到的是别的探头的回波）。每一路测到的是“沿该波束方向的径向距离”，不能直接平均——正确的做法是先从“哪一路最近”反推目标方位，用抛物线插值细化到亚波束精度，再把各路的径向距离投影回同一视线方向融合，最后送进卡尔曼。
/*
* 案例4：5 路扇形阵列 + 方位插值 + 一维卡尔曼 -> 差速跟随
*/
#include <SimpleFOC.h>
BLDCMotor wl(7), wr(7);
BLDCDriver3PWM dl(9,10,11,8), dr(22,23,24,25);
Encoder el(2,3,2048), er(32,33,2048);
void onL(){ el.handleA(); } void onR(){ er.handleA(); }
const float R_W = 0.065f, TRACK = 0.34f;
// ---------------- 阵列 ----------------
#define N_US 5
const int TRIG[N_US] = {6, 8, 10, 12, 14};
const int ECHO[N_US] = {7, 9, 11, 13, 15};
const float ANG[N_US] = {-0.628f, -0.314f, 0.0f, 0.314f, 0.628f}; // ±36°
const float MAX_R = 2.5f;
float rBuf[N_US];
float tBuf[N_US]; // 该路最近一次有效测量时间(s)
bool okBuf[N_US];
// ---------------- 轮询 ----------------
int pollIdx = 0;
unsigned long lastPollT = 0;
const unsigned long POLL_GAP = 35; // ms，必须 > 串扰衰减时间
float readUS(int i) {
digitalWrite(TRIG[i], LOW); delayMicroseconds(2);
digitalWrite(TRIG[i], HIGH); delayMicroseconds(10);
digitalWrite(TRIG[i], LOW);
long us = pulseIn(ECHO[i], HIGH, 15000);
if (us <= 0) return -1.0f;
float r = us * 0.0343f / 2.0f / 100.0f;
return (r > 0.06f && r < MAX_R) ? r : -1.0f;
}
// ---------------- 方位估计：argmin + 抛物线插值 ----------------
// r_i 在 i = 目标方位处取最小，三点拟合可得到亚波束精度
bool estimateBearing(float &phi, int &bestIdx) {
int m = -1; float rmin = 1e9f;
for (int i = 0; i < N_US; i++) {
if (okBuf[i] && rBuf[i] < rmin) { rmin = rBuf[i]; m = i; }
}
if (m < 0) return false;
bestIdx = m;
// 边缘无法三点插值，退化为波束中心
if (m == 0 || m == N_US - 1) { phi = ANG[m]; return true; }
float y0 = rBuf[m-1], y1 = rBuf[m], y2 = rBuf[m+1];
if (!okBuf[m-1] || !okBuf[m+1]) { phi = ANG[m]; return true; }
// 抛物线顶点相对中心偏移（单位：波束间隔）
float denom = (y0 - 2*y1 + y2);
float delta = (fabsf(denom) > 1e-6f) ? (0.5f * (y0 - y2) / denom) : 0.0f;
delta = constrain(delta, -1.0f, 1.0f);
phi = ANG[m] + delta * (ANG[1] - ANG[0]);
return true;
}
// ---------------- 视线距离融合 ----------------
// 目标在方位 phi、距离 R 时，第 i 路读到 r_i ≈ R·cos(ANG_i - phi)
// 反解 R_i = r_i / cos(...)，按余弦加权（越正对越可信）
float fuseRange(float phi) {
float num = 0, den = 0;
for (int i = 0; i < N_US; i++) {
if (!okBuf[i]) continue;
float dA = ANG[i] - phi;
float c = cosf(dA);
if (c < 0.5f) continue; // 偏角太大，投影放大误差
float Ri = rBuf[i] / c;
float w = c * c;
num += Ri * w; den += w;
}
return (den > 1e-4f) ? num / den : -1.0f;
}
// ---------------- 一维卡尔曼：状态 [R, Rdot] ----------------
float X[2] = {1.2f, 0.0f}; // 距离 m，径向速度 m/s
float P[2][2] = {{0.5f,0},{0,0.5f}};
const float SIG_R = 0.04f; // 超声单次测距噪声 m（实测标定）
const float Q_R = 0.002f, Q_V = 0.08f; // 过程噪声
void kfPredict(float dt) {
X[0] += X[1] * dt;
// P = F P F' + Q, F = [1 dt; 0 1]
float p00 = P[0][0] + dt*(P[1][0] + P[0][1]) + dt*dt*P[1][1] + Q_R*dt;
float p01 = P[0][1] + dt*P[1][1];
float p10 = P[1][0] + dt*P[1][1];
float p11 = P[1][1] + Q_V*dt;
P[0][0]=p00; P[0][1]=p01; P[1][0]=p10; P[1][1]=p11;
}
void kfUpdate(float z) {
// 新息
float y = z - X[0];
float S = P[0][0] + SIG_R*SIG_R;
// 门控：|y| > 4σ 视为野值丢弃（遮挡、多径、别的物体）
if (fabsf(y) > 4.0f * sqrtf(S)) return;
float K0 = P[0][0] / S;
float K1 = P[1][0] / S;
X[0] += K0 * y;
X[1] += K1 * y;
float p00 = (1-K0)*P[0][0];
float p01 = (1-K0)*P[0][1];
float p10 = P[1][0] - K1*P[0][0];
float p11 = P[1][1] - K1*P[0][1];
P[0][0]=p00; P[0][1]=p01; P[1][0]=p10; P[1][1]=p11;
}
// ---------------- 控制 ----------------
const float D_REF = 1.20f, D_STOP = 0.70f;
float vOut = 0, wOut = 0;
void setup() {
for (int i = 0; i < N_US; i++) {
pinMode(TRIG[i], OUTPUT); pinMode(ECHO[i], INPUT);
okBuf[i] = false;
}
el.init(); el.enableInterrupts(onL);
er.init(); er.enableInterrupts(onR);
wl.linkSensor(&el); wr.linkSensor(&er);
wl.controller = wr.controller = MotionControlType::velocity;
wl.PID_velocity.P = 0.35f; wl.PID_velocity.I = 4.0f;
wr.PID_velocity.P = 0.35f; wr.PID_velocity.I = 4.0f;
wl.voltage_limit = wr.voltage_limit = 12.0f;
wl.current_limit = wr.current_limit = 4.0f;
wl.init(); wr.init();
dl.voltage_power_supply = dr.voltage_power_supply = 12.0f;
dl.init(); dr.init();
wl.linkDriver(&dl); wr.linkDriver(&dr);
wl.initFOC(); wr.initFOC();
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;
// ---- 1. 分时轮询：每 35ms 触发一路 ----
if (now - lastPollT >= POLL_GAP) {
lastPollT = now;
int i = pollIdx;
pollIdx = (pollIdx + 1) % N_US;
float r = readUS(i);
if (r > 0) {
rBuf[i] = r; tBuf[i] = now / 1000.0f; okBuf[i] = true;
} else if (now/1000.0f - tBuf[i] > 0.6f) {
okBuf[i] = false; // 超时未回波才置失效，不要一丢就清
}
}
// ---- 2. 方位 + 距离融合 ----
float phi = 0; int bi = 0;
bool have = estimateBearing(phi, bi);
if (have) {
float R = fuseRange(phi);
if (R > 0) {
kfPredict(dt);
kfUpdate(R);
}
} else {
kfPredict(dt); // 无观测也要预测，保持估计连续
}
// ---- 3. 控制 ----
float dEst = X[0], vEst = X[1];
if (!have || dEst > MAX_R) {
vOut = 0; wOut = 0; // 彻底丢失：原地停
} else {
float eV = dEst - D_REF;
float vTgt = constrain(1.2f * eV + 0.5f * vEst, -0.25f, 0.85f);
if (dEst < D_STOP) vTgt = 0; // 太近直接停
wOut = constrain(2.2f * phi, -0.9f, 0.9f); // 对准方位
vOut += constrain(vTgt - vOut, -2.0f*dt, 1.0f*dt);
}
// ---- 4. 执行 ----
float vL = vOut - wOut*TRACK*0.5f, vR = vOut + wOut*TRACK*0.5f;
wl.loopFOC(); wr.loopFOC();
wl.move(vL/R_W); wr.move(vR/R_W);
Serial.print("phi="); Serial.print(phi, 3);
Serial.print(" R="); Serial.print(dEst, 2);
Serial.print(" vR="); Serial.print(vEst, 2);
Serial.print(" w="); Serial.println(wOut, 2);
delay(20);
}
关键点：tBuf 只用于超时判定，真正参与滤波的是“本次测量到达时刻的 dt”。如果按固定 20 ms 预测，而实际每 35 ms 才来一次观测，卡尔曼会系统性地低估运动——这是低帧率阵列最常见的隐性误差来源。
5、：阵列三角定位 + 二维卡尔曼 + 马氏距离门控
适用硬件：ESP32 + 差速 BLDC + 5 路超声阵列 + 编码器里程计。
核心思路：案例一只能跟踪“正前方”的目标，侧向目标要先转过去才准。这一版把每一路回波直接换算成机器人坐标系里的点，再转到世界坐标系，用二维卡尔曼跟踪目标的世界坐标。好处是：机器人自己的运动被里程计补偿掉了，锁定目标后即使车在转向，估计也不抖。
/*
* 案例5：单回波 -> 世界坐标系点 -> 二维卡尔曼 [x,y,vx,vy]
*/
#include <SimpleFOC.h>
BLDCMotor wl(7), wr(7);
// 驱动/编码器初始化同案例一（略）
const float R_W = 0.065f, TRACK = 0.34f;
#define N_US 5
const int TRIG[N_US] = {6,8,10,12,14};
const int ECHO[N_US] = {7,9,11,13,15};
const float ANG[N_US] = {-0.628f,-0.314f,0.0f,0.314f,0.628f};
const float MAX_R = 2.5f;
// ---- 机器人位姿 ----
float px = 0, py = 0, pth = 0, vCur = 0, wCur = 0;
// ---------------- 二维卡尔曼 ----------------
float X[4] = {1.2f, 0, 0, 0}; // x y vx vy（世界系）
float P[4][4];
const float SIG_XY = 0.06f; // 单点定位噪声（含角度误差放大）
const float Q_POS = 0.001f, Q_VEL = 0.08f;
void kfInit() {
memset(P, 0, sizeof(P));
for (int i = 0; i < 4; i++) P[i][i] = 0.5f;
}
void kfPredict(float dt) {
// x += vx*dt ; y += vy*dt
X[0] += X[2]*dt;
X[1] += X[3]*dt;
// P = F P F' + Q
float F[4][4] = {{1,0,dt,0},{0,1,0,dt},{0,0,1,0},{0,0,0,1}};
float FP[4][4] = {0};
for (int i=0;i<4;i++) for (int j=0;j<4;j++) {
float s = 0;
for (int k=0;k<4;k++) s += F[i][k]*P[k][j];
FP[i][j] = s;
}
float FPF[4][4] = {0};
for (int i=0;i<4;i++) for (int j=0;j<4;j++) {
float s = 0;
for (int k=0;k<4;k++) s += FP[i][k]*F[j][k]; // F' = F transposed
FPF[i][j] = s;
}
for (int i=0;i<4;i++) for (int j=0;j<4;j++) P[i][j] = FPF[i][j];
P[0][0] += Q_POS*dt; P[1][1] += Q_POS*dt;
P[2][2] += Q_VEL*dt; P[3][3] += Q_VEL*dt;
}
// 观测只含 (x, y)，H = [I2 | 0]
// 返回 false 表示被门控拒绝
bool kfUpdate(float zx, float zy) {
float y0 = zx - X[0];
float y1 = zy - X[1];
// S = H P H' + R (2x2)
float S00 = P[0][0] + SIG_XY*SIG_XY;
float S01 = P[0][1];
float S11 = P[1][1] + SIG_XY*SIG_XY;
float det = S00*S11 - S01*S01;
if (fabsf(det) < 1e-9f) return false;
// 马氏距离平方 d² = y' S^-1 y，2 自由度 99% 门限 9.21
float Si00 = S11/det, Si01 = -S01/det, Si11 = S00/det;
float d2 = y0*(Si00*y0 + Si01*y1) + y1*(Si01*y0 + Si11*y1);
if (d2 > 9.21f) return false;
// K = P H' S^-1 (4x2)
float K[4][2];
for (int i = 0; i < 4; i++) {
float p00 = P[i][0], p01 = P[i][1];
K[i][0] = p00*Si00 + p01*Si01;
K[i][1] = p00*Si01 + p01*Si11;
}
for (int i = 0; i < 4; i++) {
X[i] += K[i][0]*y0 + K[i][1]*y1;
}
// P = (I - K H) P
float NP[4][4];
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
NP[i][j] = P[i][j] - K[i][0]*P[0][j] - K[i][1]*P[1][j];
}
}
memcpy(P, NP, sizeof(P));
return true;
}
// ---------------- 单点定位 ----------------
float readUS(int i) {
digitalWrite(TRIG[i], LOW); delayMicroseconds(2);
digitalWrite(TRIG[i], HIGH); delayMicroseconds(10); digitalWrite(TRIG[i], LOW);
long us = pulseIn(ECHO[i], HIGH, 15000);
if (us <= 0) return -1;
float r = us * 0.0343f / 2.0f / 100.0f;
return (r > 0.08f && r < MAX_R) ? r : -1;
}
int pollIdx = 0;
unsigned long lastPoll = 0;
bool everLocked = false;
// ---- 阵列扫描：把所有有效回波转成世界系点，取最接近预测的一个 ----
bool acquire(float &zx, float &zy) {
float bestD2 = 1e9f; bool found = false;
float predX = X[0], predY = X[1];
for (int i = 0; i < N_US; i++) {
float r = readUS(i);
if (r < 0) continue;
// 机器人系 -> 世界系
float a = pth + ANG[i];
float wx = px + r * cosf(a);
float wy = py + r * sinf(a);
if (!everLocked) {
// 首次锁定：取正前方最近的一个
if (i == 2) { zx = wx; zy = wy; found = true; }
continue;
}
float dx = wx - predX, dy = wy - predY;
float d2 = dx*dx + dy*dy;
if (d2 < bestD2) { bestD2 = d2; zx = wx; zy = wy; found = true; }
}
// 关联半径：预测位置 0.45m 内才算同一个目标
if (found && everLocked && bestD2 > 0.45f*0.45f) return false;
return found;
}
// ---------------- 控制 ----------------
const float D_REF = 1.20f;
float vOut = 0, wOut = 0;
void setup() {
for (int i = 0; i < N_US; i++) { pinMode(TRIG[i], OUTPUT); pinMode(ECHO[i], INPUT); }
// 电机初始化同案例一
kfInit();
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;
// ---- 1. 阵列扫描（一次完整扫描 ~175ms）----
if (now - lastPoll > 180) {
lastPoll = now;
float zx, zy;
if (acquire(zx, zy)) {
if (!everLocked) { X[0]=zx; X[1]=zy; everLocked = true; }
else kfUpdate(zx, zy);
}
}
// ---- 2. 预测（每次循环都跑，与观测异步）----
kfPredict(dt);
// ---- 3. 由世界系估计反算方位与距离 ----
float dx = X[0] - px, dy = X[1] - py;
float R = hypotf(dx, dy);
float phi = atan2f(dy, dx) - pth;
while (phi > PI) phi -= 2*PI;
while (phi < -PI) phi += 2*PI;
// ---- 4. 控制 ----
if (!everLocked || R > MAX_R) {
vOut = 0; wOut = 0;
} else {
float eV = R - D_REF;
float vTgt = constrain(1.2f*eV, -0.25f, 0.85f);
if (R < 0.70f) vTgt = 0;
wOut = constrain(2.2f*phi, -0.9f, 0.9f);
vOut += constrain(vTgt - vOut, -2.0f*dt, 1.0f*dt);
}
// ---- 5. 执行 + 里程 ----
float vL = vOut - wOut*TRACK*0.5f, vR = vOut + wOut*TRACK*0.5f;
wl.loopFOC(); wr.loopFOC();
wl.move(vL/R_W); wr.move(vR/R_W);
float vl = wl.shaft_velocity*R_W, vr = wr.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;
Serial.print("R="); Serial.print(R, 2);
Serial.print(" phi="); Serial.print(phi, 3);
Serial.print(" locked="); Serial.println(everLocked);
delay(20);
}
关键点：门控用的是马氏距离（d² > 9.21 拒绝）而不是固定半径。原因在于滤波器自身的协方差在变化：刚开始锁定时 P 很大，允许的偏差应该宽；稳定跟踪后 P 很小，同样 20 cm 的跳变就该被怀疑。固定阈值在这两种状态下必有一头是错的。
案例6：FreeRTOS 三层 + 预测外推 + COAST/SEARCH/LOST 分级
适用硬件：ESP32 双核 + 差速 BLDC + 5 路超声 + 编码器。
核心思路：超声波阵列的完整扫描要 ~175 ms，而执行环要 1 kHz——中间差了三个数量级。这一版把“扫描”“估计”“执行”拆成三个任务，估计层用预测把 5.7 Hz 的观测“续”成 100 Hz 的连续状态；同时定义四级丢失处理：TRACK（正常）→ COAST（无观测但预测有效）→ SEARCH（主动转头扫）→ LOST（停车待命）。
/*
* 案例6：扫描(5.7Hz) / 估计(100Hz) / 执行(1kHz) 三层 + 丢失分级
*/
#include <SimpleFOC.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
BLDCMotor wl(7), wr(7);
// 初始化同案例一（略）
SemaphoreHandle_t mux;
const float R_W = 0.065f, TRACK = 0.34f;
#define N_US 5
const int TRIG[N_US] = {6,8,10,12,14};
const int ECHO[N_US] = {7,9,11,13,15};
const float ANG[N_US] = {-0.628f,-0.314f,0.0f,0.314f,0.628f};
const float MAX_R = 2.5f;
enum TrackState { TS_TRACK, TS_COAST, TS_SEARCH, TS_LOST };
volatile TrackState g_ts = TS_LOST;
// ---- 共享估计 ----
volatile float g_x = 1.2f, g_y = 0.0f, g_vx = 0.0f, g_vy = 0.0f;
volatile float g_var = 1.0f; // 位置方差（置信度）
volatile unsigned long g_lastObs = 0;
// ---- 机器人位姿 ----
volatile float g_px = 0, g_py = 0, g_pth = 0;
// ---- 执行输出 ----
volatile float c_v = 0, c_w = 0;
volatile bool c_halt = false;
float X[4] = {1.2f, 0, 0, 0};
float P[4][4];
const float SIG_XY = 0.06f, Q_POS = 0.001f, Q_VEL = 0.08f;
void kfInit() {
memset(P, 0, sizeof(P));
for (int i = 0; i < 4; i++) P[i][i] = 0.5f;
}
void kfPredict(float dt) {
X[0] += X[2]*dt; X[1] += X[3]*dt;
float F[4][4] = {{1,0,dt,0},{0,1,0,dt},{0,0,1,0},{0,0,0,1}};
float FP[4][4] = {0}, FPF[4][4] = {0};
for (int i=0;i<4;i++) for (int j=0;j<4;j++) {
float s=0; for (int k=0;k<4;k++) s += F[i][k]*P[k][j]; FP[i][j]=s;
}
for (int i=0;i<4;i++) for (int j=0;j<4;j++) {
float s=0; for (int k=0;k<4;k++) s += FP[i][k]*F[j][k]; FPF[i][j]=s;
}
for (int i=0;i<4;i++) for (int j=0;j<4;j++) P[i][j]=FPF[i][j];
P[0][0]+=Q_POS*dt; P[1][1]+=Q_POS*dt;
P[2][2]+=Q_VEL*dt; P[3][3]+=Q_VEL*dt;
}
bool kfUpdate(float zx, float zy) {
float y0 = zx - X[0], y1 = zy - X[1];
float S00 = P[0][0] + SIG_XY*SIG_XY, S01 = P[0][1];
float S11 = P[1][1] + SIG_XY*SIG_XY;
float det = S00*S11 - S01*S01;
if (fabsf(det) < 1e-9f) return false;
float Si00 = S11/det, Si01 = -S01/det, Si11 = S00/det;
float d2 = y0*(Si00*y0 + Si01*y1) + y1*(Si01*y0 + Si11*y1);
if (d2 > 9.21f) return false;
float K[4][2];
for (int i=0;i<4;i++) {
K[i][0] = P[i][0]*Si00 + P[i][1]*Si01;
K[i][1] = P[i][0]*Si01 + P[i][1]*Si11;
}
for (int i=0;i<4;i++) X[i] += K[i][0]*y0 + K[i][1]*y1;
float NP[4][4];
for (int i=0;i<4;i++) for (int j=0;j<4;j++)
NP[i][j] = P[i][j] - K[i][0]*P[0][j] - K[i][1]*P[1][j];
memcpy(P, NP, sizeof(P));
return true;
}
float readUS(int i) {
digitalWrite(TRIG[i], LOW); delayMicroseconds(2);
digitalWrite(TRIG[i], HIGH); delayMicroseconds(10); digitalWrite(TRIG[i], LOW);
long us = pulseIn(ECHO[i], HIGH, 15000);
if (us <= 0) return -1;
float r = us * 0.0343f / 2.0f / 100.0f;
return (r > 0.08f && r < MAX_R) ? r : -1;
}
// ---- 任务1：阵列扫描 ----
void taskScan(void*) {
TickType_t last = xTaskGetTickCount();
bool locked = false;
for (;;) {
float predX, predY, rpx, rpy, rpth;
xSemaphoreTake(mux, portMAX_DELAY);
predX = X[0]; predY = X[1];
rpx = g_px; rpy = g_py; rpth = g_pth;
xSemaphoreGive(mux);
float bestX = 0, bestY = 0, bestD2 = 1e9f;
bool found = false;
for (int i = 0; i < N_US; i++) {
float r = readUS(i);
if (r < 0) continue;
float a = rpth + ANG[i];
float wx = rpx + r*cosf(a), wy = rpy + r*sinf(a);
if (!locked) {
if (i == 2) { bestX = wx; bestY = wy; found = true; }
continue;
}
float dx = wx - predX, dy = wy - predY;
float d2 = dx*dx + dy*dy;
if (d2 < bestD2) { bestD2 = d2; bestX = wx; bestY = wy; found = true; }
}
if (found) {
if (!locked) {
xSemaphoreTake(mux, portMAX_DELAY);
X[0] = bestX; X[1] = bestY; X[2] = 0; X[3] = 0;
locked = true;
xSemaphoreGive(mux);
} else if (bestD2 < 0.45f*0.45f) {
xSemaphoreTake(mux, portMAX_DELAY);
kfUpdate(bestX, bestY);
g_lastObs = millis();
xSemaphoreGive(mux);
} else if (bestD2 > 1.2f*1.2f) {
locked = false; // 目标跑太远：重新捕获
}
}
vTaskDelayUntil(&last, pdMS_TO_TICKS(180));
}
}
// ---- 任务2：估计 + 状态机 100Hz ----
void taskEstimate(void*) {
TickType_t last = xTaskGetTickCount();
unsigned long lostT0 = 0;
float wSearch = 0.6f;
for (;;) {
xSemaphoreTake(mux, portMAX_DELAY);
kfPredict(0.01f);
float var = P[0][0] + P[1][1];
float x = X[0], y = X[1];
float rpx = g_px, rpy = g_py, rpth = g_pth;
unsigned long lastObs = g_lastObs;
xSemaphoreGive(mux);
float dx = x - rpx, dy = y - rpy;
float R = hypotf(dx, dy);
float phi = atan2f(dy, dx) - rpth;
while (phi > PI) phi -= 2*PI;
while (phi < -PI) phi += 2*PI;
unsigned long age = millis() - lastObs;
TrackState ts;
if (age < 400) ts = TS_TRACK;
else if (age < 1500) ts = TS_COAST;
else if (age < 5000) ts = TS_SEARCH;
else ts = TS_LOST;
// ---- 分级动作 ----
float vTgt = 0, wTgt = 0;
const float D_REF = 1.20f;
switch (ts) {
case TS_TRACK:
vTgt = constrain(1.2f*(R - D_REF), -0.25f, 0.85f);
if (R < 0.70f) vTgt = 0;
wTgt = constrain(2.2f*phi, -0.9f, 0.9f);
wSearch = 0.6f;
break;
case TS_COAST:
// 预测外推继续跟，但降速（不确定性在增长）
vTgt = constrain(0.6f*(R - D_REF), -0.15f, 0.40f);
if (R < 0.80f) vTgt = 0;
wTgt = constrain(1.5f*phi, -0.6f, 0.6f);
break;
case TS_SEARCH:
// 主动转头扫，速度归零
vTgt = 0;
wTgt = wSearch;
if (age > 3000) wSearch = -wSearch; // 每 2s 换向
break;
case TS_LOST:
vTgt = 0; wTgt = 0;
break;
}
// 置信度衰减：方差大就进一步限速
if (var > 0.30f && ts == TS_TRACK) vTgt *= 0.5f;
xSemaphoreTake(mux, portMAX_DELAY);
g_ts = ts; g_var = var;
c_v = vTgt; c_w = wTgt;
c_halt = (ts == TS_LOST);
xSemaphoreGive(mux);
vTaskDelayUntil(&last, pdMS_TO_TICKS(10));
}
}
// ---- 任务3：执行 1kHz ----
void taskExec(void*) {
TickType_t last = xTaskGetTickCount();
float vR = 0, wR = 0;
for (;;) {
float v, w; bool halt;
xSemaphoreTake(mux, portMAX_DELAY);
v = c_v; w = c_w; halt = c_halt;
xSemaphoreGive(mux);
if (halt) { v = 0; w = 0; }
vR += constrain(v - vR, -0.002f, 0.001f);
wR += constrain(w - wR, -0.005f, 0.005f);
float vL = vR - wR*TRACK*0.5f, vRt = vR + wR*TRACK*0.5f;
wl.loopFOC(); wr.loopFOC();
wl.move(vL/R_W); wr.move(vRt/R_W);
float vl = wl.shaft_velocity*R_W, vr = wr.shaft_velocity*R_W;
xSemaphoreTake(mux, portMAX_DELAY);
g_px += 0.5f*(vl+vr)*cosf(g_pth)*0.001f;
g_py += 0.5f*(vl+vr)*sinf(g_pth)*0.001f;
g_pth += (vr-vl)/TRACK*0.001f;
xSemaphoreGive(mux);
vTaskDelayUntil(&last, pdMS_TO_TICKS(1));
}
}
void setup() {
for (int i = 0; i < N_US; i++) { pinMode(TRIG[i], OUTPUT); pinMode(ECHO[i], INPUT); }
// 电机初始化同案例一
kfInit();
mux = xSemaphoreCreateMutex();
xTaskCreatePinnedToCore(taskScan, "scan", 4096, NULL, 2, NULL, 0);
xTaskCreatePinnedToCore(taskEstimate, "est", 4096, NULL, 2, NULL, 0);
xTaskCreatePinnedToCore(taskExec, "exec", 4096, NULL, 3, NULL, 1);
Serial.begin(115200);
}
void loop() { vTaskDelay(portMAX_DELAY); }
关键点：COAST 阶段的控制律是“同构但更保守”——同一条比例律，增益降到 0.6/1.5、限幅收到 ±0.15/±0.6。这样从 TRACK 到 COAST 是连续过渡而不是突变，车不会因为“突然丢了”而急刹。状态之间的切换靠 age 单一变量驱动，避免了多标志位互相打架。
要点解读
一、阵列的价值不只是“多测几次”，而在“用几何换精度”
五路超声一字排开，真正的信息在回波距离的分布形状里：目标正对哪一路，哪一路就读得最近；相邻两路的距离差决定了目标偏离中心多少。案例一用 argmin + 抛物线插值把方位精度从“波束宽度 ±18°”提到 ±3° 左右，靠的是几何而不是信号处理。反过来，如果只是把五路读数的平均值当距离，方位信息就全丢了——这是用阵列最常见也最可惜的浪费。
二、必须分时轮询，代价是低帧率，而卡尔曼正是为这个代价准备的
HC-SR04 的波束角约 15°，同时触发会互相收到对方的回波，读出来的数据完全不可信。分时轮询后，5 路扫完要 175 ms（5.7 Hz），这个帧率对跟随控制是远远不够的。所以架构上必须有“预测填空”：观测来时更新，观测不来时纯预测（kfPredict），并且预测用的是真实流逝时间 dt 而不是固定周期。这一条如果做错，滤波器会系统性地滞后于目标运动，表现为“跟随总是慢半拍”。
三、卡尔曼调参的本质是 Q/R 的比值，而门控比调参更重要
Q 是“你多相信运动模型”，R 是“你多相信传感器”。超声的 R 很实在（单次测距噪声 3~5 cm 可实测），所以难点在 Q：Q 大则跟得快但抖，Q 小则平滑但迟钝。更关键的是门控：没有门控的卡尔曼遇到一次野值（别人的腿、地面反射、串扰）就会被拖偏，而且要好几帧才能拉回来。案例一用 4σ 一维门控，案例二、三用马氏距离（d² > 9.21，2 自由度 99%），本质都是“新息异常就整帧丢弃，只预测不更新”。野值处理比滤波器精度更决定实际稳定性。
四、单点超声的方位与横向速度本质不可观，必须靠机器人自己运动补
一路超声只能测“沿视线方向的距离”，目标横向移动时它的读数几乎不变——这意味着横向速度在数学上不可观测，卡尔曼收敛出来的 vy 是不可信的。解决办法有两个：① 靠阵列的视差（同一目标在不同波束上的距离差隐含方位）；② 靠机器人自身转向制造可观性（案例三 SEARCH 阶段的转头扫描）。工程上要接受一点：超声波方案天生不擅长估计横向速度，与其硬调参数，不如让车多转两下。
五、丢失必须分级处理，绝不能“一丢就停”或“一丢就冲”
案例三的四级（TRACK/COAST/SEARCH/LOST）不是冗余设计，而是对应四种不同的信息状态：有观测（正常跟）、无观测但预测可信（减速跟）、预测已不可信但目标可能还在（停车转头找）、彻底丢失（停车待命）。每一级的时间预算是硬性的（400 ms / 1.5 s / 5 s），不能靠“感觉”。更关键的是估计层必须把置信度（协方差）传给执行层——var > 0.30 时主动降速，这比任何阈值判断都更本质：不确定性大时，慢就是安全。
请注意：以上案例仅作为思路拓展的参考示例，不保证完全正确、适配所有场景或可直接编译运行。由于硬件平台、实际使用场景、Arduino 版本的差异，均可能影响代码的适配性与使用方法的选择。在实际编程开发时，请务必根据自身硬件配置、使用场景及具体功能需求进行针对性调整，并通过多次实测验证效果；同时需确保硬件接线正确，充分了解所用传感器、执行器等设备的技术规范与核心特性。对于涉及硬件操作的代码，使用前务必核对引脚定义、电平参数等关键信息的准确性与安全性，避免因参数错误导致硬件损坏或运行异常。
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
1
0- 0
已为社区贡献480条内容
所有评论(0)