Lab 6(a): IMU, PID, and Odometry

Objective

The goal of this lab is to setup the IMU to enable your robot to estimate its orientation, and attempt to do PID control on the rotational speed of the actual robot.

Set up the IMU

Installed the SparkFun 9DOF IMU Breakout - ICM 20948
Connected the IMU to the Artemis board using a QWIIC connector.
Scanned the I2C channel to find the sensor, matched the address we expected:
The following data is printed out when the arduino code is compiled and uploaded.

It provided various values and their meanings were determined after running the following tests:
- Flip: Here you can see the accelerometer value travels from one side of the x-axis to the other side (i.e. mirrored across the x-axis) when the robot is flipped.
- Rotate: As the robot is rotated in each of the x, y,and z directions, the gyroscope and accelerometer values increase about that axis.
- Translate: As the robot is translated in each of the x, y,and z directions, the gyroscope and accelerometer values increase in the direction of that respective axis.
The data values were determined to be:
- Acc: This is the data from the accelerometer in each of the directions. [ax,ay,az]
- Gyr: This is the gyroscope that provides the angular speed in each of the directions, it is a 3d vector that measures rotational speed around each axis [x,y,z].
- Mag: Values from the magnetometer - This measures the magnetic forces.
- Tmp: Measures the temperature of the IMU in Celsius

Accelerometer

Using the equations:
$a$ _x = 1gsin(Θ) a_z = 1gcos(Θ) a_x/a_z = tan(Θ) We can find the roll and tilt to be:
$Θ = atan(a$ _x a_z ) Φ = atan(a_y a_z ) After including the math.h library, it was easy to compute and print the roll and tilt values as shown below:
- The pitch and roll for {-90,0,90} degrees are shown below:
  
  Roll
  
  Pitch
- As shown in class, the Low pass complimentary filter can calculated as shown below:
  
  The new plot of pitch compared to the old plot is very close (as shown below).

Gyroscope

Comparing the output to the pitch and roll values from the accelerometer and the filtered response:

The values don't overlap completely but the behavior is somewhat similar. Also, the Pitch and roll from the Gyro slowly decreases with time which is the reason for the Pitch and roll to be lower for the Gyro. The behavior of the graph in pitch, roll, and yaw is as expected.
The Low pass complimentary filter can calculated as shown below:

The filtered values for both gyroscope and accelerometer measurements are almost identical for pitch:

Same is true for the roll as shown below:

Magnetometer

PID Control

Program to make the motors spin in opposite directions at increasing, followed by decreasing, speeds while recording yaw:



res_cmd->command_type = BYTESTREAM_TX;
res_cmd->length = 14;
//TODO: Put an example of a 32-bit integer and a 64-bit integer
//for the stream. Be sure to add a corresponding case in the
//python program.
memcpy(res_cmd->data, &timecurr, 4); // time
memcpy(res_cmd->data + 4, &currspeed, 4);
memcpy(res_cmd->data + 8, &thd, 8);
Serial.println(thd);

//Serial.printf("Stream %d \n", bytestream_active);
amdtpsSendData((uint8_t *)res_cmd, 14);
STEP = 0.5;
if (currspeed <= maxspeed && STOP == 0) { // Within range
	if (ramp == 1) { // Ramp up

	digitalWrite( LEDPIN, LEFT_MOTOR );
	digitalWrite( LEDPIN, RIGHT_MOTOR );
	currspeed = currspeed + STEP;
	myMotorDriver.setDrive( LEFT_MOTOR, 1, currspeed); //Drive motor i forward at full speed
	myMotorDriver.setDrive( RIGHT_MOTOR, 1, 0.9*currspeed); //Drive motor i forward at full speed

	}
	else // Ramp down
	{
	digitalWrite( LEDPIN, LEFT_MOTOR );
	digitalWrite( LEDPIN, RIGHT_MOTOR );
	currspeed = currspeed - STEP;
	myMotorDriver.setDrive( LEFT_MOTOR, 1, currspeed);
	myMotorDriver.setDrive( RIGHT_MOTOR, 1, 0.9*currspeed);
	if (currspeed == 0) { // stop if ramped down
		STOP = 1;
	}
	}
}
if (currspeed == maxspeed)
{
	ramp = 0;
}

if ( myICM.dataReady() ) {
	myICM.getAGMT();                // The values are only updated when you call 'getAGMT'

	// yaw from Gyro
	timecurr = millis();
	dt = (timecurr - timeprev) / 1000;
	timeprev = timecurr;
	yaw = yaw + (myICM.gyrZ()) * dt;
	thd = myICM.gyrZ();
}

if (STOP == 1) {
	digitalWrite( LEDPIN, LEFT_MOTOR );
	digitalWrite( LEDPIN, RIGHT_MOTOR );
	myMotorDriver.setDrive( LEFT_MOTOR, 0, 0);
	myMotorDriver.setDrive( RIGHT_MOTOR, 0, 0);
}
}
timeprev = timecurr;

