【花雕学编程】Arduino FOC 之多目标逆解与误差反馈

最新推荐文章于 2024-09-29 10:34:16 发布

驴友花雕

最新推荐文章于 2024-09-29 10:34:16 发布

阅读量1k

点赞数 22

分类专栏： Arduino FOC 文章标签：嵌入式硬件单片机 c++ Arduino动手做多目标逆解与误差反馈 Arduino FOC

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

版权

Arduino FOC 专栏收录该内容

528 篇文章 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 之多目标逆解与误差反馈
一、主要特点
多目标逆解能力：该方法能够同时处理多个目标位置的逆运动学解，适用于复杂的机器人的路径规划和运动控制。
误差反馈机制：通过实时监测和反馈当前状态与目标状态之间的误差，能够动态调整控制策略，提高位置控制的精确度和稳定性。
自适应性：根据实时反馈的误差，控制算法能够自适应地调整参数，以适应不同的工作环境和任务需求。
高效性：结合Arduino FOC技术，能够在资源有限的嵌入式系统中高效实现多目标逆解，适应实时控制的需求。
二、应用场景
工业机器人：在复杂的工业环境中，机器人需要同时处理多个抓取或装配任务，利用多目标逆解能够优化工作效率。
服务机器人：在服务行业中，机器人需要根据环境变化快速调整路径，误差反馈机制可以确保机器人准确到达目标位置。
仿生机器人：在仿生机器人中，多目标逆解与误差反馈能够实现更自然的运动控制，模拟生物的灵活性和适应性。
医疗设备：在医疗手术中，机器人需要在多目标位置间精准移动，误差反馈可以提高手术安全性和精确度。
三、注意事项
模型准确性：多目标逆解的性能依赖于运动学模型的准确性，需确保机器人的运动学模型建立正确，以减少计算误差。
目标位置的选择：选择合理的目标位置至关重要，需确保目标位置在机械系统的可达范围内，避免不必要的运动冲突。
计算复杂性：多目标逆解可能涉及复杂的数学计算，需优化算法，以确保在实时控制中保持高效性。
误差处理：对误差的反馈和处理需及时有效，避免累积误差影响系统稳定性和控制精度。
传感器选择：高精度的传感器对误差反馈至关重要，需选择可靠的传感器以提高位置测量的准确性。
总结
多目标逆解与误差反馈技术在工业机器人、服务机器人和医疗设备等领域具有广泛应用。通过高效的多目标运动控制和实时误差调整，可以实现高精度的运动跟踪。在实施过程中，需关注运动学模型的准确性、目标位置的合理选择及误差处理的有效性，以确保系统的高效性和可靠性。合理的设计与优化将推动该技术在自动化和智能化领域的发展。

在这里插入图片描述
1、基础多目标逆解

#include <Arduino.h>

const int motor1Pin = 9; // 电机1的PWM引脚
const int motor2Pin = 10; // 电机2的PWM引脚
const int motor3Pin = 11; // 电机3的PWM引脚

float targetPositions[3][3] = {
    {10.0, 10.0, 5.0}, 
    {15.0, 15.0, 10.0}, 
    {20.0, 10.0, 5.0}
}; // 目标位置数组
int targetIndex = 0; // 当前目标索引

float currentAngles[3] = {0.0, 0.0, 0.0}; // 当前关节角度
const float L1 = 10.0; // 连杆1长度
const float L2 = 10.0; // 连杆2长度
const float L3 = 10.0; // 连杆3长度

void setup() {
    Serial.begin(9600);
    pinMode(motor1Pin, OUTPUT);
    pinMode(motor2Pin, OUTPUT);
    pinMode(motor3Pin, OUTPUT);
}

void loop() {
    if (targetIndex < 3) {
        if (cartesianToJoint(targetPositions[targetIndex], currentAngles)) {
            for (int i = 0; i < 3; i++) {
                analogWrite(motor1Pin + i, constrain(currentAngles[i] * 255 / PI, 0, 255)); // 控制电机
            }
            
            Serial.print("Current Angles: (");
            Serial.print(currentAngles[0]);
            Serial.print(", ");
            Serial.print(currentAngles[1]);
            Serial.print(", ");
            Serial.print(currentAngles[2]);
            Serial.println(")");

            // 检查是否到达目标
            if (fabs(currentAngles[0] - targetPositions[targetIndex][0]) < 0.1 &&
                fabs(currentAngles[1] - targetPositions[targetIndex][1]) < 0.1 &&
                fabs(currentAngles[2] - targetPositions[targetIndex][2]) < 0.1) {
                targetIndex++; // 移动到下一个目标
            }
        } else {
            Serial.println("Target unreachable.");
        }
    }
    delay(1000); // 每秒更新一次
}

