【花雕学编程】Arduino FOC 之五连杆绘图仪的正解逆解

最新推荐文章于 2024-08-16 16:53:28 发布

驴友花雕

最新推荐文章于 2024-08-16 16:53:28 发布

阅读量372

点赞数 7

分类专栏： Arduino FOC 文章标签：嵌入式硬件单片机 c++ Arduino FOC 五连杆绘图仪的正解逆解

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

版权

Arduino FOC 专栏收录该内容

372 篇文章 3 订阅

订阅专栏

在这里插入图片描述

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、应用场景
目标定位：根据目标点的位置和姿态，计算出机械臂各关节需要转动的角度，从而控制机械臂到达目标点。
运动规划：根据目标点的位置和姿态，计算出机械臂的运动路径，从而控制机械臂完成运动。

4、需要注意的事项
奇异点：五连杆机械臂存在奇异点，即在某些关节角度组合下，机械臂无法移动或无法完成特定动作。
精度问题：逆解的计算精度会影响机械臂的运动精度，需要选择合适的解法和参数。
工作空间：五连杆机械臂的工作空间有限，需要根据实际应用场景选择合适的机械臂结构和尺寸。

总结
Arduino FOC 五连杆绘图仪的正解逆解，是控制五连杆机械臂的关键技术。正解用于计算末端执行器的位置和姿态，逆解用于计算关节角度。在应用该技术时，需要考虑奇异点、精度问题、工作空间等因素，进行反复调试，才能实现最佳控制效果。

在这里插入图片描述
1、基于几何关系的正解

#include <Arduino.h>
#include <Servo.h> // 伺服电机库

// 定义伺服电机引脚
const int servo1Pin = 9;
const int servo2Pin = 10;
const int servo3Pin = 11;
const int servo4Pin = 12;

// 定义连杆长度
const float L1 = 100.0; // 杆1长度
const float L2 = 150.0; // 杆2长度
const float L3 = 100.0; // 杆3长度
const float L4 = 150.0; // 杆4长度
const float L5 = 100.0; // 杆5长度

// 定义伺服电机对象
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;

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

  // 初始化伺服电机
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);
  servo3.attach(servo3Pin);
  servo4.attach(servo4Pin);

  // 设置初始角度
  servo1.write(0);
  servo2.write(0);
  servo3.write(0);
  servo4.write(0);
}

void loop() {
  // 目标坐标
  float x = 100.0;
  float y = 100.0;

  // 计算关节角度
  float theta1 = atan2(y, x);
  float theta2 = acos((x * x + y * y - L2 * L2 - L3 * L3) / (2 * L2 * L3));
  float theta3 = acos((L2 * L2 + L3 * L3 - x * x - y * y) / (2 * L2 * L3));
  float theta4 = acos((L4 * L4 + L5 * L5 - (x - L1 * cos(theta1)) * (x - L1 * cos(theta1)) - (y - L1 * sin(theta1)) * (y - L1 * sin(theta1))) / (2 * L4 * L5));
  float theta5 = acos((L4 * L4 + (x - L1 * cos(theta1)) * (x - L1 * cos(theta1)) + (y - L1 * sin(theta1)) * (y - L1 * sin(theta1)) - L5 * L5) / (2 * L4 * (x - L1 * cos(theta1))));

  // 设置伺服电机角度
  servo1.write(theta1 * 180 / PI);
  servo2.write(theta2 * 180 / PI);
  servo3.write(theta3 * 180 / PI);
  servo4.write(theta4 * 180 / PI);

  delay(1000);
}

要点解读：
代码使用 Servo.h 库控制伺服电机。
代码定义连杆长度和目标坐标。
代码根据几何关系计算关节角度，并使用 write() 函数设置伺服电机角度。
代码通过调整目标坐标来控制绘图仪绘制不同的图形。

2、基于数值方法的逆解

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

// 定义伺服电机引脚
const int servo1Pin = 9;
const int servo2Pin = 10;
const int servo3Pin = 11;
const int servo4Pin = 12;

// 定义连杆长度
const float L1 = 100.0; // 杆1长度
const float L2 = 150.0; // 杆2长度
const float L3 = 100.0; // 杆3长度
const float L4 = 150.0; // 杆4长度
const float L5 = 100.0; // 杆5长度

// 定义伺服电机对象
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;

// 定义误差容限
const float tolerance = 0.01;

// 定义逆解函数
void inverseKinematics(float x, float y, float &theta1, float &theta2, float &theta3, float &theta4) {
  // 迭代计算关节角度
  float theta1_prev = 0.0;
  float theta2_prev = 0.0;
  float theta3_prev = 0.0;
  float theta4_prev = 0.0;
  float error = 1.0;

  while (error > tolerance) {
    // 计算当前关节位置
    float x1 = L1 * cos(theta1);
    float y1 = L1 * sin(theta1);
    float x2 = x1 + L2 * cos(theta2);
    float y2 = y1 + L2 * sin(theta2);
    float x3 = x2 + L3 * cos(theta3);
    float y3 = y2 + L3 * sin(theta3);
    float x4 = x3 + L4 * cos(theta4);
    float y4 = y3 + L4 * sin(theta4);

    // 计算误差
    error = sqrt((x - x4) * (x - x4) + (y - y4) * (y - y4));

    // 更新关节角度
    theta1_prev = theta1;
    theta2_prev = theta2;
    theta3_prev = theta3;
    theta4_prev = theta4;

    // 使用数值方法迭代更新关节角度
    // ...
  }
}

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

  // 初始化伺服电机
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);
  servo3.attach(servo3Pin);
  servo4.attach(servo4Pin);

  // 设置初始角度
  servo1.write(0);
  servo2.write(0);
  servo3.write(0);
  servo4.write(0);
}

