Lab 9: Mapping

The goal of this lab is to use the IMU and time of flight sensors on Meep to create a map of the environment. We sample distances from multiple points in the area, use transformation matricies to translate the measurements from the robot frame to the world frame, then convert this scan to a Line-Based Map that can be used in the simulation for lab 10.

Orientation Control Mapping

Implementation

To get a full map of the environment, we get a 360 degrees of distance measurements from set points in the environment. We then compile these readings together to make a map. In order to get the 360 degree mapping, Meep must read distance values while spinning in place. I decided to do this using the orientation control I tuned in lab 6. I created the command MAP which allows to caller to determine how many datapoints they want from the turn. Meep will calculate the setpoint angles based on the number of readings desired.

case MAP: {
  success = robot_cmd.get_next_value(x_coord);
  success = robot_cmd.get_next_value(y_coord);
  success = robot_cmd.get_next_value(num_angles);
  success = robot_cmd.get_next_value(Kp);
  success = robot_cmd.get_next_value(Ki);
  success = robot_cmd.get_next_value(Kd);
  
  angle_step = 360/num_angles;
  angle_idx = 0;
  i = 0;
  mapping = true;
}

From there, we simply record and transmit mapping data as shown below:

void make_mapping(){
  
  // We start by getting a yaw value
  if (myICM.status = ICM_20948_Stat_FIFOMoreDataAvail) {
    myICM.readDMPdataFromFIFO(&imu_data);
  }
  while(!((myICM.status == ICM_20948_Stat_Ok) || (myICM.status == ICM_20948_Stat_FIFOMoreDataAvail))){
    if (myICM.status = ICM_20948_Stat_FIFOMoreDataAvail) {
      myICM.readDMPdataFromFIFO(&imu_data);
    }
  }
  get_roll_pitch_yaw(0);

  // We set the first yaw value as our first setpoint
  curr_setpoint = yaw_readings[0];
  // and record values from both our time of flights
  record_tof_vals(num_datapoints, curr_setpoint);
  

  for (int j = 0; j < num_angles; j++){
    // we head to the next angle
    curr_setpoint += angle_step;
    if (curr_setpoint > 180){
      curr_setpoint -= 360;
    }
    else if (curr_setpoint < -180) {
      curr_setpoint += 360;
    }
    float e = yaw_readings[i-1] - curr_setpoint;
    if (e > 180){
      e -= 360;
    }
    else if (e < -180) {
      e += 360;
    }
    
    // while we aren't at the desired angle, we call `pid_angle`
    while (e > 5 || e < -5 ) {
      pid_angle(curr_setpoint);
      e = yaw_readings[i-1] - curr_setpoint;
      if (e > 180){
        e -= 360;
      }
      else if (e < -180) {
        e += 360;
      }
    }
    
    // once we get there, we stop and record the time of flight and angle data
    brake();
    tx_estring_value.clear();
    tx_estring_value.append("GETTING DATA");
    tx_characteristic_string.writeValue(tx_estring_value.c_str());  
    record_tof_vals(num_datapoints, curr_setpoint);  
  }


  ////////////////// Start Transmition ////////////////////////////

  // Once we finish, we send the data over
  tx_estring_value.clear();
  tx_estring_value.append("Transmitting");
  tx_characteristic_string.writeValue(tx_estring_value.c_str());  

  for (int j = 0; j < angle_idx; j++){
    tx_estring_value.clear();
    tx_estring_value.append(time_stamps[j]);
    tx_estring_value.append(":");
    tx_estring_value.append(tof1_readings[j]);
    tx_estring_value.append(":");
    tx_estring_value.append(tof2_readings[j]);
    tx_estring_value.append(":");
    tx_estring_value.append(gyro_yaw_readings[j]);
    tx_estring_value.append(":");
    tx_estring_value.append(setpoint_angle[j]);
    tx_characteristic_string.writeValue(tx_estring_value.c_str());
    if (myICM.status = ICM_20948_Stat_FIFOMoreDataAvail) {
      myICM.readDMPdataFromFIFO(&imu_data);
    }
  }
  tx_estring_value.clear();
  tx_estring_value.append("Done");
  tx_characteristic_string.writeValue(tx_estring_value.c_str());          
  Serial.println("Done");
  mapping = false;

When actually recording the data, I record num_vals datapoints to try to offset the error and noise discussed below.

void record_tof_vals(int num_vals, int angle){
  for (int j = 0; j < num_vals; j++){
    while (!distanceSensor2.checkForDataReady()) {
      if (myICM.status = ICM_20948_Stat_FIFOMoreDataAvail) {
        myICM.readDMPdataFromFIFO(&imu_data);
      }
    }
    while (!distanceSensor1.checkForDataReady()) {
      if (myICM.status = ICM_20948_Stat_FIFOMoreDataAvail) {
        myICM.readDMPdataFromFIFO(&imu_data);
      }
    }
    if (myICM.status = ICM_20948_Stat_FIFOMoreDataAvail) {
      myICM.readDMPdataFromFIFO(&imu_data);
    }
    while(!((myICM.status == ICM_20948_Stat_Ok) || (myICM.status == ICM_20948_Stat_FIFOMoreDataAvail))){
      if (myICM.status = ICM_20948_Stat_FIFOMoreDataAvail) {
        myICM.readDMPdataFromFIFO(&imu_data);
      }
    }
    time_stamps[angle_idx] = millis();
    get_roll_pitch_yaw(angle_idx);
    gyro_yaw_readings[angle_idx] = yaw_readings[angle_idx];
    tof1_readings[angle_idx] = distanceSensor1.getDistance();
    tof2_readings[angle_idx] = distanceSensor2.getDistance();
    setpoint_angle[angle_idx] = angle;

    distanceSensor1.clearInterrupt();
    distanceSensor2.clearInterrupt();
    angle_idx++;
    if (angle_idx >= TIME_ARR_SIZE){
      angle_idx = 0;
    }
  }
}

Testing

To test this, I created a box for Meep to try mapping

That resulted in the map below.

Since I noticed the side time of flight having some much smaller readings, I adjusted its angle. That resulted in the graph below

My sensor readings mostly agree. The discrepency between them is likely the result of having different distances due to their physical placement. As the car rotates, the front sensor will typically get closer to the wall it is reading from than the side sensor. Even in this small space, we already see quite a bit of error in both the sensors. I would reason that, in an empty room, its likely that one of the sensors would hallunicate some sort of wall or blockage that does not exist. It is likely that we are going to get a lot of noise data points and will have to look for where walls appear on mulitple trials to confirm where the real walls are located.

Real Environment

This is the environment we are trying to map. I measured the distance at 20 different angles, taking 5 measurements at each to try to offset bad readings. I got data from several locations in the environment that were a set distance apart. Specifically, I grabed data from (0,0), (-3,-2), (5,3), (0,3), and (5,-3). At each point, Meep would rotate and gather data.

The mapping from each point can easily be seen in polar coordinates as shown below.

I continued mapping, getting the data below.

Transforming the data

From here, we have data in the robot frame. To combine our data together, we can use a tranformation matrix to rotate and translate our data to the world frame, which will allow us to combine the data together. Our rotation matrix consists of a rotation (in blue) and translation (in purple). Since I started every trial with the bot facing along the positive x in our environment, we can map the inital angle to theta below and adjust from there.

While using the inital theta as the rotation works for some of the data, there are many places where I was not precise with placing the robot’s facing angle. I adjust these by graphing the transformed data, then measuring the angle offset.

The location of the sensors in the robot frame is not precisely (0,0). To be more accurate, I should have used a translation matrix to adjust for the location of the sensors. I found that the readings from the sensors mostly lined up so I decided to forgo this step when processing the data.

Results

After adjusting the rotation values, I ended up with the maps below

These mappings roughly map to the real environment as shown below

# locations in mm
x_off = [0, 0, 1524, 1524, -914.4]
y_off = [0, 914.4, 914.4, -914.4, -609.6]

# adjusted offsets for the rotation matrix
theta_off = np.deg2rad(np.array([68.586, 54.65656937979465, 64.538,  115.60284796050428, 60.6559914284864]))

# plot everything
plt.figure()

for i in range(5):
    # convert degrees to radians
    theta = np.deg2rad(data_arrays[f"yaw_{i}"] + 90) # add 90 to the side sensor so that it aligns with the front sensor
    theta2 = np.deg2rad(data_arrays[f"yaw_{i}"])

    theta_offset = theta_off[i]
    
    rotation_matrix = np.array([[np.cos(theta_offset), -np.sin(theta_offset), x_off[i]], 
                                [np.sin(theta_offset), np.cos(theta_offset), -y_off[i]], 
                                [   0,                  0,                   1]])

    
    r = data_arrays[f"tof1_{i}"]
    r2 = data_arrays[f"tof2_{i}"]
    
    # polar to cartesian
    x = r * np.cos(theta)
    y = r * np.sin(theta)
    x2 = r2 * np.cos(theta2)
    y2 = r2 * np.sin(theta2)

    # transform the readings
    roatated = rotation_matrix @ [x, y, np.ones_like(x)]
    roatated2 = rotation_matrix @ [x2, y2, np.ones_like(x2)]

    # then graph them
    plt.scatter(roatated[0], roatated[1], s=1, label = f"tof 1 data {i}")
    plt.scatter(roatated2[0], roatated2[1], s=1, label = f"tof 2 data {i}")

plt.gca().set_aspect('equal')
plt.xlabel("X")
plt.ylabel("Y")
plt.title("2D Map from Side TOF")
# plt.legend() # The legend blocks the plot so I commented it out for the screenshots. Its mostly useful for debugging
plt.show()

Collaboration and Sources

I consulted Jack Long’s website and discussed rotation vector mapping with Dyllan Hofflich.