bool cartesianToJoint(float target[3], float jointAngles[3]) {
    float x = target[0];
    float y = target[1];
    float z = target[2];

    float r = sqrt(x * x + y * y);
    
    jointAngles[0] = atan2(y, x); // 关节1角度
    jointAngles[1] = atan2(z, r) - acos((L1 * L1 + r * r - L2 * L2) / (2 * L1 * r)); // 关节2角度
    jointAngles[2] = acos((L1 * L1 + L2 * L2 - r * r) / (2 * L1 * L2)); // 关节3角度

    return (r <= (L1 + L2 + L3)); // 可达性检查
}

要点解读：
基础逆解实现：该程序实现了基础的多目标逆解，通过目标位置计算关节角度，适用于三连杆机械臂。
目标位置数组：使用数组存储多个目标位置，可以顺序访问并依次移动到每个目标。
电机控制：通过PWM信号控制电机，根据计算出的关节角度调节机械臂姿态。
可达性检查：在进行逆解计算时，检查目标位置是否在机械臂可达范围内，避免错误运动。
实时状态输出：每次循环输出当前关节角度，便于调试和验证控制效果。

2、带误差反馈的多目标控制

#include <Arduino.h>

const int motor1Pin = 9;
const int motor2Pin = 10;
const int motor3Pin = 11;

float targetPositions[3][3] = {
    {10.0, 10.0, 5.0}, 
    {15.0, 15.0, 10.0}, 
    {20.0, 10.0, 5.0}
}; 
int targetIndex = 0; 

float currentAngles[3] = {0.0, 0.0, 0.0}; 
const float L1 = 10.0; 
const float L2 = 10.0; 
const float L3 = 10.0; 

void setup() {
    Serial.begin(9600);
    pinMode(motor1Pin, OUTPUT);
    pinMode(motor2Pin, OUTPUT);
    pinMode(motor3Pin, OUTPUT);
}

void loop() {
    if (targetIndex < 3) {
        if (cartesianToJoint(targetPositions[targetIndex], currentAngles)) {
            for (int i = 0; i < 3; i++) {
                analogWrite(motor1Pin + i, constrain(currentAngles[i] * 255 / PI, 0, 255)); 
            }
            
            // 计算误差
            float positionError[3];
            for (int i = 0; i < 3; i++) {
                positionError[i] = targetPositions[targetIndex][i] - currentAngles[i];
            }

            Serial.print("Current Angles: (");
            Serial.print(currentAngles[0]);
            Serial.print(", ");
            Serial.print(currentAngles[1]);
            Serial.print(", ");
            Serial.print(currentAngles[2]);
            Serial.println(")");

            // 检查是否到达目标
            if (fabs(positionError[0]) < 0.1 && fabs(positionError[1]) < 0.1 && fabs(positionError[2]) < 0.1) {
                targetIndex++; 
            }
        } else {
            Serial.println("Target unreachable.");
        }
    }
    delay(1000); 
}

bool cartesianToJoint(float target[3], float jointAngles[3]) {
    float x = target[0];
    float y = target[1];
    float z = target[2];

    float r = sqrt(x * x + y * y);
    
    jointAngles[0] = atan2(y, x); 
    jointAngles[1] = atan2(z, r) - acos((L1 * L1 + r * r - L2 * L2) / (2 * L1 * r)); 
    jointAngles[2] = acos((L1 * L1 + L2 * L2 - r * r) / (2 * L1 * L2)); 

    return (r <= (L1 + L2 + L3)); 
}

要点解读：
误差反馈机制：该程序在目标位置控制中引入了误差反馈机制，通过计算当前关节角度与目标角度之间的误差进行控制。
性能优化：利用误差反馈可以提高系统的响应速度和精确度，使得机械臂能够更快地修正偏差。
电机控制：通过PWM信号控制电机，确保运动过程平稳且准确。
实时状态监测：每次循环输出当前关节角度，便于观察和验证控制效果。
扩展性强：代码结构清晰，便于后续扩展，如增加更多的目标位置或改进控制算法。

3、动态目标更新与误差反馈

#include <Arduino.h>

const int motor1Pin = 9;
const int motor2Pin = 10;
const int motor3Pin = 11;

float targetPositions[3][3] = {
    {10.0, 10.0, 5.0}, 
    {15.0, 15.0, 10.0}, 
    {20.0, 10.0, 5.0}
}; 
int targetIndex = 0; 

float currentAngles[3] = {0.0, 0.0, 0.0}; 
const float L1 = 10.0; 
const float L2 = 10.0; 
const float L3 = 10.0; 

void setup() {
    Serial.begin(9600);
    pinMode(motor1Pin, OUTPUT);
    pinMode(motor2Pin, OUTPUT);
    pinMode(motor3Pin, OUTPUT);
}

