Bootstrap

【花雕学编程】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

在这里插入图片描述
1、主要特点:
采用双足设计,通过精确的扭矩控制实现机器人的动态平衡。
基于 Arduino FOC 框架,可利用其强大的电机控制能力来实现平衡控制。
算法相对简单,但可以稳定地维持机器人的直立姿态。
通过传感器数据反馈,实现对机器人倾斜角度和角速度的闭环控制。
可以集成惯性测量单元(IMU)等传感器,获取机器人的姿态信息。
算法具有良好的鲁棒性,可抵抗一定程度的外部干扰。

2、应用场景:
教育机器人,可用于学习平衡控制算法的设计和实现。
娱乐机器人,可制作出能够进行动态平衡表演的双足机器人。
辅助步行设备,可用于设计具有动态平衡功能的助行机器人。
特殊环境作业机器人,可在狭小空间或不稳定地形上进行作业。
科研实验平台,可用于研究和验证新型平衡控制算法。

3、需要注意的事项:
机械结构的设计要确保双足机器人的稳定性和抗干扰能力。
电机和驱动器的选型需要满足快速精确的转矩控制需求。
传感器系统的选型和布局要能够准确感知机器人的倾斜角度和角速度。
平衡控制算法需要结合机器人的动力学特性进行优化设计和调试。
整体系统需要具备可靠的安全保护机制,如摔倒检测、姿态限制等。
平衡控制算法需要与上层导航、动作规划等功能模块进行良好的集成。

总的来说,Arduino FOC 中的简单双足平衡控制算法是一个非常实用的基础功能模块。在实际应用中,需要重点关注机械设计、驱动选型、传感器集成、算法优化和整体系统集成等方面,确保机器人能够在各种复杂环境下保持稳定的直立姿态,完成各种任务需求。

在这里插入图片描述
以下是几个基于Arduino FOC的双足平衡控制算法的实际运用程序参考代码案例。这些代码案例可以帮助你理解如何在Arduino上实现双足机器人的平衡控制。

1、PID控制器

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <Servo.h>

Adafruit_BNO055 bno = Adafruit_BNO055();
Servo servo1, servo2;

float targetAngle = 0.0;  // 目标倾斜角度
float Kp = 0.5;          // 比例系数
float Ki = 0.1;          // 积分系数
float Kd = 0.2;          // 微分系数

float errorSum = 0.0;    // 误差累积
float prevError = 0.0;   // 上一次误差

void setup()
{
  Serial.begin(9600);
  
  if (!bno.begin())
  {
    Serial.print("BNO055 not detected");
    while (1);
  }
  bno.setExtCrystalUse(true);
  
  servo1.attach(9);
  servo2.attach(10);
}

void loop()
{
  sensors_event_t event;
  bno.getEvent(&event);
  
  float roll = event.orientation.x;
  float pitch = event.orientation.y;
  
  float error = targetAngle - roll;
  
  // PID控制器计算输出
  float output = Kp * error + Ki * errorSum + Kd * (error - prevError);
  
  // 更新误差累积和上一次误差
  errorSum += error;
  prevError = error;
  
  // 控制舵机角度
  float servoAngle = map(output, -90, 90, 0, 180);
  servo1.write(servoAngle);
  servo2.write(servoAngle);
  
  delay(50);
}

要点解读：
代码使用Adafruit BNO055库来与Arduino连接和读取姿态传感器数据。
在setup()函数中，初始化串口通信、姿态传感器和舵机，并检查姿态传感器是否正常工作。
在loop()函数中，通过bno.getEvent()函数获取姿态传感器事件，并将姿态数据存储在event对象中。
根据目标倾斜角度和当前的滚动角计算误差。
使用PID控制器计算输出值，根据比例、积分和微分系数以及误差累积和上一次误差。
根据输出值映射舵机角度，并使用servo.write()函数将角度值写入相应的舵机对象中。

2、限幅PID控制器

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <Servo.h>

Adafruit_BNO055 bno = Adafruit_BNO055();
Servo servo1, servo2;

