Welcome to Self-balancing Robot Car repository

08.Car upright control (PD)

Car upright control (PD)

Car upright control (PD)Physical analysisPD controllerImplementation codeSoftware code

The tutorial mainly introduces the upright control of the balance car.

Physical analysis

Assuming that when the car tilts to the right, the wheels of the car are controlled to make the car accelerate, and the acceleration is a.

Force analysis diagram

Force analysis

If the force of the inverted pendulum is analyzed with the wheel of the car as the reference system (non-inertial reference system), the ball above the car will be subjected to an inertial force that is opposite to the acceleration of the wheel and proportional to the acceleration.

F = m g s i n θ - m a c o s θ \approx m g θ - m k_{1} θ


xxxxxxxxxx
θ is very small → sinθ = 0, cosθ = 1; k_1 is the proportionality coefficient between acceleration a and the inclination angle of the car

If k1 > g, the direction of the restoring force is opposite to the direction of displacement.

In order to allow the inverted pendulum to return to the vertical position and stabilize as quickly as possible, it is necessary to increase the damping force; the increased damping force is proportional to the speed of the deflection angle and in the opposite direction, so the formula can be changed to:

F = m g θ - m k_{1} θ - m k_{2} θ^{'} = m g θ - m (k_{1} θ + k_{2} θ^{'})


xxxxxxxxxx
(k_2 is the proportionality coefficient between damping force and angular velocity, θ' is the differential of the angle (i.e. angular velocity))

According to the inverted pendulum model, the algorithm for controlling the acceleration of the car wheel can be obtained: as long as k1 > g, k2 > 0, the car can be balanced.

a = k_{1} θ + k_{2} θ^{'}

According to the above content, establish the proportional differential negative feedback control of the speed to make the balance car maintain stable conditions through closed-loop control.


xxxxxxxxxx
The MPU6050 measures the tilt angle θ and angular acceleration θ' of the balance car. The PD controller calculates the control signal based on the measured tilt angle and angular acceleration to adjust the speed of the balance car so that the vehicle can remain upright.

PD controller

The main function of differential (D) control in PID control is to reduce the dynamic error and oscillation of the system, and the main function of integral (I) control is to eliminate static error.

Proportional control (P) can quickly reach the set speed, and differential (D) control is equivalent to damping force, which can effectively suppress the vibration of the car model.

Implementation code


x
int Balance_PD(float Angle,float Gyro)
{
float Angle_bias,Gyro_bias;
int balance;
Angle_bias=Mid_Angle-Angle; //Find the median value of the balance angle and mechanical related
Gyro_bias=0-Gyro;
balance=-Balance_Kp/100*Angle_bias-Gyro_bias*Balance_Kd/100; //Calculate the motor PWM PD control for balance control kp is the P coefficient kd is the D coefficient

return balance;
}

Software code

Balance car PID control basics: 08-13 tutorial only provides one project file.


Product supporting materials source code path: Attachment → Source code summary → 3.PID_Course → 08-13.Balanced_Car_PID

Balance principle of car

Car speed control (PI)