I'm trying to use a VectorNav VN100 IMU to map a path through an underground tunnel (GPS denied environment) and am wondering what is the best approach to take to do this. I get lots of data points from the VN100 these include: orientation/pose (Euler angles, quaternions), and acceleration and gyroscope values in three dimensions. The acceleration and gyro values are given in raw and filtered formats where filtered outputs have been filtered using an onboard Kalman filter. In addition to IMU measurements I also measure GPS-RTK coordinates in three dimensions at the start and end-points of the tunnel. How should I approach this mapping problem? I'm quite new to this area and do not know how to extract position from the acceleration and orientation data. I know acceleration can be integrated once to give velocity and that in turn can be integrated again to get position but how do I combine this data together with orientation data (quaternions) to get the path?
2 Answers
In robotics, Mapping means representing the environment using perception sensor (like 2D,3D laser or Cameras).
Once you got the map, it can be used by robot to know its location(Localization). Map is also used for find a path between locations to move from one place to another place(Path planning).
In your case you need a perception sensor to get the better location estimation. With only IMU you can track the position using Extended Kalman filter(EKF) but it drifts quickly.
Robot Operating System has EKF implementation you can refer it.
- 2,332
- 1
- 20
- 34
-
Thanks clarification of terms and for the ROS recommendation. I'm new to the field so I'm constantly getting my terms mixed up. We use a SiCK TiM LiDAR scanning in the vertical plane to get the tunnel/pipe profile, our robots are controlled manually and we don't need to implement a full SLAM solution (yet). I am hoping to implement a solution that uses the IMU data to get a line (in 3D) between the two GPS coordinates, then overlay the LiDAR data to get a map of the tunnel/pipe. What formula can I use to get this line using accelerations and quaternions? – AVD Jan 02 '20 at 15:41
Ok so I came across a solution that gets me somewhat closer to my goal of finding the path travelled underground, although it is by no means the final solution I'm posting my algorithm here in the hopes that it helps someone else.
My method is as follows:
- Rotate the Acceleration vector A = [Ax, Ay, Az] output by the VectorNav VN100 into the North, East, Down frame by multiplying by the quaternion VectorNav output Q = [q0, q1, q2, q3]. How to multiply a vector by a quaternion is outlined in this other post. Basically you take the acceleration vector and add a fourth component on to the end of it to act as the scalar term, then multiply by the quaternion and it's conjugate (N.B. the scalar terms in both matrices should be in the same position, in this case the scalar quaternion term is the first term, so therefore a zero scalar term should be added on to the start of the acceleration vector) e.g. A = [0,Ax,Ay,Az]. Then perform the following multiplication: A_ned = Q A Q* where Q* is the complex conjugate of the quaternion (i, j, and k terms are negated).
- Integrate the rotated acceleration vector to get the velocity vector: V_ned
- Integrate the Velocity vector to get the position in north, east, down: R_ned
There is substantial drift in the velocity and position due to sensor bias which causes drift. This can be corrected for somewhat if we know the start and end velocity and start and end positions. In this case the start and end velocities were zero so I used this to correct the drift in the velocity vector. Uncorrected Velocity Corrected Velocity
My final comparison between IMU position vs GPS is shown here (read: there's still a long way to go). GPS-RTK data vs VectorNav IMU data Now I just need to come up with a sensor fusion algorithm to try to improve the position estimation...
- 3
- 3