2015-11-13 11:38:13 [显示全部楼层]
9975浏览
查看: 9975|回复: 16

自平衡机器人算法实现

[复制链接]
大家好,我是这里的新人,是一名机械工程的学生,最近想用自平衡机器人实现自动化算法,但是在实际操作过程中遇到了一些问题,希望有专业知识的朋友可以指点一下

小车先读取MPU6050模块上的角度及角速度,还有马达上编码器上读速度读数,以这些度数乘以一些系数,获得马达所需电压,然后用adafruit motor sheild控制输出电压。但是遇到以下问题

给马达零电压,也就是不控制马达是,能够读出角度及角速度,但是一旦控制马达有转速时。会产生I2C read failuer, 或者I2C timeout的错误提示,然后程序就不跑了。

我附上自己写的代码,请各位帮助看一下,由于自己是机械学生,代码能力有限,请各位见谅。
  1. #include <Wire.h>
  2. #include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter
  3. #include <Adafruit_MotorShield.h>
  4. #include "utility/Adafruit_PWMServoDriver.h"
  5. //**********define motors****************//
  6. Adafruit_MotorShield AFMS = Adafruit_MotorShield();
  7. Adafruit_DCMotor *M_L = AFMS.getMotor(1);
  8. Adafruit_DCMotor *M_R = AFMS.getMotor(2);
  9. #define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
  10. Kalman kalmanX; // Create the Kalman instances
  11. Kalman kalmanY;
  12. //**************  1 ------ Left     2----------Right   ******************
  13. //*********Motor Left defines*****************
  14. #define encodPinA1      12                       // encoder A pin
  15. #define encodPinB1      11                       // encoder B pin
  16. //*********Motor Right defines*****************
  17. #define encodPinA2      3                       // encoder A pin
  18. #define encodPinB2      4                       // encoder B pin
  19. #define CURRENT_LIMIT   1000                     // high current warning
  20. #define LOW_BAT         10000                   // low bat warning
  21. #define LOOPTIME        10                     // PID loop time
  22. #define NUMREADINGS     10                      // samples for Amp average
  23. int encoder0PinALast_1 = LOW;
  24. int n_1 = LOW;
  25. int encoder0PinALast_2 = LOW;
  26. int n_2 = LOW;
  27. int readings[NUMREADINGS];
  28. unsigned long lastMilli = 0;                    // loop timing
  29. unsigned long lastMilliPrint = 0;               // loop timing
  30. float speed_act_1 = 0;                              // speed (actual value)
  31. float speed_act_2 = 0;                              // speed (actual value)
  32. int voltage = 0;                                // in mV
  33. int current = 0;                                // in mA
  34. volatile long count_1 = 0;                        // rev counter of Left Motor
  35. volatile long count_2 = 0;                        // rev counter of Right Motor
  36. //********************gain design ************************************
  37. float K1_1 = -0.178184631752522;
  38. float K1_2 = -0.071916510883366;
  39. float K1_3 = -0.098188582242225;
  40. float K1_4 = -0.005587314894462;
  41. float K2_1 = -0.003404946688540;
  42. float K2_2 = -7.908193215839999e-04;
  43. // ********motor control*******
  44. //***************motor gain*******
  45. float Kp_m = 2;
  46. float Ki_m= 100;
  47. //*********motor input***********;
  48. float vol_in_L;
  49. float vol_in_R;
  50. float w_dot_req_L;
  51. float w_dot_req_R;
  52. float w_dot_act_L;
  53. float w_dot_act_R;
  54. //****************CONTROL SIGNAL***********************************
  55. float X_in = 0;
  56. float X_dot_in = 0;
  57. float yaw_in = 0;
  58. float yaw_dot_in = 0;
  59. //*****************Parameters*************
  60. float wheel_r = 0.045085; //wheel radius meter
  61. float pi = 3.1415926;
  62. float D = 0.24384; //m lateral distance between the contact patches of the wheel
  63. float deg2rad = 2*pi/(360);
  64. float rad2deg = 360/(2*pi);
  65. float J = 0.0079; // moment of inertia about rotation axis
  66. //************States**********
  67. float X_act;
  68. float X_act_dot;
  69. float yaw_act;
  70. float yaw_act_dot;
  71. //***************system control input********
  72. float torque_sum;
  73. float torque_min;
  74. float torque_L;
  75. float torque_R;
  76. //****************debug*************print
  77. float error_w_dot_L;
  78. float error_w_dot_R;
  79. float int_error_w_dot_L = 0;
  80. float error_w_dot_L_old = 0;
  81. float int_error_w_dot_R = 0;
  82. float error_w_dot_R_old = 0;
  83. /* IMU Data */
  84. double accX, accY, accZ;
  85. double gyroX, gyroY, gyroZ;
  86. //int16_t tempRaw;
  87. double gyroXangle, gyroYangle; // Angle calculate using the gyro only
  88. double compAngleX, compAngleY; // Calculated angle using a complementary filter
  89. double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter
  90. uint32_t timer;
  91. uint8_t i2cData[14]; // Buffer for I2C data
  92. // TODO: Make calibration routine
  93. void setup() {
  94.   AFMS.begin();
  95.   Serial.begin(9600);
  96.   Wire.begin();
  97. #if Arduino >= 157
  98.   Wire.setClock(400000UL); // Set I2C frequency to 400kHz
  99. #else
  100.   TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
  101. #endif
  102.   i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
  103.   i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
  104.   i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
  105.   i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
  106.   while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
  107.   while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
  108.   while (i2cRead(0x75, i2cData, 1));
  109.   if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
  110.     Serial.print(F("Error reading sensor"));
  111.     while (1);
  112.   }
  113.   delay(100); // Wait for sensor to stabilize
  114.   /* Set kalman and gyro starting angle */
  115.   while (i2cRead(0x3B, i2cData, 6));
  116.   accX = (i2cData[0] << 8) | i2cData[1];
  117.   accY = (i2cData[2] << 8) | i2cData[3];
  118.   accZ = (i2cData[4] << 8) | i2cData[5];
  119.   // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
  120.   // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
  121.   // It is then converted from radians to degrees
  122. #ifdef RESTRICT_PITCH // Eq. 25 and 26
  123.   double roll  = atan2(accY, accZ) * RAD_TO_DEG;
  124.   double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
  125. #else // Eq. 28 and 29
  126.   double roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
  127.   double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
  128. #endif
  129.   kalmanX.setAngle(roll); // Set starting angle
  130.   kalmanY.setAngle(pitch);
  131. //  gyroXangle = roll;
  132. //  gyroYangle = pitch;
  133. //  compAngleX = roll;
  134. //  compAngleY = pitch;
  135.   timer = micros();
  136.   analogReference(EXTERNAL);                            // Current external ref is 3.3V
  137.   pinMode(encodPinA1, INPUT);
  138.   pinMode(encodPinB1, INPUT);
  139.   digitalWrite(encodPinA1, HIGH);                      // turn on pullup resistor
  140.   digitalWrite(encodPinB1, HIGH);
  141.   pinMode(encodPinA2, INPUT);
  142.   pinMode(encodPinB2, INPUT);
  143.   digitalWrite(encodPinA2, HIGH);                      // turn on pullup resistor
  144.   digitalWrite(encodPinB2, HIGH);
  145.   
  146.   attachInterrupt(1, rencoder, FALLING);
  147.   for(int i=0; i<NUMREADINGS; i++)   readings[i] = 0;  // initialize readings to 0
  148.   
  149. }
  150. void loop() {
  151.   
  152. while (i2cRead(0x3B, i2cData, 14));
  153.   accX = ((i2cData[0] << 8) | i2cData[1]);
  154.   accY = ((i2cData[2] << 8) | i2cData[3]);
  155.   accZ = ((i2cData[4] << 8) | i2cData[5]);
  156.   gyroX = (i2cData[8] << 8) | i2cData[9];
  157.   gyroY = (i2cData[10] << 8) | i2cData[11];
  158.   gyroZ = (i2cData[12] << 8) | i2cData[13];
  159.   
  160. double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
  161.   timer = micros();
  162. #ifdef RESTRICT_PITCH // Eq. 25 and 26
  163.   double roll  = atan2(accY, accZ) * RAD_TO_DEG;
  164.   double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
  165. #else // Eq. 28 and 29
  166.   double roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
  167.   double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
  168. #endif
  169.   double gyroXrate = gyroX / 131.0; // Convert to deg/s
  170.   double gyroYrate = gyroY / 131.0; // Convert to deg/s
  171. #ifdef RESTRICT_PITCH
  172.   // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  173.   if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
  174.     kalmanX.setAngle(roll);
  175.     compAngleX = roll;
  176.     kalAngleX = roll;
  177.     gyroXangle = roll;
  178.   } else
  179.     kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
  180.   if (abs(kalAngleX) > 90)
  181.     gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
  182.   kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
  183. #else
  184.   // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  185.   if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
  186.     kalmanY.setAngle(pitch);
  187.     compAngleY = pitch;
  188.     kalAngleY = pitch;
  189.     gyroYangle = pitch;
  190.   } else
  191.     kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter
  192.   if (abs(kalAngleY) > 90)
  193.     gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
  194.   kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
  195. #endif
  196.   gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
  197.   gyroYangle += gyroYrate * dt;
  198.   // Reset the gyro angle when it has drifted too much
  199.   if (gyroXangle < -180 || gyroXangle > 180)
  200.     gyroXangle = kalAngleX;
  201.   if (gyroYangle < -180 || gyroYangle > 180)
  202.     gyroYangle = kalAngleY;
  203.     if((millis()-lastMilli) >= LOOPTIME){
  204.      lastMilli=millis();
  205.     getMotorData();
  206.     X_act = count_1*2*pi/1600*wheel_r;
  207.     X_act_dot = speed_act_1*2*pi*wheel_r/60;
  208.     yaw_act = (count_1-count_2)*2*pi*wheel_r/(16*30*D);
  209.     yaw_act_dot = (speed_act_1-speed_act_2)*2*pi*wheel_r/(16*30*D);
  210.     torque_sum = -(K1_1*(X_act-X_in)-K1_2*(X_act_dot-X_dot_in)-K1_3*(kalAngleY*deg2rad)-K1_4*gyroYrate*deg2rad);
  211.     torque_min = 0; // -(K2_1*(yaw_act-yaw_in)-K2_2*(yaw_act_dot-yaw_dot_in));
  212.     torque_L = (torque_sum+torque_min)/2;
  213.     torque_R = torque_sum-torque_L;
  214.     w_dot_req_L = torque_L/J;
  215.     w_dot_req_R = torque_R/J;
  216.     error_w_dot_L = w_dot_req_L-w_dot_act_L;
  217.     error_w_dot_R = w_dot_req_R-w_dot_act_R;
  218.     int_error_w_dot_L = int_error_w_dot_L+error_w_dot_L*(LOOPTIME/1000);
  219.     int_error_w_dot_R = int_error_w_dot_R+error_w_dot_R*(LOOPTIME/1000);
  220.     vol_in_L = error_w_dot_L*Kp_m+int_error_w_dot_L*Ki_m;
  221.     vol_in_R = error_w_dot_R*Kp_m+int_error_w_dot_R*Ki_m;
  222.       if(vol_in_L>0) {
  223.       M_L->run(FORWARD);
  224.       }
  225.       if(vol_in_L<0) {
  226.       M_L->run(BACKWARD);
  227.       }
  228.       if(vol_in_R>0) {
  229.       M_R->run(FORWARD);
  230.       }
  231.       if(vol_in_R<0) {
  232.       M_R->run(BACKWARD);
  233.       }
  234.   
  235.   PWM_val_1 = volt2pwm(abs(vol_in_L));
  236.   PWM_val_2 = volt2pwm(abs(vol_in_R));
  237.   M_L->setSpeed(PWM_val_1);
  238.   M_R->setSpeed(PWM_val_2);
  239.     printdata();
  240.     }
  241.    
  242. }
  243. void printdata(){
  244.   //Serial.print("KalAngleY = ");
  245.   Serial.print(PWM_val_1); Serial.print(" ");
  246.    Serial.println(PWM_val_2);
  247. //  Serial.print("\t");
  248. }
  249. void rencoder()  {                                   
  250. n_1 = digitalRead(encodPinA1);
  251. if ((encoder0PinALast_1 == LOW) && (n_1 == HIGH)){
  252.   if (digitalRead(encodPinB1) == LOW) {
  253.   count_1--;
  254.   } else{
  255.     count_1++;
  256.     }
  257. }
  258. encoder0PinALast_1 = n_1;
  259. n_2 = digitalRead(encodPinA2);
  260. if ((encoder0PinALast_2 == LOW) && (n_2 == HIGH)){
  261.   if (digitalRead(encodPinB2) == LOW) {
  262.   count_2--;
  263.   } else{
  264.     count_2++;
  265.     }
  266. }
  267. encoder0PinALast_2 = n_2;
  268. }
  269. void getMotorData()  {                                                   
  270. static long countAnt_1 = 0;
  271. static long countAnt_2 = 0;
  272. static float speed_act_1_old = 0;
  273. static float speed_act_2_old = 0;
  274. static float speed_act_1_oold = 0;
  275. static float speed_act_2_oold = 0;
  276.   speed_act_1 = (count_1 - countAnt_1)*2*pi*1000/(16*30*LOOPTIME);//   radia/s
  277.   countAnt_1 = count_1;
  278.   speed_act_2 = (count_2 - countAnt_2)*2*pi*1000/(16*30*LOOPTIME);
  279.   countAnt_2 = count_2;
  280.   w_dot_act_L = (3*speed_act_1-4*speed_act_1_old+speed_act_1_oold)*1000/(LOOPTIME);
  281.   w_dot_act_R = (3*speed_act_2-4*speed_act_2_old+speed_act_2_oold)*1000/(LOOPTIME);
  282.   speed_act_1_old = speed_act_1;
  283.   speed_act_1_oold = speed_act_1_old;
  284.   speed_act_2_old = speed_act_2;
  285.   speed_act_2_oold = speed_act_2_old;
  286. }
  287. //void MotorVoltageUpdate ()  {
  288. //
  289. //
  290. //  
  291. //  error_w_dot_L = w_dot_req_L-w_dot_act_L;
  292. //  error_w_dot_R = w_dot_req_R-w_dot_act_R;
  293. //  int_error_w_dot_L = int_error_w_dot_L+error_w_dot_L*(LOOPTIME/1000);
  294. //  int_error_w_dot_R = int_error_w_dot_R+error_w_dot_R*(LOOPTIME/1000);
  295. //  vol_in_L = error_w_dot_L*Kp_m+int_error_w_dot_L*Ki_m;
  296. //  vol_in_R = error_w_dot_R*Kp_m+int_error_w_dot_R*Ki_m;
  297. //
  298. //    if(vol_in_L>0) {
  299. //      digitalWrite(InA1, LOW);
  300. //      digitalWrite(InB1, HIGH);
  301. //      }
  302. //      if(vol_in_L<0) {
  303. //        digitalWrite(InA1,HIGH);
  304. //        digitalWrite(InB1, LOW);
  305. //      }
  306. //      if(vol_in_R>0) {
  307. //      digitalWrite(InA2, LOW);
  308. //      digitalWrite(InB2, HIGH);
  309. //      }
  310. //      if(vol_in_R<0) {
  311. //        digitalWrite(InA2,HIGH);
  312. //        digitalWrite(InB2, LOW);
  313. //      }
  314. //  
  315. //  PWM_val_1 = volt2pwm(abs(vol_in_L));
  316. //  PWM_val_2 = volt2pwm(abs(vol_in_R));
  317. //  
  318. //}
  319. int volt2pwm(float vol){
  320.   int pwm;
  321.   pwm = 0.6272*vol*vol*vol-6.475*vol*vol+27.72*vol+11.53;
  322.   return constrain(pwm,0,255);
  323. }