void loop() {
    if (targetIndex < 3) {
        // 动态更新目标位置
        targetPositions[targetIndex][0] += 0.1; // 动态调整X坐标
        targetPositions[targetIndex][1] += 0.1; // 动态调整Y坐标

        if (cartesianToJoint(targetPositions[targetIndex], currentAngles)) {
            for (int i = 0; i < 3; i++) {
                analogWrite(motor1Pin + i, constrain(currentAngles[i] * 255 / PI, 0, 255)); 
            }
            
            float positionError[3];
            for (int i = 0; i < 3; i++) {
                positionError[i] = targetPositions[targetIndex][i] - currentAngles[i];
            }

            Serial.print("Current Angles: (");
            Serial.print(currentAngles[0]);
            Serial.print(", ");
            Serial.print(currentAngles[1]);
            Serial.print(", ");
            Serial.print(currentAngles[2]);
            Serial.println(")");

            if (fabs(positionError[0]) < 0.1 && fabs(positionError[1]) < 0.1 && fabs(positionError[2]) < 0.1) {
                targetIndex++; 
            }
        } else {
            Serial.println("Target unreachable.");
        }
    }
    delay(1000); 
}

bool cartesianToJoint(float target[3], float jointAngles[3]) {
    float x = target[0];
    float y = target[1];
    float z = target[2];

    float r = sqrt(x * x + y * y);
    
    jointAngles[0] = atan2(y, x); 
    jointAngles[1] = atan2(z, r) - acos((L1 * L1 + r * r - L2 * L2) / (2 * L1 * r)); 
    jointAngles[2] = acos((L1 * L1 + L2 * L2 - r * r) / (2 * L1 * L2)); 

    return (r <= (L1 + L2 + L3)); 
}

要点解读：
动态目标更新：该程序在控制中动态调整目标位置，模拟实时目标变化，使机械臂能够适应动态环境。
误差反馈机制：通过计算当前关节角度与动态目标之间的误差进行控制，确保机械臂能够及时修正姿态。
电机控制：通过PWM信号控制电机，确保运动过程平稳且精确。
实时状态监测：每次循环输出当前关节角度，便于调试和验证控制效果。
增强的适应性：通过动态更新目标位置，增强了系统对环境变化的适应能力。

在这里插入图片描述
4、基本多目标逆解

#include <SimpleFOC.h>

// 定义机械臂的关节数
const int DOF = 5;

// 定义关节角度数组
float jointAngles[DOF] = {0, 0, 0, 0, 0};

// 定义多个目标位置
float targetPositions[3][3] = {
  {10, 10, 10},
  {20, 20, 20},
  {30, 30, 30}
};

// 定义雅可比矩阵
float jacobian[3][DOF];

// 初始化电机
BLDCMotor motor[DOF] = {BLDCMotor(7), BLDCMotor(7), BLDCMotor(7), BLDCMotor(7), BLDCMotor(7)};

void setup() {
  for (int i = 0; i < DOF; i++) {
    motor[i].init();
  }
}

void loop() {
  for (int t = 0; t < 3; t++) {
    // 计算雅可比矩阵
    calculateJacobian(jointAngles, jacobian);

    // 计算伪逆
    float pseudoInverse[DOF][3];
    calculatePseudoInverse(jacobian, pseudoInverse);

    // 计算关节角度变化
    float deltaAngles[DOF];
    for (int i = 0; i < DOF; i++) {
      deltaAngles[i] = 0;
      for (int j = 0; j < 3; j++) {
        deltaAngles[i] += pseudoInverse[i][j] * (targetPositions[t][j] - currentPosition[j]);
      }
    }

    // 更新关节角度
    for (int i = 0; i < DOF; i++) {
      jointAngles[i] += deltaAngles[i];
      motor[i].move(jointAngles[i]);
    }
    delay(1000); // 等待一段时间以观察效果
  }
}

void calculateJacobian(float* angles, float jacobian[3][DOF]) {
  // 计算雅可比矩阵的代码
}

void calculatePseudoInverse(float jacobian[3][DOF], float pseudoInverse[DOF][3]) {
  // 计算伪逆矩阵的代码
}

5、带有误差反馈的多目标逆解

#include <SimpleFOC.h>

// 定义机械臂的关节数
const int DOF = 5;

// 定义关节角度数组
float jointAngles[DOF] = {0, 0, 0, 0, 0};

// 定义多个目标位置
float targetPositions[3][3] = {
  {10, 10, 10},
  {20, 20, 20},
  {30, 30, 30}
};

// 定义雅可比矩阵
float jacobian[3][DOF];

// 初始化电机
BLDCMotor motor[DOF] = {BLDCMotor(7), BLDCMotor(7), BLDCMotor(7), BLDCMotor(7), BLDCMotor(7)};

