【花雕学编程】Arduino FOC 之带PID控制的双关节同步

驴友花雕

于 2024-09-27 07:30:42 发布

阅读量390

点赞数 8

分类专栏： Arduino FOC 文章标签：嵌入式硬件单片机 c++ 带PID控制的双关节同步 Arduino FOC

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

版权

Arduino FOC 专栏收录该内容

523 篇文章 10 订阅

订阅专栏

在这里插入图片描述

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 之带 PID 控制的双关节同步
一、主要特点
精准控制：
采用 PID 控制算法，能够对双关节的运动进行精准控制，确保每个关节以设定的速度和加速度同步运动，从而提高整体系统的精度。
实时反馈：
结合传感器（如编码器），系统可以实时监测各个关节的位置和速度，及时调整控制输出，实现动态响应。
稳定性：
PID 控制器通过调整比例、积分和微分参数，能够有效抑制系统的振荡和超调，提高系统的稳定性。
易于实现：
基于 Arduino 平台，PID 控制的实现相对简单，开发者可以利用现有的库和示例代码快速搭建控制系统。
灵活性：
PID 控制器的参数可以根据具体应用进行调节，以适应不同的负载和运动特性，增强系统的适应能力。
二、应用场景
机器人手臂：
在工业机器人或服务机器人中，双关节同步控制能够实现高精度的抓取和放置操作，适用于装配、焊接等任务。
数控机床：
在数控加工中，双关节的同步运动能够提高加工精度和效率，适用于复杂形状的零件加工。
运动平台：
在模拟飞行、虚拟现实或教育机器人中，实现双关节的同步控制能够提供更自然的运动体验。
医疗设备：
在手术机器人和其他医疗设备中，精确的双关节控制能够确保操作的安全性和可靠性。
三、注意事项
参数调节：
PID 控制器的参数（比例、积分、微分）需根据实际情况进行调节，避免设置不当导致系统不稳定或响应过慢。
传感器精度：
选择高精度的传感器（如高分辨率编码器），以确保位置和速度反馈的准确性，防止因传感器误差影响控制效果。
关节限制：
在控制算法中需考虑各个关节的运动范围，避免超出机械臂的可操作范围，确保安全性。
外部干扰：
在实际应用中，外部因素（如负载变化、环境干扰等）可能影响控制效果，需设计有效的干扰抑制策略。
实时性要求：
控制算法应具备良好的实时性，能够快速响应输入信号和反馈信息，以保持系统的稳定运行。
总结
带 PID 控制的双关节同步在机器人手臂、数控机床和医疗设备等领域具有广泛应用。通过高效的控制算法和实时反馈机制，可以实现高精度的运动控制。在实施过程中，需关注参数调节、传感器选择及外部干扰处理，以确保系统的高效性和可靠性。合理的设计与优化将推动该技术在自动化和智能化领域的发展。

在这里插入图片描述
1、基本PID控制实现

#include <Arduino.h>

float targetPosition1 = 90.0; // 目标位置1
float targetPosition2 = 90.0; // 目标位置2
float currentPosition1 = 0.0; // 当前角度1
float currentPosition2 = 0.0; // 当前角度2

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

float previousError1 = 0.0;
float integral1 = 0.0;
float previousError2 = 0.0;
float integral2 = 0.0;

void setup() {
  Serial.begin(9600);
  // 在这里初始化舵机或其他执行器
}

void loop() {
  // 读取当前角度（这里用模拟值代替）
  currentPosition1 = readPosition1();
  currentPosition2 = readPosition2();

  // PID控制
  float output1 = PIDControl(currentPosition1, targetPosition1, Kp, Ki, Kd, previousError1, integral1);
  float output2 = PIDControl(currentPosition2, targetPosition2, Kp, Ki, Kd, previousError2, integral2);

  // 控制舵机或执行器
  controlActuator1(output1);
  controlActuator2(output2);

  // 输出调试信息
  Serial.print("Output1: "); Serial.print(output1);
  Serial.print(", Output2: "); Serial.println(output2);
  delay(100);
}

float PIDControl(float current, float target, float Kp, float Ki, float Kd, float &previousError, float &integral) {
  float error = target - current;
  integral += error;
  float derivative = error - previousError;
  previousError = error;

  return Kp * error + Ki * integral + Kd * derivative;
}

float readPosition1() {
  // 这里可以添加读取舵机位置的代码
  return currentPosition1; // 模拟当前角度
}

float readPosition2() {
  // 这里可以添加读取舵机位置的代码
  return currentPosition2; // 模拟当前角度
}

void controlActuator1(float output) {
  // 控制第一个舵机的代码
}

void controlActuator2(float output) {
  // 控制第二个舵机的代码
}

2、双关节同步控制

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

