Preforming a Wheelie

Instead of going the traditional route of doing localization, I was inspired by Stephen Wagner’s final project of balancing the car as an inverted pendulum. However, he was able to only get the car to balance when it started near a vertical position. In his final thoughts, he stated that it would be cool if a future reader was able to get the car to flip up on its own and balance itself. Therefore, I decided to form a group with Aravind Ramaswami and Nita Kattimani to tackle this challenge.

Approach

To make a robot flip up on its own and balance itself, we first modeled the robot as a non-linear dynamic model and linearized it about its its vertical equilibrium point. Using this model, we constructed a Kalman Filter observer to estimate the state of the robot. We also used pole placement to derive state space controller gains that stabalize the system. To actually get the car to flip upward, we created an open loop controller that causes the car to flip. Finally, we created a state machine which activates all of these pieces at the correct time to allow the car to successfully preform the maneuver.

System Modeling

Problem Setup

We modeled our car balancing problem as a wheel with a rod attached to it. Therefore, we can describe our system using the following state vector consisting of the cart’s x position, velocity, theta, and angular velocity.

Then, we can define the following parameters:

With these parameters, we can define the following quantities for the position of the center of mass of the system.

Energy Definitions

We are going to take a lagrangian mechanics approach to deriving the equations of motion of our system. In particular, we can express the kinetic energy of the wheel using the following expression.

Next, we can express the kinetic energy of the rod using the following expression.

Finally, we can express the potential energy of the rod with the expression below.

With these expressions, the lagrangian can be calculated by substracting the kinetic energy from the potential energy.

Euler-Legrange Equation

Next, we need to apply the Euler-Legrange equation to each coordinate of our system (x and theta).

theta:

Linearizing Equations of Motion

The coupled differential equations for x and theta are shown below.

We will linearize these equations about an upright position which in our case is theta = 0. To do this, we will make the following small angle approximations.

This gives us the following linear differential equations.

State Space Model

We have uncoupled equations for both theta and x. Unfortunately, we have no good way of measuring our x position. Therefore, we will disregard our x equation and only focus on the theta equation. The theta equation can be written in the following state space form.

Car Controller

Controller Design

With a state space model of our system, we next need to check whether our system is controllable and observable given measurements of theta (IMU) and angular velocity (theta).

Since both matrices have a rank of 2, the system is indeed controllable and observable. Therefore, we can place the poles of our system wherever we want. We modeled our physical system in the following form:

We selected that alpha_1 = 6.21 and alpha_2 = 60. Note that these parameters were not rigerously selected and were in fact a little bullshitted. To actually preform pole placement, we descretized the system in MATLAB using dt = 0.017s and used the pole placement command to determine gains corresponding to lambda = 0.87,0.75. When implementing the controller in reality, we found that these values worked best. They led to a gain of 0.04 on theta and 0.008 on angular velocity. However, we reduced the derivative gain to 0.002 as 0.008 amplified our sensor noise a little bit too much. This gives us our final gains of 0.04 on theta and 0.008 on position.

Controller Implementation

The controller was implemented using the code below. Note that since our state vector consists of theta and theta dot, our controller is effectively a PD controller. Therefore, I essentially took my code from lab 6 and tweaked the gains as shown below.

Note that I added an extra agrument om_dot corresponding to the angular velocity of the car. In lab 6, I did not have this as I integrated the DMP values to determine the angular velocity. However, I realized that using the gyroscope produced a much less noisy estimate of angular velocity so I created an arguement that accepts that.

Here is a video hilighting the robustness of the controller.

Kalman Filter

The Kalman filter code that we used is basically identical to the code used in Lab 7 as shown below.

We simply tweaked the system matrices to be the ones we derived above. In addition, we selected our sigma_u matrix to be very large in comparison to our sigma z matrix as we trusted our sensors much more than our model. (Note the bullshitting of our parameters described above) A much more detailed description of sigma u and sigma z can be found in the lab 7 writeup.

Open Loop Control and State Machine

Unfortunately, our controller can only stabalize the car once it has reached about 30 degrees in angle. (We determined this from experimentation) Therefore, we need a series of open loop commands which will drive the car at high speed, break, and then reverse the car so that it initiates a flip. Therefore, we developed the following overall state machine for our car which will allow it to preform the flip.

State Machine Drawing

Essentially, the car begins in an idle state of rest. Then, when a Delay_Stop command (a python BLE command) is called, the car enters FORWARD where is drives forward at full speed. After about 272ms, the car enters BREAK where the car locks its motors for another 100ms. Then, the car enters REVERSE for 270ms. Finally the car enters STOP where the car stops its motors. During this entire process, as soon as the car’s angle exceeds 30 degrees (the error is less than 60 degrees), the controller and Kalman Filter activates causing the car to balance itself on its wheels.

This is implemented in Arduino as shown below. Note that Flip0, Flip1,Flip2, and Flip3 correspond to FORWARD, BREAK, REVERSE, and STOP respectively.

When the bank angle exceeds 30 degrees, the following code is executed activating the controller and Kalman filter.

Results

Shown below are two videos of the car preforming the stunt with accociated plots of the controller effort and raw orientation and Kalman filter estimates. The Kalman filter line begins when the controller is activated on the orientation plot. There is also a red line on the command signal plot denoting when the car switches from open loop to close loop control. As shown by the vidoes and plots, the car was able to successfully initiate a flip and balance the car on two wheels.

Example 1

orientation_t2

u_sig_t2

Example 2

orientation_t3

u_sig_t3

Group Contributions

Aravind Ramaswami worked on the Dynamics modeling and Kalman filter implementation. I worked on the controller design and implementation. Nita Kattimani worked on the state machine and open loop flip machine. However, we all helped contribute to all parts of the project.

Conclusion

Overall, I had so much fun in this class. I am really glad that I got to bring together a wide variety of content learned both in this class and in others to allow our car to preform something really cool. I would also like to thank my group members Aravind Ramaswami and Nita Kattimani for helping make this final project happen.