// 误差反馈参数
float errorThreshold = 0.1;

void setup() {
  for (int i = 0; i < DOF; i++) {
    motor[i].init();
  }
}

void loop() {
  for (int t = 0; t < 3; t++) {
    // 计算雅可比矩阵
    calculateJacobian(jointAngles, jacobian);

    // 计算伪逆
    float pseudoInverse[DOF][3];
    calculatePseudoInverse(jacobian, pseudoInverse);

    // 计算关节角度变化
    float deltaAngles[DOF];
    for (int i = 0; i < DOF; i++) {
      deltaAngles[i] = 0;
      for (int j = 0; j < 3; j++) {
        deltaAngles[i] += pseudoInverse[i][j] * (targetPositions[t][j] - currentPosition[j]);
      }
    }

    // 更新关节角度并进行误差反馈
    for (int i = 0; i < DOF; i++) {
      jointAngles[i] += deltaAngles[i];
      motor[i].move(jointAngles[i]);
    }

    // 检查误差
    float error = calculateError(targetPositions[t], currentPosition);
    if (error < errorThreshold) {
      break; // 如果误差小于阈值，则停止调整
    }
    delay(1000); // 等待一段时间以观察效果
  }
}

void calculateJacobian(float* angles, float jacobian[3][DOF]) {
  // 计算雅可比矩阵的代码
}

void calculatePseudoInverse(float jacobian[3][DOF], float pseudoInverse[DOF][3]) {
  // 计算伪逆矩阵的代码
}

float calculateError(float* target, float* current) {
  float error = 0;
  for (int i = 0; i < 3; i++) {
    error += pow(target[i] - current[i], 2);
  }
  return sqrt(error);
}

6、带有动态调整的多目标逆解

#include <SimpleFOC.h>

// 定义机械臂的关节数
const int DOF = 5;

// 定义关节角度数组
float jointAngles[DOF] = {0, 0, 0, 0, 0};

// 定义多个目标位置
float targetPositions[3][3] = {
  {10, 10, 10},
  {20, 20, 20},
  {30, 30, 30}
};

// 定义雅可比矩阵
float jacobian[3][DOF];

// 初始化电机
BLDCMotor motor[DOF] = {BLDCMotor(7), BLDCMotor(7), BLDCMotor(7), BLDCMotor(7), BLDCMotor(7)};

// 误差反馈参数
float errorThreshold = 0.1;

void setup() {
  for (int i = 0; i < DOF; i++) {
    motor[i].init();
  }
}

void loop() {
  for (int t = 0; t < 3; t++) {
    // 计算雅可比矩阵
    calculateJacobian(jointAngles, jacobian);

    // 计算伪逆
    float pseudoInverse[DOF][3];
    calculatePseudoInverse(jacobian, pseudoInverse);

    // 计算关节角度变化
    float deltaAngles[DOF];
    for (int i = 0; i < DOF; i++) {
      deltaAngles[i] = 0;
      for (int j = 0; j < 3; j++) {
        deltaAngles[i] += pseudoInverse[i][j] * (targetPositions[t][j] - currentPosition[j]);
      }
    }

    // 更新关节角度并进行误差反馈
    for (int i = 0; i < DOF; i++) {
      jointAngles[i] += deltaAngles[i];
      motor[i].move(jointAngles[i]);
    }

    // 检查误差并动态调整
    float error = calculateError(targetPositions[t], currentPosition);
    if (error < errorThreshold) {
      adjustTarget(targetPositions[t]);
    }
    delay(1000); // 等待一段时间以观察效果
  }
}

void calculateJacobian(float* angles, float jacobian[3][DOF]) {
  // 计算雅可比矩阵的代码
}

void calculatePseudoInverse(float jacobian[3][DOF], float pseudoInverse[DOF][3]) {
  // 计算伪逆矩阵的代码
}

float calculateError(float* target, float* current) {
  float error = 0;
  for (int i = 0; i < 3; i++) {
    error += pow(target[i] - current[i], 2);
  }
  return sqrt(error);
}

void adjustTarget(float* target) {
  // 动态调整目标位置的代码
}

要点解读
雅可比矩阵计算：每个案例都展示了如何计算机械臂的雅可比矩阵，这是逆运动学求解的基础。
伪逆矩阵求解：通过伪逆法求解雅可比矩阵的逆，能够处理机械臂的冗余自由度问题，实现精确控制。
误差反馈：第5个和第6个案例展示了如何通过误差反馈机制，实时调整关节角度，确保机械臂能够精确到达目标位置。
动态调整：第6个案例进一步展示了如何在误差反馈基础上进行动态调整，提高系统的适应性和响应速度。
实时控制：在loop函数中进行实时控制，确保机械臂能够快速响应目标位置的变化，适应复杂的工作环境。

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

在这里插入图片描述