void loop() {
  // 目标坐标
  float x = 100.0;
  float y = 100.0;

  // 计算关节角度
  float theta1, theta2, theta3, theta4;
  inverseKinematics(x, y, theta1, theta2, theta3, theta4);

  // 设置伺服电机角度
  servo1.write(theta1 * 180 / PI);
  servo2.write(theta2 * 180 / PI);
  servo3.write(theta3 * 180 / PI);
  servo4.write(theta4 * 180 / PI);

  delay(1000);
}

要点解读：
代码使用 Servo.h 库控制伺服电机。
代码定义连杆长度、目标坐标和误差容限。
代码使用 inverseKinematics() 函数进行逆解，通过迭代计算关节角度，直到误差小于容限。
代码使用 write() 函数设置伺服电机角度。

3、基于 Arduino FOC 库的控制

#include <Arduino.h>
#include <FOC.h> // FOC 库
#include <Servo.h>

// 定义伺服电机引脚
const int servo1Pin = 9;
const int servo2Pin = 10;
const int servo3Pin = 11;
const int servo4Pin = 12;

// 定义连杆长度
const float L1 = 100.0; // 杆1长度
const float L2 = 150.0; // 杆2长度
const float L3 = 100.0; // 杆3长度
const float L4 = 150.0; // 杆4长度
const float L5 = 100.0; // 杆5长度

// 定义伺服电机对象
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;

// 定义 FOC 对象
FOC motor1;
FOC motor2;
FOC motor3;
FOC motor4;

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

  // 初始化伺服电机
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);
  servo3.attach(servo3Pin);
  servo4.attach(servo4Pin);

  // 初始化 FOC 对象
  motor1.init(4, 0.1, 0.001);
  motor2.init(4, 0.1, 0.001);
  motor3.init(4, 0.1, 0.001);
  motor4.init(4, 0.1, 0.001);

  // 设置电机控制模式
  motor1.setMode(FOC_MODE_POSITION);
  motor2.setMode(FOC_MODE_POSITION);
  motor3.setMode(FOC_MODE_POSITION);
  motor4.setMode(FOC_MODE_POSITION);

  // 启动电机
  motor1.run();
  motor2.run();
  motor3.run();
  motor4.run();
}

void loop() {
  // 目标坐标
  float x = 100.0;
  float y = 100.0;

  // 计算关节角度
  float theta1, theta2, theta3, theta4;
  inverseKinematics(x, y, theta1, theta2, theta3, theta4);

  // 设置电机目标位置
  motor1.setTargetPosition(theta1);
  motor2.setTargetPosition(theta2);
  motor3.setTargetPosition(theta3);
  motor4.setTargetPosition(theta4);

  // 读取传感器数据
  motor1.updateSensorData();
  motor2.updateSensorData();
  motor3.updateSensorData();
  motor4.updateSensorData();

  // 执行 FOC 控制算法
  motor1.control();
  motor2.control();
  motor3.control();
  motor4.control();

  // 更新电机状态
  motor1.updateMotorState();
  motor2.updateMotorState();
  motor3.updateMotorState();
  motor4.updateMotorState();

  // 设置伺服电机角度
  servo1.write(motor1.getPosition() * 180 / PI);
  servo2.write(motor2.getPosition() * 180 / PI);
  servo3.write(motor3.getPosition() * 180 / PI);
  servo4.write(motor4.getPosition() * 180 / PI);

  delay(1000);
}

// 定义逆解函数
void inverseKinematics(float x, float y, float &theta1, float &theta2, float &theta3, float &theta4) {
  // ...
}

要点解读：
代码使用 FOC.h 库控制伺服电机，并使用 Servo.h 库设置伺服电机角度。
代码定义连杆长度和目标坐标。
代码使用 inverseKinematics() 函数进行逆解，计算关节角度。
代码使用 FOC 控制算法控制电机，并根据计算的关节角度设置电机目标位置。
代码通过读取传感器数据、执行 FOC 控制算法和更新电机状态来实现精确的控制。

在这里插入图片描述
4、正解解算

#include <Arduino.h>

// 定义连杆长度
const float L1 = 100; // 杆1长度
const float L2 = 80; // 杆2长度
const float L3 = 60; // 杆3长度
const float L4 = 40; // 杆4长度
const float L5 = 20; // 杆5长度

// 定义关节角度
float theta1, theta2, theta3, theta4, theta5;

// 定义目标点坐标
float x, y;

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

