Description:
In the previous project, we have introduced how to use MPU-6050 chip to calculate the inclined angle and angular velocity value so as to control the car’s balance.
In this section, we will set about to balance the car upright on a horizontal plane via PD (proportion and differential ).
However, if placed on a slope or pushed slightly by hand, the car will accelerate in the inclined direction and fall down.
The corresponding inclined angle value can be printed out on the serial monitor.
Upright balance principle:
The self-balancing robot stands upright on two coaxial wheels.
The gravity center of robot’s structure design is not on the axis center of the two coaxial wheels and the air flow effect, it will fall in one direction when standing.
To prevent the robot from falling down, it is necessary to add a suitable opposite direction force in the falling direction.
This suitable reverse direction force is provided by a DC geared motor that drives two wheels.
How does a gear DC motor provide a reverse force?
The connection between the self-balancing robot body and the wheels is equivalent to a hinge. Figure below is a simplified diagram of a self-balancing robot.
If the wheel accelerates to the right, due to the effect of inertia, the center of gravity of the body will be subjected to a leftward inertial force F.
Just like on the car, if the car accelerates, the person on the car will lean backwards.
But how big is the force in the opposite direction?
Now we come to analyze the force of the self-balancing robot body at an oblique angle θ.
From the force analysis, we can get the Force balance equation below：
So can turn into the restoring force of robot:
From the force balance equation, the reverse direction force is determined by the acceleration a of the wheel; the magnitude of the acceleration a is determined by the inclination angle θ of the robot body.
Thus, if we know the angle of the inclination angle θ, and then control the value of the acceleration a according to the magnitude of θ;
It seems that the self-balancing robot will not fall down, that is, a negative feedback control.
However, it is difficult for the robot to stand upright and stably through simply adjusting the acceleration a by the inclination angle θ.
Because the value of a is difficult to adjust to the exact value, which is often greater than the appropriate value. So the robot tends to the other direction, swing back and fort in the vertical position, with a larger amplitude.
To stabilize the robot return to the vertical position as soon as possible, you need to increase the damping force.
Supposed that the negative feedback control is that the wheel acceleration a is proportional to the inclination angle θ, and the ratio is k1. That is:
Linearize the formula:
because the inclination angle θ is relatively small, get:
The increased damping force is proportional to the speed of the declination;
The factor of proportionality is k2, and the direction is opposite.
Then change the formula into:
Then can get the final arithmetic of wheel acceleration a:
θ: inclination angle ; θ': angle speed
K1; k2: the factor of proportionality.
It is also Kp and Kd in the PD algorithm adjustment of the robot upright balance described later.
Up to now, in theory, the balance robot can be balanced upright.
Source Code:
#include <MsTimer2.h> //internal timer 2
#include <PinChangeInt.h> //this library can make all pins of arduino REV4 as external interrupt
#include <MPU6050.h> //MPU6050 library
#include <Wire.h> //IIC communication library
MPU6050 mpu6050; //Instantiate an MPU6050 object; name mpu6050
int16_t ax, ay, az, gx, gy, gz; //Instantiate an MPU6050 object; name mpu6050
//TB6612 pins
const int right_R1=8;
const int right_R2=12;
const int PWM_R=10;
const int left_L1=7;
const int left_L2=6;
const int PWM_L=9;
///////////////////////angle parameters//////////////////////////////
float Angle;
float angle_X; //calculate the inclined angle variable of X-axis by accelerometer
float angle_Y; //calculate the inclined angle variable of Y-axis by accelerometer
float angle0 = 1; //Actual measured angle (ideally 0 degrees)
float Gyro_x,Gyro_y,Gyro_z; //Angular angular velocity for gyroscope calculation
///////////////////////angle parameters//////////////////////////////
///////////////////////Kalman_Filter////////////////////////////
float Q_angle = 0.001; //Covariance of gyroscope noise
float Q_gyro = 0.003; //Covariance of gyroscope drift noise
float R_angle = 0.5; //Covariance of accelerometer
char C_0 = 1;
float dt = 0.005; // The value of dt is the filter sampling time.
float K1 = 0.05; // a function containing the Kalman gain is used to calculate the deviation of the optimal estimate
float K_0,K_1,t_0,t_1;
float angle_err;
float q_bias; //gyroscope drift
float accelz = 0;
float angle;
float angleY_one;
float angle_speed;
float Pdot[4] = { 0, 0, 0, 0};
float P[2][2] = {{ 1, 0 }, { 0, 1 }};
float PCt_0, PCt_1, E;
//////////////////////Kalman_Filter/////////////////////////
//////////////////////PD parameters///////////////////////////////
double kp = 34, ki = 0, kd = 0.62; //Angle loop parameter
double setp0 = 0; //Angle balance point
int PD_pwm; //angle output
float pwm1=0,pwm2=0;
//void anglePWM();
void setup()
{
//set the control motor’s pin to OUTPUT
pinMode(right_R1,OUTPUT);
pinMode(right_R2,OUTPUT);
pinMode(left_L1,OUTPUT);
pinMode(left_L2,OUTPUT);
pinMode(PWM_R,OUTPUT);
pinMode(PWM_L,OUTPUT);
//Initial state value
digitalWrite(right_R1,1);
digitalWrite(right_R2,0);
digitalWrite(left_L1,0);
digitalWrite(left_L2,1);
analogWrite(PWM_R,0);
analogWrite(PWM_L,0);
// Join I2C bus
Wire.begin(); //Join the I2C bus sequence
Serial.begin(9600); //open serial monitor, set the baud rate to 9600
delay(1500);
mpu6050.initialize(); //initialize MPU6050
delay(2);
//5ms use timer2 to set the timer interrupt (Note: using timer2 will affect the PWM output of pin3 pin11.)
MsTimer2::set(5, DSzhongduan); //5ms execute the function DSzhongduan once
MsTimer2::start(); // start the interrupt
}
void loop()
{
Serial.print("angle = ");
Serial.println(angle);
Serial.print("Angle = ");
Serial.println(Angle);
/*Serial.print("Gyro_x = ");
Serial.println(Gyro_x);
Serial.print("K_Gyro_x = ");
Serial.println(angle_speed);*/
//Serial.println(PD_pwm);
//Serial.println(pwm1);
//Serial.println(pwm2);
}
/////////////////////////////////interrupt////////////////////////////
void DSzhongduan()
{
sei(); //Allow overall interrupt
mpu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //IIC to get MPU6050 six-axis data ax ay az gx gy gz
angle_calculate(ax, ay, az, gx, gy, gz, dt, Q_angle, Q_gyro, R_angle, C_0, K1); //get angle and Kalman_Filter
PD(); // angle loop of PD control
anglePWM();
}
///////////////////////////////////////////////////////////
/////////////////////////////angle calculation///////////////////////
void angle_calculate(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz,float dt,float Q_angle,float Q_gyro,float R_angle,float C_0,float K1)
{
Angle = -atan2(ay , az) * (180/ PI); //Radial rotation angle calculation formula; negative sign is direction processing
Gyro_x = -gx / 131; //The X-axis angular velocity calculated by the gyroscope; the negative sign is the direction processing
Kalman_Filter(Angle, Gyro_x); // Kalman Filter
//Rotation Angle Z axis parameter
Gyro_z = -gz / 131; //Z-axis angular velocity
//accelz = az / 16.4;
float angleAx = -atan2(ax, az) * (180 / PI); //Calculate the angle with the x-axis
Gyro_y = -gy / 131.00; //Y-axis angular velocity
Yiorderfilter(angleAx, Gyro_y); //first-order filter
}
////////////////////////////////////////////////////////////////
///////////////////////////////KalmanFilter/////////////////////
void Kalman_Filter(double angle_m, double gyro_m)
{
angle += (gyro_m - q_bias) * dt; //Prior estimate
angle_err = angle_m - angle;
Pdot[0] = Q_angle - P[0][1] - P[1][0]; //Differential of azimuth error covariance
Pdot[1] = - P[1][1];
Pdot[2] = - P[1][1];
Pdot[3] = Q_gyro;
P[0][0] += Pdot[0] * dt; //A priori estimation error covariance differential integral
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;
//Intermediate variable of matrix multiplication
PCt_0 = C_0 * P[0][0];
PCt_1 = C_0 * P[1][0];
//Denominator
E = R_angle + C_0 * PCt_0;
//gain value
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0; //Intermediate variable of matrix multiplication
t_1 = C_0 * P[0][1];
P[0][0] -= K_0 * t_0; //Posterior estimation error covariance
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0;
P[1][1] -= K_1 * t_1;
q_bias += K_1 * angle_err; //Posterior estimate
angle_speed = gyro_m - q_bias; //The differential of the output value gives the optimal angular velocity
angle += K_0 * angle_err; ////Posterior estimation to get the optimal angle
}
/////////////////////first-order Filter/////////////////
void Yiorderfilter(float angle_m, float gyro_m)
{
angleY_one = K1 * angle_m + (1 - K1) * (angleY_one + gyro_m * dt);
}
//////////////////angle PD////////////////////
void PD()
{
PD_pwm = kp * (angle + angle0) + kd * angle_speed; //PD angle loop control
}
////////////////////////////PWM end value/////////////////////////////
void anglePWM()
{
pwm2=-PD_pwm; //The final value assigned to the motor PWM
pwm1=-PD_pwm;
if(pwm1>255) //limit PWM value not greater than 255
{
pwm1=255;
}
if(pwm1<-255)
{
pwm1=-255;
}
if(pwm2>255)
{
pwm2=255;
}
if(pwm2<-255)
{
pwm2=-255;
}
if(angle>80 || angle<-80) //When the self-balancing trolley’s tilt angle is greater than 45 degrees, the motor will stop.
{
pwm1=pwm2=0;
}
if(pwm2>=0) //determine the motor’s steering and speed by the positive and negative of PWM
{
digitalWrite(left_L1,LOW);
digitalWrite(left_L2,HIGH);
analogWrite(PWM_L,pwm2);
}
else
{
digitalWrite(left_L1,HIGH);
digitalWrite(left_L2,LOW);
analogWrite(PWM_L,-pwm2);
}
if(pwm1>=0)
{
digitalWrite(right_R1,LOW);
digitalWrite(right_R2,HIGH);
analogWrite(PWM_R,pwm1);
}
else
{
digitalWrite(right_R1,HIGH);
digitalWrite(right_R2,LOW);
analogWrite(PWM_R,-pwm1);
}
}
Test Result
Installed well the balance car, upload the source code and power on; turn the power switch ON.
The balance car will stand upright on the desktop.
Then open the Arduino IDE, set the baud rate to 9600, the serial monitor will pop up the inclined value before filtering and after filtering.
- Self-balancing Car Kit For Arduino Robot Tutorial
- Self-balancing Car Kit For Arduino Robot Project 1: Getting Started with Main Board and ARDUINO
- Self-balancing Car Kit For Arduino Robot Project 2: Button and Buzzer
- Self-balancing Car Kit For Arduino Robot Project 3: TB6612 Motor Driving
- Self-balancing Car Kit For Arduino Robot Project 4: Hall Encoder Test
- Self-balancing Car Kit For Arduino Robot Project 5: Internal Timer Interrupt
- Self-balancing Car Kit For Arduino Robot Project 6: Bluetooth Test
- Self-balancing Car Kit For Arduino Robot Project 7: MPU6050 Test
- Self-balancing Car Kit For Arduino Robot Project 8: Calculating Inclined Angle And Angular Velocity Values
- Self-balancing Car Kit For Arduino Robot Project 9: PID Principle
- Self-balancing Car Kit For Arduino Robot Project 10: Upright Loop Adjustment
- Self-balancing Car Kit For Arduino Robot Project 11: Speed Loop Adjustment
- Self-balancing Car Kit For Arduino Robot Project 12: Steering Loop Control
- Self-balancing Car Kit For Arduino Robot Project 13: Bluetooth Control
- Self-balancing Car Kit For Arduino Robot Project 14: Adjusting Balance Angle and Bluetooth Control