复制代码


苦海  初级技匠

发表于 2015-11-13 11:49:16

看着像是供电的问题,试试电机单独供电。(比如Arduino接数字电源A 电机+电机驱动板接数字电源B)
回复

使用道具 举报

FuzzyLogic32  学徒
 楼主|

发表于 2015-11-13 11:51:04

我觉得有可能是马达功率太高,导致MPU 6050电压不稳定,于是装了一个pull up电阻,但是还是出同样的错。然后采用分开的电压输入,依旧出错。

我没有连接MPU 6050上的int pin。怀疑是不是需要interrupt
回复

使用道具 举报

Ash  管理员

发表于 2015-11-13 11:51:42

苦海 发表于 2015-11-13 11:49
看着像是供电的问题,试试电机单独供电。(比如Arduino接数字电源A 电机+电机驱动板接数字电源B) ...

刚想邀请你来回答这个问题
回复

使用道具 举报

Ash  管理员

发表于 2015-11-13 11:53:07

邀请回答 @Ricky  
回复

使用道具 举报

FuzzyLogic32  学徒
 楼主|

发表于 2015-11-13 11:56:26

还有void MotorVoltageUpdate () 方程里老是不更新w_dot的数据,所以只好注释掉,全部定义成全局变量,放在主循环里了
回复

