Arduino BLDC 超声波与 PID 控制跟随机器人实战

电机驱动与电源管理

• 驱动器匹配：Arduino 的 IO 口无法直接驱动 BLDC 电机，必须通过电子调速器（ESC）。需确保 ESC 支持所需的通信协议（如 DShot、PWM），并正确配置其刹车和响应参数。
• 电源隔离：大电流的电机回路会产生电压波动，容易导致 Arduino 复位。建议使用独立的稳压模块（如 LM2596）为逻辑电路和电机电路分别供电，并共地处理。

代码实现示例

以下代码展示了不同场景下的核心逻辑，实际使用时请根据硬件引脚调整。

1. 单点超声波基本跟随

场景：固定跟随一个移动目标，保持设定距离。核心逻辑：超声波测距 + 单路 PID 速度控制。

#include <SimpleFOC.h>
#include <NewPing.h>
#include <PID_v1.h>

#define TRIG_PIN 9
#define ECHO_PIN 10
#define MAX_DISTANCE 200

NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);

// BLDC 电机对象
BLDCMotor motorL(11);
BLDCMotor motorR(11);

// PID 参数
double setpoint = 30.0; // 目标跟随距离：30cm
double input, output;
double Kp = 0.5, Ki = 0.1, Kd = 0.05;
PID myPID(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

void setup() {
  Serial.begin(115200);
  // 初始化 PID
  myPID.SetMode(AUTOMATIC);
  myPID.SetOutputLimits(-0.5, 0.5); // 输出限制在 -0.5~0.5
  
  // 电机初始化
  motorL.init();
  motorR.init();
  motorL.initFOC();
  motorR.initFOC();
}

void loop() {
  // 1. 超声波测距
  input = sonar.ping_cm();
  if (input == 0) input = 100; // 无信号时设为远距离

  // 2. PID 计算输出
  myPID.Compute();

  // 3. 根据 PID 输出控制电机
  // output 为正：向前追赶；为负：后退拉开
  float speedL = 0.2 + output;
  float speedR = 0.2 + output;

  // 4. 输出限制
  speedL = constrain(speedL, -0.5, 0.5);
  speedR = constrain(speedR, -0.5, 0.5);
  
  motorL.move(speedL);
  motorR.move(speedR);
  motorL.loopFOC();
  motorR.loopFOC();
  
  Serial.print("Distance:");
  Serial.print(input);
  Serial.print(" PIDout:");
  Serial.println(output);
}

2. 双超声波差速转向跟随

场景：不仅能前后跟随，还能在目标横向移动时自动转向。核心逻辑：左右双超声波 + 双路 PID（距离 PID + 角度 PID）。

#include <SimpleFOC.h>
#include <NewPing.h>
#include <PID_v1.h>

#define SONAR_L_TRIG 2
#define SONAR_L_ECHO 3
#define SONAR_R_TRIG 4
#define SONAR_R_ECHO 5

NewPing sonarL(SONAR_L_TRIG, SONAR_L_ECHO, 200);
NewPing sonarR(SONAR_R_TRIG, SONAR_R_ECHO, 200);

BLDCMotor motorL(11);
BLDCMotor motorR(11);

// 双路 PID：距离控制和角度控制
double setpointDist = 30.0, setpointAngle = 0.0;
double distInput, distOutput, angleInput, angleOutput;
PID pidDist(&distInput, &distOutput, &setpointDist, 0.4, 0.05, 0.02, DIRECT);
PID pidAngle(&angleInput, &angleOutput, &setpointAngle, 0.3, 0.01, 0.01, DIRECT);

void setup() {
  Serial.begin(115200);
  // 初始化 PID
  pidDist.SetMode(AUTOMATIC);
  pidDist.SetOutputLimits(-0.4, 0.4);
  pidAngle.SetMode(AUTOMATIC);
  pidAngle.SetOutputLimits(-0.3, 0.3);
  
  // 电机初始化
  motorL.init();
  motorR.init();
  motorL.initFOC();
  motorR.initFOC();
}

void loop() {
  // 1. 读取左右距离
  float distL = sonarL.ping_cm();
  float distR = sonarR.ping_cm();
  if (distL == 0) distL = 100;
  if (distR == 0) distR = 100;

  // 2. 计算距离输入（平均距离）和角度输入（距离差）
  distInput = (distL + distR) / 2.0;
  angleInput = distL - distR; // 正值为目标偏左

  // 3. 计算 PID 输出
  pidDist.Compute();
  pidAngle.Compute();

  // 4. 混合控制：差速转向
  // 左轮速度 = 距离控制 + 角度控制
  // 右轮速度 = 距离控制 - 角度控制
  float speedL = 0.2 + distOutput + angleOutput;
  float speedR = 0.2 + distOutput - angleOutput;

  // 5. 限幅
  speedL = constrain(speedL, -0.6, 0.6);
  speedR = constrain(speedR, -0.6, 0.6);
  
  motorL.move(speedL);
  motorR.move(speedR);
  motorL.loopFOC();
  motorR.loopFOC();
}

3. 自适应 PID 跟随（带死区与饱和保护）

场景：工业级应用，加入非线性处理和异常保护。核心逻辑：死区控制 + PID 自适应 + 安全保护。

#include <SimpleFOC.h>
#include <NewPing.h>
#include <PID_v1.h>

// ... 引脚定义与案例一类似
double setpoint = 30.0;
double input, output;
PID myPID(&input, &output, &setpoint, 0.5, 0.1, 0.05, DIRECT);

// 自适应参数
float errorDeadZone = 2.0; // 死区±2cm
float errorThreshold = 20.0; // 误差超过 20cm 时启用强 PID
float maxSpeed = 0.4;

void setup() {
  // 初始化...
  myPID.SetMode(AUTOMATIC);
  myPID.SetOutputLimits(-maxSpeed, maxSpeed);
}

void loop() {
  input = sonar.ping_cm();
  if (input == 0) { safetyStop(); return; }
  
  float error = input - setpoint;

  // 1. 死区控制：误差在死区内不调节
  if (abs(error) < errorDeadZone) {
    motorL.move(0);
    motorR.move(0);
    return;
  }

  // 2. 自适应 PID：大误差时增强 P 项
  if (abs(error) > errorThreshold) {
    myPID.SetTunings(1.0, 0.0, 0.0); // 纯 P 控制
  } else {
    myPID.SetTunings(0.5, 0.1, 0.05); // 正常 PID
  }

  // 3. PID 计算
  myPID.Compute();

  // 4. 前馈补偿
  float feedForward = 0.0;
  if (error > 0) feedForward = 0.1; // 距离过远，给予基础前向速度
  
  float speedL = output + feedForward;
  float speedR = output + feedForward;

  // 5. 软限制
  speedL = constrain(speedL, -0.5, 0.5);
  speedR = constrain(speedR, -0.5, 0.5);
  
  motorL.move(speedL);
  motorR.move(speedR);
  motorL.loopFOC();
  motorR.loopFOC();
}

void safetyStop() {
  // 软停止
  static float speedL = 0, speedR = 0;
  speedL *= 0.9;
  speedR *= 0.9;
  motorL.move(speedL);
  motorR.move(speedR);
}

关键要点解读

超声波传感器的局限与应对

回波干扰在狭小空间或面对吸音材料时可能发生。代码中应加入有效性校验，连续多次无效读数时紧急停车。此外，超声波锥形波束（约 15°）可能同时检测到障碍物和目标，双超声波方案可提供简单的横向定位，避免目标偏离中线时机器人'视而不见'。

PID 控制的三重调校

• P（比例）：决定响应速度。P 值过小会导致跟随迟缓，过大则会在目标距离附近震荡。
• I（积分）：消除稳态误差。如果机器人最终停在 32cm 而非 30cm，需增加 Ki。但 I 值过大会导致'积分饱和'，响应滞后。
• D（微分）：抑制超调。当目标突然靠近，微分项可提前减速。案例三的自适应 PID 在大误差时切为纯 P 控制，避免积分项累积造成危险。

'差速转向'是实现横向跟随的关键

angleInput = distL - distR 是核心。当目标偏左时，左声纳距离更近，angleInput 为负，pidAngle 输出负值，使左轮减速、右轮加速，从而实现转向。这种双 PID 混控是轮式机器人跟踪移动目标的经典方案。

非线性的必要处理

• 死区（Dead Zone）：如案例三，±2cm 内不响应，避免机器人在目标距离附近'抽搐'。这是节省能量、减少机械磨损的关键。
• 输出限幅：PID 输出必须限制在电机安全范围内。案例一通过 SetOutputLimits() 限制，案例三通过 constrain() 实现平滑限幅，避免急启急停。

BLDC 闭环控制是精度保障

使用 analogWrite() 的开环控制，负载变化时速度会漂移，导致跟随距离波动。采用 SimpleFOC 库的闭环控制，电机速度严格跟随 motor.move() 的指令，这是 PID 算法能稳定工作的基础。编码器反馈的实时速度信息还可用于速度前馈，进一步提升响应。

其他参考方案

4. 超声波跟随小车（单传感器避障跟随）

适用于基础入门，核心在于 PWM 映射与基础 PID。

#include <NewPing.h>
#include <PID_v1.h>

#define TRIG_PIN 12
#define ECHO_PIN 11
#define MAX_DISTANCE 200

NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);
double setpoint = 30.0;
double input, output;
PID myPID(&input, &output, &setpoint, 2.0, 5.0, 1.0, DIRECT);