Servo joint1; // 第一个关节
Servo joint2; // 第二个关节

float targetPosition1 = 90.0; // 目标位置1
float targetPosition2 = 90.0; // 目标位置2

float Kp = 1.0, Ki = 0.1, Kd = 0.05;
float previousError1 = 0.0, integral1 = 0.0;
float previousError2 = 0.0, integral2 = 0.0;

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

void loop() {
  // 读取当前角度
  float currentPosition1 = joint1.read();
  float currentPosition2 = joint2.read();

  // PID控制
  float output1 = PIDControl(currentPosition1, targetPosition1, Kp, Ki, Kd, previousError1, integral1);
  float output2 = PIDControl(currentPosition2, targetPosition2, Kp, Ki, Kd, previousError2, integral2);

  // 控制舵机
  joint1.write(currentPosition1 + output1);
  joint2.write(currentPosition2 + output2);

  // 输出调试信息
  Serial.print("Joint1 Output: "); Serial.print(output1);
  Serial.print(", Joint2 Output: "); Serial.println(output2);
  delay(100);
}

float PIDControl(float current, float target, float Kp, float Ki, float Kd, float &previousError, float &integral) {
  float error = target - current;
  integral += error;
  float derivative = error - previousError;
  previousError = error;

  return Kp * error + Ki * integral + Kd * derivative;
}

3、带限制的PID控制

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

Servo joint1; // 第一个关节
Servo joint2; // 第二个关节

float targetPosition1 = 90.0; // 目标位置1
float targetPosition2 = 90.0; // 目标位置2

float Kp = 1.0, Ki = 0.1, Kd = 0.05;
float previousError1 = 0.0, integral1 = 0.0;
float previousError2 = 0.0, integral2 = 0.0;

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

void loop() {
  // 读取当前角度
  float currentPosition1 = joint1.read();
  float currentPosition2 = joint2.read();

  // PID控制
  float output1 = PIDControl(currentPosition1, targetPosition1, Kp, Ki, Kd, previousError1, integral1);
  float output2 = PIDControl(currentPosition2, targetPosition2, Kp, Ki, Kd, previousError2, integral2);

  // 限制输出范围
  output1 = constrain(output1, -10, 10);
  output2 = constrain(output2, -10, 10);

  // 控制舵机
  joint1.write(currentPosition1 + output1);
  joint2.write(currentPosition2 + output2);

  // 输出调试信息
  Serial.print("Joint1 Output: "); Serial.print(output1);
  Serial.print(", Joint2 Output: "); Serial.println(output2);
  delay(100);
}

float PIDControl(float current, float target, float Kp, float Ki, float Kd, float &previousError, float &integral) {
  float error = target - current;
  integral += error;
  float derivative = error - previousError;
  previousError = error;

  return Kp * error + Ki * integral + Kd * derivative;
}

要点解读
PID控制原理：
PID（比例-积分-微分）控制器通过计算当前值与目标值之间的误差，利用比例、积分和微分三种控制方式的组合来调节输出，确保系统快速而稳定地达到目标。比例控制用于响应当前误差，积分控制用于消除稳态误差，微分控制用于预测未来误差。
双关节同步控制：
通过两个舵机的PID控制器，实现双关节的同步控制。无论是独立控制每个关节，还是通过共享目标位置和PID算法，可以确保两个关节协调运动，保持机械臂的稳定性。
限制输出范围：
在第三个代码示例中，使用constrain函数限制PID输出范围，防止舵机过快移动而导致的震荡或损坏。这种限制对于保持系统的平稳性至关重要。
实时监测与调试：
在每个循环中，使用串口输出当前的控制信息（如输出值和当前角度），有助于调试和监测系统状态。这对于调整PID参数和优化控制算法非常重要。
扩展性与应用：
这些代码结构简单，易于扩展。可以根据需求增加更多的功能，例如添加传感器输入、实现更复杂的控制算法（如自适应控制或模糊控制），以应对复杂的动态环境和任务需求。

在这里插入图片描述
4、基本双关节同步控制

#include <SimpleFOC.h>

// 定义电机和驱动器
BLDCMotor motor1 = BLDCMotor(7);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(9, 10, 11);
BLDCMotor motor2 = BLDCMotor(7);
BLDCDriver3PWM driver2 = BLDCDriver3PWM(5, 6, 7);

// 定义PID控制器
PIDController pid1 = PIDController(1.0, 0.1, 0.01, 0.01);
PIDController pid2 = PIDController(1.0, 0.1, 0.01, 0.01);

void setup() {
  Serial.begin(115200);

  // 初始化驱动器
  driver1.voltage_power_supply = 12;
  driver1.init();
  motor1.linkDriver(&driver1);
  motor1.init();

  driver2.voltage_power_supply = 12;
  driver2.init();
  motor2.linkDriver(&driver2);
  motor2.init();

  // 初始化PID控制器
  pid1.init();
  pid2.init();
}

