ARDUINO控制云台舵机,基于MPU6050+PID算法保持位姿

&黄昏的乐师

已于 2023-12-22 23:01:58 修改

阅读量1.4k

点赞数 5

文章标签：单片机 c# 学习笔记

于 2023-11-17 23:50:23 首次发布

本文链接：https://blog.csdn.net/2301_76165902/article/details/134472356

版权

工具

arduino nano

MPU6050三轴陀螺仪

sg90舵机两个

二自由度云台

介绍

《模拟鸡头》

转动云台，让头部保持在一个位置不转动，通过舵机自我调节。

基于arduino nano单片机，陀螺仪解算出姿态角度，利用PID算法算出角度差值赋给舵机。

需要了解一定的PID算法，并且学会调参（主要是调参比较难），还要学会MPU6050（或其他陀螺仪，能解算出姿态就行）的应用。

PID 实指“比例”、“积分”、“微分”，这三项构成 PID 基本要素。数字 PID 控制算法通常分为位置式 PID 控制算法和增量式 PID 控制算法。

比例P : $e_n$ 目标值 - 当前值

积分I : $e_n+e_{n-1}+...+e_{0}$ 误差的累加

微分D : $e_n-e_{n-1}$ 这次误差-上次误差

P（比例）作用：对当前时刻的偏差进行比例放大。

I（积分）环节：对过去所有时间的偏差进行积分。

D（微分）环节：通过偏差的偏差，对控制系统的输出走向进行预判，起超前调节的作用。

常用的位置式PID：

可以加一个积分限幅，因为误差达到饱和时为防止一直积累，可以增加限幅来停止积分作用

所以我感觉可以只用单一的P控制器或者PD来结算误差，毕竟现在不要求太高精度，如果要达到好的效果就要用到积分。

这里提供PID模拟器，可以帮助你理解PID的作用

链接：https://pan.baidu.com/s/1MiCkuNO5Izwt-dLrM5QnBg
提取码：HHHH

主要流程如下图

代码如下

PID部分

int pid1(int a,int b){
  float kp=0.68,ki=0.002,kd=0.3;
  float pwm;
  a= map(a,-45,45,0,180);
  curren_err = a-b;
  
  leijia+=curren_err;
  if(leijia>5)leijia=5;
  if(leijia<-5)leijia=-5;
  pwm = kp*curren_err+ki*leijia+kd*(curren_err-last_err);
  last_err = curren_err;
  return pwm;
}

在循环中应用此函数算出误差即可

全部代码如下

#include <MPU6050_tockn.h>
#include <Servo.h>
Servo duo1;
Servo duo2;
float y,x;
MPU6050 mpu6050(Wire);
float tar=0,curren_err,last_err,pre_err,leijia;

int pid1(int a,int b){
  float kp=0.68,ki=0.002,kd=0.3;
  float pwm;
  a= map(a,-45,45,0,180);
  curren_err = a-b;
  
  leijia+=curren_err;
  if(leijia>5)leijia=5;
  if(leijia<-5)leijia=-5;
  pwm = kp*curren_err+ki*leijia+kd*(curren_err-last_err);
  last_err = curren_err;
  return pwm;
}

void setup() {
  // put your setup code here, to run once:
duo1.attach(3);
duo2.attach(5);
  Serial.begin(9600);
  Wire.begin();
  mpu6050.begin();
  mpu6050.calcGyroOffsets(true);

}

void loop() {
  // put your main code here, to run repeatedly:
  mpu6050.update();
  //Serial.print(mpu6050.getAngleY());
  Serial.print(" ");
  //Serial.println(mpu6050.getAngleX());
  y=mpu6050.getAngleY();
  x=mpu6050.getAngleX();
duo1.write(pid1(y,tar));
duo2.write(pid1(x,tar));
 Serial.print(pid1(y,tar));
 Serial.print(" ");
 Serial.println(pid1(x,tar));
}