const int motorL_PWM = 5;
const int motorR_PWM = 6;

void setup() {
  Serial.begin(9600);
  myPID.SetMode(AUTOMATIC);
  pinMode(motorL_PWM, OUTPUT);
  pinMode(motorR_PWM, OUTPUT);
}

void loop() {
  input = sonar.ping_cm();
  if (input == 0) input = MAX_DISTANCE;
  
  myPID.Compute();
  
  int speed = constrain(output, -255, 255);
  if (speed > 0) {
    analogWrite(motorL_PWM, speed);
    analogWrite(motorR_PWM, speed);
  } else {
    analogWrite(motorL_PWM, 0);
    analogWrite(motorR_PWM, 0);
  }
  
  Serial.print("Distance: ");
  Serial.print(input);
  Serial.print(" -> Speed: ");
  Serial.println(speed);
  delay(50);
}

5. 双超声波传感器差速跟随

通过左右轮速度差实现转向，需确保电机极性一致。

#include <NewPing.h>
#include <PID_v1.h>

#define LEFT_TRIG 12
#define LEFT_ECHO 11
#define RIGHT_TRIG 10
#define RIGHT_ECHO 9
#define MAX_DISTANCE 100

NewPing leftSonar(LEFT_TRIG, LEFT_ECHO, MAX_DISTANCE);
NewPing rightSonar(RIGHT_TRIG, RIGHT_ECHO, MAX_DISTANCE);

