【花雕学编程】Arduino FOC 之实时路径跟踪的梯形速度规划-CSDN博客

本文链接：https://blog.csdn.net/weixin_41659040/article/details/142606158

在这里插入图片描述

Arduino是一个开放源码的电子原型平台，它可以让你用简单的硬件和软件来创建各种互动的项目。Arduino的核心是一个微控制器板，它可以通过一系列的引脚来连接各种传感器、执行器、显示器等外部设备。Arduino的编程是基于C/C++语言的，你可以使用Arduino IDE（集成开发环境）来编写、编译和上传代码到Arduino板上。Arduino还有一个丰富的库和社区，你可以利用它们来扩展Arduino的功能和学习Arduino的知识。

Arduino的特点是：
1、开放源码：Arduino的硬件和软件都是开放源码的，你可以自由地修改、复制和分享它们。
2、易用：Arduino的硬件和软件都是为初学者和非专业人士设计的，你可以轻松地上手和使用它们。
3、便宜：Arduino的硬件和软件都是非常经济的，你可以用很低的成本来实现你的想法。
4、多样：Arduino有多种型号和版本，你可以根据你的需要和喜好来选择合适的Arduino板。
5、创新：Arduino可以让你用电子的方式来表达你的创意和想象，你可以用Arduino来制作各种有趣和有用的项目，如机器人、智能家居、艺术装置等。

在这里插入图片描述
Arduino FOC（Field Oriented Control，场向量控制）是一种先进的电机控制技术，它允许精确控制电机的转矩和速度。这种控制技术特别适用于无刷直流电机（BLDC）和步进电机。在Arduino平台上实现FOC可以提供平滑的运行和高度的扭矩、速度和位置控制，它通过精确控制电机的电流和电压来实现高效率、高精度和低噪声的操作。

主要特点:
1、高性能电机控制：FOC是一种高级的电机控制算法，可以精准控制PMSM(永磁同步电机)和BLDC(无刷直流)电机，实现平滑的转速和扭矩输出。
2、闭环控制架构：FOC采用闭环反馈控制，通过检测电机的位置和速度数据，实时调整输出电压和电流，确保电机动作符合预期。
3、模块化设计：Arduino FOC库采用模块化设计，包含电机建模、速度/位置/电流控制环、PWM生成等子模块，用户可根据需求灵活组合使用。
4、可移植性强：Arduino FOC可移植到多种硬件平台，如Arduino、ESP32、STM32与树莓派等，适用于功率从几十瓦到几千瓦的电机系统。
5、参数自动识别：FOC库具有自动识别电机参数的功能，可以大幅简化电机控制系统的调试过程。

应用场景:
1、工业自动化：在工厂的机器人、传送带、CNC加工设备等领域，Arduino FOC可提供高性能的电机控制解决方案。
2、电动车辆：电动自行车、电动汽车、电动叉车等车载电机驱动系统，可以采用Arduino FOC进行精准控制。
3、家用电器：在电风扇、洗衣机、空调等家用电器中，Arduino FOC可实现细腻的电机速度和扭矩控制。
4、航模和无人机：航模飞机、无人机等对电机控制性能要求很高的领域，Arduino FOC能够提供高精度的电机驱动。
5、机器人：工业机器人、服务机器人、仿生机器人等对电机控制性能有严格要求的领域，Arduino FOC是一个不错的选择。

需要注意的事项:
1、硬件要求：Arduino FOC对控制器的性能(如CPU频率、RAM/ROM容量等)有一定要求，需要选择合适的硬件平台。
2、调试复杂性：FOC算法涉及电机建模、坐标变换、PI调节器等诸多环节，调试和调优过程相对复杂，需要一定的专业知识。
3、噪声抑制：电机驱动电路容易产生噪声干扰，需要采取合理的屏蔽和滤波措施，确保信号质量。
4、安全防护：电机驱动系统可能会产生过电流、过压等故障,需要配备可靠的保护电路，确保人身和设备安全。
5、系统集成：将Arduino FOC集成到完整的电机驱动系统中时，需要考虑机械、电力、控制等各个方面的协调配合。

