【花雕学编程】Arduino FOC 之基于雅可比矩阵的步态生成

驴友花雕

于 2024-09-14 07:31:55 发布

阅读量450

点赞数 8

分类专栏： Arduino FOC 文章标签：矩阵嵌入式硬件单片机 c++ Arduino动手做基于雅可比矩阵的步态生成 Arduino FOC

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

版权

Arduino FOC 专栏收录该内容

500 篇文章 8 订阅

订阅专栏

在这里插入图片描述

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、主要特点
精确运动控制：
基于雅可比矩阵的步态生成技术能够精准地将关节空间的运动转换为任务空间的运动，确保机器人的运动路径符合预定的目标。
实时性：
结合Arduino FOC技术，能够实现高效的实时控制，快速响应传感器数据和环境变化，确保步态生成的动态适应性。
灵活性：
该方法支持多种步态生成，可以灵活调整步态参数，以适应不同的运动场景和任务需求。
多自由度支持：
能够处理多自由度的机器人系统，如双足机器人或四足机器人，通过雅可比矩阵进行复杂步态的生成和控制。
开源和可扩展性：
基于Arduino的开源平台，开发者可以方便地修改和扩展步态生成算法，便于进行实验和创新。

2、应用场景
仿生机器人：
在仿生机器人领域，基于雅可比矩阵的步态生成可以实现更自然的运动，提升机器人的灵活性和适应性。
人形机器人：
在人形机器人中，步态生成技术能够实现稳定的行走和运动，适用于人机交互和服务等应用。
智能移动平台：
在智能轮椅或自动驾驶车辆中，通过步态生成算法实现灵活的导航和路径规划，提升用户体验。
教育与研究：
作为教育工具，帮助学生理解机器人运动学和控制理论，促进相关研究和实验。
工业自动化：
在工业机器人中，基于步态生成的技术可以用于物料搬运、装配等任务，提高工作效率和精度。

3、需要注意的事项
雅可比矩阵的准确性：
确保所使用的雅可比矩阵计算准确，以保证步态生成的有效性和精确性。
参数调节：
步态生成过程中，需根据实际情况调节步态参数，如步幅、频率等，以适应不同的工作条件。
动态稳定性：
在复杂环境中，需设计机制确保机器人的动态稳定性，避免因步态不良导致的倾覆或摔倒。
传感器反馈：
结合传感器数据进行实时反馈和调整，以应对外部环境的变化，确保步态的灵活性和适应性。
安全性设计：
在实际应用中，需加强安全性设计，如设置急停装置和碰撞检测，确保机器人在运行过程中的安全。
通过结合Arduino FOC技术与基于雅可比矩阵的步态生成方法，可以实现高效、灵活的运动控制，广泛应用于仿生机器人、人形机器人等多个领域。

在这里插入图片描述
1、基本雅可比矩阵计算

#include <Arduino.h>

float linkLengths[3] = {5.0, 4.0, 3.0}; // 连杆长度
float jointAngles[3] = {0.0, 0.0, 0.0}; // 关节角度

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

void loop() {
  float J[3][3]; // 雅可比矩阵

  // 计算雅可比矩阵
  computeJacobian(J, jointAngles);
  
  // 输出雅可比矩阵
  Serial.println("Jacobian Matrix:");
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      Serial.print(J[i][j]);
      Serial.print("\t");
    }
    Serial.println();
  }

  delay(1000); // 更新频率
}

void computeJacobian(float J[3][3], float angles[3]) {
  // 计算雅可比矩阵的简化示例
  J[0][0] = -linkLengths[0] * sin(angles[0]) - linkLengths[1] * sin(angles[0] + angles[1]);
  J[0][1] = -linkLengths[1] * sin(angles[0] + angles[1]);
  J[0][2] = 0;

  J[1][0] = linkLengths[0] * cos(angles[0]) + linkLengths[1] * cos(angles[0] + angles[1]);
  J[1][1] = linkLengths[1] * cos(angles[0] + angles[1]);
  J[1][2] = 0;

  J[2][0] = 1; // Z轴
  J[2][1] = 1;
  J[2][2] = 1;
}

