【花雕学编程】Arduino FOC 之关节位置/速度/电流环闭环控制

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 之关节位置/速度/电流环闭环控制
1、主要特点
分层控制策略: 关节位置、速度和电流环闭环控制采用分层控制策略，分别针对不同的控制目标（位置、速度和电流）进行优化，确保系统的整体性能。
高精度控制: 通过位置环、速度环和电流环的相互配合，可以实现高精度的运动控制，确保电机在运行过程中的位置和速度稳定，避免过冲和振荡。
实时反馈: 系统通过传感器（如编码器和电流传感器）实时监测关节状态，能够迅速响应外部干扰和负载变化，保证控制的可靠性和稳定性。
鲁棒性强: 该控制策略能够有效应对系统参数变化、外部扰动和负载不确定性，提升控制系统的鲁棒性。

2、应用场景
机器人关节控制: 在机械臂和机器人系统中，关节位置、速度和电流环闭环控制可以实现精确的运动控制，适应复杂的操作需求。
电动工具: 在电动工具（如电动扳手和电动螺丝刀）中，通过闭环控制可以确保工具在工作过程中的稳定性和精确性，提高工作效率。
自动化设备: 在自动化生产线中，电机的精确控制能够提高设备的生产效率，确保产品质量。
医疗设备: 在医疗机械臂和手术辅助设备中，精准的关节控制能够确保医疗操作的安全性和有效性。

3、需要注意的事项
控制参数调优: 每个环路的控制参数（如PID参数）需要进行合理调优，以确保系统的稳定性和响应速度，避免过度振荡或缓慢响应。
传感器精度: 传感器的精度直接影响控制效果，需确保编码器和电流传感器的校准准确，以提供可靠的反馈信息。
系统延迟: 控制系统的响应时间和传感器的延迟可能影响整体性能，需设计合适的补偿机制，以确保系统在动态环境中的稳定性。
负载变化: 不同的负载对控制性能有显著影响，需考虑负载变化对控制策略的影响，并进行相应的调整。
热管理: 在高负载工作下，电机可能会产生过热，需设计合适的散热方案以避免过热导致的性能下降或设备损坏。

总结
Arduino FOC 中的关节位置/速度/电流环闭环控制为实现高效、精确的电机控制提供了一种有效的方法，广泛应用于机器人关节控制、电动工具、自动化设备和医疗设备等领域。通过合理的参数调优和实时反馈机制，可以确保系统的稳定性和可靠性。在实际开发中，应重视控制参数调优、传感器精度和系统延迟等关键因素，以实现高质量的运动控制。

1、位置环闭环控制

#include <Arduino.h>

// PID 参数
float Kp = 1.0;  
float Ki = 0.1;  
float Kd = 0.01;  

float target_position = 0.0; // 目标位置
float current_position = 0.0; // 当前实际位置

// PID 控制变量
float error, integral, derivative, previous_error;

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

void loop() {
    // 读取当前位置（模拟）
    current_position = readSensor(); // 模拟传感器读取

    // 计算误差
    error = target_position - current_position;

    // PID 控制
    integral += error;
    derivative = error - previous_error;
    float output = Kp * error + Ki * integral + Kd * derivative;

    // 控制电机（模拟）
    controlMotor(output);

    // 更新前一误差
    previous_error = error;

    // 打印信息
    Serial.print("Target Position: ");
    Serial.print(target_position);
    Serial.print(" | Current Position: ");
    Serial.println(current_position);

    delay(100);
}

float readSensor() {
    // 模拟读取传感器值
    return 0.0; // 占位
}

void controlMotor(float output) {
    // 模拟电机控制
}

2、速度环闭环控制

#include <Arduino.h>

// PID 参数
float Kp = 0.5;  
float Ki = 0.05;  
float Kd = 0.1;  

float target_velocity = 0.0; // 目标速度
float current_velocity = 0.0; // 当前实际速度

// PID 控制变量
float error, integral, derivative, previous_error;

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

void loop() {
    // 读取当前速度（模拟）
    current_velocity = readVelocitySensor(); // 模拟传感器读取

    // 计算误差
    error = target_velocity - current_velocity;

    // PID 控制
    integral += error;
    derivative = error - previous_error;
    float output = Kp * error + Ki * integral + Kd * derivative;

    // 控制电机（模拟）
    controlMotor(output);

    // 更新前一误差
    previous_error = error;

    // 打印信息
    Serial.print("Target Velocity: ");
    Serial.print(target_velocity);
    Serial.print(" | Current Velocity: ");
    Serial.println(current_velocity);

    delay(100);
}

float readVelocitySensor() {
    // 模拟读取速度传感器值
    return 0.0; // 占位
}

void controlMotor(float output) {
    // 模拟电机控制
}

3、电流环闭环控制

#include <Arduino.h>

// PID 参数
float Kp = 0.8;  
float Ki = 0.1;  
float Kd = 0.05;  

float target_current = 0.0; // 目标电流
float current_current = 0.0; // 当前实际电流

// PID 控制变量
float error, integral, derivative, previous_error;

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

void loop() {
    // 读取当前电流（模拟）
    current_current = readCurrentSensor(); // 模拟传感器读取

    // 计算误差
    error = target_current - current_current;

    // PID 控制
    integral += error;
    derivative = error - previous_error;
    float output = Kp * error + Ki * integral + Kd * derivative;

    // 控制电机（模拟）
    controlMotor(output);

    // 更新前一误差
    previous_error = error;

    // 打印信息
    Serial.print("Target Current: ");
    Serial.print(target_current);
    Serial.print(" | Current Current: ");
    Serial.println(current_current);

    delay(100);
}

