自平衡机器人算法实现
大家好，我是这里的新人，是一名机械工程的学生，最近想用自平衡机器人实现自动化算法，但是在实际操作过程中遇到了一些问题，希望有专业知识的朋友可以指点一下小车先读取MPU6050模块上的角度及角速度，还有马达上编码器上读速度读数，以这些度数乘以一些系数，获得马达所需电压，然后用adafruit motor sheild控制输出电压。但是遇到以下问题
给马达零电压，也就是不控制马达是，能够读出角度及角速度，但是一旦控制马达有转速时。会产生I2C read failuer, 或者I2C timeout的错误提示，然后程序就不跑了。
我附上自己写的代码，请各位帮助看一下，由于自己是机械学生，代码能力有限，请各位见谅。#include <Wire.h>
#include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_PWMServoDriver.h"
//**********define motors****************//
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *M_L = AFMS.getMotor(1);
Adafruit_DCMotor *M_R = AFMS.getMotor(2);
#define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;
//**************1 ------ Left 2----------Right ******************
//*********Motor Left defines*****************
#define encodPinA1 12 // encoder A pin
#define encodPinB1 11 // encoder B pin
//*********Motor Right defines*****************
#define encodPinA2 3 // encoder A pin
#define encodPinB2 4 // encoder B pin
#define CURRENT_LIMIT 1000 // high current warning
#define LOW_BAT 10000 // low bat warning
#define LOOPTIME 10 // PID loop time
#define NUMREADINGS 10 // samples for Amp average
int encoder0PinALast_1 = LOW;
int n_1 = LOW;
int encoder0PinALast_2 = LOW;
int n_2 = LOW;
int readings;
unsigned long lastMilli = 0; // loop timing
unsigned long lastMilliPrint = 0; // loop timing
float speed_act_1 = 0; // speed (actual value)
float speed_act_2 = 0; // speed (actual value)
int voltage = 0; // in mV
int current = 0; // in mA
volatile long count_1 = 0; // rev counter of Left Motor
volatile long count_2 = 0; // rev counter of Right Motor
//********************gain design ************************************
float K1_1 = -0.178184631752522;
float K1_2 = -0.071916510883366;
float K1_3 = -0.098188582242225;
float K1_4 = -0.005587314894462;
float K2_1 = -0.003404946688540;
float K2_2 = -7.908193215839999e-04;
// ********motor control*******
//***************motor gain*******
float Kp_m = 2;
float Ki_m= 100;
//*********motor input***********;
float vol_in_L;
float vol_in_R;
float w_dot_req_L;
float w_dot_req_R;
float w_dot_act_L;
float w_dot_act_R;
//****************CONTROL SIGNAL***********************************
float X_in = 0;
float X_dot_in = 0;
float yaw_in = 0;
float yaw_dot_in = 0;
//*****************Parameters*************
float wheel_r = 0.045085; //wheel radius meter
float pi = 3.1415926;
float D = 0.24384; //m lateral distance between the contact patches of the wheel
float deg2rad = 2*pi/(360);
float rad2deg = 360/(2*pi);
float J = 0.0079; // moment of inertia about rotation axis
//************States**********
float X_act;
float X_act_dot;
float yaw_act;
float yaw_act_dot;
//***************system control input********
float torque_sum;
float torque_min;
float torque_L;
float torque_R;
//****************debug*************print
float error_w_dot_L;
float error_w_dot_R;
float int_error_w_dot_L = 0;
float error_w_dot_L_old = 0;
float int_error_w_dot_R = 0;
float error_w_dot_R_old = 0;
/* IMU Data */
double accX, accY, accZ;
double gyroX, gyroY, gyroZ;
//int16_t tempRaw;
double gyroXangle, gyroYangle; // Angle calculate using the gyro only
double compAngleX, compAngleY; // Calculated angle using a complementary filter
double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter
uint32_t timer;
uint8_t i2cData; // Buffer for I2C data
// TODO: Make calibration routine
void setup() {
AFMS.begin();
Serial.begin(9600);
Wire.begin();
#if ARDUINO >= 157
Wire.setClock(400000UL); // Set I2C frequency to 400kHz
#else
TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
#endif
i2cData = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
i2cData = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
i2cData = 0x00; // Set Gyro Full Scale Range to ±250deg/s
i2cData = 0x00; // Set Accelerometer Full Scale Range to ±2g
while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
while (i2cRead(0x75, i2cData, 1));
if (i2cData != 0x68) { // Read "WHO_AM_I" register
Serial.print(F("Error reading sensor"));
while (1);
}
delay(100); // Wait for sensor to stabilize
/* Set kalman and gyro starting angle */
while (i2cRead(0x3B, i2cData, 6));
accX = (i2cData << 8) | i2cData;
accY = (i2cData << 8) | i2cData;
accZ = (i2cData << 8) | i2cData;
// Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
// It is then converted from radians to degrees
#ifdef RESTRICT_PITCH // Eq. 25 and 26
double roll= atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
double roll= atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
kalmanX.setAngle(roll); // Set starting angle
kalmanY.setAngle(pitch);
//gyroXangle = roll;
//gyroYangle = pitch;
//compAngleX = roll;
//compAngleY = pitch;
timer = micros();
analogReference(EXTERNAL); // Current external ref is 3.3V
pinMode(encodPinA1, INPUT);
pinMode(encodPinB1, INPUT);
digitalWrite(encodPinA1, HIGH); // turn on pullup resistor
digitalWrite(encodPinB1, HIGH);
pinMode(encodPinA2, INPUT);
pinMode(encodPinB2, INPUT);
digitalWrite(encodPinA2, HIGH); // turn on pullup resistor
digitalWrite(encodPinB2, HIGH);
attachInterrupt(1, rencoder, FALLING);
for(int i=0; i<NUMREADINGS; i++) readings = 0;// initialize readings to 0
}
void loop() {
while (i2cRead(0x3B, i2cData, 14));
accX = ((i2cData << 8) | i2cData);
accY = ((i2cData << 8) | i2cData);
accZ = ((i2cData << 8) | i2cData);
gyroX = (i2cData << 8) | i2cData;
gyroY = (i2cData << 8) | i2cData;
gyroZ = (i2cData << 8) | i2cData;
double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
timer = micros();
#ifdef RESTRICT_PITCH // Eq. 25 and 26
double roll= atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
double roll= atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
double gyroXrate = gyroX / 131.0; // Convert to deg/s
double gyroYrate = gyroY / 131.0; // Convert to deg/s
#ifdef RESTRICT_PITCH
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
kalmanX.setAngle(roll);
compAngleX = roll;
kalAngleX = roll;
gyroXangle = roll;
} else
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
if (abs(kalAngleX) > 90)
gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
#else
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
kalmanY.setAngle(pitch);
compAngleY = pitch;
kalAngleY = pitch;
gyroYangle = pitch;
} else
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter
if (abs(kalAngleY) > 90)
gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
#endif
gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
gyroYangle += gyroYrate * dt;
// Reset the gyro angle when it has drifted too much
if (gyroXangle < -180 || gyroXangle > 180)
gyroXangle = kalAngleX;
if (gyroYangle < -180 || gyroYangle > 180)
gyroYangle = kalAngleY;
if((millis()-lastMilli) >= LOOPTIME){
lastMilli=millis();
getMotorData();
X_act = count_1*2*pi/1600*wheel_r;
X_act_dot = speed_act_1*2*pi*wheel_r/60;
yaw_act = (count_1-count_2)*2*pi*wheel_r/(16*30*D);
yaw_act_dot = (speed_act_1-speed_act_2)*2*pi*wheel_r/(16*30*D);
torque_sum = -(K1_1*(X_act-X_in)-K1_2*(X_act_dot-X_dot_in)-K1_3*(kalAngleY*deg2rad)-K1_4*gyroYrate*deg2rad);
torque_min = 0; // -(K2_1*(yaw_act-yaw_in)-K2_2*(yaw_act_dot-yaw_dot_in));
torque_L = (torque_sum+torque_min)/2;
torque_R = torque_sum-torque_L;
w_dot_req_L = torque_L/J;
w_dot_req_R = torque_R/J;
error_w_dot_L = w_dot_req_L-w_dot_act_L;
error_w_dot_R = w_dot_req_R-w_dot_act_R;
int_error_w_dot_L = int_error_w_dot_L+error_w_dot_L*(LOOPTIME/1000);
int_error_w_dot_R = int_error_w_dot_R+error_w_dot_R*(LOOPTIME/1000);
vol_in_L = error_w_dot_L*Kp_m+int_error_w_dot_L*Ki_m;
vol_in_R = error_w_dot_R*Kp_m+int_error_w_dot_R*Ki_m;
if(vol_in_L>0) {
M_L->run(FORWARD);
}
if(vol_in_L<0) {
M_L->run(BACKWARD);
}
if(vol_in_R>0) {
M_R->run(FORWARD);
}
if(vol_in_R<0) {
M_R->run(BACKWARD);
}
PWM_val_1 = volt2pwm(abs(vol_in_L));
PWM_val_2 = volt2pwm(abs(vol_in_R));
M_L->setSpeed(PWM_val_1);
M_R->setSpeed(PWM_val_2);
printdata();
}
}
void printdata(){
//Serial.print("KalAngleY = ");
Serial.print(PWM_val_1); Serial.print(" ");
Serial.println(PWM_val_2);
//Serial.print("\t");
}
void rencoder(){
n_1 = digitalRead(encodPinA1);
if ((encoder0PinALast_1 == LOW) && (n_1 == HIGH)){
if (digitalRead(encodPinB1) == LOW) {
count_1--;
} else{
count_1++;
}
}
encoder0PinALast_1 = n_1;
n_2 = digitalRead(encodPinA2);
if ((encoder0PinALast_2 == LOW) && (n_2 == HIGH)){
if (digitalRead(encodPinB2) == LOW) {
count_2--;
} else{
count_2++;
}
}
encoder0PinALast_2 = n_2;
}
void getMotorData(){
static long countAnt_1 = 0;
static long countAnt_2 = 0;
static float speed_act_1_old = 0;
static float speed_act_2_old = 0;
static float speed_act_1_oold = 0;
static float speed_act_2_oold = 0;
speed_act_1 = (count_1 - countAnt_1)*2*pi*1000/(16*30*LOOPTIME);// radia/s
countAnt_1 = count_1;
speed_act_2 = (count_2 - countAnt_2)*2*pi*1000/(16*30*LOOPTIME);
countAnt_2 = count_2;
w_dot_act_L = (3*speed_act_1-4*speed_act_1_old+speed_act_1_oold)*1000/(LOOPTIME);
w_dot_act_R = (3*speed_act_2-4*speed_act_2_old+speed_act_2_oold)*1000/(LOOPTIME);
speed_act_1_old = speed_act_1;
speed_act_1_oold = speed_act_1_old;
speed_act_2_old = speed_act_2;
speed_act_2_oold = speed_act_2_old;
}
//void MotorVoltageUpdate (){
//
//
//
//error_w_dot_L = w_dot_req_L-w_dot_act_L;
//error_w_dot_R = w_dot_req_R-w_dot_act_R;
//int_error_w_dot_L = int_error_w_dot_L+error_w_dot_L*(LOOPTIME/1000);
//int_error_w_dot_R = int_error_w_dot_R+error_w_dot_R*(LOOPTIME/1000);
//vol_in_L = error_w_dot_L*Kp_m+int_error_w_dot_L*Ki_m;
//vol_in_R = error_w_dot_R*Kp_m+int_error_w_dot_R*Ki_m;
//
// if(vol_in_L>0) {
// digitalWrite(InA1, LOW);
// digitalWrite(InB1, HIGH);
// }
// if(vol_in_L<0) {
// digitalWrite(InA1,HIGH);
// digitalWrite(InB1, LOW);
// }
// if(vol_in_R>0) {
// digitalWrite(InA2, LOW);
// digitalWrite(InB2, HIGH);
// }
// if(vol_in_R<0) {
// digitalWrite(InA2,HIGH);
// digitalWrite(InB2, LOW);
// }
//
//PWM_val_1 = volt2pwm(abs(vol_in_L));
//PWM_val_2 = volt2pwm(abs(vol_in_R));
//
//}
int volt2pwm(float vol){
int pwm;
pwm = 0.6272*vol*vol*vol-6.475*vol*vol+27.72*vol+11.53;
return constrain(pwm,0,255);
}
看着像是供电的问题，试试电机单独供电。（比如Arduino接数字电源A 电机+电机驱动板接数字电源B） 我觉得有可能是马达功率太高，导致MPU 6050电压不稳定，于是装了一个pull up电阻，但是还是出同样的错。然后采用分开的电压输入，依旧出错。
我没有连接MPU 6050上的int pin。怀疑是不是需要interrupt 苦海 发表于 2015-11-13 11:49
看着像是供电的问题，试试电机单独供电。（比如Arduino接数字电源A 电机+电机驱动板接数字电源B） ...
刚想邀请你来回答这个问题 {:5_148:} 邀请回答 @Ricky 还有void MotorVoltageUpdate () 方程里老是不更新w_dot的数据，所以只好注释掉，全部定义成全局变量，放在主循环里了 苦海 发表于 2015-11-13 11:49
看着像是供电的问题，试试电机单独供电。（比如Arduino接数字电源A 电机+电机驱动板接数字电源B） ...
刚开始的时候是，arduino接电脑，电机驱动接接电源，还是不行，现在用了adafruit的电机驱动，更加糟糕 我把电路图也上传吧，图中的电池直接改成12V直流，arduino 接电脑，motor controller改成了adafruit电机驱动板，所以就没有电机的input HIGH LOW和analog write了。
MPU 6050上的SDA,SCL 由并联了两个4.7k欧姆的电阻，来稳定电压，还是没有用。
https://mc.dfrobot.com.cn/home.php?mod=space&uid=721144&do=album&picid=2267&goto=down#pic_block FuzzyLogic32 发表于 2015-11-13 11:59
刚开始的时候是，arduino接电脑，电机驱动接接电源，还是不行，现在用了adafruit的电机驱动，更加糟糕 ...
你的系统是带光耦隔离的么？Adafruit之外的另一块驱动是什么？电机是哪种电机 多大? 看说明，一定是供电问题。 电流过大引起电源降低，造成无法读数。 Ricky 发表于 2015-11-13 19:17
看说明，一定是供电问题。 电流过大引起电源降低，造成无法读数。
我试过将mpu6050的供电改成独立电池供电，可是还是出错。我没有连interrupt可能产生这样的问题么? 苦海 发表于 2015-11-13 18:17
你的系统是带光耦隔离的么？Adafruit之外的另一块驱动是什么？电机是哪种电机 多大? ...
光耦隔离是什么？我用的是arduino mega。电机是pololu 12v 29:1 电机。最大电流2500mA。
谢谢帮助！ Ricky 发表于 2015-11-13 19:17
看说明，一定是供电问题。 电流过大引起电源降低，造成无法读数。
谢谢帮助！ 赞一个 FuzzyLogic32 发表于 2015-11-14 00:20
我试过将mpu6050的供电改成独立电池供电，可是还是出错。我没有连interrupt可能产生这样的问题么? ...
arduino 也要和 mpu6050一起用单独的供电，电机用另外的电源供电。 正确的做法是 ，Arduino和mpu6050 共用一组电源5v输入， 电机单独一路电源，但是两个系统要共地。 Ricky 发表于 2015-11-16 15:30
正确的做法是 ，Arduino和mpu6050 共用一组电源5v输入， 电机单独一路电源，但是两个系统要共地。 ...
我试过了这样的做法，后来还是不行，然后我有看了一下光栅编码器的度数，发现噪音特别大，导致了输出电压非常不稳定，请问这个有办法解决么
页:
[1]