总的来说，Arduino FOC是一种功能强大、性能优秀的电机控制解决方案,适用于工业自动化、电动车辆、家用电器等众多领域。但在硬件选型、算法调试、噪声抑制和安全防护等方面都需要谨慎考虑，以确保系统稳定可靠地运行。

附录：系列目录
1、Arduino FOC的特点、场景和使用事项
http://t.csdnimg.cn/WZhYL
2、Arduino FOC 之简单FOC库 - 跨平台的无刷直流和步进电机FOC实现
http://t.csdnimg.cn/p9ADE
3、Arduino FOC 之无刷直流电机速度控制
http://t.csdnimg.cn/gZ7CY
4、Arduino FOC 之步进电机位置控制
http://t.csdnimg.cn/VYbIb
5、Arduino FOC 之无刷直流电机电流控制
http://t.csdnimg.cn/wWGVu
6、Arduino FOC 之 SimpleFOC 库的主要函数
http://t.csdnimg.cn/S26MC
7、Arduino FOC 之 ArduinoFOC库的核心函数
http://t.csdnimg.cn/3VLzF
8、Arduino FOC 之传感器校准
http://t.csdnimg.cn/NS3TR
9、Arduino FOC 之SimpleFOCShield v2.0.4无刷电机驱动板
http://t.csdnimg.cn/g9mP7
10、Arduino FOC 之 AS5600角度读取
http://t.csdnimg.cn/dmI6F
11、Arduino FOC 之 FOC算法
http://t.csdnimg.cn/ENxc0
12、Arduino FOC 之 SimpleFOC库的适配电机方案
http://t.csdnimg.cn/QdH6k

在这里插入图片描述
Arduino FOC 之实时路径跟踪的梯形速度规划
一、主要特点
梯形速度曲线：
梯形速度规划通过在加速、匀速和减速三个阶段构建速度曲线，形成一个梯形轮廓。该方法能够有效控制运动过程中的加速和减速，避免突发性加速导致的系统振荡。
平滑性：
梯形速度规划确保在加速和减速阶段之间的平滑过渡，能够减少机械磨损和提高系统的稳定性，适合对运动平滑性要求较高的场合。
实时性：
结合 Arduino 平台，可以实现实时路径跟踪，能够快速响应路径变化，适应动态环境中的实时控制需求。
简单易实现：
梯形速度规划的算法实现相对简单，适合初学者和快速原型开发，便于在嵌入式系统中应用。
灵活性：
可以根据不同的任务需求灵活调整加速度、最大速度和行程时间等参数，满足多种应用场景的需求。

二、应用场景
工业自动化：
在自动化生产线中，实时路径跟踪的梯形速度规划能够提高物料搬运和装配的效率，确保生产过程的顺畅。
移动机器人：
在移动机器人导航中，梯形速度规划可以确保机器人在跟踪路径时平稳移动，减少碰撞风险，提高导航精度。
CNC 加工：
在数控机床中，梯形速度规划能够改善切削过程中的速度控制，提高加工精度和表面质量。
教育与研究：
作为控制理论和机器人技术的实验平台，梯形速度规划可以帮助学生理解运动控制和路径规划的基本原理。

三、注意事项
参数设置：
梯形速度规划的参数（如最大加速度、最大速度等）需根据具体应用进行合理设置，以避免不必要的振荡或超调。
路径平滑性：
在路径规划过程中，应确保路径的平滑性，避免出现突兀的转弯或停顿，影响运动的稳定性。
实时性要求：
控制算法需具备良好的实时性，能够快速响应输入信号和路径变化，以保持系统的稳定运行。
外部干扰处理：
需考虑环境变化（如负载变化、地面不平等）对速度控制的影响，设计有效的干扰抑制策略。
反馈机制：
实时路径跟踪需要有效的反馈机制（如位置传感器、速度传感器）来确保控制精度，及时调整控制策略。

