Overview
the aim of balance car is keeping the balance of the body of the car, to modify the speed and movement direction of the body of car, the power to maintain body balance,speed, and steering comes from the two wheels of the car body.Wheel rotation is driven by two DC geared motors.therefore,from the control point of view, the vehicle body is a control object, and its control input is the rotation speed of the two motors.
- control body balance (fundamental):Keeping upright and balanced state of the car body by controlling the forward and reverse motion of two motors
- control body speed: controlling the speed of the car body by adjusting the inclination of the body.In essence, wheel speed is controlled by controlling the speed of the motor.
- control body direction:Car body steering controlled by controlling the rotational differential between two motors
After completing the above three steps, you can achieve the use of a balanced car，the essence of the above three steps is still the control of the motor. So the problem eventually became the control of the motor,only need to apply appropriate control signals on the two motors to complete the balance car control.
The following will introduce the above three points respectively.
Principle introduction
Balance control of the car body
The intuitive experience of controlling the car body’s balance comes from people’s daily life experience.People can keep a straight stick upright on the tip of the finger through a simple exercise .it needs two conditions:the palm that holding a wooden stick can be moved;The eyes can observe the inclination and tilting trend (angular speed) of the stick ,so the stick can keep upright through offset the inclination and trend of sticks when the palm move,These two conditions are indispensable,in fact, it is the negative feedback mechanism in control.
Body balance is controlled also by negative feedback.It is relatively simple compared to holding the stick upright due to two wheels on the ground of the car body.The body will only tilt in the direction of the wheel’s rolling. Keeping the body balanced through offset the trend in one dimension by controlling the rotation of the wheel.As shown.
So how make the wheels work, maintain the balance and stability of the car body? In order to balance of the body,we need to install gyroscopes on the car ,the balance car is fitted with the MPU6050 triaxial gyro module.It needs a control signal on the motor to keep the body in balance by collecting gyro data, In order to maintain balance, we use the angle PD (proportional-derivative) control algorithm.In other words, the PD controller is responsible for vehicle body balance,The PD controller is implemented by the angleout() function in the program.To keeping balance of the car body,the user only needs to change the kp (proportional term) and kd (integral term) parameters. kalmanfilter.angle and kalmanfilter.Gyro_x are Theresults after the data that is collected by MPU6050 module is filtered by kalman filter.The angle 0 is the angle when the car body keeps balance.
void anqleout()
{
balancecar.anqleoutput =kp*(kalmanfilter.angle + angle + angle0) + kd*kalmanfilter.Gyro_x;//PD
}
Body speed control
After the above steps,the car can already stand upright,however, there are two problems:
- can not move when standing upright
- can’t stand still in one place when standing upright
In order to solve the above two problems, you need to add a speed controller.On the one hand, the speed control makes the car more stable when it is at rest, and it will not tilt sideways,on the other hand, the car can be driven upright.Speed loop use a PI (Proportional Integral) controller.Speed PI controller is implemented by the speedpiout function in the BalanceCar library.
Outputs = balancecar.speedpiout(kp_speed, ki_speed, kd_speed, front, back, setp0);//speed PI
The user only needs to change the parameters of kp_speed (proportional item) and ki_speed (integral item), then can add speed control to the car. Among them, kd_speed (differential term) can be set to zero, front and back are signs of the forward and backward.
Body direction control
After the above two steps,the car has been able to remain stable and upright.it needs to add a direction ring to control the direction of the car when going straight to prevents the car from yawing due to the actual deviation of the left and right motors.In addition, the direction ring is also used for the speed adjustment of the car turning.Direction control can be controlled by PD (proportional derivative) control.The direction PD controller is implemented by the turnspin function in the BalanceCar library.
balancecar.turnspin(turnl, turnr, spinl, spinr, kp_turn, kd_turn, kalmanfilter.Gyro_z);//direction PD
The user only needs to modify kp_turn (proportional term) and kd_turn (differential term) to adjust parameters of the directional ring
Adjust the motor speed
Balance control, speed control and direction control ,all of them an output,three outputs are added to the motor through the pwma function in the BalanceCar library.At this point, the balance car is completed.