void loop() {
  // 获取当前角度
  float angle1 = motor1.shaft_angle;
  float angle2 = motor2.shaft_angle;

  // 计算PID输出
  float pid_output1 = pid1(angle1, target_angle1);
  float pid_output2 = pid2(angle2, target_angle2);

  // 设置电机速度
  motor1.move(pid_output1);
  motor2.move(pid_output2);

  Serial.print("Angle1: "); Serial.print(angle1);
  Serial.print(" | Angle2: "); Serial.println(angle2);

  delay(10);
}

5、带负载的双关节同步控制

#include <SimpleFOC.h>

// 定义电机和驱动器
BLDCMotor motor1 = BLDCMotor(7);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(9, 10, 11);
BLDCMotor motor2 = BLDCMotor(7);
BLDCDriver3PWM driver2 = BLDCDriver3PWM(5, 6, 7);

// 定义PID控制器
PIDController pid1 = PIDController(1.0, 0.1, 0.01, 0.01);
PIDController pid2 = PIDController(1.0, 0.1, 0.01, 0.01);

void setup() {
  Serial.begin(115200);

  // 初始化驱动器
  driver1.voltage_power_supply = 12;
  driver1.init();
  motor1.linkDriver(&driver1);
  motor1.init();

  driver2.voltage_power_supply = 12;
  driver2.init();
  motor2.linkDriver(&driver2);
  motor2.init();

  // 初始化PID控制器
  pid1.init();
  pid2.init();
}

void loop() {
  // 获取当前角度
  float angle1 = motor1.shaft_angle;
  float angle2 = motor2.shaft_angle;

  // 计算PID输出
  float pid_output1 = pid1(angle1, target_angle1);
  float pid_output2 = pid2(angle2, target_angle2);

  // 设置电机速度
  motor1.move(pid_output1);
  motor2.move(pid_output2);

  // 模拟负载变化
  float load1 = random(-10, 10) / 10.0;
  float load2 = random(-10, 10) / 10.0;
  motor1.move(pid_output1 + load1);
  motor2.move(pid_output2 + load2);

  Serial.print("Angle1: "); Serial.print(angle1);
  Serial.print(" | Angle2: "); Serial.println(angle2);

  delay(10);
}

6、双关节同步与无线控制

#include <SimpleFOC.h>
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>

// 定义电机和驱动器
BLDCMotor motor1 = BLDCMotor(7);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(9, 10, 11);
BLDCMotor motor2 = BLDCMotor(7);
BLDCDriver3PWM driver2 = BLDCDriver3PWM(5, 6, 7);

// 定义PID控制器
PIDController pid1 = PIDController(1.0, 0.1, 0.01, 0.01);
PIDController pid2 = PIDController(1.0, 0.1, 0.01, 0.01);

// 定义无线模块
RF24 radio(9, 10);
const byte address[6] = "00001";

void setup() {
  Serial.begin(115200);
  Wire.begin();

  // 初始化驱动器
  driver1.voltage_power_supply = 12;
  driver1.init();
  motor1.linkDriver(&driver1);
  motor1.init();

  driver2.voltage_power_supply = 12;
  driver2.init();
  motor2.linkDriver(&driver2);
  motor2.init();

  // 初始化PID控制器
  pid1.init();
  pid2.init();

  // 初始化无线模块
  radio.begin();
  radio.openWritingPipe(address);
  radio.setPALevel(RF24_PA_MIN);
}

void loop() {
  // 获取当前角度
  float angle1 = motor1.shaft_angle;
  float angle2 = motor2.shaft_angle;

  // 计算PID输出
  float pid_output1 = pid1(angle1, target_angle1);
  float pid_output2 = pid2(angle2, target_angle2);

  // 设置电机速度
  motor1.move(pid_output1);
  motor2.move(pid_output2);

  // 发送数据
  float data[2] = {angle1, angle2};
  radio.write(&data, sizeof(data));

  Serial.print("Angle1: "); Serial.print(angle1);
  Serial.print(" | Angle2: "); Serial.println(angle2);

  delay(10);
}

要点解读
电机和驱动器初始化：每个程序都初始化了电机和驱动器，这是实现双关节同步控制的基础。
PID控制器：使用PID控制器来调节电机的输出，使其达到目标角度。PID参数需要根据实际情况进行调节。
实时反馈：通过获取电机的当前角度，实时调整电机的输出，确保双关节的同步性。
负载模拟：在第5个案例中，模拟了负载的变化，展示了系统在不同负载下的响应能力。
无线控制：第6个案例展示了如何通过无线模块实现远程控制和数据传输，增加了系统的灵活性和应用范围。

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

在这里插入图片描述