使用道具 举报

FuzzyLogic32  学徒
 楼主|

发表于 2015-11-13 11:59:21

苦海 发表于 2015-11-13 11:49
看着像是供电的问题,试试电机单独供电。(比如Arduino接数字电源A 电机+电机驱动板接数字电源B) ...

刚开始的时候是,arduino接电脑,电机驱动接接电源,还是不行,现在用了adafruit的电机驱动,更加糟糕
回复

使用道具 举报

FuzzyLogic32  学徒
 楼主|

发表于 2015-11-13 12:09:48

我把电路图也上传吧,图中的电池直接改成12V直流,arduino 接电脑,motor controller改成了adafruit电机驱动板,所以就没有电机的input HIGH LOW和analog write了。
MPU 6050上的SDA,SCL 由并联了两个4.7k欧姆的电阻,来稳定电压,还是没有用。

https://mc.dfrobot.com.cn/home.p ... goto=down#pic_block
回复

使用道具 举报

苦海  初级技匠

发表于 2015-11-13 18:17:59

FuzzyLogic32 发表于 2015-11-13 11:59
刚开始的时候是,arduino接电脑,电机驱动接接电源,还是不行,现在用了adafruit的电机驱动,更加糟糕 ...

你的系统是带光耦隔离的么?Adafruit之外的另一块驱动是什么?电机是哪种电机 多大?
回复

