Lab 9: Mapping
Orientation PID Controller
The rotation was performed using my previous orientation PID controller based on integrated gyro readings. I kept my original tuning from lab 6 (Kp=0.4 Ki=0.1, Kd=0.0002), as it was working well for the turns.
PID Performance
The data above shows the results of a single robot scan. The PID terms graph shows the error oscillating between 0 and 25 degrees as the robot continously completes a 25 degree turn and updates its target angle. The yaw graph ( wrapped to 360 degrees) shows robot's orientation as it completes a full scan. When testing my controller I found that the robot tyically turned within 1 to 3 degrees of the target angle.
Turn Video
Accuracy
In a 4 m by 4 m empty square room, with the robot turning on axis at the center, the mapping error is dominated by yaw error rather than TOF noise. Based on the average yaw change I measured during turning, the heading error is about 1° to 3°. Using this, the average map error is about 4 to 12 cm, while the maximum error is about 5 to 15 cm for points near the corners of the room. The TOF characterization from Lab 3 showed a variance of about 1 to 25 mm, so its contribution is much smaller than the angular error. Therefore, the overall map accuracy is mainly limited by how accurately the robot turns and maintains its heading during the scan.
Mapping Code
Artemis Code
My code was structured around the use of my mapping() function which called via a bluetooth START_MAPPING command. The function starts the orientation pid with the set point of 0 and then does a for loop of 14 measurments where while the absolute value of pid error is greater than 3 my imu_pid_step() function is called. The error had to be limited to 3 to guarantee that the robot would not get stuck trying to correct a small error without finishing its full rotation. Once the error is less than 3, the robot stops and takes a TOF readings. Then the robot records its data which can be transmitted via MAP_DATA bluetooth command and increments the next target angle by 25 degrees. The function heavily relies on all pid related terms being global variables accesible to both the pid functions and the mapping function.
MAP
The picture of the map I used is below. The center X marks the origin of my plane and the otther Xs mark the locations of test spots. The spots are reported in meters away from the origin.
Individual Scan Polar Plots
For each location three rotations were performed to create three separate scans.
Location 1 (0.57, -0.16)
Location 2 (0.14, 0.33)
Location 3 (-0.45, 0.33)
Location 4 (-0.37, -0.33)
Transformation Matrices
The TOF sensor is mounted 8 cm in front of the robot center, along the robot’s forward direction. Since the robot starts aligned with the global \(+x\)-axis, the sensor offset in the robot frame is \( \begin{bmatrix}0.08 \\ 0\end{bmatrix} \) meters.
The general transformation from robot frame to inertial frame is: $$ \begin{bmatrix} x_w \\ y_w \\ 1 \end{bmatrix} = \begin{bmatrix} \cos\theta & -\sin\theta & x_r \\ \sin\theta & \cos\theta & y_r \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 0.08 + d\cos\alpha \\ d\sin\alpha \\ 1 \end{bmatrix} $$ where \((x_r, y_r)\) is the robot position, \(\theta\) is the robot yaw in the inertial frame, \(d\) is the TOF reading, and \(\alpha\) is the scan angle relative to the robot’s forward axis.
Line-Based Map
The integrated line map was able to capture the real walls of the room but also had a lot of noise particularly in the middle of the room. This is likely due to poor sensor mounting/tilting with the sensor not retriving the full range when the signal reflects off the floor before hitting a wall.
Resources
I checked my transformation matrices with Jack Long's webpage.