search

sbrodeur / ros-icreate-bbb

ROS (Robotic Operating System) packages for the iRobot Create with onboard BeagleBone Black
BSD 3-Clause "New" or "Revised" License
3 stars 2 forks source link

Validate X-Y-Z axes on the robot #8

Closed sbrodeur closed 7 years ago

sbrodeur commented 7 years ago

Define the X-Y-Z axes on the robot and make sure this is coherent with odometry and IMU output.

sgcarrier commented 7 years ago

Adjusted and standardized X-Y-Z axis. In self-reference, Y points straight ahead, X points right and Z points up. In orientation, Y points North, X points East, and Z points up. IMU madgwick (topic /imu/data) gives fairly accurate IMU info now.

sbrodeur commented 7 years ago

I would like to see if we obtain something similar to Figure 7 in the link below after calibrating the magnetometer: http://www.sensorsmag.com/sensors/motion-velocity-displacement/compensating-tilt-hard-iron-and-soft-iron-effects-6475

sgcarrier commented 7 years ago

magnetic_profile_x_y

We suffer from both hard and soft distortion. Hard distortion is more present in the Y axis ( X axis has almost none). We also have the soft-distortion ellipse form. Note that this data was collected with the offsets I made.

I'm not sure if the Madgwick filter we use to fuse mag and acc data takes this soft distortion into account.

It is also to note that we havent removed the bank and elevation angle in the magnetic data like in the article, so that may explain the soft-distortion ellipse we see.

sbrodeur commented 7 years ago

You may be right with this. I think that if we plot the same data in 3D (X-Y-Z), we should be able to estimate the shift to the reference axes, and remove the bank and elevation angle effects. I can do that if you give me the data that was used to generate the figure, including the Z axis data.

sgcarrier commented 7 years ago

mag2.zip Here is the csv and spreadsheet file. The data different from the previous graph since my collected data was overwritten but it gives the same curves and all.

sbrodeur commented 7 years ago

See script for calibration: https://github.com/sbrodeur/ros-icreate-bbb/blob/master/src/imu/scripts/calibrate_mag.py

Usage:

python calibrate_mag.py --input=magPoints_2.csv --display-data

It should display the data in 3D (navigable) and return the transformation (rotation matrix) required for calibration:

INFO:__main__:Using input cvs file: /home/simon/git/ros-icreate-bbb/data/magPoints_2.csv
INFO:__main__:X scale: -0.000020, 0.000018 (range of 0.000038)
INFO:__main__:Y scale: -0.000017, 0.000019 (range of 0.000036)
INFO:__main__:Z scale: -0.000041, -0.000040 (range of 0.000001)
INFO:__main__:Use this rotation matrix: 
 [[ 1.          0.          0.00988014]
 [ 0.          1.         -0.07183293]
 [-0.00978334  0.07740914  1.        ]]

data

sbrodeur commented 7 years ago

The rotation matrix R should multiply the data x (i.e. xc = R x) directly at the IMU capture. It can be hard-coded or passed as a ROS parameter to the driver node.

sgcarrier commented 7 years ago

Applied corrections and this is the new graph magnetic_profile_x_y_corrected

I applied the offset Y needed as well so its pretty centered. There is a slight ellipse shape, like in the previous graph.

Here is the graph of Z for X and Y , showing that the circle is now "flat"

magnetic_profile_xy_z_corrected

sbrodeur commented 7 years ago

See last commit efd77a5 for handling soft-iron effects. This requires an additional rotation matrix Rxy and multiplicative factors fx and fy. The idea to correct for the ellipse shape is to apply the rotation Rxy, rescale with fx and fy, then apply the inverse rotation Rxy^T (transposed of the rotation matrix) to get back to the original space.

INFO:__main__:Using input cvs file: /home/simon/git/ros-icreate-bbb/src/imu/scripts/magPoints_2.csv
INFO:__main__:X scale: -0.000019, 0.000018 (range of 0.000037)
INFO:__main__:Y scale: -0.000017, 0.000019 (range of 0.000037)
INFO:__main__:Z scale: -0.000041, -0.000040 (range of 0.000001)
INFO:__main__:Use this rotation matrix to correct for bank angle (z axis): 
 [[ 1.          0.          0.00988014]
 [ 0.          1.         -0.07183293]
 [-0.00978334  0.07740914  1.        ]]
INFO:__main__:Use this rotation matrix to correct for soft-iron effects (x and y axis): 
 [[-0.78891389  0.61450376  0.        ]
 [-0.61450376 -0.78891389  0.        ]
 [ 0.          0.          1.        ]]
INFO:__main__:Use these factors to correct for soft-iron effects (x-y plane): 
 [ 0.93339828  1.07683662]
INFO:__main__:All done.

figure_1

sgcarrier commented 7 years ago

magnetic_profile_x_y_corrected_2

The new correction works really well! The only thing is the offset on Y that I cant get right, but i think the reason for this is that I apply the offset before the rotations, when I should do it after. I will apply corrections and check it out.

sgcarrier commented 7 years ago

magnetic_profile_x_y_correctedalligned This is the latest calibration. It's practically aligned perfectly and has a good circle shape.

sbrodeur commented 7 years ago

Excellent! We can close the issue when the changes to the offsets will be committed.

sgcarrier commented 7 years ago

Committed. For future reference : a8af559af217208a5e5bfa4e4b416b9da26a271b

sbrodeur commented 7 years ago

While the IMU-calculated odometry follows the reference axes determined below, the odometry calculated from the internal robot sensors switches Y with X axes: https://github.com/sbrodeur/ros-icreate-bbb/blob/master/src/irobot_create/bin/driver.py#L175

I am also able to see that in the visualizations.

sbrodeur commented 7 years ago

See commit ee3e09d for a fix. This will need to be tested with the robot.