The motion of the robot programmed as shown above:

Max Rotation Speed of ~ 800 rad/s
Deadband: Motor values below which the robot doesn’t spin is ~650 and ~725 for the Right motor and Left motor, respectively.
Hysteresis: The size of the deadband is ~ 75.
As shown in the video, the right wheel starts moving before the left wheel.

Guided by the ramp response, the lowest possible speed at which one can reliably rotate the robot open loop around its own axis is ~150 deg/sec which was determined by using the ramp up input and the response below.
- Minimum sampling time of the TOF sensor can be found in Lab 5 for short mode to be 34 ms.
- If the robot starts at 0.5m from a wall pointing straight towards it, and spins as slowly as possible, the initial distance would be 0.5 m. Then at a speed of 150 deg/sec, the robot would scan every 34 ms or every 5.1 degrees. The distance would change by (0.5/cos(5.1 deg) - 0.5) = 2 mm.
  If it starts angled 45 degrees to the wall, the initial distance will be 0.5/cos(45 deg) = 0.7071, the angle change would still be 5.1 degrees, but the distance change will be (0.7071/cos(45 deg) - 0.5) = 0.5 m.
- From above, the two displacement values differ a considerate amount, which will affect the ToF Sensor measurements. Therefore, in certain situations (like the one above) the rotational scan won't be as reliable.

I tried implementing PID control to make the robot rotate slowly around its own axis at a constant speed. I chose PID because it provides a relatively faster reaction to disturbances and better accuracy. In our case, there is a lot of disturbance which the PID can help with. I applied a step input as shown in the code below, and as the robot rotated, the PID would compute a new motor speed given the gyroscope reading. I chose a setpoint of 150 after conducting multiple tests.


// ------------------------  Send Data ------------------------ //
    res_cmd->command_type = BYTESTREAM_TX;
    res_cmd->length = 14;
    //TODO: Put an example of a 32-bit integer and a 64-bit integer
    //for the stream. Be sure to add a corresponding case in the
    //python program.
    t = millis();
    memcpy(res_cmd->data, &t, 4); // time
    memcpy(res_cmd->data + 4, &thd, 4);
    memcpy(res_cmd->data + 8, &Speed, 8);

    amdtpsSendData((uint8_t *)res_cmd, 14);


    // ------------------------  measure ang.vel ------------------------ //
    if ( myICM.dataReady() ) {
      myICM.getAGMT();                // The values are only updated when you call 'getAGMT'

      thd = myICM.gyrZ();
      Input = thd;

      // ------------------------  PID ------------------------ //
      myPID.Compute();

      // ------------------------  Move Motors at Steady speeds ------------------------ //    
      Speed = min(150,Output+60);
        
      myMotorDriver.setDrive( LEFT_MOTOR, 1, Speed); //Drive motor i forward at full speed
      myMotorDriver.setDrive( RIGHT_MOTOR, 1, Speed); //Drive motor i forward at full speed
    }

The slowest I can make my robot reliably turn is at a motor speed of 120. The behavior of the robot is much smoother and stable than the open loop control. Considering that the maximum rotational speed is 200 rad/s, which is much lower than that of the open loop, therefore, this would be an optimal speed at which we can measure the distance using the ToF sensor.

I applied the PID control to one motor only (the battery was not at its highest). The slowest I can make the speed is 110. The intuition for why one can achieve much lower speeds now is because the robot is not being restricted to rotate about its own center of mass. I set the motor controller on the passive side to be 60. Given the lowest speed, the orientation of the robot changes very slowly as shown in the figure (~100 rad/s). Mapping a 4x4 box should not be an issue with a lower angular velocity. The accuracy would be better for the measurements directly oriented at the wall, and worse for the ones oriented on an angle but since it is in the middle, the angle should be relatvely lower than being really close to a wall.

Lab 6(b): Odometry and Ground Truth in the Virtual Robot

Code


def update_plot(robot):
   # Code goes here
    
    # Odom
    [xo,yo,tho] = robot.get_pose();
    robot.send_to_plot(xo, yo, ODOM)
    
    # GT
    [xg,yg,thg] = robot.get_gt_pose();
    robot.send_to_plot(xg, yg, GT)
    
    time.sleep(0.1) 
    
while(1):
	update_plot(robot)

Questions

As shown in the code, I added a delay of 0.5 seconds, so I send data every 0.5 seconds.
After conducting tests for plotting large number of points and/or over long durations, I noticed that the odometry pose increases in error, whereas, the Ground truth stays consistently accurate, as shown below.
The odometry trajectory looks similar to that of the ground truth for the first few time steps, but as time goes on, the error increases and so does the deviation of the odom pose from the GT pose, as shown above.
When the robot is stationary, the Ground truth is stationary at the true Pose, however, the odometry has noise which is why it moves around the true pose as shown below:
The noise in the odometry pose estimate increases with speed and time since the error acculumates over time.