Lab 12 - Path Planning

Lab 12 - Path Planning

The objective of Lab 12 is to have the robot navigate through a set of waypoints in that environment as quickly and accurately as possible.

Prelab

This lab is a compilation of all the tasks performed in previous labs, so the prelab consists on reviewing past labs.

Path Planning

The lab handout states that this lab is open ended. Therefore, I decided to implement a path planning algorithm, where the robot has to hit a series of waypoints in the map in order. I divided this lab in a few subtasks.

1. Rotation and Forward Movement Commands

In previous labs, the PID controller was set to move the robot to the wall up to a certain distance, and to rotate the robot so that it stays in a desired position. I modified the controller so that we can ask the robot to move certain distance forward, and rotate certain angles.

The commands for Bluetooth are ble.send_command(CMD.MOVE_DISTANCE,"300|0.0001|0|0.01") and ble.send_command(CMD.MOVE_ANGLE,"180|5|0|1.2"), where the first argument is the distance/angle we want to move (in mm/degrees); and the last three arguments are the Kp, Ki and Kd parameters of the PID controller.

In the case of the forward movement, the PID controller was modified so that the Set Point is the difference between the initial distance to the wall and the desired forward distance:

1
2
3
4
5

pid.setPoint = pid.first_distance - pid.setDistance;

/*...*/

pid.error = pid.curr_distance - pid.setPoint;

The controller ends when the error is 0 for at least 5 consecutive cycles. Note that errors smaller than 5mm are considered 0.

1
2
3
4
5
6

if(pid.last_n_error_0 >= 5){
    pid.run_distance = false;
    pid.pid = 0;
    brake();
    Serial.println("Test ended. Brake");
}

The orientation controller is very similar to the modifications used for mapping the environment. However, this time the motor only rotates once. The Set Point is defined as the desired angle, and the initial yaw is initialized to 0.

1
2
3
4
5
6

ori.yaw = 0;
ori.setPoint = ori.setAngle;

/*...*/

ori.error = ori.setPoint - ori.yaw;

Again, the controller ends when the error is 0 for at least 5 consecutive cycles. Note that errors smaller than 2 degrees are considered 0.

1
2
3
4
5
6

if(ori.last_n_error_0 >= 5){
    ori.run_angle = false;
    ori.pid = 0;
    brake();
    Serial.println("Test ended. Brake");
}

With all these modifications we can ask the robot to move forward a certain distance, or to rotate certain angle. If we want to set the distance/angle as the value in a variable, we can create a string with that value, and then send the command with that string.

1
2
3
4
5
6
7

# MOVE DISTANCE COMMAND
distance_command = "{}|0.0001|0|0.01".format(int(distance))
ble.send_command(CMD.MOVE_DISTANCE, distance_command)

# ROTATE ANGLE COMMAND
angle_command = "{}|5|0|1.2".format(int(delta_theta_deg))
ble.send_command(CMD.MOVE_ANGLE, angle_command)

2. Moving from one Point to another

Now that we have the necessay commands to move certain distance, or rotate certain angle, we need to create a function that moves the robot from one point to another. This function will receive the initial position as (x,y,theta) and the desired position as (x,y). The function obtains the rotation needed to go from one point to another, as well as the distance between them. Note that the robot might be pointig to a different position than the desired one, therefore we need the initial rotation. Once we have computed those paremeters, we can ask the robot to move accordingly. To make sure each movement is done before running the next one, a user input is needed before continuing.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

def move_robot(initial_position, desired_position):
    x0, y0, alpha0 = initial_position
    x1, y1 = desired_position

    # Step 1: Calculate the angle to the target point
    theta = np.arctan2(y1 - y0, x1 - x0)
    
    # Step 2: Calculate the rotation angle needed
    delta_theta = theta - np.radians(alpha0)
    
    # Normalize the rotation angle to the range [-pi, pi]
    delta_theta = (delta_theta + np.pi) % (2 * np.pi) - np.pi
    
    # Step 3: Calculate the distance to the target point
    distance = np.hypot(x1 - x0, y1 - y0)
    distance = distance*1000
    
    # Convert delta_theta to degrees (this value will be used outside the function)
    global delta_theta_deg
    delta_theta_deg = np.degrees(delta_theta)
    
    # Output the commands
    angle_command = "{}|5|0|1.2".format(int(delta_theta_deg))
    ble.send_command(CMD.MOVE_ANGLE, angle_command)
    input(f"Rotating by {delta_theta_deg:.2f} degrees. Press Enter when rotation has finished...")
    
    distance_command = "{}|0.0001|0|0.01".format(int(distance))
    ble.send_command(CMD.MOVE_DISTANCE, distance_command)
    input(f"Moving forward by {distance:.2f} units. Press Enter when movement has finished...")

    ble.send_command(CMD.BRAKE,"")