要点解读：
雅可比矩阵：该程序计算三维空间中机械臂的雅可比矩阵，适用于运动学分析。
关节角度：通过输入关节角度，动态计算雅可比矩阵，以适应不同姿态。
连杆长度：通过数组存储连杆长度，增强代码的可维护性。
输出格式：通过串口输出雅可比矩阵，便于调试和验证计算结果。
更新频率：通过delay(1000)控制输出频率，适合于实时监控。

2、步态生成与目标位置控制

#include <Arduino.h>

float jointAngles[3] = {0.0, 0.0, 0.0}; // 初始关节角度
float target[3] = {8.0, 6.0, 0.0}; // 目标位置
float linkLengths[3] = {5.0, 4.0, 3.0}; // 连杆长度
float learningRate = 0.1; // 学习率

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

void loop() {
  float current[3];
  forwardKinematics(jointAngles, current); // 计算当前末端位置

  // 计算误差
  float error[3];
  for (int i = 0; i < 2; i++) {
    error[i] = target[i] - current[i];
  }

  // 计算雅可比矩阵
  float J[3][3];
  computeJacobian(J, jointAngles);

  // 更新关节角度
  for (int i = 0; i < 3; i++) {
    jointAngles[i] += learningRate * (J[i][0] * error[0] + J[i][1] * error[1]);
  }

  // 输出当前关节角度
  Serial.print("Joint Angles: ");
  for (int i = 0; i < 3; i++) {
    Serial.print(jointAngles[i]);
    Serial.print("\t");
  }
  Serial.println();

  delay(500); // 更新频率
}

void forwardKinematics(float angles[3], float position[3]) {
  // 计算末端位置的简化示例
  position[0] = linkLengths[0] * cos(angles[0]) + linkLengths[1] * cos(angles[0] + angles[1]);
  position[1] = linkLengths[0] * sin(angles[0]) + linkLengths[1] * sin(angles[0] + angles[1]);
  position[2] = 0; // 假设Z轴不变
}

要点解读：
步态生成：该程序通过雅可比矩阵实现末端位置控制，适用于动态路径跟踪。
误差计算：通过计算目标位置与当前末端位置之间的误差，驱动关节角度调整。
学习率：learningRate控制关节角度更新的速度，需根据实际需要进行调整。
前向运动学：实现简单的前向运动学计算，确保末端位置的准确性。
实时输出：通过串口输出当前关节角度，便于调试和监控系统状态。

3、多轴步态控制

#include <Arduino.h>

float jointAngles[3] = {0.0, 0.0, 0.0}; // 初始关节角度
float target[3] = {8.0, 6.0, 0.0}; // 目标位置
float linkLengths[3] = {5.0, 4.0, 3.0}; // 连杆长度
float learningRate = 0.1; // 学习率

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

void loop() {
  float current[3];
  forwardKinematics(jointAngles, current); // 计算当前末端位置

  // 计算误差
  float error[3];
  for (int i = 0; i < 2; i++) {
    error[i] = target[i] - current[i];
  }

  // 计算雅可比矩阵
  float J[3][3];
  computeJacobian(J, jointAngles);

  // 更新关节角度
  for (int i = 0; i < 3; i++) {
    jointAngles[i] += learningRate * (J[i][0] * error[0] + J[i][1] * error[1]);
  }

  // 输出当前关节角度
  Serial.print("Joint Angles: ");
  for (int i = 0; i < 3; i++) {
    Serial.print(jointAngles[i]);
    Serial.print("\t");
  }
  Serial.println();

  // 限制关节角度范围
  for (int i = 0; i < 3; i++) {
    if (jointAngles[i] < -M_PI) jointAngles[i] = -M_PI;
    if (jointAngles[i] > M_PI) jointAngles[i] = M_PI;
  }

  delay(500); // 更新频率
}

要点解读：
多轴控制：实现多轴的步态控制，适用于复杂机械臂的操作需求。
误差反馈：通过实时计算末端位置与目标位置的误差，进行动态调整。
角度限制：加入关节角度范围限制，以防止机械臂过度扭转，保护设备。
动态更新：实现动态更新关节角度，适应不同的目标位置。
调试输出：通过串口输出当前关节角度，便于监控系统运行状态。

