Lab 9: Mapping
The purpose of this lab was to map out a static room. To accomplish this I had to place my robot in several marked spots in the room and have the robot spin once at each spot while collecting ToF readings.
First, I decided to spin using orientation control. I decided to spin the robot at 25 degree increments and to take one ToF reading at each increment. This meant that for one full spin (360 degrees) I would end up with 15 ToF readings at angles of 0, 25, 50, 75, 100, …., 350 degrees. I wrote a new command to accomplish this first. First one ToF reading is taken while the robot hasn’t moved yet and is at 0 degrees. Then it loops 14 times, and at each iteration first rotates the car 25 degrees, and then takes one ToF reading. I used the same orientationPID function from last lab with some small tweaks to use PID to rotate the car 25 degrees. Except after the yaw is within 3 degrees of 25 or if 3 seconds have passed, I write 0 to both motors to turn them off after the PID. Below is a code snippet of the command. Each time I run the PID the yaw starts from 0 so each iteration I tell the robot to rotate 25 degrees.
I tested the PID parameters until I could get good performance. I ended up raising Kp a lot up to 2500 and lowering Kd down to 10. This was because the car often undershot. Below is a video of the car successfully rotating and taking 15 ToF readings. The car finishes at an angle because the last rotation takes it to 350 degrees not 360. But at the start the first reading is at 0 degrees so data is still taken from that angle.
Here is a plot of the yaw values during the rotation. During each rotation the yaw is reaching around 22-23 degrees most of the time and sometimes overshooting 25. The yaw estimate is prone to drifting so small error like this didn’t make a huge impact.
Here are the ToF readings collected.
I then had to put my robot in the lab arena and execute the turn and collect ToF readings at 5 different locations. Below are two images of the arena. There are two obstacles, and each black square on the floor is one location to execute a turn at. The location of the red car in the middle is also one such point, and is also the origin of the arena used for the mapping later.
After collecting 15 ToF readings at each point, I plotted them in polar coordinates to verify the results. This was simple to check because each reading is separated by 25 degrees, from 0 to 350 degrees. Position 1 is the leftmost point, position 2 is the origin, position 3 is the lower right point seen in the second Arena image, position 4 is directly above the origin, and position 5 is behind the obstacle in the top right. Below are the polar plots. I started my robot facing towards the top of the arena each location.
To create a map of the arena using the 75 ToF readings I collected, I needed to transform the ToF distance to an x,y coordinate in the coordinate system of the arena. To accomplish this I needed to construct a transformation matrix, that when multiplied by the ToF coordinate gives the arena coordinate of the reading. The origin of the arena is at position 2, and the arena coordinate system has the z axis facing upwards perpendicular to the arena, the x axis to the right, and the y axis forwards. Since I started my robot facing forwards at each point, I defined my robot’s coordinate system the same as the arena’s (z upwards, x to the right, and y forwards) except with the origin being the robot itself. This meant that all of the ToF readings had coordinates relative to the robot of (0, distance, 0). The following image from the lecture slides shows how to construct the transformation matrix.
My robot always was flat on the ground so the pitch and roll were always 0 degrees. The yaw of the robot was a multiple of 25 degrees for each ToF reading, so in the rotation section of the transformation matrix the only terms that mattered are sin/cos of the yaw (psi). Any other sin term is 0 and any other cos term is 1. The translation part of the matrix is different for each point in the Arena. For example, point is located two tiles lower of the origin and three tiles to the left. Thus, dx = -914.4 (3 feet in mm), dy = -609.6 (2 feet in mm), and dz = 0 for all locations. Below is a code snippet that constructs the rotation matrix for position 1 and converts all of the ToF readings from that position into global coordinates. Since the transformation matrix is homogenous a 1 must be appended to the end of the ToF coordinate array, and only the first two parts of the resulting arena coordinate are necessary (x and y components).
I repeated this for all 5 positions. The only transformation matrix components that changed were the translations, since each position had a different (x,y) in arena coordinates. Since each position took ToF readings at increments of 25 degrees, the rotation part of the matrix is the same for each location. That gave me 5 pairs of (x,y) lists of arena coordinates. I then plotted them all together but used different colors for points found at different positions. The resulting plot is below.
The plot shows that the robot did a pretty solid job of mapping out the room. The walls of the arena are clearly defined, and it picked up both obstacles in the scatter plot, even though the size of the obstacles seems a little small due to not having a ton of data points. There are also some outlier readings outside of the arena.
I then used the data to draw lines for the boundaries of the arena and the obstacles in it. Below is the plot of the arena mapping with these lines overlayed.