float targetAngle = 0.0;  // 目标倾斜角度
float Kp = 0.5;          // 比例系数
float Ki = 0.1;          // 积分系数
float Kd = 0.2;          // 微分系数
float outputMin = -90.0; // 输出最小值
float outputMax = 90.0;  // 输出最大值

float errorSum = 0.0;    // 误差累积
float prevError = 0.0;   // 上一次误差

void setup()
{
  Serial.begin(9600);
  
  if (!bno.begin())
  {
    Serial.print("BNO055 not detected");
    while (1);
  }
  bno.setExtCrystalUse(true);
  
  servo1.attach(9);
  servo2.attach(10);
}

void loop()
{
  sensors_event_t event;
  bno.getEvent(&event);
  
  float roll = event.orientation.x;
  float pitch = event.orientation.y;
  
  float error = targetAngle - roll;
  
// 限幅PID控制器计算输出
  float output = Kp * error + Ki * errorSum + Kd * (error - prevError);
  output = constrain(output, outputMin, outputMax);  // 限制输出在最小值和最大值之间
  
  // 更新误差累积和上一次误差
  errorSum += error;
  prevError = error;
  
  // 控制舵机角度
  float servoAngle = map(output, -90, 90, 0, 180);
  servo1.write(servoAngle);
  servo2.write(servoAngle);
  
  delay(50);
}

要点解读：
与上一个代码案例相比，添加了outputMin和outputMax变量，用于限制PID控制器的输出值在指定范围内。
在计算PID控制器输出值后，使用constrain()函数将输出值限制在outputMin和outputMax之间。

3、模糊控制器

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <Servo.h>
#include <Fuzzy.h>

Adafruit_BNO055 bno = Adafruit_BNO055();
Servo servo1, servo2;

float targetAngle = 0.0;  // 目标倾斜角度

Fuzzy fuzzyController;

void setup()
{
  Serial.begin(9600);
  
  if (!bno.begin())
  {
    Serial.print("BNO055 not detected");
    while (1);
  }
  bno.setExtCrystalUse(true);
  
  servo1.attach(9);
  servo2.attach(10);
  
  fuzzyController.begin();
  
  // 设置模糊控制器输入和输出的模糊集合函数
  fuzzyController.setRule(0, "NB", "NS", "NB", "NS");
  fuzzyController.setRule(1, "NB", "NS", "NM", "NS");
  fuzzyController.setRule(2, "NB", "NS", "ZE", "ZS");
  fuzzyController.setRule(3, "NB", "NS", "PM", "PS");
  fuzzyController.setRule(4, "NB", "NS", "PB", "PS");
  // ... 添加更多规则
  
  fuzzyController.setRule(30, "PB", "PS", "NB", "NS");
  fuzzyController.setRule(31, "PB", "PS", "NM", "NS");
  fuzzyController.setRule(32, "PB", "PS", "ZE", "ZS");
  fuzzyController.setRule(33, "PB", "PS", "PM", "PS");
  fuzzyController.setRule(34, "PB", "PS", "PB", "PS");
  
  // 设置模糊控制器输入和输出的范围
  fuzzyController.setInputRange(-90, 90);
  fuzzyController.setOutputRange(-90, 90);
  
  // 设置模糊控制器的模糊化函数和解模糊化函数
  fuzzyController.setDefuzzifyMethod(MAX);
  fuzzyController.setFuzzyFunction(Fuzzy::triangular);
}
  
void loop()
{
  sensors_event_t event;
  bno.getEvent(&event);
  
  float roll = event.orientation.x;
  float pitch = event.orientation.y;
  
  // 模糊控制器计算输出
  float output = fuzzyController.fuzzify(roll);
  
  // 控制舵机角度
  float servoAngle = map(output, -90, 90, 0, 180);
  servo1.write(servoAngle);
  servo2.write(servoAngle);
  
  delay(50);
}