void loop() {
  // 设置目标点坐标
  x = 100;
  y = 50;

  // 正解解算
  solveForwardKinematics(x, y);

  // 输出关节角度
  Serial.print("theta1: ");
  Serial.print(theta1);
  Serial.print(" theta2: ");
  Serial.print(theta2);
  Serial.print(" theta3: ");
  Serial.print(theta3);
  Serial.print(" theta4: ");
  Serial.print(theta4);
  Serial.print(" theta5: ");
  Serial.println(theta5);

  delay(1000);
}

// 正解解算函数
void solveForwardKinematics(float x, float y) {
  // ... (根据五连杆机构的几何关系计算关节角度)
}

要点解读：
定义连杆长度和目标点坐标。
使用 solveForwardKinematics() 函数根据目标点坐标计算关节角度。
输出计算得到的关节角度。

5、逆解解算

#include <Arduino.h>

// 定义连杆长度
const float L1 = 100; // 杆1长度
const float L2 = 80; // 杆2长度
const float L3 = 60; // 杆3长度
const float L4 = 40; // 杆4长度
const float L5 = 20; // 杆5长度

// 定义关节角度
float theta1, theta2, theta3, theta4, theta5;

// 定义目标点坐标
float x, y;

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

void loop() {
  // 设置关节角度
  theta1 = 30;
  theta2 = 45;
  theta3 = 60;
  theta4 = 75;
  theta5 = 90;

  // 逆解解算
  solveInverseKinematics(theta1, theta2, theta3, theta4, theta5);

  // 输出目标点坐标
  Serial.print("x: ");
  Serial.print(x);
  Serial.print(" y: ");
  Serial.println(y);

  delay(1000);
}

// 逆解解算函数
void solveInverseKinematics(float theta1, float theta2, float theta3, float theta4, float theta5) {
  // ... (根据关节角度计算目标点坐标)
}

要点解读：
定义连杆长度和关节角度。
使用 solveInverseKinematics() 函数根据关节角度计算目标点坐标。
输出计算得到的目标点坐标。

6、基于 Arduino FOC 的五连杆绘图仪控制

#include <Arduino.h>
#include <FOC.h>

// 定义电机引脚
const int motor1Pin = 2;
const int motor2Pin = 3;
const int motor3Pin = 4;
const int motor4Pin = 5;
const int motor5Pin = 6;

// 定义 FOC 控制参数
FOC motor1;
FOC motor2;
FOC motor3;
FOC motor4;
FOC motor5;

// 定义连杆长度
const float L1 = 100; // 杆1长度
const float L2 = 80; // 杆2长度
const float L3 = 60; // 杆3长度
const float L4 = 40; // 杆4长度
const float L5 = 20; // 杆5长度

// 定义关节角度
float theta1, theta2, theta3, theta4, theta5;

// 定义目标点坐标
float x, y;

void setup() {
  // 初始化电机引脚
  pinMode(motor1Pin, OUTPUT);
  pinMode(motor2Pin, OUTPUT);
  pinMode(motor3Pin, OUTPUT);
  pinMode(motor4Pin, OUTPUT);
  pinMode(motor5Pin, OUTPUT);

  // 初始化 FOC 控制参数
  motor1.init(motor1Pin);
  motor2.init(motor2Pin);
  motor3.init(motor3Pin);
  motor4.init(motor4Pin);
  motor5.init(motor5Pin);
  // ... (设置电机参数)

  Serial.begin(115200);
}

void loop() {
  // 设置目标点坐标
  x = 100;
  y = 50;

  // 正解解算
  solveForwardKinematics(x, y);

  // 控制电机转动
  motor1.setAngle(theta1);
  motor2.setAngle(theta2);
  motor3.setAngle(theta3);
  motor4.setAngle(theta4);
  motor5.setAngle(theta5);
  // ... (控制电机运行)

  delay(1000);
}

// 正解解算函数
void solveForwardKinematics(float x, float y) {
  // ... (根据五连杆机构的几何关系计算关节角度)
}

要点解读：
使用 Arduino FOC 控制五台步进电机，分别对应五连杆机构的五个关节。
使用正解解算函数计算关节角度，并控制电机转动到对应角度。
通过控制电机转动，实现五连杆绘图仪的运动。

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

在这里插入图片描述

驴友花雕

关注

7
点赞
踩
4

收藏

觉得还不错? 一键收藏
打赏
0
评论
【花雕学编程】Arduino FOC 之五连杆绘图仪的正解逆解

您的硬件平台、使用场景和Arduino版本可能影响使用方法的选择。在Arduino平台上实现FOC可以提供平滑的运行和高度的扭矩、速度和位置控制，它通过精确控制电机的电流和电压来实现高效率、高精度和低噪声的操作。5、创新：Arduino可以让你用电子的方式来表达你的创意和想象，你可以用Arduino来制作各种有趣和有用的项目，如机器人、智能家居、艺术装置等。1、高性能电机控制：FOC是一种高级的电机控制算法，可以精准控制PMSM(永磁同步电机)和BLDC(无刷直流)电机，实现平滑的转速和扭矩输出。
复制链接

扫一扫