Bug Zero Algorithm Tutorial: Motion to Goal with Webots and Python

Introduction to Motion to Goal and Bug Zero

In mobile robotics, navigating from a start point to a goal while avoiding obstacles is fundamental. This tutorial guides you through implementing motion to goal and the Bug Zero algorithm using the FAIRIS Lite robot in Webots 2023b with an RGBD camera. Whether you're a student tackling CDA 4621 Lab 3 or a hobbyist exploring robot navigation, these concepts are essential. We'll use Python code examples and connect them to real-world scenarios, like how autonomous delivery robots navigate crowded sidewalks or how AI-powered vacuum cleaners avoid furniture.

Setting Up the Environment

Ensure you have Webots 2023b and the FAIRIS Lite robot model. The robot is equipped with an RGBD camera that provides both image data and object recognition. Access the camera via robot.rgb_camera. The function getRecognitionObjects() returns a list of RecognitionObject instances, each containing attributes like getId(), getPosition(), getSize(), getPositionOnImage(), getSizeOnImage(), and getColors(). This sensor is crucial for detecting the goal (a yellow cylinder) and obstacles.

Example: Detecting Objects

rec_objects = robot.rgb_camera.getRecognitionObjects()
if len(rec_objects) > 0:
    landmark = rec_objects[0]
    object_id = landmark.getId()
    object_position = landmark.getPosition()  # [X, Y, Z] relative to camera
    object_size = landmark.getSize()  # [Y, Z]
    object_position_on_image = landmark.getPositionOnImage()  # [X, Y] pixels
    object_size_on_image = landmark.getSizeOnImage()  # [X, Y] pixels
    object_color = landmark.getColors()  # [R, G, B]

This code checks for any detected object and extracts its properties. For motion to goal, we focus on the goal's position on the image to center it.

Motion to Goal: Step-by-Step

The goal is a yellow cylinder. The robot must detect it, rotate to center it in the camera frame, move forward, and stop 0.5 meters away.

Step 1: Detect the Goal

Use the camera's object recognition. Filter by color or ID if multiple objects exist. For simplicity, assume the first detected object is the goal.

def detect_goal():
    rec_objects = robot.rgb_camera.getRecognitionObjects()
    for obj in rec_objects:
        if obj.getColors() == [1.0, 1.0, 0.0]:  # Yellow
            return obj
    return None

Step 2: Center the Goal Using PID

Get the goal's x-coordinate on the image. The image width is typically 640 pixels. Compute error = (goal_x - image_center_x). Use a proportional controller (P) to adjust rotational speed.

def center_goal(goal):
    image_width = robot.rgb_camera.getWidth()
    center_x = image_width / 2
    goal_x = goal.getPositionOnImage()[0]
    error = goal_x - center_x
    # Proportional gain
    Kp = 0.01
    angular_speed = -Kp * error
    # Set motor speeds (differential drive)
    left_speed = -angular_speed
    right_speed = angular_speed
    robot.set_motors(left_speed, right_speed)

This loop continues until the error is small (e.g., <5 pixels).

Step 3: Move Forward and Stop

Once centered, move forward using a forward PID controller based on the goal's distance (Z from getPosition). Stop when distance <= 0.5 meters.

def move_to_goal(goal):
    distance = goal.getPosition()[2]  # Z distance
    error = distance - 0.5  # desired distance
    Kp_forward = 0.5
    forward_speed = Kp_forward * error
    # Clamp speed
    forward_speed = max(min(forward_speed, 5.0), -5.0)
    robot.set_motors(forward_speed, forward_speed)
    # Stop condition
    if abs(error) < 0.05:
        robot.set_motors(0, 0)
        return True
    return False

This approach is similar to how a drone might hover at a fixed distance from a target using its camera.

Bug Zero Algorithm: Navigating Around Obstacles

The Bug Zero algorithm is a reactive path-planning method: move straight to goal; if obstacle detected, follow its boundary until the path to goal is clear again. This is akin to how a person might walk around a table in a crowded room.