As can be seen from the code, this function receives two postions: the initial, and the destiantion. It computes the necessary rotation and distance between points. Then, the robot rotates so that it faces the destination. The user has to make sure the rotation is done. Then, after pressing Enter, the robot will move forward to the end point. User input is needed again to make sure that the movement has finished. Last, the robot stops completely.

The last step is obtaining the current postion, as well as the new one.

3. Desired Waypoints

The waypoints the robot has to hit were given in the lab handout. They can be easily stored in an array that will be used later. Note that the coordinates are converter to meters, as the whole system operates in meters.

1
2

waypoints = [(-4, -3), (-2, -1), (1, -1), (2, -3), (5, -3), (5, -2), (5, 3), (0, 3), (0, 0)]
waypoints = np.array(waypoints)*0.3048

4. Current Position

We can use the localization function from Lab 11 to get the current position of the robot. The results are stored in the variable loc.bel. Using the get_max() function, we can obtain the most probable state from the beliefs. Then, we can transform the state to our system of coordinates (x,y,theta).

1
2

argmax_bel = get_max(loc.bel)
current_belief = loc.mapper.from_map(*argmax_bel[0])

Now that we know the desired and current position, we can ask the robot to move from one point to another.

1
2
3
4

initial_position = current_belief  # x0, y0, alpha0 (alpha0 in degrees)
desired_position = waypoints[i]    # x1, y1

move_robot(initial_position, desired_position)

5. Integration with Bayes Filter

The last step is to integrate everything together. The robot will start in the first waypoint, and we will run the update step of the Bayes Filter (with uniform prior beliefs) to determine its current position (even though we already know it). The iteration variable i is initialized to 1, as that is the index of the next waypoint we want to hit. Now, we know the current position (given by current_belief), and the desired one (given by waypoints[i]). The robot will move from one point to another.

Next, we will run another iteration of the Bayes Filter. This time, we will incorporate the prediction step. This step receives the previous and current poses, that we already know. The previous pose is given by current_belief, that hasn’t been updated yet. For the current pose, we need to add the angle of the robot to the position given by waypoints[i].

1
2
3
4
5

global delta_theta_deg
current_odom = waypoints[i]
current_odom = np.insert(current_odom, 2, round(current_belief[2]+delta_theta_deg), axis=0)

prev_odom = current_belief

The full Bayes Filter will be executed, and the new current position will be obtained.

1
2
3
4
5
6
7
8
9
10
11
12
13

# Prediction Step
loc.prediction_step(current_odom, prev_odom)

# Get Observation Data by executing a 360 degree rotation motion
await loc.get_observation_data()

# Run Update Step
loc.update_step()
loc.plot_update_step_data(plot_data=True)

# Get Current Position
argmax_bel = get_max(loc.bel)
current_belief = loc.mapper.from_map(*argmax_bel[0])

After each iteration, the iteration variable will be updated so that the robot moves to the next position (i+=1).

Results

The following video shows the results of the best run. Note that, as in Lab 11, the robot did not always predict its current position accurately. Therefore, to improve the results, I decided to move the robot to the position it thinks it is, as the algorithm wouldn’t work otherwise. Additionally, the ToF sensor sometimes did get wrong readings if the wall is to far away, so the forward movement wasn’t always accurate. To improve this, I temporarily added a wall close to the desired point when moving forward, so that the readings were accurate.

As can be seen, the robot’s prediction isn’t always exact, but is very close: usually one tile away. The movement of the robot is often quite precise, and it can get to every waypoint quite accurately. However, when moving to the top right most point, as the distance is too large, the forward movement wasn’t accurete. For that reason, the robot was manually place in the desired point. Last, the robot did not exactly end in the last point (it ended a tile away), which, again, can be explain with the ToF sensor accuracy when the distance is long (Note that my ToF sensor is set on Long Distance Reading Mode, as discussed in Lab 3).

Conclusion

The Path Planning algorithm developed in this lab is a powerful tool to follow a path, given a series of waypoints. A more robust and reliable ToF sensor can help the robot move and predict its state more accurately. A more precise robot movement, can also significantly improve the results. Even with the current limitations, the robot was able to follow the path quite accurately (with some external help).