Design of Two Wheel Inverted Pendulum: JANUARY 19 2020

Design of Two Wheel Inverted
Pendulum
JANUARY 19 2020
The importance of TWIP Robot
The basic idea of the TWIP
 Physical parameters of Two-wheel Self-balancing robots are the following:

 (wm,um,vm) The axis midpoint coordinates of the two wheels,
 (wr,ur,vr) The coordinates of right wheel,
 (wl,ul,vl) The coordinates of right wheel,
 (wb,ub,vb) The coordinate of robot’s center,
 Ε Body pitch angle,
 ϕl,r Wheel angle (l,
 R indicates left and right),
 η Body angle,
 Jβ Wheel inertia moment,
 M Body weight,
 L Distance of the center of mass from the wheel axle,
 Jα Body pitch inertia moment,
 ϕml,r DC motor angle
How to balance this unbalanced system
Control method: PID
Tuning the controller
Continue: controlling method
 In accordance with these parameters, the equations of motion of the robot could be
established by Lagrangian theory, and state equations are formed after proporate choice of
status variables, figure on the left is angle of robot tilt, figure on the right shows tiling
velocity
Angle conversion
 Angle sensor is contained in IMU 6050, the conversion relationship is as follows:

 acceler = acce_Ad−4096∗1.623/3−3
 The data acceler is x-axis digital increment, and the x-axis voltage of adxl335 is 1.623 v when x-axis is
horizontal, and digital value -3 as a compensation.
 The next step is to obtain the sine value, on the x-axis direction as follows:
 0.32sin(theta)=acceler∗3/4096
 So sine value is as follows:
 sin(theta)=acceler∗0.0022888
 Get the angle (in radians):
 theta=asin(acceler∗0.0022888).
Angle velocity conversion
 The angular velocity sensor is a gyroscopic MEMS in the 6050 IMU. The angular
velocity digital increment is as follows:
 gyro=AdcAin1−(4096∗1.23)/3
 The angular velocity value (in radians) is as follows:
 gyro∗=0.05859375∗(3.1415926/180).
Kalman filter
 double kalmanCalculate(float newAngle, float newRate,int looptime) {

 dt = looptime / 1000.0;
angle += dt * (newRate – bias); //angle = rate * timesample
 P_00 += dt * ( dt * P_11 – P_01 – P_10 + Q_angle);
P_01 -= dt * P_11;
P_10 -= dt * P_11;
P_11 += Q_gyro * dt;
 S = P_00 + R_angle;
K_0 = P_00 / S;
K_1 = P_10 / S;
 y = newAngle – angle;
angle += K_0 * y;
bias += K_1 * y;
 P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;
 return angle;
} //Kalman
Important note
 We used a sort of trial and error to tune our PID controller applied in the code, we also changed the controller from
UNO to mega for extra clock speed and the advantage of the two extra analog input ports A6&A7:
 PID stands for Proportional, Integral, and Derivative. Each of these terms provides a unique response to our self-
balancing robot.
 The proportional term, as its name suggests, generates a response that is proportional to the error. For our system,
the error is the angle of inclination of the robot.
 The integral term generates a response based on the accumulated error. This is essentially the sum of all the errors
multiplied by the sampling period. This is a response based on the behavior of the system in past.
 The derivative term is proportional to the derivative of the error. This is the difference between the current error
and the previous error divided by the sampling period. This acts as a predictive term that responds to how the robot
might behave in the next sampling loop.
 Multiplying each of these terms by their corresponding constants (i.e, Kp, Ki and Kd) and summing the result, we
generate the output which is then sent as command to drive the motor.
Tuning the PID

 Set Ki and Kd to zero and gradually increase Kp so that the robot starts to oscillate about
the zero position.
 Increase Ki so that the response of the robot is faster when it is out of balance. Ki should
be large enough so that the angle of inclination does not increase. The robot should come
back to zero position if it is inclined.
 Increase Kd so as to reduce the oscillations. The overshoots should also be reduced by
now.
 Repeat the above steps by fine tuning each parameter to achieve the best result.
Thanks for your precious time and attention

Design of Two Wheel Inverted Pendulum: JANUARY 19 2020

Uploaded by

Copyright:

Available Formats

Design of Two Wheel Inverted Pendulum: JANUARY 19 2020

Uploaded by

Document Information

Original Description:

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Design of Two Wheel Inverted Pendulum: JANUARY 19 2020

Uploaded by

Copyright:

Available Formats

Design of Two Wheel Inverted

 Physical parameters of Two-wheel Self-balancing robots are the following:

 Angle sensor is contained in IMU 6050, the conversion relationship is as follows:

 double kalmanCalculate(float newAngle, float newRate,int looptime) {

You might also like

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.