基础循迹跑通了下一步挑战直角弯道。直角弯和普通弯道不一样。普通弯道是渐变的曲率缓慢变化PID控制能平滑处理。直角弯是突变的曲率从0直接跳到无穷大循迹算法会懵——它以为还在直道上结果下一秒车头已经对准墙壁了。这篇文章讲怎么处理直角弯道怎么检测直角、怎么从差速循迹切换到定点旋转、切换时机怎么把握。1 直角弯道的特殊性1.1 普通弯道 vs 直角弯道普通弯道比如15°弯╭──────╮ ──────╯ ╰────── 曲率缓慢变化 传感器响应渐进式偏差 控制策略差速调整直角弯道90°弯│ │ ──────╯ 曲率瞬间跳变 传感器响应几乎全在一侧 控制策略差速失效普通弯道感知阵列会给出连续的偏差信号机器人可以一边修正一边前进。直角弯道偏差会持续为同一个方向的大值比如连续10帧都是10差速调整根本转不过来。1.2 差速控制的局限差速控制的原理是左轮慢、右轮快或反之两轮速度差产生转向。问题在于速度差有限转向速度有限。假设基础速度140最大调整量100左轮0右轮240最极端的差速左轮不动、右轮全速转向速度是有限的。如果弯道足够急差速转不过去。直角弯道的曲率半径趋近于0差速控制根本处理不了。1.3 定点旋转的优势定点旋转的原理是一轮正转、一轮反转。左轮 -MAX (反转) 右轮 MAX (正转) 效果绕两轮中心点旋转 转向半径 0转向半径为零意味着无论弯道多急都能转过去。这是处理直角弯道的正确方法。2 直角识别算法2.1 识别思路直角弯道的特征连续多帧偏差值很大且方向相同。正常循迹时感知信号会不断变化偏差值时正时负。直角弯道上偏差会持续为同一个方向的大值比如连续10帧都是10。可以用一个计数器来捕捉这个模式int consecutiveBigError 0; // 连续大偏差计数 const int CORNER_THRESHOLD 8; // 触发直角的阈值2.2 偏差方向判断怎么判断偏差方向相同简单方法判断偏差绝对值是否超过某个阈值。if (abs(error) 8) { consecutiveBigError; } else { consecutiveBigError 0; }abs(error) 8意味着机器人认为自己在严重偏离中要么左偏要么右偏。连续8帧以上都在严重偏离就可以判定为直角弯道。2.3 代码实现cpp复制2.4 参数调整CORNER_THRESHOLD是经验值需要根据实际情况调整阈值效果太小(5)普通弯道也会触发误判多合适(7-10)直角触发普通弯道不触发太大(15)直角可能漏检我的实测2cm导引线、速度140、50Hz控制频率设8比较合适。3 定点旋转实现3.1 电机控制改造原来forward()只管正转定点旋转需要正反转同时进行。扩展Motor类class Motor { public: void forward(int speed) { digitalWrite(in1, HIGH); digitalWrite(in2, LOW); analogWrite(pwm, constrain(speed, 0, 255)); } void backward(int speed) { digitalWrite(in1, LOW); digitalWrite(in2, HIGH); analogWrite(pwm, constrain(speed, 0, 255)); } void stop() { digitalWrite(in1, LOW); digitalWrite(in2, LOW); analogWrite(pwm, 0); } };3.2 定点旋转函数void turnInPlace(int direction, int speed) { // direction: 1左转(逆时针), -1右转(顺时针) if (direction 1) { // 左转左轮反转右轮正转 leftMotor.backward(speed); rightMotor.forward(speed); } else { // 右转左轮正转右轮反转 leftMotor.forward(speed); rightMotor.backward(speed); } }3.3 旋转方向判断进入直角模式后需要判断是左转还是右转。这个信息可以从累计的偏差方向获取int cornerDirection 0; // 1左转, -1右转 void updateCornerDetection(int error) { if (abs(error) 8) { consecutiveBigError; // 记录旋转方向 if (error 0) cornerDirection 1; // 左偏要右转找线 if (error 0) cornerDirection -1; // 右偏要左转找线 // 进入直角模式 if (consecutiveBigError CORNER_THRESHOLD !inCornerMode) { inCornerMode true; turnInPlace(cornerDirection, 150); // 开始定点旋转 } } else { consecutiveBigError 0; cornerDirection 0; } }3.4 退出条件定点旋转要转多少不能无限转下去。退出条件是中间传感器检测到导引线。void handleCornerMode() { if (!inCornerMode) return; // 继续定点旋转 turnInPlace(cornerDirection, 150); // 检查是否完成旋转 // 中间传感器(L2和R2之间的MID)检测到导引线 if (irData[2] IR_BLACK) { // 完成旋转退出直角模式 inCornerMode false; consecutiveBigError 0; // 恢复差速循迹 leftMotor.stop(); rightMotor.stop(); delay(50); // 短暂停顿让车身稳定 Serial.println(Corner completed, resuming line follow); } }4 完整代码集成4.1 全局变量// 直角检测相关 int consecutiveBigError 0; const int CORNER_THRESHOLD 8; bool inCornerMode false; int cornerDirection 0; const int CORNER_SPEED 150; // 定点旋转速度4.2 修改后的loopvoid loop() { // 1. 读取感知数据 readIR(); // 2. 计算偏差 error getError(); // 3. 直角检测 updateCornerDetection(error); // 4. 根据模式选择控制策略 if (inCornerMode) { // 直角模式定点旋转 handleCornerMode(); } else { // 正常模式PID循迹 pidOut pidCalculate(error); setMotor(pidOut); } // 5. 串口调试可选 #ifdef DEBUG printDebug(); #endif delay(10); }4.3 直角模式完整状态机5 调试经验5.1 旋转速度选择CORNER_SPEED设150旋转速度大概1.5圈/秒实测。90°转弯大约需要0.6秒。如果觉得太慢可以设200但太快可能转过头。5.2 阈值微调遇到问题时的调试思路问题可能原因调整方法直角误触发阈值太小增大CORNER_THRESHOLD直角漏检阈值太大减小CORNER_THRESHOLD旋转过头退出条件不严格改为连续3帧中间检测到线才退出旋转不足速度太快减小CORNER_SPEED5.3 多直角情况如果是连续两个直角锯齿形赛道需要区分是同一个直角的延续还是新的直角。可以在完成一个直角后设置一个冷却期int cornerCooldown 0; // 直角冷却计数 void updateCornerDetection(int error) { if (cornerCooldown 0) { cornerCooldown--; return; // 冷却期跳过检测 } // ...原有检测逻辑... if (inCornerMode /* 检测完成 */) { cornerCooldown 30; // 冷却30帧约0.3秒 } }6 完整示例代码整合以上所有逻辑给出完整可运行的版本#include Arduino.h // 引脚定义 #define LEFT_IN1 6 #define LEFT_IN2 7 #define LEFT_ENA 4 #define RIGHT_IN1 15 #define RIGHT_IN2 16 #define RIGHT_ENB 5 #define IR_L1 42 #define IR_L2 41 #define IR_MID 40 #define IR_R2 39 #define IR_R1 38 // 参数 #define IR_BLACK LOW #define BASE_SPEED 140 #define MIN_SPEED 25 #define MAX_ADJUST 100 #define CORNER_THRESHOLD 8 #define CORNER_SPEED 150 // PID参数 float Kp 30.0; float Ki 0.05; float Kd 25.0; // Motor类 class Motor { public: int in1, in2, pwm; Motor(int i1, int i2, int p) : in1(i1), in2(i2), pwm(p) {} void init() { pinMode(in1, OUTPUT); pinMode(in2, OUTPUT); pinMode(pwm, OUTPUT); stop(); } void forward(int speed) { digitalWrite(in1, HIGH); digitalWrite(in2, LOW); analogWrite(pwm, constrain(speed, 0, 255)); } void backward(int speed) { digitalWrite(in1, LOW); digitalWrite(in2, HIGH); analogWrite(pwm, constrain(speed, 0, 255)); } void stop() { digitalWrite(in1, LOW); digitalWrite(in2, LOW); analogWrite(pwm, 0); } }; // 全局 Motor leftMotor(LEFT_IN1, LEFT_IN2, LEFT_ENA); Motor rightMotor(RIGHT_IN1, RIGHT_IN2, RIGHT_ENB); int irPins[5] {IR_L1, IR_L2, IR_MID, IR_R2, IR_R1}; int irData[5]; float error 0, lastError 0, integral 0, pidOut 0; String lastValid 00100; // 直角检测 int consecutiveBigError 0; bool inCornerMode false; int cornerDirection 0; int cornerCooldown 0; // 函数 void readIR() { for (int i 0; i 5; i) irData[i] digitalRead(irPins[i]); } int getError() { String state ; for (int i 0; i 5; i) state (irData[i] IR_BLACK) ? 1 : 0; if (state ! 00000 state ! 11111) lastValid state; if (state 10000) return 10; if (state 11000) return 6; if (state 01000) return 3; if (state 00100) return 0; if (state 00010) return -3; if (state 00011) return -6; if (state 00001) return -10; if (state 11111) return 0; if (state 00000) { if (lastValid[0]1||lastValid[1]1) return 5; if (lastValid[3]1||lastValid[4]1) return -5; return 0; } int left (state[0]1)*2(state[1]1); int right (state[3]1)(state[4]1)*2; return left - right; } float pidCalculate(float err) { float p Kp * err; integral err; integral constrain(integral, -30, 30); float i Ki * integral; float d Kd * (err - lastError); float out p i d; out constrain(out, -MAX_ADJUST, MAX_ADJUST); lastError err; return out; } void setMotor(float pidVal) { int L constrain(BASE_SPEED - pidVal, MIN_SPEED, 255); int R constrain(BASE_SPEED pidVal, MIN_SPEED, 255); leftMotor.forward(L); rightMotor.forward(R); } void turnInPlace(int direction, int speed) { if (direction 1) { leftMotor.backward(speed); rightMotor.forward(speed); } else { leftMotor.forward(speed); rightMotor.backward(speed); } } void updateCornerDetection() { if (cornerCooldown 0) { cornerCooldown--; return; } if (abs(error) 8) { consecutiveBigError; if (error 0) cornerDirection 1; if (error 0) cornerDirection -1; if (consecutiveBigError CORNER_THRESHOLD !inCornerMode) { inCornerMode true; Serial.println(Corner: ON); } } else { consecutiveBigError 0; cornerDirection 0; } } void handleCorner() { if (!inCornerMode) return; turnInPlace(cornerDirection, CORNER_SPEED); if (irData[2] IR_BLACK) { inCornerMode false; consecutiveBigError 0; cornerCooldown 30; leftMotor.stop(); rightMotor.stop(); delay(50); Serial.println(Corner: OFF); } } void setup() { Serial.begin(115200); leftMotor.init(); rightMotor.init(); for (int i 0; i 5; i) pinMode(irPins[i], INPUT); delay(1000); } void loop() { readIR(); error getError(); updateCornerDetection(); if (inCornerMode) { handleCorner(); } else { pidOut pidCalculate(error); setMotor(pidOut); } delay(10); }7 小结直角弯道的处理总结识别方法连续多帧偏差≥8进入直角模式机动方式从差速切换到定点旋转方向判断根据偏差正负确定左转还是右转退出条件中间传感器回到导引线退出直角模式冷却机制完成直角后设置冷却期防止误触发
【进阶功能】直角弯道识别与定点旋转机动——从差速控制到定点转向
基础循迹跑通了下一步挑战直角弯道。直角弯和普通弯道不一样。普通弯道是渐变的曲率缓慢变化PID控制能平滑处理。直角弯是突变的曲率从0直接跳到无穷大循迹算法会懵——它以为还在直道上结果下一秒车头已经对准墙壁了。这篇文章讲怎么处理直角弯道怎么检测直角、怎么从差速循迹切换到定点旋转、切换时机怎么把握。1 直角弯道的特殊性1.1 普通弯道 vs 直角弯道普通弯道比如15°弯╭──────╮ ──────╯ ╰────── 曲率缓慢变化 传感器响应渐进式偏差 控制策略差速调整直角弯道90°弯│ │ ──────╯ 曲率瞬间跳变 传感器响应几乎全在一侧 控制策略差速失效普通弯道感知阵列会给出连续的偏差信号机器人可以一边修正一边前进。直角弯道偏差会持续为同一个方向的大值比如连续10帧都是10差速调整根本转不过来。1.2 差速控制的局限差速控制的原理是左轮慢、右轮快或反之两轮速度差产生转向。问题在于速度差有限转向速度有限。假设基础速度140最大调整量100左轮0右轮240最极端的差速左轮不动、右轮全速转向速度是有限的。如果弯道足够急差速转不过去。直角弯道的曲率半径趋近于0差速控制根本处理不了。1.3 定点旋转的优势定点旋转的原理是一轮正转、一轮反转。左轮 -MAX (反转) 右轮 MAX (正转) 效果绕两轮中心点旋转 转向半径 0转向半径为零意味着无论弯道多急都能转过去。这是处理直角弯道的正确方法。2 直角识别算法2.1 识别思路直角弯道的特征连续多帧偏差值很大且方向相同。正常循迹时感知信号会不断变化偏差值时正时负。直角弯道上偏差会持续为同一个方向的大值比如连续10帧都是10。可以用一个计数器来捕捉这个模式int consecutiveBigError 0; // 连续大偏差计数 const int CORNER_THRESHOLD 8; // 触发直角的阈值2.2 偏差方向判断怎么判断偏差方向相同简单方法判断偏差绝对值是否超过某个阈值。if (abs(error) 8) { consecutiveBigError; } else { consecutiveBigError 0; }abs(error) 8意味着机器人认为自己在严重偏离中要么左偏要么右偏。连续8帧以上都在严重偏离就可以判定为直角弯道。2.3 代码实现cpp复制2.4 参数调整CORNER_THRESHOLD是经验值需要根据实际情况调整阈值效果太小(5)普通弯道也会触发误判多合适(7-10)直角触发普通弯道不触发太大(15)直角可能漏检我的实测2cm导引线、速度140、50Hz控制频率设8比较合适。3 定点旋转实现3.1 电机控制改造原来forward()只管正转定点旋转需要正反转同时进行。扩展Motor类class Motor { public: void forward(int speed) { digitalWrite(in1, HIGH); digitalWrite(in2, LOW); analogWrite(pwm, constrain(speed, 0, 255)); } void backward(int speed) { digitalWrite(in1, LOW); digitalWrite(in2, HIGH); analogWrite(pwm, constrain(speed, 0, 255)); } void stop() { digitalWrite(in1, LOW); digitalWrite(in2, LOW); analogWrite(pwm, 0); } };3.2 定点旋转函数void turnInPlace(int direction, int speed) { // direction: 1左转(逆时针), -1右转(顺时针) if (direction 1) { // 左转左轮反转右轮正转 leftMotor.backward(speed); rightMotor.forward(speed); } else { // 右转左轮正转右轮反转 leftMotor.forward(speed); rightMotor.backward(speed); } }3.3 旋转方向判断进入直角模式后需要判断是左转还是右转。这个信息可以从累计的偏差方向获取int cornerDirection 0; // 1左转, -1右转 void updateCornerDetection(int error) { if (abs(error) 8) { consecutiveBigError; // 记录旋转方向 if (error 0) cornerDirection 1; // 左偏要右转找线 if (error 0) cornerDirection -1; // 右偏要左转找线 // 进入直角模式 if (consecutiveBigError CORNER_THRESHOLD !inCornerMode) { inCornerMode true; turnInPlace(cornerDirection, 150); // 开始定点旋转 } } else { consecutiveBigError 0; cornerDirection 0; } }3.4 退出条件定点旋转要转多少不能无限转下去。退出条件是中间传感器检测到导引线。void handleCornerMode() { if (!inCornerMode) return; // 继续定点旋转 turnInPlace(cornerDirection, 150); // 检查是否完成旋转 // 中间传感器(L2和R2之间的MID)检测到导引线 if (irData[2] IR_BLACK) { // 完成旋转退出直角模式 inCornerMode false; consecutiveBigError 0; // 恢复差速循迹 leftMotor.stop(); rightMotor.stop(); delay(50); // 短暂停顿让车身稳定 Serial.println(Corner completed, resuming line follow); } }4 完整代码集成4.1 全局变量// 直角检测相关 int consecutiveBigError 0; const int CORNER_THRESHOLD 8; bool inCornerMode false; int cornerDirection 0; const int CORNER_SPEED 150; // 定点旋转速度4.2 修改后的loopvoid loop() { // 1. 读取感知数据 readIR(); // 2. 计算偏差 error getError(); // 3. 直角检测 updateCornerDetection(error); // 4. 根据模式选择控制策略 if (inCornerMode) { // 直角模式定点旋转 handleCornerMode(); } else { // 正常模式PID循迹 pidOut pidCalculate(error); setMotor(pidOut); } // 5. 串口调试可选 #ifdef DEBUG printDebug(); #endif delay(10); }4.3 直角模式完整状态机5 调试经验5.1 旋转速度选择CORNER_SPEED设150旋转速度大概1.5圈/秒实测。90°转弯大约需要0.6秒。如果觉得太慢可以设200但太快可能转过头。5.2 阈值微调遇到问题时的调试思路问题可能原因调整方法直角误触发阈值太小增大CORNER_THRESHOLD直角漏检阈值太大减小CORNER_THRESHOLD旋转过头退出条件不严格改为连续3帧中间检测到线才退出旋转不足速度太快减小CORNER_SPEED5.3 多直角情况如果是连续两个直角锯齿形赛道需要区分是同一个直角的延续还是新的直角。可以在完成一个直角后设置一个冷却期int cornerCooldown 0; // 直角冷却计数 void updateCornerDetection(int error) { if (cornerCooldown 0) { cornerCooldown--; return; // 冷却期跳过检测 } // ...原有检测逻辑... if (inCornerMode /* 检测完成 */) { cornerCooldown 30; // 冷却30帧约0.3秒 } }6 完整示例代码整合以上所有逻辑给出完整可运行的版本#include Arduino.h // 引脚定义 #define LEFT_IN1 6 #define LEFT_IN2 7 #define LEFT_ENA 4 #define RIGHT_IN1 15 #define RIGHT_IN2 16 #define RIGHT_ENB 5 #define IR_L1 42 #define IR_L2 41 #define IR_MID 40 #define IR_R2 39 #define IR_R1 38 // 参数 #define IR_BLACK LOW #define BASE_SPEED 140 #define MIN_SPEED 25 #define MAX_ADJUST 100 #define CORNER_THRESHOLD 8 #define CORNER_SPEED 150 // PID参数 float Kp 30.0; float Ki 0.05; float Kd 25.0; // Motor类 class Motor { public: int in1, in2, pwm; Motor(int i1, int i2, int p) : in1(i1), in2(i2), pwm(p) {} void init() { pinMode(in1, OUTPUT); pinMode(in2, OUTPUT); pinMode(pwm, OUTPUT); stop(); } void forward(int speed) { digitalWrite(in1, HIGH); digitalWrite(in2, LOW); analogWrite(pwm, constrain(speed, 0, 255)); } void backward(int speed) { digitalWrite(in1, LOW); digitalWrite(in2, HIGH); analogWrite(pwm, constrain(speed, 0, 255)); } void stop() { digitalWrite(in1, LOW); digitalWrite(in2, LOW); analogWrite(pwm, 0); } }; // 全局 Motor leftMotor(LEFT_IN1, LEFT_IN2, LEFT_ENA); Motor rightMotor(RIGHT_IN1, RIGHT_IN2, RIGHT_ENB); int irPins[5] {IR_L1, IR_L2, IR_MID, IR_R2, IR_R1}; int irData[5]; float error 0, lastError 0, integral 0, pidOut 0; String lastValid 00100; // 直角检测 int consecutiveBigError 0; bool inCornerMode false; int cornerDirection 0; int cornerCooldown 0; // 函数 void readIR() { for (int i 0; i 5; i) irData[i] digitalRead(irPins[i]); } int getError() { String state ; for (int i 0; i 5; i) state (irData[i] IR_BLACK) ? 1 : 0; if (state ! 00000 state ! 11111) lastValid state; if (state 10000) return 10; if (state 11000) return 6; if (state 01000) return 3; if (state 00100) return 0; if (state 00010) return -3; if (state 00011) return -6; if (state 00001) return -10; if (state 11111) return 0; if (state 00000) { if (lastValid[0]1||lastValid[1]1) return 5; if (lastValid[3]1||lastValid[4]1) return -5; return 0; } int left (state[0]1)*2(state[1]1); int right (state[3]1)(state[4]1)*2; return left - right; } float pidCalculate(float err) { float p Kp * err; integral err; integral constrain(integral, -30, 30); float i Ki * integral; float d Kd * (err - lastError); float out p i d; out constrain(out, -MAX_ADJUST, MAX_ADJUST); lastError err; return out; } void setMotor(float pidVal) { int L constrain(BASE_SPEED - pidVal, MIN_SPEED, 255); int R constrain(BASE_SPEED pidVal, MIN_SPEED, 255); leftMotor.forward(L); rightMotor.forward(R); } void turnInPlace(int direction, int speed) { if (direction 1) { leftMotor.backward(speed); rightMotor.forward(speed); } else { leftMotor.forward(speed); rightMotor.backward(speed); } } void updateCornerDetection() { if (cornerCooldown 0) { cornerCooldown--; return; } if (abs(error) 8) { consecutiveBigError; if (error 0) cornerDirection 1; if (error 0) cornerDirection -1; if (consecutiveBigError CORNER_THRESHOLD !inCornerMode) { inCornerMode true; Serial.println(Corner: ON); } } else { consecutiveBigError 0; cornerDirection 0; } } void handleCorner() { if (!inCornerMode) return; turnInPlace(cornerDirection, CORNER_SPEED); if (irData[2] IR_BLACK) { inCornerMode false; consecutiveBigError 0; cornerCooldown 30; leftMotor.stop(); rightMotor.stop(); delay(50); Serial.println(Corner: OFF); } } void setup() { Serial.begin(115200); leftMotor.init(); rightMotor.init(); for (int i 0; i 5; i) pinMode(irPins[i], INPUT); delay(1000); } void loop() { readIR(); error getError(); updateCornerDetection(); if (inCornerMode) { handleCorner(); } else { pidOut pidCalculate(error); setMotor(pidOut); } delay(10); }7 小结直角弯道的处理总结识别方法连续多帧偏差≥8进入直角模式机动方式从差速切换到定点旋转方向判断根据偏差正负确定左转还是右转退出条件中间传感器回到导引线退出直角模式冷却机制完成直角后设置冷却期防止误触发