State Machine

Implement two states: GO_TO_GOAL and WALL_FOLLOW. Use LIDAR or camera to detect obstacles. Here, we'll use LIDAR for distance measurements.

STATE_GO_TO_GOAL = 0
STATE_WALL_FOLLOW = 1
state = STATE_GO_TO_GOAL
while not at_goal:
    if state == STATE_GO_TO_GOAL:
        # Move toward goal using motion to goal logic
        goal = detect_goal()
        if goal is None:
            # Goal lost, rotate to find it
            robot.set_motors(-1, 1)
        else:
            # Check for obstacles using LIDAR
            if obstacle_detected():
                state = STATE_WALL_FOLLOW
                # Record distance to goal when obstacle encountered
                leave_distance = get_distance_to_goal(goal)
            else:
                center_goal(goal)
                move_to_goal(goal)
    elif state == STATE_WALL_FOLLOW:
        # Follow obstacle boundary using wall-following behavior from Lab 2
        follow_wall()
        # Check if we can resume direct path
        goal = detect_goal()
        if goal and get_distance_to_goal(goal) < leave_distance:
            state = STATE_GO_TO_GOAL

Obstacle Detection

Use LIDAR to check if any point in front is within a threshold (e.g., <0.5 meters).

def obstacle_detected():
    lidar_values = robot.lidar.getRangeImage()
    # Check front sector (e.g., indices around center)
    front = lidar_values[300:340]  # adjust based on resolution
    return min(front) < 0.5

Alternatively, use the camera's object recognition to detect obstacles as objects that are not the goal.

Wall Following

Implement a simple wall follower: keep a fixed distance to the obstacle on one side (e.g., right side). Use PID to maintain distance.

def follow_wall():
    # Get LIDAR distances on right side
    right_dist = min(robot.lidar.getRangeImage()[400:440])  # example indices
    error = right_dist - 0.3  # desired distance
    Kp = 2.0
    angular_speed = Kp * error
    forward_speed = 2.0
    robot.set_motors(forward_speed - angular_speed, forward_speed + angular_speed)

This behavior is similar to how a self-driving car might follow a lane boundary.

Putting It All Together

Combine motion to goal and Bug Zero in a main loop. Use timestamps to avoid infinite loops. Test in a Webots environment with obstacles like boxes or walls.

def main():
    robot = MyRobot()
    state = STATE_GO_TO_GOAL
    leave_distance = None
    while robot.step(32) != -1:
        goal = detect_goal()
        if state == STATE_GO_TO_GOAL:
            if goal is None:
                robot.set_motors(-1, 1)  # rotate to find
            else:
                if obstacle_detected():
                    state = STATE_WALL_FOLLOW
                    leave_distance = goal.getPosition()[2]
                else:
                    center_goal(goal)
                    if move_to_goal(goal):
                        break  # reached goal
        elif state == STATE_WALL_FOLLOW:
            follow_wall()
            goal = detect_goal()
            if goal and goal.getPosition()[2] < leave_distance:
                state = STATE_GO_TO_GOAL
    robot.set_motors(0, 0)

Testing and Debugging Tips

Visualize: Print object position and LIDAR values to the console.
Tune PID gains: Start with small Kp to avoid oscillation.
Handle goal loss: If the robot loses sight of the goal, rotate in place to reacquire.
Edge cases: Test with concave obstacles; Bug Zero may fail in such cases, but for convex obstacles it works well.

Real-World Connections

This lab mirrors technologies used in autonomous delivery robots (like those from Starship Technologies) that navigate sidewalks and avoid pedestrians. Similarly, AI-powered lawn mowers use boundary following akin to wall following. The Bug Zero algorithm is a classic example of reactive navigation, which is still relevant in resource-constrained systems.

Conclusion

You've learned to implement motion to goal and the Bug Zero algorithm using Webots and Python. These skills form the foundation for more advanced path planning. Experiment with different environments and sensor configurations to deepen your understanding.