float readCurrentSensor() {
    // 模拟读取电流传感器值
    return 0.0; // 占位
}

void controlMotor(float output) {
    // 模拟电机控制
}

要点解读
闭环控制原理：
所有三个案例展示了闭环控制的基本概念，通过设定目标值（位置、速度或电流）并与实际读数进行比较，计算误差并使用 PID 控制算法来调整电机输出。这种方法能够提高系统的响应速度和稳定性。
PID 控制的调节：
每个案例中都包含 PID 控制参数（Kp、Ki、Kd），这些参数的设置对控制系统的性能至关重要。通过适当的调节，可以消除稳态误差，提高动态响应，确保系统在外部干扰下的稳定性。
模拟传感器读取：
代码中使用了模拟函数来表示传感器读取，这样的设计便于测试和调试。在实际应用中，需要替换为真实的传感器数据读取代码，以确保系统的准确性。
电机控制接口：
控制电机的具体实现是留空的，实际应用中需要根据所用电机驱动器的控制接口来实现。在 Arduino 平台上，常用的电机驱动库可以帮助简化控制逻辑。
应用广泛性：
位置、速度和电流环闭环控制是自动化和机器人控制中的基本组成部分，广泛应用于机械臂、伺服电机、无人机等领域。通过这些案例，用户可以快速上手并进行扩展以满足特定需求。
解释
闭环控制：是一种控制系统，利用反馈机制来调整系统的输出，以使其接近期望的目标值。
PID 控制器：是一种经典的控制算法，通过比例、积分和微分三部分的组合，调节输出以实现更好的控制效果。
电机控制：在自动化和机器人应用中，电机控制是核心部分，通过精确调节电机的动作来实现目标任务。

4、位置闭环控制

#include <SimpleFOC.h>

// 电机实例化
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11);
Encoder encoder = Encoder(2, 3, 500);
Commander commander = Commander(Serial);

void setup() {
  // 初始化电机
  motor.linkDriver(&driver);
  motor.linkSensor(&encoder);
  motor.controller = MotionControlType::angle;
  motor.PID_angle.P = 20;
  motor.PID_angle.I = 0.1;
  motor.PID_angle.D = 0.01;
  motor.voltage_limit = 12;
  motor.init();
  encoder.init();
  commander.add('A', doAngleControl, "angle");
  Serial.begin(115200);
}

void loop() {
  motor.loopFOC();
  motor.move();
  commander.run();
}

void doAngleControl(char* cmd) {
  commander.angle(&motor, cmd);
}

5、速度闭环控制

#include <SimpleFOC.h>

// 电机实例化
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11);
Encoder encoder = Encoder(2, 3, 500);
Commander commander = Commander(Serial);

void setup() {
  // 初始化电机
  motor.linkDriver(&driver);
  motor.linkSensor(&encoder);
  motor.controller = MotionControlType::velocity;
  motor.PID_velocity.P = 0.2;
  motor.PID_velocity.I = 20;
  motor.PID_velocity.D = 0.001;
  motor.voltage_limit = 12;
  motor.init();
  encoder.init();
  commander.add('V', doVelocityControl, "velocity");
  Serial.begin(115200);
}

void loop() {
  motor.loopFOC();
  motor.move();
  commander.run();
}

void doVelocityControl(char* cmd) {
  commander.velocity(&motor, cmd);
}

6、电流环闭环控制

#include <SimpleFOC.h>

// 电机实例化
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11);
Encoder encoder = Encoder(2, 3, 500);
Commander commander = Commander(Serial);

void setup() {
  // 初始化电机
  motor.linkDriver(&driver);
  motor.linkSensor(&encoder);
  motor.controller = MotionControlType::torque;
  motor.PID_current_q.P = 3;
  motor.PID_current_q.I = 300;
  motor.PID_current_q.D = 0.01;
  motor.voltage_limit = 12;
  motor.init();
  encoder.init();
  commander.add('T', doTorqueControl, "torque");
  Serial.begin(115200);
}

void loop() {
  motor.loopFOC();
  motor.move();
  commander.run();
}

void doTorqueControl(char* cmd) {
  commander.torque(&motor, cmd);
}

要点解读
初始化与配置：
每个程序都需要初始化电机、驱动器和编码器，并设置控制模式（位置、速度或电流）。
例如，位置控制使用 MotionControlType::angle，速度控制使用 MotionControlType::velocity，电流控制使用 MotionControlType::torque。
PID参数调节：
每种控制模式都有其特定的PID参数，这些参数需要根据实际应用进行调节。
例如，位置控制的PID参数为 motor.PID_angle.P，速度控制为 motor.PID_velocity.P，电流控制为 motor.PID_current_q.P。
电压限制：
为了保护电机和驱动器，设置了电压限制 motor.voltage_limit，通常根据电机的额定电压进行设置。
闭环控制：
在主循环中，通过 motor.loopFOC() 和 motor.move() 实现闭环控制，确保电机按照设定的目标运行。
命令接口：
使用 Commander 类实现串口命令接口，方便实时调整控制参数和目标值。
例如，位置控制命令为 commander.add(‘A’, doAngleControl, “angle”)，速度控制命令为 commander.add(‘V’, doVelocityControl, “velocity”)。

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