在这里插入图片描述
4、六足机器人步态生成

#include <SimpleFOC.h>

// 电机和传感器初始化
BLDCMotor motor1 = BLDCMotor(7);
BLDCMotor motor2 = BLDCMotor(7);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(9, 10, 11);
BLDCDriver3PWM driver2 = BLDCDriver3PWM(6, 5, 3);
Encoder encoder1 = Encoder(2, 3, 500);
Encoder encoder2 = Encoder(4, 5, 500);
void doA1() { encoder1.handleA(); }
void doB1() { encoder1.handleB(); }
void doA2() { encoder2.handleA(); }
void doB2() { encoder2.handleB(); }

void setup() {
  // 初始化电机
  driver1.voltage_power_supply = 12;
  driver1.init();
  motor1.linkDriver(&driver1);
  motor1.linkSensor(&encoder1);
  motor1.controller = MotionControlType::angle;
  motor1.init();
  encoder1.init();
  encoder1.enableInterrupts(doA1, doB1);

  driver2.voltage_power_supply = 12;
  driver2.init();
  motor2.linkDriver(&driver2);
  motor2.linkSensor(&encoder2);
  motor2.controller = MotionControlType::angle;
  motor2.init();
  encoder2.init();
  encoder2.enableInterrupts(doA2, doB2);
}

void loop() {
  // 雅可比矩阵计算
  float J[2][2] = {{1, 0}, {0, 1}};
  float q[2] = {motor1.shaft_angle, motor2.shaft_angle};
  float dq[2] = {0.1, 0.1}; // 关节速度

  // 计算末端速度
  float v[2] = {J[0][0] * dq[0] + J[0][1] * dq[1], J[1][0] * dq[0] + J[1][1] * dq[1]};

  // 控制电机移动
  motor1.move(q[0] + v[0]);
  motor2.move(q[1] + v[1]);
  delay(10);
}

5、四足机器人步态生成

#include <SimpleFOC.h>

// 电机和传感器初始化
BLDCMotor motor1 = BLDCMotor(7);
BLDCMotor motor2 = BLDCMotor(7);
BLDCMotor motor3 = BLDCMotor(7);
BLDCMotor motor4 = BLDCMotor(7);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(9, 10, 11);
BLDCDriver3PWM driver2 = BLDCDriver3PWM(6, 5, 3);
BLDCDriver3PWM driver3 = BLDCDriver3PWM(12, 13, 14);
BLDCDriver3PWM driver4 = BLDCDriver3PWM(15, 16, 17);
Encoder encoder1 = Encoder(2, 3, 500);
Encoder encoder2 = Encoder(4, 5, 500);
Encoder encoder3 = Encoder(6, 7, 500);
Encoder encoder4 = Encoder(8, 9, 500);
void doA1() { encoder1.handleA(); }
void doB1() { encoder1.handleB(); }
void doA2() { encoder2.handleA(); }
void doB2() { encoder2.handleB(); }
void doA3() { encoder3.handleA(); }
void doB3() { encoder3.handleB(); }
void doA4() { encoder4.handleA(); }
void doB4() { encoder4.handleB(); }

void setup() {
  // 初始化电机
  driver1.voltage_power_supply = 12;
  driver1.init();
  motor1.linkDriver(&driver1);
  motor1.linkSensor(&encoder1);
  motor1.controller = MotionControlType::angle;
  motor1.init();
  encoder1.init();
  encoder1.enableInterrupts(doA1, doB1);

  driver2.voltage_power_supply = 12;
  driver2.init();
  motor2.linkDriver(&driver2);
  motor2.linkSensor(&encoder2);
  motor2.controller = MotionControlType::angle;
  motor2.init();
  encoder2.init();
  encoder2.enableInterrupts(doA2, doB2);

  driver3.voltage_power_supply = 12;
  driver3.init();
  motor3.linkDriver(&driver3);
  motor3.linkSensor(&encoder3);
  motor3.controller = MotionControlType::angle;
  motor3.init();
  encoder3.init();
  encoder3.enableInterrupts(doA3, doB3);

  driver4.voltage_power_supply = 12;
  driver4.init();
  motor4.linkDriver(&driver4);
  motor4.linkSensor(&encoder4);
  motor4.controller = MotionControlType::angle;
  motor4.init();
  encoder4.init();
  encoder4.enableInterrupts(doA4, doB4);
}