使用道具 举报

Ricky  NPC

发表于 2015-11-13 19:17:11

看说明,一定是供电问题。 电流过大引起电源降低,造成无法读数。
回复

使用道具 举报

FuzzyLogic32  学徒
 楼主|
来自手机

发表于 2015-11-14 00:20:36

Ricky 发表于 2015-11-13 19:17
看说明,一定是供电问题。 电流过大引起电源降低,造成无法读数。

我试过将mpu6050的供电改成独立电池供电,可是还是出错。我没有连interrupt可能产生这样的问题么?
回复

使用道具 举报

FuzzyLogic32  学徒
 楼主|
来自手机

发表于 2015-11-14 00:25:01

苦海 发表于 2015-11-13 18:17
你的系统是带光耦隔离的么?Adafruit之外的另一块驱动是什么?电机是哪种电机 多大? ...

光耦隔离是什么?我用的是arduino mega。电机是pololu 12v 29:1 电机。最大电流2500mA。
谢谢帮助!
回复

使用道具 举报

FuzzyLogic32  学徒
 楼主|
来自手机

发表于 2015-11-14 00:25:42

Ricky 发表于 2015-11-13 19:17
看说明,一定是供电问题。 电流过大引起电源降低,造成无法读数。

谢谢帮助!
回复

