Lab 9: Mapping

The goal of this lab was to map a static room so that the map could later be used for localization and navigation. Since I completed this lab at home instead of in the lab space, I used my own measured scan locations rather than the predetermined class coordinates. I placed the robot at the points (0,0), (1.5,3), (2.25,6), and (-2.25,3), then rotated it in place and collected distance measurements with my ToF sensors while also logging the robot orientation from the IMU.

The main idea was still the same as the lab: collect a set of angle-distance measurements, sanity check each scan in polar form, transform those points into the room frame, and then combine all the scans into one map of my home test area.

The robot used the IMU DMP quaternion data to compute yaw, wrapped the angle into the range of -180° to 180°, and then used a PD controller to rotate to the next target orientation. Once the error was small enough, the robot stopped, read both ToF sensors, stored the data, and then incremented the target by 25°.

float wrapAngle180(float angle) {
  while (angle > 180.0f) angle -= 360.0f;
  while (angle < -180.0f) angle += 360.0f;
  return angle;
}

if (((data.header & DMP_header_bitmap_Quat6) > 0) && 
    (ToF_Sensor_1.checkForDataReady()) && 
    (ToF_Sensor_2.checkForDataReady()) && tof_count < 15) {

  Gyro_yaw = (float)atan2(t3, t4) * 180.0 / PI + 3.8;
  Gyro_yaw = wrapAngle180(Gyro_yaw);

  error1P_ori = wrapAngle180(targetOrientation1 - Gyro_yaw);

  control_ori = (kp_ori * error1P_ori) + (kd_ori * error1D_ori);
  duty2 = fabs(control_ori);

  if (fabs(error1P_ori) <= deadband_ori) {
    stop_motors();

    int distance1 = ToF_Sensor_1.getDistance();
    int distance2 = ToF_Sensor_2.getDistance();

    distance1_in = distance1 * 0.0393701f;
    distance2_in = distance2 * 0.0393701f;

    store_mapping_data(i, starttime, distance1_in, distance2_in, Gyro_yaw);

    targetOrientation1 = wrapAngle180(targetOrientation1 + 25.0f);
    tof_count++;
    i++;
    delay(1000);
  }
}

This was the key part of the lab. The robot only logged data once it was within the orientation deadband, which helped reduce bad readings caused by taking measurements while still turning.

My robot used two ToF sensors during the scan. One sensor measured what was in front of the robot, and the other measured what was to the side. This let me gather more information at each orientation instead of relying on only one sensor.

Using two sensors made the scan denser and gave me more wall points after transforming everything into the global frame. It also gave me a way to compare whether both sensors were producing measurements that made sense relative to each other.

In the final map, I used both data sets together instead of throwing one away.

def transform_gbl(rad, d, x0, y0):
    x = d * np.cos(rad) + x0
    y = d * np.sin(rad) + y0
    return x, y


def postProcess(df, x0, y0):
    loc = f"({x0},{y0})"

    rad_front = -np.deg2rad(df["Angle(deg)"] - 90)
    rad_side = -np.deg2rad(df["Angle(deg)"])

    distF = df["Front(in)"]
    distS = df["Side(in)"]

    xf, yf = transform_gbl(rad_front, distF, x0 * 12, y0 * 12)
    xs, ys = transform_gbl(rad_side, distS, x0 * 12, y0 * 12)

    return pd.concat([pd.Series(xf), pd.Series(xs)], ignore_index=True), \
           pd.concat([pd.Series(yf), pd.Series(ys)], ignore_index=True)

This transformation moved each local measurement into a fixed room coordinate system. In other words, the robot measured distances relative to itself, and then I shifted those points using the known robot position to express them in the map frame.

Since my scan coordinates were measured in feet, I multiplied x0 and y0 by 12 to convert them into inches before adding them to the transformed points.

For each of my home scan locations, I plotted the front and side ToF measurements in polar form first. This helped me check whether the scan shape looked reasonable before combining it with the rest of the room data. The locations I used were (0,0), (1.5,3), (2.25,6), and (-2.25,3).

In general, the scans matched what I expected. The major room boundaries appeared where they should, and the overall point shape made sense relative to the robot position. The biggest imperfections came from small turning drift, slight yaw estimation error, and some inconsistency in the ToF readings.

Polar Plot: Location (0,0)

Polar Plot: Location (1.5,3)

Polar Plot: Location (2.25,6)

Polar Plot: Location (-2.25,3)

The final step was to convert the merged scatter plot into a line-based map. I manually estimated the wall locations by drawing line segments over the main clusters of points from my home scans. This simplified the raw scan data into a format that can be used later in simulation.

Since the real data was not perfectly clean, this step also let me correct small errors caused by drift, slight misalignment, or noisy distance measurements. The goal was not to preserve every single point, but instead to capture the actual room geometry well enough for future localization tasks.

lines = [
    ((-45,60),(-15,60)),
    ((-15,60),(-15,53)),
    ((-15,53),(15,53)),
    ((15,53),(15,92)),
    ((15,92),(40,92)),
    ((40,92),(40,-18)),
    ((40,-18),(-7,-18)),
    ((-7,-18),(-7,25)),
    ((-7,25),(-15,25)),
    ((-15,25),(-15,17)),
    ((-15,17),(-45,17)),
    ((-45,17),(-45,60)),
    #inside box
    ((5,25),(28,25)),
    ((28,25),(28,10)),
    ((28,10),(5,10)),
    ((5,10),(5,25))
]

Overall, this lab worked well and I was able to build a room map from multiple scan locations in my home setup rather than the class lab space. The biggest thing that made this possible was using orientation control so the robot could stop before taking each measurement. That made the data much more usable than trying to read the sensors while spinning freely.

The main sources of error were slight drift while turning, imperfections in the yaw estimate, and normal ToF noise. Even if the robot mostly turned on axis, small shifts in position still affected how cleanly the scans lined up after transformation. Also, if the heading estimate was off by even a few degrees, that would slightly rotate the measured points in the global frame.

In the middle of a 4 by 4 meter square room, these angle errors would translate into larger position error as the measured distance increased. So points farther away from the robot would usually show the biggest spread. Even with that, the scans were still accurate enough to identify the walls and create a reasonable line-based map.

If I were to improve this lab further, I would try collecting more scan points, reducing turn-to-turn drift even more, and comparing repeated scans from the same location to measure repeatability.

For the completion of this lab, I used Ethan Sarpongs 2025 page to reference the stucture of the python code and the way to plot the data. I also used ChatGPT for code clarification, debugging, and to help develop this page of the webside.