Inverted Pendulum Calculations
The section goes through the derivation of the angle of inverted pendulum, on the robot. Throughout this document ‘theta’ represents the angle the pendulum makes with the vertical direction (standing straight up). The first part of this document shows the derivation of the angle theta, as seen by the camera. The second part of this document goes through the linearization of this very non-linear equation.
Angle Calculation
The one major assumption before starting this derivation is that, the robot’s placement in the y-direction does not affect the camera’s calculation of theta when the robot is traveling on a line in the x-direction (this fact has been derived, and may be included later). With this conclusion of the x and y-axis being ‘decoupled’ in a sense, we can look at the robot’s x-directional movement without bothering ourselves with the y-direction.
Figure 1 shows the layout of the field. The camera is positioned over the top of the field and looking down upon it. The position directly below the camera is x = 0. The position of the ball and robot (as seen by the camera) can be thought of as their visual projections onto the floor. These projections are represented by the shaded ovals in Figure 1. As stated previously, the angle that the pendulum makes is with respect to the vertical direction, and is positive for positive x-direction displacement.
Declaration of Variables:
- Bx -- The ‘projected’ x-position of the ball
- Rx -- The ‘projected’ x-position of the robot
- Hc -- The height of the camera
- Hp -- The length of the pendulum
- Hr -- The height of the robot
- θ -- The angle the pendulum makes with the vertical direction
- β -- The actual position of the robot
First looking at the projection of the robot onto the floor. As seen in Figure 2, this creates two similar triangles and leads to the equation:
Looking now at the projection of the ball (Fig. 3), again two similar triangles are created. This leads to the equation:
Combining the previous two equations to get rid of the beta term, we are left with:
We now have an equation in terms of the variables we desire; the robot’s x-position (Rx), the ball’s x-position (Bx), and the angle theta (θ). But we want theta (the dependent variable) to be the variable on the left hand side of the equation, not Bx. Solving the above equation for theta we have1:
Linearization2
The motivation for linearizing the above equation is that the robot doesn't have the arcsine and arctangent function; also there is a lot of multiplication and division of floating point numbers and this would be computationally intensive. Since we have the dependent variable theta on the left side of the equation, we can do linearization. The linearization point is chosen as Bx = Rx = 0, since this would mean the the robot is directly underneath the camera with the pendulum being vertical, which is desirable since we don’t want the robot to wander too far. Letting α be equal to the quotient term within the arcsine function, and carrying out the linearization3:
Therefore for our linearized equation we have:
For our current situation we have:
- Hc = 116 inches = 2.9464 meters
- Hp = 35.5 inches = 0.9017 meters
- Hr = 4.5 inches = 0.1143 meters
therefore:
Testing
Using Matlab we can make a plot of both the original nonlinear equation, and the calculated linear approximation in the same figure. From this figure we should be able to see the accuracy or closeness of our approximation. The way in which I plotted this data was a 3-d plot using the ‘mesh()’ command in Matlab. The code I used is:
<highlightSyntax language="matlab5">Bx = -0.5:0.05:0.5; Rx = -0.5:0.05:0.5;
Hp = 0.9017; Hc = 2.9464; Hr = 0.1143;
% initialize matricies NonLinEqu = -100*ones(length(Bx),length(Rx)); LinEqu = -100*ones(length(Bx),length(Rx));
% computation of both the linear and non-linear equations for i = 1:length(Bx)
NonLinEqu(i,1:length(Rx)) = asin((Bx(i)*(Hc-Hr)+Rx*(Hr-Hc))/ ...
(Hp*sqrt(Bx(i)^2+Hc^2)))-atan(Bx(i)/Hc)*ones(1,length(Rx));
LinEqu(i,1:length(Rx)) = -1.066*Rx + 0.7266*Bx(i)*ones(1,length(Rx));
end
% plotting mesh(Rx,Bx,real(NonLinEqu)); hold on; mesh(Rx,Bx,LinEqu);
xlabel('The Robots Position (R_x)'); ylabel('The balls Position (B_x)'); zlabel('The Angle of the Pendulum (\theta)'); </highlightSyntax>
From the figure we can view the linear and non-linear equations on the same plot although it may be a bit hard to see. The approximation is very close to the actual value over a pretty decent range about the equilibrium position; therefore it seems this approximation should work excellently for our application.
Conclusion
Having developed these equation, we now have a linear equation for determining the angle of the inverted pendulum by measuring the projected position of the robot and ball, which we can get from the camera. Also since this is a linear equation it will be easy for the robot to process.