void loop() {
  // 雅可比矩阵计算
  float J[4][4] = {{1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}};
  float q[4] = {motor1.shaft_angle, motor2.shaft_angle, motor3.shaft_angle, motor4.shaft_angle};
  float dq[4] = {0.1, 0.1, 0.1, 0.1}; // 关节速度

  // 计算末端速度
  float v[4] = {J[0][0] * dq[0] + J[0][1] * dq[1] + J[0][2] * dq[2] + J[0][3] * dq[3],
                J[1][0] * dq[0] + J[1][1] * dq[1] + J[1][2] * dq[2] + J[1][3] * dq[3],
                J[2][0] * dq[0] + J[2][1] * dq[1] + J[2][2] * dq[2] + J[2][3] * dq[3],
                J[3][0] * dq[0] + J[3][1] * dq[1] + J[3][2] * dq[2] + J[3][3] * dq[3]};

  // 控制电机移动
  motor1.move(q[0] + v[0]);
  motor2.move(q[1] + v[1]);
  motor3.move(q[2] + v[2]);
  motor4.move(q[3] + v[3]);
  delay(10);
}

6、双足机器人步态生成

#include <SimpleFOC.h>

// 电机和传感器初始化
BLDCMotor motor1 = BLDCMotor(7);
BLDCMotor motor2 = BLDCMotor(7);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(9, 10, 11);
BLDCDriver3PWM driver2 = BLDCDriver3PWM(6, 5, 3);
Encoder encoder1 = Encoder(2, 3, 500);
Encoder encoder2 = Encoder(4, 5, 500);
void doA1() { encoder1.handleA(); }
void doB1() { encoder1.handleB(); }
void doA2() { encoder2.handleA(); }
void doB2() { encoder2.handleB(); }

void setup() {
  // 初始化电机
  driver1.voltage_power_supply = 12;
  driver1.init();
  motor1.linkDriver(&driver1);
  motor1.linkSensor(&encoder1);
  motor1.controller = MotionControlType::angle;
  motor1.init();
  encoder1.init();
  encoder1.enableInterrupts(doA1, doB1);

  driver2.voltage_power_supply = 12;
  driver2.init();
  motor2.linkDriver(&driver2);
  motor2.linkSensor(&encoder2);
  motor2.controller = MotionControlType::angle;
  motor2.init();
  encoder2.init();
  encoder2.enableInterrupts(doA2, doB2);
}

void loop() {
  // 雅可比矩阵计算
  float J[2][2] = {{1, 0}, {0, 1}};
  float q[2] = {motor1.shaft_angle, motor2.shaft_angle};
  float dq[2] = {0.1, 0.1}; // 关节速度

  // 计算末端速度
  float v[2] = {J[0][0] * dq[0] + J[0][1] * dq[1], J[1][0] * dq[0] + J[1][1] * dq[1]};

  // 控制电机移动
  motor1.move(q[0] + v[0]);
  motor2.move(q[1] + v[1]);
  delay(10);
}

要点解读
雅可比矩阵的应用：每个案例都使用雅可比矩阵来计算关节速度和末端速度，从而实现步态生成。
电机控制：使用SimpleFOC库进行电机控制，确保电机在步态生成过程中平滑运行和精确定位。
关节速度设置：通过设置关节速度（dq）来控制机器人关节的运动，从而生成步态。
末端速度计算：利用雅可比矩阵计算末端速度（v），并将其应用于电机控制，实现步态生成。
延时控制：使用delay函数来控制步态生成的速度，确保电机有足够时间移动到下一个位置，避免过快导致的步态不稳定。

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

在这里插入图片描述