总结
实时路径跟踪的梯形速度规划在工业自动化、移动机器人和 CNC 加工等领域具有广泛应用。通过高效的速度控制和实时反馈机制，可以实现平稳的运动控制。在实施过程中，需关注参数设置、路径平滑性及实时性要求，以确保系统的高效性和可靠性。合理的设计与优化将推动该技术在智能化和自动化领域的发展。

在这里插入图片描述
1、基本梯形速度规划

#include <Arduino.h>

float targetPosition = 100.0; // 目标位置
float currentPosition = 0.0;   // 当前位移
float maxSpeed = 10.0;         // 最大速度
float acceleration = 2.0;      // 加速度
float velocity = 0.0;          // 当前速度

void setup() {
  Serial.begin(9600);
}

void loop() {
  // 梯形速度规划
  if (currentPosition < targetPosition) {
    // 加速阶段
    if (velocity < maxSpeed) {
      velocity += acceleration * 0.1; // 时间间隔0.1秒
    }
    currentPosition += velocity * 0.1;
  } else {
    // 减速阶段
    if (velocity > 0) {
      velocity -= acceleration * 0.1;
      currentPosition += velocity * 0.1;
    }
  }

  // 输出当前位置和速度
  Serial.print("Current Position: "); Serial.print(currentPosition);
  Serial.print(", Current Velocity: "); Serial.println(velocity);

  // 如果到达目标位置，则停止
  if (currentPosition >= targetPosition) {
    currentPosition = targetPosition;
    velocity = 0;
    Serial.println("Reached Target Position!");
    while (true); // 停止循环
  }

  delay(100); // 延迟100毫秒
}

2、多段路径跟踪

#include <Arduino.h>

struct Point {
  float x;
  float y;
};

Point path[] = { {0, 0}, {50, 50}, {100, 0} }; // 路径点
int numPoints = sizeof(path) / sizeof(path[0]);
int currentPoint = 0;

float maxSpeed = 10.0;
float acceleration = 2.0;
float currentPosition = 0.0;
float velocity = 0.0;

void setup() {
  Serial.begin(9600);
}

void loop() {
  if (currentPoint < numPoints) {
    float targetPosition = path[currentPoint].x; // 目标X坐标

    // 梯形速度规划
    if (currentPosition < targetPosition) {
      if (velocity < maxSpeed) {
        velocity += acceleration * 0.1;
      }
      currentPosition += velocity * 0.1;
    } else {
      if (velocity > 0) {
        velocity -= acceleration * 0.1;
        currentPosition += velocity * 0.1;
      }
    }

    // 输出当前位置和速度
    Serial.print("Current Position: "); Serial.print(currentPosition);
    Serial.print(", Current Velocity: "); Serial.println(velocity);

    // 检查是否到达目标点
    if (currentPosition >= targetPosition) {
      currentPosition = targetPosition;
      velocity = 0;
      Serial.print("Reached Point: "); Serial.println(currentPoint);
      currentPoint++;
    }
  } else {
    Serial.println("Path Completed!");
    while (true); // 停止循环
  }

  delay(100);
}

3、实时路径跟踪与控制

#include <Arduino.h>
#include <Servo.h>

Servo actuator; // 控制器

struct Point {
  float x;
  float y;
};

Point path[] = { {0, 0}, {50, 50}, {100, 0} }; // 路径点
int numPoints = sizeof(path) / sizeof(path[0]);
int currentPoint = 0;

float maxSpeed = 10.0;
float acceleration = 2.0;
float currentPosition = 0.0;
float velocity = 0.0;

void setup() {
  Serial.begin(9600);
  actuator.attach(9); // 连接到引脚9
}