使用道具 举报

dsweiliang  初级技神

发表于 2015-11-14 00:52:47

赞一个
回复

使用道具 举报

seesea  初级技匠

发表于 2015-11-14 15:47:07

FuzzyLogic32 发表于 2015-11-14 00:20
我试过将mpu6050的供电改成独立电池供电,可是还是出错。我没有连interrupt可能产生这样的问题么? ...

arduino 也要和 mpu6050一起用单独的供电,电机用另外的电源供电。
回复

使用道具 举报

Ricky  NPC

发表于 2015-11-16 15:30:26

正确的做法是 ,Arduino和mpu6050 共用一组电源5v输入, 电机单独一路电源,但是两个系统要共地。
回复

使用道具 举报

FuzzyLogic32  学徒
 楼主|

发表于 2015-11-17 05:44:50

Ricky 发表于 2015-11-16 15:30
正确的做法是 ,Arduino和mpu6050 共用一组电源5v输入, 电机单独一路电源,但是两个系统要共地。 ...

我试过了这样的做法,后来还是不行,然后我有看了一下光栅编码器的度数,发现噪音特别大,导致了输出电压非常不稳定,请问这个有办法解决么
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

为本项目制作心愿单
购买心愿单
心愿单 编辑
[[wsData.name]]

硬件清单

  • [[d.name]]
btnicon
我也要做!
点击进入购买页面
关于楼主

楼主的其它帖子

上海智位机器人股份有限公司 沪ICP备09038501号-4

© 2013-2024 Comsenz Inc. Powered by Discuz! X3.4 Licensed

mail