跳到主要内容Arduino BLDC 基于串口指令的远程控制工业巡检机器人 | 极客日志C++算法
Arduino BLDC 基于串口指令的远程控制工业巡检机器人
介绍基于 Arduino 的 BLDC 电机远程工业巡检机器人系统。涵盖 UART 串口通信架构、BLDC 驱动特性及模块化设计。详细阐述了电力石化、轨道交通等应用场景,并强调通信抗干扰、电源管理及安全机制。提供四种代码方案:基础 PWM 调速、多电机差速协同、PID 闭环控制及 ROS/Modbus 集成。重点解析协议设计、状态反馈与安全容错策略,为嵌入式机器人开发提供参考。
花里胡哨1 浏览 Arduino BLDC 基于串口指令的远程控制工业巡检机器人
基于 Arduino 的 BLDC 串口指令远程控制工业巡检机器人,是一种将嵌入式控制、高效驱动与可靠通信技术深度融合的工业自动化解决方案。该系统以 Arduino 为核心控制器,驱动 BLDC 电机实现高机动性移动,通过串口通信链路接收上位机或远程终端的指令,实现对机器人的精确操控与状态监控。
1. 主要特点
高可靠性的串口通信架构
串口通信(UART)作为工业控制领域的基石,提供了稳定、低延迟的指令传输通道。
- 协议灵活性:系统可定义自定义的二进制或 ASCII 协议。例如,通过发送字符指令(如 "F" 前进,"B" 后退,"L" 左转,"R" 右转)或结构化数据包(包含速度、方向、任务 ID 等字段),实现复杂的控制逻辑。
- 硬件接口多样性:物理层可采用标准 TTL 电平、RS232 或 RS485。其中,RS485 支持多点、长距离(可达 1200 米)差分传输,具有极强的抗共模干扰能力,非常适合工业现场复杂的电磁环境。
- 低延迟实时控制:相较于网络协议栈,串口通信的协议开销极小,数据传输延迟通常在微秒级。这使得操作员能够实时控制机器人的运动状态,快速响应突发情况。
BLDC 驱动的高机动性与长续航
BLDC 电机为巡检机器人提供了适应复杂工业环境的动力保障。
- 高能效比:BLDC 电机效率通常高达 85%-90%,配合电子调速器(ESC),在频繁启停的巡检任务中能显著降低能耗,延长单次充电的巡检时长。
- 宽调速范围与高动态响应:通过 PWM 信号精确控制 BLDC 电机,机器人可实现从极低速(用于精细检查设备仪表)到高速(用于快速转移阵地)的平滑调速。电机的快速扭矩响应特性确保了机器人在接收到急停或变向指令时能迅速执行。
- 低维护与高可靠性:无电刷设计消除了机械磨损和火花干扰,使驱动系统能在粉尘、油污等恶劣工业环境中长期稳定运行,减少了停机维护成本。
模块化与可扩展性
Arduino 的开源生态为系统功能的扩展提供了极大便利。
- 多传感器集成:通过串口、I2C 或 SPI 总线,可轻松集成红外热像仪、气体传感器、高清摄像头等多种巡检传感器。Arduino 负责协调这些传感器的数据采集,并通过串口回传给上位机。
- 分级控制架构:通常采用'上位机规划 + Arduino 执行'的模式。上位机负责复杂的路径规划、图像处理和数据分析,Arduino 则专注于底层的电机控制、传感器数据读取和紧急避障,实现了计算资源的合理分配。
2. 应用场景
- 电力与石化设施巡检:在变电站、炼油厂等高危环境中,机器人代替人工进入现场,通过串口接收远程指令,对变压器、管道、阀门等设备进行巡检。操作员可在控制室实时查看机器人回传的红外热图、气体浓度数据和视频画面,及时发现漏电、漏油、过热等隐患。
- 轨道交通与隧道检测:在地铁隧道、铁路涵洞等狭长、封闭的空间内,机器人沿轨道或预设路径行进,利用串口链路与基站保持通信。通过远程控制,操作员可指挥机器人对轨道状态、隧道壁结构进行详细检查,解决了人工巡检视野受限、效率低下的问题。
- 大型仓储与数据中心:在面积广阔的仓库或数据中心机房,机器人执行环境监测任务。通过串口指令,系统可调度机器人前往指定区域检测温湿度、烟雾浓度,或对服务器机柜的指示灯状态进行视觉识别,实现无人化、全天候的智能巡检。
- 应急救援与排爆:在发生事故或存在爆炸风险的现场,机器人作为侦查先锋,通过有线或无线串口(如数传电台)接收远程指令。其强大的越障能力和灵活的运动性能,使其能够深入复杂废墟,回传现场音视频信息,为救援决策提供依据。
3. 注意事项
通信稳定性与抗干扰
- 硬件隔离与保护:工业现场存在强烈的电磁干扰和浪涌电压。必须在串口通信线路中加入光耦隔离、磁耦隔离或 RS485 收发器(如 MAX485)的保护电路,防止干扰信号或高压尖峰损坏 Arduino 和上位机。
- 数据校验与重传机制:在软件层面,必须设计完善的数据包结构,包含帧头、地址、指令、数据、校验和(如 CRC16)及帧尾。若接收方校验失败,应丢弃数据包并请求重传,确保指令的准确无误。
电源管理与系统分区
- :BLDC 电机驱动属于强电回路,其启动和换向会产生巨大的电流冲击和电压波动。必须使用独立的 DC-DC 模块为 Arduino 及其逻辑电路供电,严禁与电机共用电源路径,防止电机噪声导致单片机复位。
微信扫一扫,关注极客日志
微信公众号「极客日志」,在微信中扫描左侧二维码关注。展示文案:极客日志 zeeklog
相关免费在线工具
- 加密/解密文本
使用加密算法(如AES、TripleDES、Rabbit或RC4)加密和解密文本明文。 在线工具,加密/解密文本在线工具,online
- Base64 字符串编码/解码
将字符串编码和解码为其 Base64 格式表示形式即可。 在线工具,Base64 字符串编码/解码在线工具,online
- Base64 文件转换器
将字符串、文件或图像转换为其 Base64 表示形式。 在线工具,Base64 文件转换器在线工具,online
- Markdown 转 HTML
将 Markdown(GFM)转为 HTML 片段,浏览器内 marked 解析;与 HTML 转 Markdown 互为补充。 在线工具,Markdown 转 HTML在线工具,online
- HTML 转 Markdown
将 HTML 片段转为 GitHub Flavored Markdown,支持标题、列表、链接、代码块与表格等;浏览器内处理,可链接预填。 在线工具,HTML 转 Markdown在线工具,online
- JSON 压缩
通过删除不必要的空白来缩小和压缩JSON。 在线工具,JSON 压缩在线工具,online
强弱电分离
电源滤波:在电源输入端和逻辑电路端并联大容量电解电容和高频陶瓷电容,以吸收电压尖峰,平滑电源噪声。
- 看门狗与超时保护:必须启用 Arduino 的硬件看门狗定时器。同时,在程序中设置通信超时机制,若在设定时间内未接收到上位机的'心跳包'或有效指令,系统应自动进入安全模式(如停止电机、启动声光报警)。
- 急停与限位保护:物理急停按钮是必须的硬件安全措施。此外,还应设置软件限位,当传感器检测到障碍物过近或电机电流异常(可能卡死)时,立即切断电机动力。
- 指令冲突处理:需设计合理的协议优先级。例如,急停指令应具有最高优先级,任何时刻接收到急停指令都必须立即响应,而速度调节指令则应平滑过渡。
- 双向通信与状态反馈:远程控制不应是单向的。Arduino 应定期或在事件触发时向上位机回传机器人的状态信息(如当前坐标、电池电压、电机温度、传感器读数),以便操作员全面掌握现场情况,实现闭环控制。
4. 代码示例
4.1 基础串口速度控制(PWM 信号)
#include <Servo.h>
Servo esc;
String inputCommand;
int currentSpeed = 1000;
void setup() {
Serial.begin(115200);
esc.attach(9, 1000, 2000);
esc.writeMicroseconds(currentSpeed);
Serial.println("BLDC Control Ready (PWM Mode)");
Serial.println("Commands: SPEED:<1000-2000> | STOP");
}
void loop() {
if (Serial.available() > 0) {
inputCommand = Serial.readStringUntil('\n');
inputCommand.trim();
if (inputCommand.startsWith("SPEED:")) {
currentSpeed = inputCommand.substring(6).toInt();
currentSpeed = constrain(currentSpeed, 1000, 2000);
esc.writeMicroseconds(currentSpeed);
Serial.print("Speed set to: ");
Serial.println(currentSpeed);
} else if (inputCommand == "STOP") {
currentSpeed = 1000;
esc.writeMicroseconds(currentSpeed);
Serial.println("Emergency Stop");
}
}
}
应用场景:通过上位机(如 Python 脚本)发送串口指令控制机器人电机启停和调速。
4.2 多电机协同控制(差速转向)
#include <Servo.h>
Servo escLeft, escRight;
String command;
void setup() {
Serial.begin(115200);
escLeft.attach(9, 1000, 2000);
escRight.attach(10, 1000, 2000);
Serial.println("Differential Drive Ready");
Serial.println("Commands: FORWARD:<speed> | TURN:<left_speed>:<right_speed> | STOP");
}
void loop() {
if (Serial.available()) {
command = Serial.readStringUntil('\n');
command.trim();
if (command.startsWith("FORWARD:")) {
int speed = command.substring(8).toInt();
speed = constrain(speed, 1000, 2000);
escLeft.writeMicroseconds(speed);
escRight.writeMicroseconds(speed);
Serial.print("Moving forward at: ");
Serial.println(speed);
} else if (command.startsWith("TURN:")) {
int colonIndex = command.indexOf(':', 5);
int leftSpeed = command.substring(5, colonIndex).toInt();
int rightSpeed = command.substring(colonIndex + 1).toInt();
escLeft.writeMicroseconds(constrain(leftSpeed, 1000, 2000));
escRight.writeMicroseconds(constrain(rightSpeed, 1000, 2000));
Serial.print("Turning - Left: ");
Serial.print(leftSpeed);
Serial.print(" | Right: ");
Serial.println(rightSpeed);
} else if (command == "STOP") {
escLeft.writeMicroseconds(1000);
escRight.writeMicroseconds(1000);
Serial.println("Stopped");
}
}
}
应用场景:差速驱动机器人(如巡检小车),通过串口指令实现直行、转向和停止。
4.3 带反馈的闭环控制(PID 调速)
#include <Servo.h>
#include <PID_v1.h>
Servo esc;
int encoderPin = 2;
volatile long pulseCount = 0;
double targetRPM = 100, actualRPM = 0;
double outputPWM = 1000;
PID myPID(&actualRPM, &outputPWM, &targetRPM, 0.5, 0.1, 0.01, DIRECT);
void countPulses() { pulseCount++; }
void setup() {
Serial.begin(115200);
esc.attach(9, 1000, 2000);
pinMode(encoderPin, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(encoderPin), countPulses, RISING);
myPID.SetMode(AUTOMATIC);
myPID.SetOutputLimits(1000, 2000);
Serial.println("Closed-Loop Control Ready");
}
void loop() {
static unsigned long lastTime = 0;
if (millis() - lastTime >= 100) {
noInterrupts();
long currentPulses = pulseCount;
pulseCount = 0;
interrupts();
actualRPM = (currentPulses / 20.0) * 60;
myPID.Compute();
esc.writeMicroseconds(outputPWM);
Serial.print("Target: ");
Serial.print(targetRPM);
Serial.print(" | Actual: ");
Serial.print(actualRPM);
Serial.print(" | PWM: ");
Serial.println(outputPWM);
lastTime = millis();
}
if (Serial.available()) {
String cmd = Serial.readStringUntil('\n');
if (cmd.startsWith("RPM:")) {
targetRPM = cmd.substring(4).toDouble();
Serial.print("New RPM Target: ");
Serial.println(targetRPM);
}
}
}
应用场景:需要精确控制轮速的巡检机器人(如爬坡时保持恒定速度),通过编码器反馈实现 PID 闭环控制。
4.4 基础串口指令控制巡检机器人
场景:室内简单巡检,通过串口接收控制指令。核心逻辑:ASCII 指令解析 + 差速控制。
#include <SimpleFOC.h>
BLDCMotor motorL = BLDCMotor(7);
BLDCMotor motorR = BLDCMotor(7);
Encoder encoderL(2, 3, 2048);
Encoder encoderR(18, 19, 2048);
void doA_L() { encoderL.handleA(); }
void doB_L() { encoderL.handleB(); }
void doA_R() { encoderR.handleA(); }
void doB_R() { encoderR.handleB(); }
float linearSpeed = 0.0;
float angularSpeed = 0.0;
float maxSpeed = 1.0;
float wheelSeparation = 0.3;
float wheelRadius = 0.05;
struct Command {
char type;
float param1;
float param2;
unsigned long timestamp;
};
Command currentCmd = {'S', 0, 0, 0};
bool newCommand = false;
String serialBuffer = "";
struct RobotState {
float posX = 0.0, posY = 0.0;
float heading = 0.0;
float battery = 12.6;
float temp = 25.0;
unsigned long uptime = 0;
};
RobotState state;
#define SONAR_TRIG 9
#define SONAR_ECHO 10
float obstacleDistance = 0.0;
void setup() {
Serial.begin(115200);
Serial1.begin(115200);
encoderL.init();
encoderR.init();
encoderL.enableInterrupts(doA_L, doB_L);
encoderR.enableInterrupts(doA_R, doB_R);
motorL.linkSensor(&encoderL);
motorR.linkSensor(&encoderR);
motorL.init();
motorR.init();
motorL.initFOC();
motorR.initFOC();
pinMode(SONAR_TRIG, OUTPUT);
pinMode(SONAR_ECHO, INPUT);
Serial.println("INSPECT> Robot Ready");
Serial.println("INSPECT> Commands: F-forward B-back L-left R-right S-stop");
Serial.println("INSPECT> V0.5-speed0.5 T0.2-turn0.2 C-config");
}
void loop() {
readSerialCommands();
if (newCommand) {
executeCommand(currentCmd);
newCommand = false;
}
motionControl();
updateRobotState();
checkObstacles();
motorL.loopFOC();
motorR.loopFOC();
static unsigned long lastReport = 0;
if (millis() - lastReport > 1000) {
sendStatusReport();
lastReport = millis();
}
}
void readSerialCommands() {
while (Serial.available()) {
char c = Serial.read();
if (c == '\n' || c == '\r') {
if (serialBuffer.length() > 0) {
parseCommand(serialBuffer);
serialBuffer = "";
}
} else {
serialBuffer += c;
}
}
}
void parseCommand(String buffer) {
buffer.trim();
buffer.toUpperCase();
Command cmd = {'S', 0, 0, millis()};
if (buffer.length() == 1) {
cmd.type = buffer[0];
switch (buffer[0]) {
case 'F':
cmd.param1 = 0.3;
break;
case 'B':
cmd.param1 = -0.3;
break;
case 'L':
cmd.type = 'T';
cmd.param1 = 0.5;
break;
case 'R':
cmd.type = 'T';
cmd.param1 = -0.5;
break;
case 'S':
cmd.type = 'S';
break;
case '?':
cmd.type = 'R';
break;
}
} else if (buffer.startsWith("V")) {
cmd.type = 'M';
buffer.remove(0, 1);
cmd.param1 = buffer.toFloat();
} else if (buffer.startsWith("T")) {
cmd.type = 'T';
buffer.remove(0, 1);
cmd.param1 = buffer.toFloat();
} else if (buffer.startsWith("C")) {
parseConfigCommand(buffer, cmd);
} else if (buffer.startsWith("G")) {
cmd.type = 'G';
parseCoordinates(buffer, cmd);
}
currentCmd = cmd;
newCommand = true;
Serial.print("INSPECT> Received: ");
Serial.println(buffer);
}
void executeCommand(Command cmd) {
switch (cmd.type) {
case 'M':
linearSpeed = constrain(cmd.param1, -maxSpeed, maxSpeed);
angularSpeed = 0;
Serial.print("INSPECT> Set speed: ");
Serial.println(linearSpeed, 2);
break;
case 'T':
angularSpeed = constrain(cmd.param1, -1.0, 1.0);
linearSpeed = 0;
Serial.print("INSPECT> Set turn: ");
Serial.println(angularSpeed, 2);
break;
case 'S':
linearSpeed = 0;
angularSpeed = 0;
Serial.println("INSPECT> Stopped");
break;
case 'R':
sendDetailedStatus();
break;
case 'G':
navigateTo(cmd.param1, cmd.param2);
break;
case 'C':
applyConfiguration(cmd);
break;
}
}
void motionControl() {
float leftSpeed = linearSpeed - (angularSpeed * wheelSeparation / 2.0);
float rightSpeed = linearSpeed + (angularSpeed * wheelSeparation / 2.0);
float leftMotorSpeed = leftSpeed / wheelRadius;
float rightMotorSpeed = rightSpeed / wheelRadius;
leftMotorSpeed = constrain(leftMotorSpeed, -20.0, 20.0);
rightMotorSpeed = constrain(rightMotorSpeed, -20.0, 20.0);
motorL.move(leftMotorSpeed);
motorR.move(rightMotorSpeed);
}
void updateRobotState() {
static float lastLeftPos = 0, lastRightPos = 0;
float leftPos = motorL.shaft_angle;
float rightPos = motorR.shaft_angle;
float deltaLeft = leftPos - lastLeftPos;
float deltaRight = rightPos - lastRightPos;
float linearDelta = (deltaLeft + deltaRight) * wheelRadius / 2.0;
float angularDelta = (deltaRight - deltaLeft) * wheelRadius / wheelSeparation;
state.heading += angularDelta;
state.posX += linearDelta * cos(state.heading);
state.posY += linearDelta * sin(state.heading);
lastLeftPos = leftPos;
lastRightPos = rightPos;
state.uptime = millis() / 1000;
state.battery = 12.0 + random(0, 20) / 10.0;
state.temp = 25.0 + random(0, 20) / 10.0;
}
void checkObstacles() {
digitalWrite(SONAR_TRIG, LOW);
delayMicroseconds(2);
digitalWrite(SONAR_TRIG, HIGH);
delayMicroseconds(10);
digitalWrite(SONAR_TRIG, LOW);
long duration = pulseIn(SONAR_ECHO, HIGH, 30000);
obstacleDistance = duration * 0.034 / 2.0;
if (obstacleDistance > 0 && obstacleDistance < 0.3) {
if (linearSpeed > 0) {
linearSpeed = 0;
angularSpeed = 0.5;
Serial.println("INSPECT> Obstacle detected! Auto avoiding");
}
}
}
void sendStatusReport() {
Serial.print("INSPECT> STATUS X:");
Serial.print(state.posX, 2);
Serial.print(" Y:");
Serial.print(state.posY, 2);
Serial.print(" H:");
Serial.print(state.heading, 2);
Serial.print(" Bat:");
Serial.print(state.battery, 1);
Serial.print("V Temp:");
Serial.print(state.temp, 1);
Serial.print("C Obst:");
Serial.print(obstacleDistance, 2);
Serial.println("m");
}
4.5 工业 Modbus RTU 串口控制
场景:工业环境,需要标准 Modbus 协议与 PLC/DCS 集成。核心逻辑:Modbus RTU 协议 + 寄存器映射。
#include <SimpleFOC.h>
#include <ModbusRTU.h>
BLDCMotor motorL, motorR;
ModbusRTU mb;
#define MODBUS_SLAVE_ID 1
#define MODBUS_BAUDRATE 9600
#define MODBUS_SERIAL Serial2
enum ModbusRegisters {
REG_SET_SPEED = 0,
REG_SET_ANGLE,
REG_MAX_SPEED,
REG_ACCELERATION,
REG_CONTROL_MODE,
REG_TARGET_X,
REG_TARGET_Y,
REG_ACTUAL_SPEED = 100,
REG_ACTUAL_ANGLE,
REG_POSITION_X,
REG_POSITION_Y,
REG_BATTERY_VOLTAGE,
REG_MOTOR_TEMP,
REG_OBSTACLE_DIST,
REG_ERROR_CODE,
REG_STATUS_WORD,
REG_COUNT = 150
};
uint16_t mbRegisters[REG_COUNT] = {0};
uint16_t mbInputs[50] = {0};
struct IndustrialState {
float speedActual = 0.0;
float angleActual = 0.0;
float posX = 0.0, posY = 0.0;
float battery = 24.0;
float temperature[2] = {25.0, 25.0};
uint16_t errorCode = 0;
uint16_t statusWord = 0x0001;
bool emergencyStop = false;
};
IndustrialState iState;
enum ControlMode { SPEED_CTRL, POSITION_CTRL, TRAJECTORY_CTRL };
ControlMode ctrlMode = SPEED_CTRL;
void setup() {
Serial.begin(115200);
MODBUS_SERIAL.begin(MODBUS_BAUDRATE, SERIAL_8N1);
mb.begin(&MODBUS_SERIAL);
mb.setBaudrate(MODBUS_BAUDRATE);
mb.slave(MODBUS_SLAVE_ID);
mb.addHreg(REG_SET_SPEED, 0, REG_COUNT);
mb.onSet(HREG(0), writeRegisterCallback, REG_COUNT);
mb.onGet(HREG(0), readRegisterCallback, REG_COUNT);
initModbusRegisters();
initBLDCMotors();
Serial.println("INSPECT> Modbus RTU Industrial Robot Ready");
Serial.println("INSPECT> Slave ID: 1, Baud: 9600, 8N1");
}
void loop() {
mb.task();
processModbusCommands();
switch (ctrlMode) {
case SPEED_CTRL:
speedControl();
break;
case POSITION_CTRL:
positionControl();
break;
case TRAJECTORY_CTRL:
trajectoryControl();
break;
}
updateInputRegisters();
safetyMonitor();
motorL.loopFOC();
motorR.loopFOC();
static unsigned long lastDiag = 0;
if (millis() - lastDiag > 1000) {
sendDiagnosticInfo();
lastDiag = millis();
}
}
void initModbusRegisters() {
mb.Hreg(REG_SET_SPEED, 0);
mb.Hreg(REG_MAX_SPEED, 100);
mb.Hreg(REG_ACCELERATION, 50);
mb.Hreg(REG_TARGET_X, 0);
mb.Hreg(REG_TARGET_Y, 0);
mb.Hreg(REG_CONTROL_MODE, SPEED_CTRL);
for (int i = 0; i < 50; i++) {
mb.Ireg(i, 0);
}
}
bool writeRegisterCallback(Modbus::ResultCode event, uint16_t address, uint16_t val) {
if (event == Modbus::EX_SUCCESS) {
Serial.print("MODBUS> Write reg ");
Serial.print(address);
Serial.print(" = ");
Serial.println(val);
switch (address) {
case REG_SET_SPEED:
handleSpeedCommand(val);
break;
case REG_CONTROL_MODE:
ctrlMode = (ControlMode)constrain(val, 0, 2);
Serial.print("MODBUS> Control mode: ");
Serial.println(ctrlMode);
break;
case REG_TARGET_X:
case REG_TARGET_Y:
if (ctrlMode == POSITION_CTRL || ctrlMode == TRAJECTORY_CTRL) {
handlePositionCommand();
}
break;
default:
mb.Hreg(address, val);
}
return true;
}
return false;
}
void handleSpeedCommand(uint16_t speedReg) {
float speedSetpoint = speedReg / 100.0;
static float currentSpeed = 0.0;
float maxAccel = mb.Hreg(REG_ACCELERATION) / 100.0;
float maxSpeed = mb.Hreg(REG_MAX_SPEED) / 100.0;
if (speedSetpoint > currentSpeed) {
currentSpeed = min(currentSpeed + maxAccel * 0.1, speedSetpoint);
} else {
currentSpeed = max(currentSpeed - maxAccel * 0.1, speedSetpoint);
}
currentSpeed = constrain(currentSpeed, -maxSpeed, maxSpeed);
iState.speedActual = currentSpeed;
float motorSpeed = currentSpeed / 0.05;
motorL.move(motorSpeed);
motorR.move(motorSpeed);
}
void handlePositionCommand() {
float targetX = mb.Hreg(REG_TARGET_X) / 100.0;
float targetY = mb.Hreg(REG_TARGET_Y) / 100.0;
float dx = targetX - iState.posX;
float dy = targetY - iState.posY;
float distance = sqrt(dx * dx + dy * dy);
if (distance > 0.01) {
float targetAngle = atan2(dy, dx);
float angleError = targetAngle - iState.angleActual;
while (angleError > PI) angleError -= 2 * PI;
while (angleError < -PI) angleError += 2 * PI;
float angularSpeed = angleError * 2.0;
float linearSpeed = min(distance * 0.5, 0.5);
float wheelSep = 0.3;
float leftSpeed = linearSpeed - (angularSpeed * wheelSep / 2.0);
float rightSpeed = linearSpeed + (angularSpeed * wheelSep / 2.0);
motorL.move(leftSpeed / 0.05);
motorR.move(rightSpeed / 0.05);
}
}
void updateInputRegisters() {
mb.Ireg(REG_ACTUAL_SPEED - 100, (uint16_t)(iState.speedActual * 100));
mb.Ireg(REG_POSITION_X - 100, (uint16_t)(iState.posX * 100));
mb.Ireg(REG_POSITION_Y - 100, (uint16_t)(iState.posY * 100));
mb.Ireg(REG_BATTERY_VOLTAGE - 100, (uint16_t)(iState.battery * 10));
mb.Ireg(REG_MOTOR_TEMP - 100, (uint16_t)(iState.temperature[0] * 10));
updateStatusWord();
mb.Ireg(REG_STATUS_WORD - 100, iState.statusWord);
mb.Ireg(REG_ERROR_CODE - 100, iState.errorCode);
}
void updateStatusWord() {
iState.statusWord = 0;
iState.statusWord |= 0x0001;
iState.statusWord |= 0x0002;
if (iState.errorCode != 0) {
iState.statusWord |= 0x0004;
}
if (iState.emergencyStop) {
iState.statusWord |= 0x0008;
}
if (iState.battery < 20.0) {
iState.statusWord |= 0x0010;
}
iState.statusWord |= 0x0020;
if (abs(iState.speedActual) < 0.01) {
iState.statusWord |= 0x0040;
}
}
void safetyMonitor() {
if (iState.battery < 18.0) {
iState.errorCode = 0x1001;
emergencyStop();
}
if (iState.temperature[0] > 75.0 || iState.temperature[1] > 75.0) {
iState.errorCode = 0x1002;
deratePower(0.5);
}
static unsigned long lastComm = 0;
if (millis() - lastComm > 5000) {
iState.errorCode = 0x1003;
emergencyStop();
}
}
4.6 ROS 串行协议集成
场景:与 ROS 系统集成,实现高级导航和感知。核心逻辑:ROS Serial 协议 + Twist 消息。
#include <SimpleFOC.h>
#include <ros.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/BatteryState.h>
#include <std_msgs/Bool.h>
ros::NodeHandle nh;
BLDCMotor motorL, motorR;
float wheel_radius = 0.05;
float wheel_base = 0.30;
float encoder_resolution = 2048 * 4;
float left_ticks = 0, right_ticks = 0;
float last_left_ticks = 0, last_right_ticks = 0;
geometry_msgs::Twist cmd_vel;
nav_msgs::Odometry odom_msg;
sensor_msgs::BatteryState battery_msg;
std_msgs::Bool emergency_msg;
ros::Publisher odom_pub("odom", &odom_msg);
ros::Publisher battery_pub("battery", &battery_msg);
ros::Publisher emergency_pub("emergency_stop", &emergency_msg);
void cmdVelCallback(const geometry_msgs::Twist& msg) {
cmd_vel = msg;
}
ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &cmdVelCallback);
void emergencyStopCallback(const std_msgs::Bool& msg) {
if (msg.data) {
emergencyStop();
} else {
resumeOperation();
}
}
ros::Subscriber<std_msgs::Bool> emergency_sub("emergency_stop", &emergencyStopCallback);
float max_linear_vel = 1.0;
float max_angular_vel = 1.0;
float linear_vel = 0.0;
float angular_vel = 0.0;
bool emergency_stop = false;
float safe_zone_x_min = 0.0, safe_zone_x_max = 10.0;
float safe_zone_y_min = 0.0, safe_zone_y_max = 10.0;
void setup() {
Serial.begin(57600);
nh.initNode();
nh.advertise(odom_pub);
nh.advertise(battery_pub);
nh.advertise(emergency_pub);
nh.subscribe(cmd_vel_sub);
nh.subscribe(emergency_sub);
initMotorsROS();
initOdometry();
initSafetySystem();
nh.loginfo("ROS Industrial Inspection Robot Ready");
odom_msg.header.frame_id = "odom";
odom_msg.child_frame_id = "base_link";
}
void loop() {
nh.spinOnce();
checkSafetyZone();
if (!emergency_stop) {
velocityControl();
} else {
motorL.move(0);
motorR.move(0);
}
updateOdometry();
publishROSData();
motorL.loopFOC();
motorR.loopFOC();
delay(10);
}
void velocityControl() {
linear_vel = constrain(cmd_vel.linear.x, -max_linear_vel, max_linear_vel);
angular_vel = constrain(cmd_vel.angular.z, -max_angular_vel, max_angular_vel);
float left_speed = (linear_vel - angular_vel * wheel_base / 2.0) / wheel_radius;
float right_speed = (linear_vel + angular_vel * wheel_base / 2.0) / wheel_radius;
left_speed = constrain(left_speed, -20.0, 20.0);
right_speed = constrain(right_speed, -20.0, 20.0);
motorL.move(left_speed);
motorR.move(right_speed);
}
void updateOdometry() {
float current_left = motorL.shaft_angle * encoder_resolution / (2 * PI);
float current_right = motorR.shaft_angle * encoder_resolution / (2 * PI);
float delta_left = current_left - last_left_ticks;
float delta_right = current_right - last_right_ticks;
last_left_ticks = current_left;
last_right_ticks = current_right;
left_ticks += delta_left;
right_ticks += delta_right;
float left_distance = delta_left * 2 * PI * wheel_radius / encoder_resolution;
float right_distance = delta_right * 2 * PI * wheel_radius / encoder_resolution;
static float x = 0.0, y = 0.0, theta = 0.0;
float linear = (left_distance + right_distance) / 2.0;
float angular = (right_distance - left_distance) / wheel_base;
x += linear * cos(theta + angular / 2.0);
y += linear * sin(theta + angular / 2.0);
theta += angular;
while (theta > PI) theta -= 2 * PI;
while (theta < -PI) theta += 2 * PI;
odom_msg.header.stamp = nh.now();
odom_msg.pose.pose.position.x = x;
odom_msg.pose.pose.position.y = y;
odom_msg.pose.pose.position.z = 0.0;
odom_msg.pose.pose.orientation.x = 0.0;
odom_msg.pose.pose.orientation.y = 0.0;
odom_msg.pose.pose.orientation.z = sin(theta / 2.0);
odom_msg.pose.pose.orientation.w = cos(theta / 2.0);
odom_msg.twist.twist.linear.x = linear_vel;
odom_msg.twist.twist.angular.z = angular_vel;
}
void publishROSData() {
static unsigned long last_odom_pub = 0;
static unsigned long last_battery_pub = 0;
unsigned long now = millis();
if (now - last_odom_pub >= 50) {
odom_pub.publish(&odom_msg);
last_odom_pub = now;
}
if (now - last_battery_pub >= 1000) {
updateBatteryMessage();
battery_pub.publish(&battery_msg);
last_battery_pub = now;
}
emergency_msg.data = emergency_stop;
emergency_pub.publish(&emergency_msg);
}
void updateBatteryMessage() {
battery_msg.header.stamp = nh.now();
battery_msg.voltage = readBatteryVoltage();
battery_msg.current = readBatteryCurrent();
battery_msg.percentage = calculateBatteryPercent(battery_msg.voltage);
battery_msg.power_supply_status = sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING;
battery_msg.power_supply_health = sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_GOOD;
battery_msg.present = true;
}
void checkSafetyZone() {
float x = odom_msg.pose.pose.position.x;
float y = odom_msg.pose.pose.position.y;
if (x < safe_zone_x_min || x > safe_zone_x_max || y < safe_zone_y_min || y > safe_zone_y_max) {
if (!emergency_stop) {
nh.logwarn("Robot outside safety zone! Emergency stop.");
emergencyStop();
geometry_msgs::Twist return_cmd;
return_cmd.linear.x = 0.0;
return_cmd.angular.z = 0.0;
cmd_vel = return_cmd;
}
}
}
void emergencyStop() {
emergency_stop = true;
linear_vel = 0.0;
angular_vel = 0.0;
motorL.move(0);
motorR.move(0);
nh.logerror("EMERGENCY STOP ACTIVATED");
}
void resumeOperation() {
if (emergency_stop) {
emergency_stop = false;
nh.loginfo("Emergency stop released");
}
}
void handleROSServices() {
}
5. 要点解读
- ASCII 协议(案例 4):简单直观,适合调试和人机交互。
parseCommand() 解析单字符(F/B/L/R)和带参数命令(V0.5/T0.2)。优点是可读性强,缺点是效率低、无校验。
- Modbus RTU(案例 5):工业标准,适合 PLC 集成。16 位寄存器映射,CRC 校验保证可靠。
writeRegisterCallback() 处理写操作,状态字提供完整状态信息。这是工业部署首选。
- ROS Serial(案例 6):机器人标准,适合与 ROS 导航栈集成。支持 Twist 速度命令、Odometry 里程计、BatteryState 等标准消息。便于与 SLAM、导航、感知模块集成。
- 速度限制:案例 4 的
constrain(linearSpeed, -maxSpeed, maxSpeed) 防止超速。工业环境通常限制在 1m/s 以内。
- 加速度限制:案例 5 的
handleSpeedCommand() 实现梯形速度曲线,避免急启急停对机械的冲击。
- 安全区域:案例 6 的
checkSafetyZone() 实现地理围栏,超出预设区域自动急停。
- 通讯超时:案例 5 的
safetyMonitor() 检测 5 秒无通讯自动停止,防止失控。
- 急停处理:所有案例都有
emergencyStop() 函数,立即置零速度并发布状态。
- 里程计计算:案例 4 和案例 6 都通过编码器积分计算位置。关键公式:position += (Δleft + Δright)/2 * cos(heading)。
- 电池监控:案例 6 的 BatteryState 消息包含电压、电流、电量百分比。工业应用需监控单节电芯电压,防止不平衡。
- 温度监控:案例 5 监控电机温度,超过 75°C 触发降额
deratePower(0.5)。
- 状态字设计:案例 5 的 16 位状态字
statusWord,每 bit 表示一种状态(就绪、错误、急停等),便于 PLC 快速读取。
- 自动避障:案例 4 的
checkObstacles() 检测 30cm 内障碍物自动转向。工业环境需考虑超声波多径反射问题。
- 点到点导航:案例 4 的
navigateTo(x,y) 实现简单航点导航。工业应用需结合反光板、二维码或 UWB 精确定位。
- 巡检路径:可扩展为预设路径循环巡检,定时报告关键设备状态(温度、振动、噪声)。
- 异常上报:通过 Modbus 异常代码或 ROS 的 logerror 上报故障,触发维护工单。
- 波特率选择:调试用 115200,工业 Modbus 用 9600/19200,ROS 用 57600/115200。长距离需降低波特率。
- 电缆要求:RS-485(Modbus)需双绞线,终端电阻 120Ω。ROS Serial 可用普通串口,但建议用 RS-485 转换抗干扰。
- 电源管理:工业机器人常采用 24VDC,需 DC-DC 降压为 12V(电机)和 5V/3.3V(控制板)。案例 5 的
battery < 18.0 欠压保护针对 24V 系统(6S 锂电)。
- 防护等级:工业环境需 IP54 以上防护,接头需防水。代码中应监控湿度传感器,高湿度报警。
- 维护接口:保留固件升级接口(如 USB DFU),支持远程升级。案例可扩展为通过 Modbus 写特定寄存器触发固件更新。
注意,以上案例只是为了拓展思路,仅供参考。它们可能有错误、不适用或者无法编译。您的硬件平台、使用场景和 Arduino 版本可能影响使用方法的选择。实际编程时,您要根据自己的硬件配置、使用场景和具体需求进行调整,并多次实际测试。您还要正确连接硬件,了解所用传感器和设备的规范和特性。涉及硬件操作的代码,您要在使用前确认引脚和电平等参数的正确性和安全性。