要点解读：
引入了Fuzzy库，用于实现模糊控制器。
在setup()函数中，初始化串口通信、姿态传感器和舵机，并检查姿态传感器是否正常工作。
配置模糊控制器的输入和输出模糊集合函数，设置规则以及输入和输出的范围。
使用fuzzyController.fuzzify()函数将输入值模糊化，并得到模糊控制器的输出值。

在这里插入图片描述
4、简单双足平衡控制程序:

#include <SimpleFOC.h>

// 电机驱动对象
BLDCMotor motor1 = BLDCMotor(11);
BLDCMotor motor2 = BLDCMotor(12);

// 角度传感器对象
MagneticSensorSPI sensor1 = MagneticSensorSPI(14, 15, 16);
MagneticSensorSPI sensor2 = MagneticSensorSPI(17, 18, 19);

// 电机控制器对象
SimpleFOCDriver driver1 = SimpleFOCDriver(9, 10);
SimpleFOCDriver driver2 = SimpleFOCDriver(7, 8);

void setup() {
  // 初始化电机和传感器
  motor1.linkSensor(&sensor1);
  motor2.linkSensor(&sensor2);
  motor1.linkDriver(&driver1);
  motor2.linkDriver(&driver2);

  // 设置电机参数
  motor1.pole_pairs = 7;
  motor2.pole_pairs = 7;
  motor1.voltage_limit = 12;
  motor2.voltage_limit = 12;

  // 初始化电机控制器
  motor1.initFOC();
  motor2.initFOC();

  // 初始化平衡控制算法
  double kp = 0.5, ki = 0.1, kd = 0.05;
  balanceController.PID(kp, ki, kd);
}

void loop() {
  // 读取电机角度
  float angle1 = sensor1.getAngle();
  float angle2 = sensor2.getAngle();

  // 计算倾斜角度
  float tiltAngle = (angle1 - angle2) / 2.0;

  // 执行平衡控制算法
  float torque = balanceController.step(tiltAngle);

  // 输出扭矩到电机
  motor1.setPhaseVoltage(torque);
  motor2.setPhaseVoltage(-torque);

  // 等待下一个循环
  motor1.loop();
  motor2.loop();
}

要点解读:
使用 SimpleFOC 库来控制两个BLDC电机,并链接到角度传感器和电机驱动器。
设置电机的极对数和电压限制。
初始化平衡控制算法,使用简单的PID控制器。
在循环中,读取两个电机的角度,计算倾斜角度,执行平衡控制算法,输出扭矩到电机。
在每个循环中,调用电机的 loop() 函数来更新电机状态。

5、使用IMU传感器的双足平衡控制程序:

#include <SimpleFOC.h>
#include <Wire.h>
#include <MPU6050.h>

// 电机驱动对象
BLDCMotor motor1 = BLDCMotor(11);
BLDCMotor motor2 = BLDCMotor(12);

// 角度传感器对象
MagneticSensorSPI sensor1 = MagneticSensorSPI(14, 15, 16);
MagneticSensorSPI sensor2 = MagneticSensorSPI(17, 18, 19);

// IMU传感器对象
MPU6050 imu;

// 电机控制器对象
SimpleFOCDriver driver1 = SimpleFOCDriver(9, 10);
SimpleFOCDriver driver2 = SimpleFOCDriver(7, 8);

void setup() {
  // 初始化电机、传感器和IMU
  motor1.linkSensor(&sensor1);
  motor2.linkSensor(&sensor2);
  motor1.linkDriver(&driver1);
  motor2.linkDriver(&driver2);
  imu.initialize();

  // 设置电机参数
  motor1.pole_pairs = 7;
  motor2.pole_pairs = 7;
  motor1.voltage_limit = 12;
  motor2.voltage_limit = 12;

  // 初始化电机控制器
  motor1.initFOC();
  motor2.initFOC();

  // 初始化平衡控制算法
  double kp = 0.5, ki = 0.1, kd = 0.05;
  balanceController.PID(kp, ki, kd);
}