void loop() {
  if (currentPoint < numPoints) {
    float targetPosition = path[currentPoint].x; // 目标X坐标

    // 梯形速度规划
    if (currentPosition < targetPosition) {
      if (velocity < maxSpeed) {
        velocity += acceleration * 0.1;
      }
      currentPosition += velocity * 0.1;
    } else {
      if (velocity > 0) {
        velocity -= acceleration * 0.1;
        currentPosition += velocity * 0.1;
      }
    }

    // 控制舵机
    actuator.write(map(currentPosition, 0, 100, 0, 180)); // 将位置映射到舵机范围

    // 输出当前位置和速度
    Serial.print("Current Position: "); Serial.print(currentPosition);
    Serial.print(", Current Velocity: "); Serial.println(velocity);

    // 检查是否到达目标点
    if (currentPosition >= targetPosition) {
      currentPosition = targetPosition;
      velocity = 0;
      Serial.print("Reached Point: "); Serial.println(currentPoint);
      currentPoint++;
    }
  } else {
    Serial.println("Path Completed!");
    while (true); // 停止循环
  }

  delay(100);
}

要点解读
梯形速度规划原理：
梯形速度规划通过加速、匀速和减速三个阶段来控制运动，使得运动过程平滑且避免剧烈变化。每个阶段的速度和位置变化通过简单的数学公式来实现，适用于机械臂和机器人等应用。
路径点的处理：
在第二个代码示例中，定义了一系列路径点，通过检测当前点与目标点的关系来实现路径跟踪。这种方法可以扩展到更复杂的路径规划，适合于自动化移动设备的导航。
实时控制与反馈：
在第三个代码示例中，结合了舵机控制和路径跟踪，实时控制舵机的位置。这种实时反馈机制能够更好地应对动态环境变化和任务需求，提高系统的灵活性和适应性。
位置与速度输出：
每个示例中都输出当前位置和速度，方便调试和监测系统状态。这对于优化控制算法和调整参数至关重要，特别是在实际应用中，能够快速识别问题并进行调整。
扩展性与应用：
这些代码结构简单，易于扩展。可以根据需求增加更多的功能，例如结合传感器输入、实现更复杂的路径规划算法或引入障碍物避让策略，以实现更复杂的操作和更高级的应用场景。

在这里插入图片描述
4、基本梯形速度规划

#include <Arduino.h>

// 定义梯形速度规划参数
float maxSpeed = 100.0; // 最大速度
float maxAccel = 50.0;  // 最大加速度
float currentSpeed = 0.0;
float targetPosition = 200.0;
float currentPosition = 0.0;

void setup() {
  Serial.begin(9600);
}

void loop() {
  // 梯形速度规划
  if (currentPosition < targetPosition) {
    if (currentSpeed < maxSpeed) {
      currentSpeed += maxAccel * 0.1; // 加速
    }
    currentPosition += currentSpeed * 0.1;
  } else {
    if (currentSpeed > 0) {
      currentSpeed -= maxAccel * 0.1; // 减速
    }
    currentPosition += currentSpeed * 0.1;
  }
  Serial.print("Position: ");
  Serial.println(currentPosition);
  delay(100);
}

5、结合FOC的梯形速度规划

#include <Arduino.h>
#include <SimpleFOC.h>

// 定义梯形速度规划参数
float maxSpeed = 100.0;
float maxAccel = 50.0;
float currentSpeed = 0.0;
float targetPosition = 200.0;
float currentPosition = 0.0;

// 电机和编码器初始化
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11);
Encoder encoder = Encoder(2, 3, 500);
void doA() { encoder.handleA(); }
void doB() { encoder.handleB(); }

void setup() {
  Serial.begin(9600);
  motor.linkDriver(&driver);
  motor.linkSensor(&encoder);
  motor.controller = MotionControlType::velocity;
  motor.init();
  driver.init();
  encoder.init();
  attachInterrupt(digitalPinToInterrupt(2), doA, CHANGE);
  attachInterrupt(digitalPinToInterrupt(3), doB, CHANGE);
}

void loop() {
  // 梯形速度规划
  if (currentPosition < targetPosition) {
    if (currentSpeed < maxSpeed) {
      currentSpeed += maxAccel * 0.1;
    }
    currentPosition += currentSpeed * 0.1;
  } else {
    if (currentSpeed > 0) {
      currentSpeed -= maxAccel * 0.1;
    }
    currentPosition += currentSpeed * 0.1;
  }
  motor.move(currentSpeed);
  Serial.print("Position: ");
  Serial.println(currentPosition);
  delay(100);
}

