5
$\begingroup$

I am currently working on a balancing robot project, which features fairly low-cost sensors such as an 9-Dof IMU with the measurement states

$\textbf{x}_\text{IMU} = \left[a_x, a_y, a_z, g_x, g_y, g_z, m_x,m_y,m_z \right]^\text{T}$.

Currently I use the accelerometer and gyroscope readings, fused by a complimentary filter to get the angular deviation of the robot's upright (stable) position. The magnetometer values are tilt-compensated and yield the robots orientation with respect to the earth-magnetic field (awful when close to magnetic distortion). Furthermore I have pretty decent rotational encoders mounted on the wheels which deliver information on a wheel's velocity.

$\textbf{x}_\text{ENC} = \left[v_l,v_r\right]^\text{T}$.

Given these measurements i want to try to get the robots pose (position + heading).

$\textbf{x}_\text{ROB} = \left[x,y,\theta\right]^\text{T}$

I do have minor theoretical knowledge on EKF or KF, but it is not sufficient for me to actually derive a practical implementation. Note that my computational resources are fairly limited (Raspberry Pi B+ with RTOS) and that I want to avoid using ROS or any other non-std libs. Can anybody help me on how to actually approach this kind of problem? overview

Improve this question
$\endgroup$
1
  • $\begingroup$ Welcome to Robotics Flo Ryan, great first question, I look forward to reading the answers you get. $\endgroup$
    – Mark Booth
    Aug 15, 2015 at 9:55

1 Answer 1

Reset to default
3
$\begingroup$

So you have acceleration readings from your IMU (linear and angular), and you get velocity readings (linear only) from wheel encoders.

Get velocity from linear and angular accelerations with

$$ v = v + a*\mbox{dT} $$

Get angular velocity from your wheel encoders by exploiting geometry of the vehicle

$$ \dot{\theta} = \mbox{atan2}((v_r - v_l) , \mbox{wheel base}) $$

Now you have two sources of linear and angular velocities - the IMU and the wheel encoders. Get to linear and angular positions with

$$ x = x + v*\mbox{dT} $$

Here I've used $x$ but of course it's just a variable to use for whatever position you're interested in using.

Now you have heading and position from the IMU (via linear x and y accelerations and the z gyro), you have heading and position from the wheel encoders, and heading via the magnetometer.

I would suggest filtering IMU outputs before integration, but you can use whatever filter you want - lag, Kalman, or something more complicated. If computational simplicity is a priority I would highly suggest you check out the lag filter. It's basically a complimentary filter using the current sample and previous sample instead of two different sensors.

Finally, you can fuse the sensor readings with the complimentary filter. I don't think I can advise you as to which input you should place more emphasis because that will depend primarily on wheel slip (how much you "trust" the wheel encoder).

Improve this answer
$\endgroup$
7
  • $\begingroup$ Thanks! The gyroscope gives us angular velocities. I can follow your post but the fact that the Imu's frame tilts around its x axis makes it a lot harder i guess. Did you leave out the magnetometer readings as they are to imprecise ? I added a small sketch. Hope it helps! $\endgroup$
    – Flo Ryan
    Aug 15, 2015 at 19:16
  • $\begingroup$ No, you can use the magnetometer along with the heading estimates from the gyro and wheel encoders. You can use a complimentary filter to combine the three estimates all at once or you can combine the first two and then combine that output with the third (cascade the complimentary filter). $\endgroup$
    – Chuck ♦
    Aug 15, 2015 at 19:43
  • $\begingroup$ Regarding the IMU rotation, your robot balances so the rotation should be minimal. You can use small angle approximation for cosine, $1 - \theta^2/2$, where the angle is in radians. However, if you do the math here, 5 degrees is $5*(\pi/180)=0.087$ radians. Squared divided by two gives you 0.0038, so the cosine small angle approximation for 5 degrees is 0.9962. I've found that, unless you need to be super precise, <reading> cos$\theta \approx$ <reading> is generally a safe small angle approximation. $\endgroup$
    – Chuck ♦
    Aug 15, 2015 at 19:54
  • $\begingroup$ A final comment on the small angle approximation - compare what I've shown above to the measurement error of the IMU. Allowing $\cos{\theta}=1$ introduces an error of up to 0.4%, which may be negligible compared to measurement error. Additionally, the error is symmetric (same error applied forward and backward), so any error introduced during forward motion will get canceled during backwards motion. You could always do proper trig compensation of course, but I don't know if it's necessary for your application. $\endgroup$
    – Chuck ♦
    Aug 15, 2015 at 20:01
  • $\begingroup$ Thanks again. One issue is remaining.The rotation around the x and the y axis calculated by the atan2 of the IMU yields fairly good results, which is due to the constant gravitational force. However estimates for the rotation around the z-Axis which is paralell to the gravitational force vector are not feasible as the forces created by the movement of the robot are too small. I will consider using the magnetometer for this. I do also get your point on the small angular approximation, I don't know why but I feel the urge to over-complicate things. I guess I'll stick with your suggestion, though $\endgroup$
    – Flo Ryan
    Aug 15, 2015 at 21:09

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.