void loop() {
  // 读取IMU数据
  float ax, ay, az;
  imu.getAcceleration(&ax, &ay, &az);

  // 计算倾斜角度
  float tiltAngle = atan2(ay, az) * 180 / PI;

  // 执行平衡控制算法
  float torque = balanceController.step(tiltAngle);

  // 输出扭矩到电机
  motor1.setPhaseVoltage(torque);
  motor2.setPhaseVoltage(-torque);

  // 等待下一个循环
  motor1.loop();
  motor2.loop();
}

要点解读:
除了之前的角度传感器,这个程序还使用了 MPU6050 IMU传感器来读取倾斜角度。
在 setup() 函数中,初始化了 MPU6050 传感器。
在 loop() 函数中,读取 IMU 数据,计算倾斜角度,并执行平衡控制算法。
使用 IMU 数据可以更准确地感知机器人的倾斜状态,从而实现更精准的平衡控制。

6、带有外部平衡控制算法的双足平衡控制程序:

#include <SimpleFOC.h>

// 电机驱动对象
BLDCMotor motor1 = BLDCMotor(11);
BLDCMotor motor2 = BLDCMotor(12);

// 角度传感器对象
MagneticSensorSPI sensor1 = MagneticSensorSPI(14, 15, 16);
MagneticSensorSPI sensor2 = MagneticSensorSPI(17, 18, 19);

// 电机控制器对象
SimpleFOCDriver driver1 = SimpleFOCDriver(9, 10);
SimpleFOCDriver driver2 = SimpleFOCDriver(7, 8);

// 外部平衡控制算法对象
BalanceController balancer;

void setup() {
  // 初始化电机和传感器
  motor1.linkSensor(&sensor1);
  motor2.linkSensor(&sensor2);
  motor1.linkDriver(&driver1);
  motor2.linkDriver(&driver2);

  // 设置电机参数
  motor1.pole_pairs = 7;
  motor2.pole_pairs = 7;
  motor1.voltage_limit = 12;
  motor2.voltage_limit = 12;

  // 初始化电机控制器
  motor1.initFOC();
  motor2.initFOC();

  // 初始化平衡控制算法
  balancer.init(0.5, 0.1, 0.05);
}

void loop() {
  // 读取电机角度
  float angle1 = sensor1.getAngle();
  float angle2 = sensor2.getAngle();

  // 计算倾斜角度
  float tiltAngle = (angle1 - angle2) / 2.0;

  // 执行平衡控制算法
  float torque = balancer.balance(tiltAngle);

  // 输出扭矩到电机
  motor1.setPhaseVoltage(torque);
  motor2.setPhaseVoltage(-torque);

  // 等待下一个循环
  motor1.loop();
  motor2.loop();
}

要点解读:
这个程序使用了一个外部的 BalanceController 对象来实现平衡控制算法,而不是使用简单的 PID 控制器。
在 setup() 函数中,初始化了 BalanceController 对象,设置了 PID 参数。
在 loop() 函数中,读取电机角度,计算倾斜角度,然后调用 BalanceController 的 balance() 函数来计算输出扭矩。
这种方式可以让你更方便地实现和调试平衡控制算法,而不需要直接修改 SimpleFOC 库的代码。

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

在这里插入图片描述

悦读

道可道，非常道；名可名，非常名。 无名，天地之始，有名，万物之母。 故常无欲，以观其妙，常有欲，以观其徼。 此两者，同出而异名，同谓之玄，玄之又玄，众妙之门。

最新收录
python岭回归、Lasso、随机森林、XGBoost、Keras神经网络、kmeans聚类链家租房数据地理可视化分析
Visual Studio 2010旗舰版的vb.net版本录音程序代码QZQ
Vulnhub靶场案例渗透[8]- HackableII
组合数学 8种盒子放球问题
USB 的工作原理：枚举和配置（第 3 部分）
python爬虫代理IP池(proxy pool)
不想git更新代码时产生merge记录，可以使用git rebase命令
kafka安装部署，和基本操作
5、设计与实现资源加载器，从Spring.xml解析和注册Bean对象
支持python开发的环境有哪些特点_Python虚拟环境详细教程，一篇带你入坑
;