6、多轴实时路径跟踪

#include <Arduino.h>
#include <SimpleFOC.h>

// 定义梯形速度规划参数
float maxSpeed = 100.0;
float maxAccel = 50.0;
float currentSpeed1 = 0.0;
float currentSpeed2 = 0.0;
float targetPosition1 = 200.0;
float targetPosition2 = 150.0;
float currentPosition1 = 0.0;
float currentPosition2 = 0.0;

// 电机和编码器初始化
BLDCMotor motor1 = BLDCMotor(7);
BLDCMotor motor2 = BLDCMotor(7);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(9, 10, 11);
BLDCDriver3PWM driver2 = BLDCDriver3PWM(5, 6, 7);
Encoder encoder1 = Encoder(2, 3, 500);
Encoder encoder2 = Encoder(4, 5, 500);
void doA() { encoder1.handleA(); }
void doB() { encoder1.handleB(); }
void doC() { encoder2.handleA(); }
void doD() { encoder2.handleB(); }

void setup() {
  Serial.begin(9600);
  motor1.linkDriver(&driver1);
  motor2.linkDriver(&driver2);
  motor1.linkSensor(&encoder1);
  motor2.linkSensor(&encoder2);
  motor1.controller = MotionControlType::velocity;
  motor2.controller = MotionControlType::velocity;
  motor1.init();
  motor2.init();
  driver1.init();
  driver2.init();
  encoder1.init();
  encoder2.init();
  attachInterrupt(digitalPinToInterrupt(2), doA, CHANGE);
  attachInterrupt(digitalPinToInterrupt(3), doB, CHANGE);
  attachInterrupt(digitalPinToInterrupt(4), doC, CHANGE);
  attachInterrupt(digitalPinToInterrupt(5), doD, CHANGE);
}

void loop() {
  // 梯形速度规划
  if (currentPosition1 < targetPosition1) {
    if (currentSpeed1 < maxSpeed) {
      currentSpeed1 += maxAccel * 0.1;
    }
    currentPosition1 += currentSpeed1 * 0.1;
  } else {
    if (currentSpeed1 > 0) {
      currentSpeed1 -= maxAccel * 0.1;
    }
    currentPosition1 += currentSpeed1 * 0.1;
  }
  if (currentPosition2 < targetPosition2) {
    if (currentSpeed2 < maxSpeed) {
      currentSpeed2 += maxAccel * 0.1;
    }
    currentPosition2 += currentSpeed2 * 0.1;
  } else {
    if (currentSpeed2 > 0) {
      currentSpeed2 -= maxAccel * 0.1;
    }
    currentPosition2 += currentSpeed2 * 0.1;
  }
  motor1.move(currentSpeed1);
  motor2.move(currentSpeed2);
  Serial.print("Position1: ");
  Serial.println(currentPosition1);
  Serial.print("Position2: ");
  Serial.println(currentPosition2);
  delay(100);
}

要点解读
梯形速度规划：通过设定最大速度和最大加速度，实现平滑的加速和减速，确保运动的平稳性和精确性。
实时路径跟踪：结合FOC控制技术，实时调整电机速度和位置，确保机械臂或其他设备按照预定路径移动。
电机和编码器初始化：在FOC控制中，电机和编码器的初始化和配置是关键步骤，确保系统能够正确读取和控制电机位置。
多轴控制：在多轴系统中，分别对每个轴进行梯形速度规划和实时控制，确保各轴协调运动。
实时控制循环：在主循环中，持续调用FOC控制函数，实时调整电机速度和位置，以实现目标位置的精确控制。

注意，以上案例只是为了拓展思路，仅供参考。它们可能有错误、不适用或者无法编译。您的硬件平台、使用场景和Arduino版本可能影响使用方法的选择。实际编程时，您要根据自己的硬件配置、使用场景和具体需求进行调整，并多次实际测试。您还要正确连接硬件，了解所用传感器和设备的规范和特性。涉及硬件操作的代码，您要在使用前确认引脚和电平等参数的正确性和安全性。

在这里插入图片描述