Trouble getting reliable heading from MPU9250 using RTIMULib

I’ve been working on a small autonomous vehicle project and getting reliable heading feedback from my MPU9250 IMU using RTIMULib has been an issue that’s haunted me for weeks. For background, it’s a four-wheeled terrestrial robot built on a standard 1:10 RC car chassis. It’s controlled by a Raspberry Pi 3B running ROS Kinetic, and I’m using RTIMULib2 to get data from the IMU. The operating environment is a typical parking lot with a very bumpy surface. The IMU is mounted alone on a plastic rod sticking straight up about a foot above the rest of the robot to isolate it from the other electronics, batteries, motors, etc. Below is all of the code I believe is relevant to the library implementation:

 //create RTIMUSettings type object called imu_settings to set initial IMU settings that will later be used to create IMU object RTIMUSettings *imu_settings = new RTIMUSettings(calibration_file_path.c_str(), calibration_file_name.c_str()); //create RTIMU type object called imu using previously determined settings RTIMU *imu = RTIMU::createIMU(imu_settings); //initialize IMU if (!imu->IMUInit()) { ROS_ERROR("[imu_node] failed to initialize IMU"); ROS_BREAK(); } //set fusion coefficient and enable gyro, accelerometer, and compass imu->setSlerpPower(slerp_power); imu->setGyroEnable(true); imu->setAccelEnable(true); imu->setCompassEnable(true); while (ros::ok()) { if (imu->IMURead()) { //get current IMU data RTIMU_DATA imu_data = imu->getIMUData(); //get yaw value and convert to degrees [deg] heading_msg.heading_angle = imu_data.fusionPose.z() / PI * 180; //normalize yaw value to compass heading in degrees (0 - 360 deg) if (heading_msg.heading_angle < 90) heading_msg.heading_angle += 270; else heading_msg.heading_angle -= 90; } } 

The entire file can be viewed on Github here.

Like I said earlier, I’ve been working on this for a while now so I’ve gathered a lot of information on things that haven’t worked but, frankly, I’m not knowledgeable enough to make any sense of it and figure out how to fix it.

The Problem

I’m calculating a desired heading from a vector drawn between the vehicle’s current position and a GPS position I’d like it to navigate to. I use the IMU to get the vehicle’s actual heading, then calculate the error between the actual heading and the desired heading, and use it to calculate a corrected steering output. The robot works admirably going due north or south. It does not work going east or west. The actual behavior depends strongly on the slerp_power variable above, of which I have tried a number of different values with varying results as explained below. In all cases I have been using the RTQF filter (default in RTIMULib), though I have also tested the Kalman filter and will include that information later. In all cases north to south navigation and vice versa works fine.

At the default slerp value, 0.02, the vehicle initially navigates in the correct direction for up to a second or two before heading hopelessly off in the wrong direction. Stopping the robot for a few seconds then starting it again without making any changes to anything produces the same result: heading is reported corrected when the robot initially starts moving but after only a second or two gets very wrong, and the robot heads for the hills on its own thinking it’s traveling in the correct direction all the while. The robot will correctly navigate in a straight line to the next point if it’s started and stopped every second or so.

Lowering the slerp value down to a minimum of 0.001 improves how long the reported heading stays accurate considerably. At 0.001 the robot can travel due east/west for up to ten seconds before heading starts to drift, and the drift occurs much more gradually. At first it drifts slightly to the left (north if traveling east and vice versa) until it’s in line with the destination waypoint on the north-south axis, at which it point it suddenly corrects towards the point and travels due north/south to reach it successfully. This “hooking” phenomenon gets progressively worse until the reported heading is completely useless.

Slerp values below 0.001, down to 0, seem to have insufficient or no reference to the world frame.

At slerp values above 0.02 the IMU does not even initially report a correct heading, and the robot usually just drives in very tight circles as the heading values jump all over the place.

My Understanding

As far as I understand it, the slerp value determines the weighting of the gyro feedback from the IMU vs. feedback from the magnetometer and accelerometers. This seems to indicate to me that my problem lies in getting accurate reading from the latter sensors since increasing the slerp value quickly produces useless data from the IMU. Using very low slerp values (~0.001) relies almost entirely on the gyros so seems to work much better, but is also subject to considerable drift since there’s less bias on data from the magnetometer which I need to maintain reference to the world frame. My first question then is whether my understanding of what’s happening is correct? My second question is, of course, how do I fix it?

Things I’ve Tried

  1. Recalibrating the IMU about a hundred times using RTIMULibCal, and running all three calibration routines. I’ve also read every word of the calibration .pdf file about thirty times because despite this, I still believe bad calibration to be the most likely issue if for no other reason than I can’t imagine what else it could be.
  2. A different MPU9250 sensor to rule out a hardware issue with exactly the same results.
  3. Rotating the IMU. I wanted to see if rotating the IMU 90 degrees would shift the problem from going east-west to north-south, and it did. With the IMU rotated it reports heading perfectly going east-west but does not work going north-south.
  4. The Kalman filter. This filter, with slerp values around 0.1 to 0.2, sort of worked better. It produced serious oscillation unlike the smooth output from RTQF, but the vehicle did navigate in generally the correct direction. I need to do a lot more testing with this, but right now I am leaning towards using the Kalman filter and attempting to damp the oscillation with the control algorithm.
  5. Using SPI instead of I2C. No difference.
  6. Testing in different locations. I recalibrated the IMU for every new location but the behavior was exactly the same regardless.

I apologize for the long-winded post, but I would very, very much appreciate any insight whatsoever into this issue. I’ve been pulling my hair out trying to solve this, but nothing I do seems to help.

submitted by /u/futility_jp
[link] [comments]