double leftDist, rightDist, error, output;
PID myPID(&error, &output, &setpoint, 1.5, 0.1, 0.5, DIRECT);

const int motorL_PWM = 5;
const int motorR_PWM = 6;
const int baseSpeed = 150;

void setup() {
  Serial.begin(9600);
  myPID.SetMode(AUTOMATIC);
  pinMode(motorL_PWM, OUTPUT);
  pinMode(motorR_PWM, OUTPUT);
}

void loop() {
  leftDist = leftSonar.ping_cm();
  rightDist = rightSonar.ping_cm();
  if (leftDist == 0) leftDist = MAX_DISTANCE;
  if (rightDist == 0) rightDist = MAX_DISTANCE;
  
  error = leftDist - rightDist;
  myPID.Compute();
  
  int leftSpeed = constrain(baseSpeed - output, 0, 255);
  int rightSpeed = constrain(baseSpeed + output, 0, 255);
  
  analogWrite(motorL_PWM, leftSpeed);
  analogWrite(motorR_PWM, rightSpeed);
  
  Serial.print("Left: ");
  Serial.print(leftDist);
  Serial.print(" Right: ");
  Serial.print(rightDist);
  Serial.print(" -> L/R Speed: ");
  Serial.print(leftSpeed);
  Serial.print("/");
  Serial.println(rightSpeed);
  delay(50);
}

6. 带速度限制的智能跟随

根据距离动态调整基础速度，提升跟随平滑性。

#include <NewPing.h>
#include <PID_v1.h>

#define TRIG_PIN 12
#define ECHO_PIN 11
#define MAX_DISTANCE 150

NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);
double setpoint = 40.0;
double input, output;
PID myPID(&input, &output, &setpoint, 1.8, 0.5, 0.2, DIRECT);

const int motorL_PWM = 5;
const int motorR_PWM = 6;
const int maxSpeed = 200;

void setup() {
  Serial.begin(9600);
  myPID.SetMode(AUTOMATIC);
  myPID.SetOutputLimits(-maxSpeed, maxSpeed);
  pinMode(motorL_PWM, OUTPUT);
  pinMode(motorR_PWM, OUTPUT);
}

void loop() {
  input = sonar.ping_cm();
  if (input == 0) input = MAX_DISTANCE;
  
  myPID.Compute();
  
  int baseSpeed = map(constrain(input, 20, 100), 20, 100, 80, 200);
  int leftSpeed = baseSpeed + output;
  int rightSpeed = baseSpeed - output;
  
  leftSpeed = constrain(leftSpeed, 0, maxSpeed);
  rightSpeed = constrain(rightSpeed, 0, maxSpeed);
  
  analogWrite(motorL_PWM, leftSpeed);
  analogWrite(motorR_PWM, rightSpeed);
  
  Serial.print("Distance: ");
  Serial.print(input);
  Serial.print(" -> L/R Speed: ");
  Serial.print(leftSpeed);
  Serial.print("/");
  Serial.println(rightSpeed);
  delay(30);
}

以上案例仅供参考，实际编程时需根据自己的硬件配置、使用场景和具体需求进行调整。涉及硬件操作的代码，要在使用前确认引脚和电平等参数的正确性和安全性。