【花雕学编程】Arduino BLDC 之基于超声波与PID控制的简单跟随机器人
基于 Arduino 的无刷直流电机（BLDC）超声波与 PID 控制简单跟随机器人，是一个将经典自动控制理论与现代高效驱动技术相结合的典型机电一体化项目。该系统通过超声波传感器获取环境距离信息，利用 PID 算法实时解算运动指令，并由 Arduino 驱动 BLDC 电机执行，从而实现对目标物体的稳定、平滑跟随。
1、主要特点
三角测量与单发双收架构
这是实现“定向”跟随而非“盲目”避障的核心感知逻辑。
单发双收拓扑： 系统通常采用一个手持式超声波发射模块和两个安装在机器人前端左右两侧的接收模块（单发双收）。这种布局构成了一个简单的三角形测量系统。
偏差解算原理： 当目标（人）正对机器人时，左右两个接收模块测得的距离 ，系统可以精确判断目标的偏航角度，从而实现方向控制。
双环 PID 串级控制
为了实现平稳的跟随效果，系统通常采用速度环（内环）与方向环（外环）的串级 PID 控制结构。
方向环（外环）： 以左右超声波距离差 ，通过 PD 或 PID 算法计算出所需的转向角速度（或左右轮速差） 。该环路负责消除方向偏差，使机器人对准目标。
速度环（内环）： 以当前与目标的距离
BLDC 电机的高效动力响应
相较于传统的有刷电机，BLDC 电机为跟随机器人的动态响应和续航提供了坚实基础。
快速动态响应： BLDC 电机具备高扭矩密度和快速的电磁时间常数，能够迅速响应 PID 控制器发出的频繁加减速和转向指令，使机器人的运动轨迹平滑、无滞后。
长续航与低维护： 由于 BLDC 电机无电刷磨损，效率高，发热量小，配合电子调速器（ESC）的高效驱动，使得机器人能够进行长时间的连续跟随作业，特别适合户外或大面积场景。
2、 应用场景
智能行李箱与随行载具：
在机场、车站或校园中，机器人底盘可作为智能行李车，自动跟随主人身后，解放双手。超声波方案成本低廉，且不受光线条件影响，适合室内外通用。
农业辅助与物料搬运：
在农场或仓库，工人可以利用该机器人搬运工具或货物。通过简单的手持发射器，机器人能稳定地跟在工人身后，适应复杂的地形和环境变化。
教育与创客实践平台：
作为高校《自动控制原理》、《传感器技术》和《机电一体化》课程的经典实验项目，学生可以通过搭建该系统，直观地理解三角测量原理、PID 参数整定方法以及 BLDC 电机的控制特性。
展厅导览与广告跟随：
在大型展会中，机器人可以搭载广告屏或展品，自动跟随讲解员移动，确保观众始终能清晰地看到展示内容，提升互动体验。
3、注意事项
传感器选型与安装
抗干扰设计： 超声波传感器易受环境温度、湿度及气流影响，且多个超声波模块同时工作时可能产生串扰。建议在软件中加入时间片轮询机制或频率编码技术，并对采样数据进行滑动平均滤波或卡尔曼滤波，以剔除异常值。
安装位置： 两个接收探头之间的基线距离应尽可能长（在结构允许范围内），以提高角度测量的分辨率。同时，探头应安装在机器人前端较高位置，避免地面反射干扰。
PID 参数整定与系统稳定性
积分饱和（Integral Windup）： 在跟随过程中，若机器人因障碍物或机械卡死无法到达目标位置，积分项会持续累积，导致解除卡死后机器人疯狂加速。必须加入积分限幅或积分分离策略。
微分项噪声： 微分项对测量噪声敏感，容易引起电机抖动。可以适当降低微分增益 ，或在微分环节前加入一阶低通滤波器。
采样周期一致性： PID 控制的稳定性高度依赖于固定的控制周期。必须使用硬件定时器中断（如 Timer1）来触发 PID 计算，严禁使用 delay() 函数导致控制周期抖动。
电机驱动与电源管理
驱动器匹配： Arduino 的 IO 口无法直接驱动 BLDC 电机，必须通过电子调速器（ESC）。需确保 ESC 支持所需的通信协议（如 DShot、PWM），并正确配置其刹车和响应参数。
电源隔离： 大电流的电机回路会产生电压波动，容易导致 Arduino 复位。建议使用独立的稳压模块（如 LM2596）为逻辑电路和电机电路分别供电，并共地处理。
1、单点超声波基本跟随
场景：固定跟随一个移动目标，保持设定距离。
核心逻辑：超声波测距 + 单路PID速度控制。
#include<SimpleFOC.h>// BLDC控制库#include<NewPing.h>// 超声波库#include<PID_v1.h>// PID库#defineTRIG_PIN9#defineECHO_PIN10#defineMAX_DISTANCE200 NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);// BLDC电机对象 BLDCMotor motorL =BLDCMotor(11); BLDCMotor motorR =BLDCMotor(11);// PID参数double setpoint =30.0;// 目标跟随距离：30cmdouble input, output;double Kp =0.5, Ki =0.1, Kd =0.05; PID myPID(&input,&output,&setpoint, Kp, Ki, Kd, DIRECT);voidsetup(){ 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();}voidloop(){// 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>#defineSONAR_L_TRIG2#defineSONAR_L_ECHO3#defineSONAR_R_TRIG4#defineSONAR_R_ECHO5 NewPing sonarL(SONAR_L_TRIG, SONAR_L_ECHO,200); NewPing sonarR(SONAR_R_TRIG, SONAR_R_ECHO,200); BLDCMotor motorL, motorR;// 双路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);voidsetup(){ Serial.begin(115200);// 初始化PID pidDist.SetMode(AUTOMATIC); pidDist.SetOutputLimits(-0.4,0.4); pidAngle.SetMode(AUTOMATIC); pidAngle.SetOutputLimits(-0.3,0.3);// 电机初始化...}voidloop(){// 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;// 死区±2cmfloat errorThreshold =20.0;// 误差超过20cm时启用强PIDfloat maxSpeed =0.4;voidsetup(){// 初始化... myPID.SetMode(AUTOMATIC); myPID.SetOutputLimits(-maxSpeed, maxSpeed);}voidloop(){ 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 =smoothLimit(speedL,0.1);// 平滑限幅 speedR =smoothLimit(speedR,0.1); motorL.move(speedL); motorR.move(speedR); motorL.loopFOC(); motorR.loopFOC();}voidsafetyStop(){// 软停止staticfloat speedL =0, speedR =0; speedL *=0.9; speedR *=0.9; motorL.move(speedL); motorR.move(speedR);}要点解读
超声波传感器的局限与应对
回波干扰：在狭小空间或面对吸音材料时，超声波可能失效。代码中应加入有效性校验（如案例三的safetyStop()），连续多次无效读数时紧急停车。
波束角度：超声波锥形波束（约15°）可能同时检测到障碍物和目标。双超声波方案（案例二）可提供简单的横向定位，避免目标偏离中线时机器人“视而不见”。
PID控制的三重调校
P（比例）：决定响应速度。P值过小会导致跟随迟缓，过大则会在目标距离附近震荡。案例一中Kp=0.5是温和起点。
I（积分）：消除稳态误差。如果机器人最终停在32cm而非30cm，需增加Ki。但I值过大会导致“积分饱和”，响应滞后。
D（微分）：抑制超调。当目标突然靠近，微分项可提前减速。案例三的自适应PID在大误差时切为纯P控制，避免积分项累积造成危险。
“差速转向”是实现横向跟随的关键
案例二的angleInput = distL - distR是核心。当目标偏左时，左声纳距离更近，angleInput为负，pidAngle输出负值，使左轮减速、右轮加速，从而实现转向。这种双PID混控是轮式机器人跟踪移动目标的经典方案。
非线性的必要处理
死区（Dead Zone）：如案例三，±2cm内不响应，避免机器人在目标距离附近“抽搐”。这是节省能量、减少机械磨损的关键。
输出限幅：PID输出必须限制在电机安全范围内。案例一通过SetOutputLimits()限制，案例三通过smoothLimit()实现平滑限幅，避免急启急停。
BLDC闭环控制是精度保障
使用analogWrite()的开环控制，负载变化时速度会漂移，导致跟随距离波动。采用SimpleFOC库的闭环控制，电机速度严格跟随motor.move()的指令，这是PID算法能稳定工作的基础。编码器反馈的实时速度信息还可用于速度前馈，进一步提升响应。
4、超声波跟随小车（单传感器避障跟随）
#include<NewPing.h>#include<PID_v1.h>#defineTRIG_PIN12#defineECHO_PIN11#defineMAX_DISTANCE200 NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);double setpoint =30.0;// 目标跟随距离（cm）double input, output; PID myPID(&input,&output,&setpoint,2.0,5.0,1.0, DIRECT);// Kp, Ki, Kdconstint motorL_PWM =5;constint motorR_PWM =6;voidsetup(){ Serial.begin(9600); myPID.SetMode(AUTOMATIC);pinMode(motorL_PWM, OUTPUT);pinMode(motorR_PWM, OUTPUT);}voidloop(){ input = sonar.ping_cm();// 获取超声波距离if(input ==0) input = MAX_DISTANCE;// 过滤无效值 myPID.Compute();// 计算PID输出// 电机控制：输出值映射到PWM范围（0-255）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>#defineLEFT_TRIG12#defineLEFT_ECHO11#defineRIGHT_TRIG10#defineRIGHT_ECHO9#defineMAX_DISTANCE100 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);constint motorL_PWM =5;constint motorR_PWM =6;constint baseSpeed =150;voidsetup(){ Serial.begin(9600); myPID.SetMode(AUTOMATIC);pinMode(motorL_PWM, OUTPUT);pinMode(motorR_PWM, OUTPUT);}voidloop(){ 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();// 差速控制：根据PID输出调整左右轮速度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>#defineTRIG_PIN12#defineECHO_PIN11#defineMAX_DISTANCE150 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);constint motorL_PWM =5;constint motorR_PWM =6;constint maxSpeed =200;voidsetup(){ Serial.begin(9600); myPID.SetMode(AUTOMATIC); myPID.SetOutputLimits(-maxSpeed, maxSpeed);// 限制输出范围pinMode(motorL_PWM, OUTPUT);pinMode(motorR_PWM, OUTPUT);}voidloop(){ 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);}要点解读
超声波传感器选择与滤波
HC-SR04：成本低，但盲区大（建议最小距离≥2cm），需通过多次采样取中值滤波（如NewPing库的ping_median()）。
JSN-SR04T：防水型，适合户外场景，但响应速度较慢。
多传感器布局：双传感器可实现方向判断（如案例5），但需注意交叉干扰（建议夹角≥30°）。
PID参数调试技巧
手动整定法：先调Kp（比例）使快速响应但不震荡，再调Kd（微分）抑制超调，最后调Ki（积分）消除稳态误差。
动态目标值：如案例6中根据距离调整setpoint，可提升跟随平滑性。
输出限幅：通过SetOutputLimits()防止电机过载（如案例6的±200）。
电机控制策略
差速转向：通过左右轮速度差实现转向（案例5），需确保电机极性一致。
速度映射：使用map()将PID输出映射到PWM范围（案例6），避免非线性响应。
死区处理：当距离误差较小时（如±2cm），强制输出为零以减少抖动。
实时性与抗干扰设计
采样频率：超声波单次测量需30ms，建议循环延迟≥50ms（如案例4的delay(50)）。
异常值过滤：忽略超过MAX_DISTANCE的读数（案例4），或使用滑动窗口滤波。
电源隔离：BLDC电机与传感器共用电源时，添加大电容（如1000μF）稳定电压。
安全与扩展功能
急停机制：当距离<10cm时强制停止（可添加if (input < 10) stopMotors()）。
无线遥控备份：通过蓝牙（如HC-05）覆盖语音或手动控制模式。
视觉融合：未来可集成OpenMV或树莓派，用摄像头辅助目标识别。
注意，以上案例只是为了拓展思路，仅供参考。它们可能有错误、不适用或者无法编译。您的硬件平台、使用场景和Arduino版本可能影响使用方法的选择。实际编程时，您要根据自己的硬件配置、使用场景和具体需求进行调整，并多次实际测试。您还要正确连接硬件，了解所用传感器和设备的规范和特性。涉及硬件操作的代码，您要在使用前确认引脚和电平等参数的正确性和安全性。
