Lab12 - Path Planning and Execution

Published:

Objective

Until now, the robot has been built with debugging functions, closed loop control, the abilities to map the environment, and the localization functions. Here, we are given the court map and the robot car should traverse the 9 waypoints.

waypoints

Keywords

Localization, PID Control

Lab Tasks

This is an open ended lab task, and I mainly implemented two methods to try this lab task.

Localization

Based on the localization results of four sample waypoints from lab11, I first tried the localization method to traverse the waypoints.

The general idea is - the car first rotates the starting angle based on the last calculated control information, and then performs the translation for calculated distance. After the car reaches one waypoint, the car starts to do the observation by continuously self-rotating and getting the distance readings. Then, based on the observation after the bayes filter prediction, the control information can be calculated accordingly.

Here is how I implemented the observation function:

perform_observation_code

In this function, I use the variance of the distance readings to get the stable distance information by sampling groups of data from ToFs.

Here is how I implemented the get_cur_pose function:

get_cur_pose_code

The compute_control part is just the same as the lab11.

The main navigation function is like:

navigation_code

So, I can just pass the waypoints matrix to the navigation function and perform the waypoints traverse.

Before I pass the waypoints, I need to do the pre-process of the data like:

waypoints_data_preprocess

where I added a third column as the “starting angle” of the robot, and I set all of the angle to be 0. The whole matrix is like:

waypoints_data

The localization method is tested:

Video:

Localization Test

The localization plot is:

localization_test_prediction_plot

From the test, we can know that the performance of the localization was really bad. Although from the lab11, the precision of the localization was quite good. But in the whole run, there were much more noise for the sensors. Noise can be from the error of the robot (x, y) real location, the difference between the ToF readings and the actual distance, and the small error from the car’s orientation. All those errors would be accumulated and magnified hugely, which will result in the bad performance in the car’s run.

Localization is a good method theoretically. But it can be really affected by the noise. To improve this performace, we need to optimize the sensors and the control algorithm. However, due to the time limit and the resource constraints, I did not continue to use this method.

PID Control (Open-Loop Control)

Due to the noise effect, I decided to apply the pure PID control (i.e. open loop control). In this way, the car does not need to perform the observation rotation loop and it can avoid much more noise. However, for this method, I have to optimize my PID control for the going forward motion and the orientation.

First, as for the PID orientation control, I tested series of angles: -45, -45, -90, 45, 45, 270, 180:

pid_angle_precision

And the video:

pid_rotate_precision

which is quite good.

Second, as for the PID forward control, I tested many rounds, and here is when I explicitly required my car to go forward for 6 inches(tiles):

fd_6_tile_precision

which is also quite good.

Moreover, I also added the front safety distance control. I let my car stop within 1 inch(tile) from the wall no matter what distance is told to perform.

Here is the demo video, I “explicitly” let my car move 8 inches but it stopped in front of the wall:

fd_8_but_safety

With all those above, I tested the open loop control to traverse all the waypoints:

path_execution

which is quite good.

End

This is the end of the course ECE4160 - Fast Robots. I really learned a lot from this course. I would express my sincere gratitude to Dr. Jonathan Jaramillo, and all the TAs! Thank you guys!