Lab 10: Path Planning and Execution
Objective
The objective of this lab is to have the robot be placed randomly inside a map and find its goal location in the simulator and in real life.
Path Planning in Simulator
To start, I converted my real map to a grid map with the following code below. The ones indicate wall or an obstacle and zeros indicate free space that the robot can roam around in.
I've decided to use Bug 0 algorithm in order to find the path. Bug 0 algorithm is a simple concept where the robot heads to the direction of the goal, while keeping in mind where the walls and obstacles are. If the robot faces a large obstacle, it will follow the edge of the obstacle until the it can head toward the goal again. The algorithm can be seen below.
Using the code, I was able to successfully find all the waypoints that are required for robot's motion.
I used feedback linearization in order to make the robot move. The function outputs linear and angular velocity, which makes it easier to put in controls for the simulated robot. The code can be seen below.
Now that I have a way to input commands into the robot, I was able to visit each waypoint by iterating through the list of waypoints.
Before I tested the code on the simulator, I had to change the simulation map to match the one given above. This was done in the playground.world file.
With the map adjusted properly, I tested my algorithm, which turns out works pretty well. In the beginning of the video, you can see the path with the red dot as the starting point and the green dot as the end point. The initial path from the first point to the path is missing only because that was necessary for plotting the path but not necessary for path planning.
Path Planning in Real Life
For implementation of path planning, I utilized both jupyter lab algorithm as converted the commands to the arduino code to be run on the robot. Initially, I wanted to run the algorithm online and base the commands from feedback linearization, which is what I used for the simulation. However, since I am unable to communicate to the robot from jupyter lab, I decided that I would compute all the commands necessary to navigate the robot and then implement those commands in arduino ide.
Grid localization allows the robot to move in right angles and straight lines. I decided to take that advantage and code the robot such that it moves in only 4 directions: north, south, east, and west with respect to the global frame (not the robot). In Jupyter, I converted the waypoints to directions that the robot would need to take. Using, the directions, the robot would move north, south, east, or west at each "grid" on the real map. The picture below is the path that I want the robot to follow in real life. The red indicates the starting point and green indicates the end point.
Here is the code that was implemented to move the robot depending on the direction. I would turn the robot until it is in the right orientation and head straight to the next grid space.
if (direct == 1) //going north
{
Serial.println("North");
if (yaw_g > 92) {
while (yaw_g > 91) {
myICM.getAGMT();
dt = (micros() - last_time) / 1000000;
last_time = micros();
yaw_g = yaw_g + myICM.gyrZ() * dt;
//go counterclockwise
myMotorDriver.setDrive(0, 0, motor_speed);
myMotorDriver.setDrive(1, 0, motor_speed);
}
}
else if (yaw_g < 88) {
//go clockwise
while (yaw_g < 89) {
myICM.getAGMT();
dt = (micros() - last_time) / 1000000;
last_time = micros();
yaw_g = yaw_g + myICM.gyrZ() * dt;
myMotorDriver.setDrive(0, 1, motor_speed);
myMotorDriver.setDrive(1, 1, motor_speed);
}
}
else {
myMotorDriver.setDrive(0, 0, 0);
myMotorDriver.setDrive(1, 0, 0);
delay(200);
myMotorDriver.setDrive(0, 0, motor_speed);
myMotorDriver.setDrive(1, 1, motor_speed);
delay(100);
myMotorDriver.setDrive(0, 0, 0);
myMotorDriver.setDrive(1, 0, 0);
i++;
}
}
//=========================================================================================================
if (direct == 2)//going south
{
Serial.println("South!");
if (yaw_g > 272) {
while (yaw_g > 272) {
myICM.getAGMT();
dt = (micros() - last_time) / 1000000;
last_time = micros();
yaw_g = yaw_g + myICM.gyrZ() * dt;
//go counterclockwise
myMotorDriver.setDrive(0, 0, motor_speed);
myMotorDriver.setDrive(1, 0, motor_speed);
}
}
else if (yaw_g < 268) {
//go clockwise
while (yaw_g < 268) {
myICM.getAGMT();
dt = (micros() - last_time) / 1000000;
last_time = micros();
yaw_g = yaw_g + myICM.gyrZ() * dt;
myMotorDriver.setDrive(0, 1, motor_speed);
myMotorDriver.setDrive(1, 1, motor_speed);
}
}
else {
myMotorDriver.setDrive(0, 0, 0);
myMotorDriver.setDrive(1, 0, 0);
delay(200);
myMotorDriver.setDrive(0, 0, motor_speed);
myMotorDriver.setDrive(1, 1, motor_speed);
delay(100);
myMotorDriver.setDrive(0, 0, 0);
myMotorDriver.setDrive(1, 0, 0);
i++;
}
}
//=========================================================================================================
if (direct == 3)//going east
{
Serial.println("East!");
if (yaw_g > 2) {
while (yaw_g > 2) {
myICM.getAGMT();
dt = (micros() - last_time) / 1000000;
last_time = micros();
yaw_g = yaw_g + myICM.gyrZ() * dt;
//go counterclockwise
myMotorDriver.setDrive(0, 0, motor_speed);
myMotorDriver.setDrive(1, 0, motor_speed);
}
}
else if (yaw_g < -2) {
//go clockwise
while (yaw_g < -2) {
myICM.getAGMT();
dt = (micros() - last_time) / 1000000;
last_time = micros();
yaw_g = yaw_g + myICM.gyrZ() * dt;
myMotorDriver.setDrive(0, 1, motor_speed);
myMotorDriver.setDrive(1, 1, motor_speed);
}
}
else {
myMotorDriver.setDrive(0, 0, 0);
myMotorDriver.setDrive(1, 0, 0);
delay(200);
myMotorDriver.setDrive(0, 0, motor_speed);
myMotorDriver.setDrive(1, 1, motor_speed);
delay(100);
myMotorDriver.setDrive(0, 0, 0);
myMotorDriver.setDrive(1, 0, 0);
i++;
}
}
//=========================================================================================================
if (direct == 4)//goin west
{
Serial.println("West!");
if (yaw_g > 182) {
while (yaw_g > 182) {
myICM.getAGMT();
dt = (micros() - last_time) / 1000000;
last_time = micros();
yaw_g = yaw_g + myICM.gyrZ() * dt;
//go counterclockwise
myMotorDriver.setDrive(0, 0, motor_speed);
myMotorDriver.setDrive(1, 0, motor_speed);
}
}
else if (yaw_g < 178) {
//go clockwise
while (yaw_g < 178) {
myICM.getAGMT();
dt = (micros() - last_time) / 1000000;
last_time = micros();
yaw_g = yaw_g + myICM.gyrZ() * dt;
myMotorDriver.setDrive(0, 1, motor_speed);
myMotorDriver.setDrive(1, 1, motor_speed);
}
}
else {
myMotorDriver.setDrive(0, 0, 0);
myMotorDriver.setDrive(1, 0, 0);
delay(200);
myMotorDriver.setDrive(0, 0, motor_speed);
myMotorDriver.setDrive(1, 1, motor_speed);
delay(100);
myMotorDriver.setDrive(0, 0, 0);
myMotorDriver.setDrive(1, 0, 0);
i++;
}
}
Here is a video of how it went:
The two tapes indicate the starting point and the end point
Possible Improvements Given Infinite Time
- The bug algorithm is a simple method for path planning, but definitely not the best since it can get stuck if there were maps that were more of a maze. This algorithm would not be the best method for navigating through complicated maps.
- I would implement feedback linearization to the actual robot. It provided accurate controls for the simulator and it would have been interesting to see it on the real robot. Implementing it through bluetooth would have taken me a long time personally, which is why I decided to do the directional method for the actual robot.
- If the real robot could localize at each timestep, I believe it would have improved its trajectory. The current method was not bad, but depends purely on the yaw angle to determine its direction. It is following a step by step command instead of localizing at everystep of the way, which would give a better sense of where to go next.