4
$\begingroup$

Context: I am working with the SFU Mountain Dataset [http://autonomylab.org/sfu-mountain-dataset/]

The UGV image - via the SFU Mountain Dataset website: enter image description here

I have used the following state update equations (Husky A200 - differential drive)

State Update - from Prob. Robotics, Thrun et. al [x' y' theta'] represent the state at the next time step

enter image description here State Update - from Prob. Robotics, Thrun et. al.

After plotting the x and y positions based on just the wheel encoder data (v_fwd and w -> the dataset provides these directly, instead on the vr and vl), the curve seems to be quite weird and unexpected. Wheel Odometry Data - http://autolab.cmpt.sfu.ca/files/datasets/sfu-mountain-workshop-version/sfu-mountain-torrent/encoder-dry-a.tgz

Blue - Wheel Odom | Red - GPS Blue - Wheel Odom | Red - GPS

Actual path! Actual path!

Question: Is the above curve expected (considering the inaccuracy of wheel odometry) or is there something I'm missing? If the wheel encoder data is that bad, will an EKF (odom + imu) even work?

PS: I'm not worried about the EKF (update step) just as yet. What concerns me more is the horrible wheel odometry data.

Improve this question
$\endgroup$
1
  • $\begingroup$ I experienced slip of wheels even in somewhat ideal lab conditions (flat, clean, dry floor), which made separate slip-free encoder wheels necessary. This is a woodland trail. $\endgroup$
    – Bending Unit 22
    Feb 13, 2016 at 17:27

3 Answers 3

Reset to default
3
$\begingroup$

A few things:

  1. I took a look at your data set. Did you make sure you used the time column correctly? The first entry is "1429481388546050050" without the decimal. To make it in seconds, it should be 1429481388.546050050.

  2. Your motion model is fine (I've used it before, for people who want to see it derived, it is very similar to this one). However, to avoid dividing by zero, you should be checking the value for angular velocity, and if it below a certain threshold, you need to change it to

$$ \begin{bmatrix} x_{k+1} \\ y_{k+1} \\ \theta_{k+1} \end{bmatrix} = \begin{bmatrix} x_k \\ y_k \\ \theta_k \end{bmatrix} + \begin{bmatrix} v_k\Delta t\cos(\theta_k) \\ v_k\Delta t\sin(\theta_k) \\ \omega_k\Delta t \end{bmatrix} $$

  1. The wheel encoders on the Husky are actually pretty good. If you can get your hands on the raw data (i.e., the ticks), you might get a better result. However, this requires that it was somewhat well calibrated (i.e., how many ticks per metre of travel).

  2. If this were an indoor test, I would tell you that yes, you should definitely get better results. But it looks like the Husky was travelling on pretty rough terrain. If you've compensated for the two things I mentioned, it might just be the tough environment!

Improve this answer
$\endgroup$
1
  • $\begingroup$ Thanks! I have scaled and subtracted the time stamps appropriately. I will try changing the equations for the w = 0 condition and get back. In the case that it still turns out to be bad, are you aware of any other datasets that have decently dependable wheel odom + imu values? $\endgroup$
    – AKag
    Feb 14, 2016 at 7:24
2
$\begingroup$

Thanks for the update. Now it looks like $x_c$ and $y_c$ denote the origin/starting position, and $\theta$ is positive, measured CCW from the positive x-axis. Now I am even more concerned about the equations you're using. Consider just $x$. You have:

$$ x_k = x_{k-1} - \frac{v}{\omega} \sin{(\theta)} + \frac{v}{\omega} \sin{(\theta + \omega \Delta t)} $$

So,

  1. Consider the term $\frac{v}{\omega}$. If there is no yawing motion, $\omega = 0$, then what happens? That term is divide-by-zero. If $\omega$ happens to be near zero, the term explodes to infinity.
  2. Consider #1 to not be broken, and imagine no yaw at a heading of $\theta = 0$, which means the vehicle is pointing in-line with the +x axis. However, assuming #1 is a non-issue, x values update directly proportional to $\sin{\theta}$, which evaluates to zero. That is, per the diagram and code provided, when the vehicle is in line with the x-axis it is not possible to change the x-coordinate of the vehicle.
  3. Ignoring #1 and #2 above, there is no time-dependency on the non-x terms. This means that, whether the sample frequency is 10Hz or 1000Hz, each sample will accumulate with the same weight. This is akin to me saying I've traveled another 60 miles every time I look at the speedometer in my car, regardless of how frequently I check it. I (you) don't include how long the vehicle has been at that speed, so any check of vehicle speed is improperly weighted.

Currently you only show (a picture of!) the algorithm you're using to generate positions. The more you can post the easier it is to help - the actual code would help the most.

Improve this answer
$\endgroup$
3
  • $\begingroup$ 1. You're correct. When using this motion model, you're actually supposed to switch to another one when ω is below a threshold (see my answer). 2. When $\omega\neq 0$ (i.e., #1 not broken) and $v>0$, the third term will be positive, making $x$ increase. 3. I think you should take a look at the formulation. The $\Delta t$ in the third term here actually accounts for how long you've been travelling at the velocities since the last update. $\endgroup$
    – kamek
    Feb 14, 2016 at 4:12
  • $\begingroup$ Chuck, that's a good catch! I'll try it out with kamek's suggestion, and check if it gets any better. $\endgroup$
    – AKag
    Feb 14, 2016 at 7:30
  • $\begingroup$ @AKag - no problem, but again, please post the actual code you're using on the next update (if there is one). Something I thought about since I answered is that your vehicle looks like a 4 wheeled vehicle, but you state it's differentially steered. I don't know if all four wheels are driven, and if not, whether the encoder data is coming from driving or passive wheels. Passive wheels would neatly sidestep most of your wheel slip error. As a neat check, once you've parsed the data, try summing the path and compare that path length to summed GPS path length. $\endgroup$
    – Chuck ♦
    Feb 14, 2016 at 18:57
1
$\begingroup$

Yes, I have experienced this. Wheel encoders are great for a second to second hint for an ekf predict step, but are generally awful for long term, long distance prediction. Odometery and imu can do better, but both are integrators and will accumulate error quickly. Add GPS, terrain features, or other global estimates for a real solution.

Improve this answer
$\endgroup$
2
  • $\begingroup$ Thanks for the help! But, shouldn't I get a curve (from the pure wheel odom reading) that at least resembles the actual/gps curve? I expected it to be of a similar 'shape', with a large drift from the gps data. Do the equations that I'm using make sense? $\endgroup$
    – AKag
    Feb 13, 2016 at 22:37
  • $\begingroup$ Maybe. I'm sure others can and will address the details. I'm just saying I've seen it that bad before $\endgroup$
    – Josh Vander Hook
    Feb 14, 2016 at 2:41

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.