Realistic Velocity Calculation

[Joshua Sicz]

I have been helping out our student programmers on our team and we haven't found a good why to calculate velocity. We have a accelerometer that gives us reading in g's. So we just convert it to ft/s^2 and then take the timedifference between that last time the code was ran to calculate velocity. But what we are finding is the accelerometer doesn't give us consistent values.

Does any team have experience in this? Anything would help thanks!

[Ideal_Nerd]

You could use a encoder on the wheels to calculate the speed of your robot and use the gyro/accelerometer to determine the direction.

[asid61]

Originally Posted by Ideal_Nerd

You could use a encoder on the wheels to calculate the speed of your robot and use the gyro/accelerometer to determine the direction.

I believe many tems use an encoder for speeds. We do.

[GeeTwo]

Originally Posted by Joshua Sicz

I have been helping out our student programmers on our team and we haven't found a good why to calculate velocity. We have a accelerometer that gives us reading in g's. So we just convert it to ft/s^2 and then take the timedifference between that last time the code was ran to calculate velocity. But what we are finding is the accelerometer doesn't give us consistent values.

Does any team have experience in this? Anything would help thanks!

In order to even have a chance at making this work, you would also need a gyroscope. At each time step, you would have to rotate the previous time step's x and y by the amount of angle you've turned (theta), then add the change in speed. Assuming x is to the right and y is forward, and theta is rotation to the right, you'd have to do:

• ynew = yold*cos(theta) + xold*sin(theta) + yacc*rdelta
• xnew = xold*cos(theta) - yold*sin(theta) + xacc*tdelta

This is known as inertial navigation, and you can very quickly get lost in the intricacies if you're actually trying to use it for more than a few seconds, or in more than two dimensions, or across more than an area the size of, say, an FRC arena. Even staying in a 27-foot square box and limiting yourself to 2 minutes and 30 seconds, it would probably be a good idea to have a "reset" button that you can hit when the robot is actually stopped that sets both values to zero.

[hexane]

Quadrature Encoders are probably the most accurate way to tell distance. To figure out velocity, make a variable that you set to the position of the encoder at the end of a program cycle, then figure out the change in position divided by the change in time.

[Joshua Sicz]

Originally Posted by Ideal_Nerd

You could use a encoder on the wheels to calculate the speed of your robot and use the gyro/accelerometer to determine the direction.

Yea we have encoders but what may happen is you could be stuck and your wheels turning with you actually going anywhere.

[GeeTwo]

Originally Posted by Joshua Sicz

Yea we have encoders but what may happen is you could be stuck and your wheels turning with you actually going anywhere.

HMMM.. I wonder if anyone has ever implemented an optical mouse solution applied to the carpet beneath an FRC robot...you'd need to use two of them in different locations to also measure rotation, but it'd be a great way to know if you lost traction. Oh, well - not my problem this year; shove it into the "off-season projects" folder, er, file cabinet.

[Caleb Sykes]

Originally Posted by GeeTwo

HMMM.. I wonder if anyone has ever implemented an optical mouse solution applied to the carpet beneath an FRC robot...you'd need to use two of them in different locations to also measure rotation, but it'd be a great way to know if you lost traction. Oh, well - not my problem this year; shove it into the "off-season projects" folder, er, file cabinet.

Not quite an optical mouse, but I know that a couple of teams have mounted unpowered omni wheels beneath their robot that are connected to encoders in order to measure distance. Syncing the data with this with an angular measurement could then determine velocity and position. I think I remember 111 doing something like this last year, but I am unable to find the thread describing it.

[GeeTwo]

Originally Posted by Caleb Sykes

Not quite an optical mouse, but I know that a couple of teams have mounted unpowered omni wheels beneath their robot that are connected to encoders in order to measure distance. Syncing the data with this with an angular measurement could then determine velocity and position. I think I remember 111 doing something like this last year, but I am unable to find the thread describing it.

And if you arranged them in a triangle with all the axes pointing towards the same point, you'd have a kiwi speedometer!

[Joshua Sicz]

Originally Posted by GeeTwo

In order to even have a chance at making this work, you would also need a gyroscope. At each time step, you would have to rotate the previous time step's x and y by the amount of angle you've turned (theta), then add the change in speed. Assuming x is to the right and y is forward, and theta is rotation to the right, you'd have to do:

• ynew = yold*cos(theta) + xold*sin(theta) + yacc*rdelta
• xnew = xold*cos(theta) - yold*sin(theta) + xacc*tdelta

This is known as inertial navigation, and you can very quickly get lost in the intricacies if you're actually trying to use it for more than a few seconds, or in more than two dimensions, or across more than an area the size of, say, an FRC arena. Even staying in a 27-foot square box and limiting yourself to 2 minutes and 30 seconds, it would probably be a good idea to have a "reset" button that you can hit when the robot is actually stopped that sets both values to zero.

Awesome! We have actually played around we this exact thing this season and have got it to work with exactly they way you have said it to be done. Now I just need to get velocity from the x and y direction relative to the frame. Getting the code to output a realistic velocity because we get random numbers out of the accelerator.

[Joshua Sicz]

Originally Posted by GeeTwo

And if you arranged them in a triangle with all the axes pointing towards the same point, you'd have a kiwi speedometer!

All of this is quite interesting. I will have to try a mouse on the carpet.

[Spoam]

You can find the velocity by numerically integrating the acceleration values (e.g. trapezoid sum them, or continuously add the acc value*time step). This is probably very imprecise though, so I would test it empirically and not have anything vital depend on those values.

[Greg McKaskle]

Integrating the acceleration to get velocity is theoretically correct and simple, but won't yield good results unless you can handle lots of pesky details.

The biggest problem is that the accelerometer is operating in the earth's gravity field. You have a big force that just doesn't go away. You can sit still and calibrate it out, but if you tilt just a tiny bit, gravity's "down" vector starts to show up in your other orthogonal dimensions you really care about. And since you are accumulating, it adds up quick and appears that your robot has been accelerating and is now traveling across the field. My understanding is that good IMUs also contain a good inclinometer ( a specialized gyro) so that they measure the tilt and correct for it.

If this were straightforward, you'd also see tons of phone apps that not only counted steps, but told you how fast you were walking/driving and didn't need GPS and cell triangulation to know your location. Cars wouldn't measure wheel speed either. They would use the same technique.

So the concept is a good one to discuss and experiment with, but a little bit of additional analysis will show the need to cancel gravity for matches in order for this to work well enough.

Greg McKaskle

[Michael Hill]

Originally Posted by Caleb Sykes

Not quite an optical mouse, but I know that a couple of teams have mounted unpowered omni wheels beneath their robot that are connected to encoders in order to measure distance. Syncing the data with this with an angular measurement could then determine velocity and position. I think I remember 111 doing something like this last year, but I am unable to find the thread describing it.

We did this on my high school team (45) back in 2004(?). I think we had white tape strips on black wheels for a primitive encoder.

[slibert]

Originally Posted by Greg McKaskle

Integrating the acceleration to get velocity is theoretically correct and simple, but won't yield good results unless you can handle lots of pesky details.

The biggest problem is that the accelerometer is operating in the earth's gravity field. You have a big force that just doesn't go away. You can sit still and calibrate it out, but if you tilt just a tiny bit, gravity's "down" vector starts to show up in your other orthogonal dimensions you really care about. And since you are accumulating, it adds up quick and appears that your robot has been accelerating and is now traveling across the field. My understanding is that good IMUs also contain a good inclinometer ( a specialized gyro) so that they measure the tilt and correct for it.

If this were straightforward, you'd also see tons of phone apps that not only counted steps, but told you how fast you were walking/driving and didn't need GPS and cell triangulation to know your location. Cars wouldn't measure wheel speed either. They would use the same technique.

So the concept is a good one to discuss and experiment with, but a little bit of additional analysis will show the need to cancel gravity for matches in order for this to work well enough.

Greg McKaskle

Just wanted to amplify Greg's point. It's complicated, so will take some focused effort and time, and understanding of the physics and related math.

The process is (a) the gravity vector has to be calculated, and to be done accurately, (b) the gyro and accelerometer data must be fused and filtered. Then (c), that vector is subtracted from calibrated accelerometer data to yield linear acceleration, (d)
rotated to be in "world" coordinates (overcoming pitch/roll effects and rotation), then (e) double-integrated to yield distance-traveled vectors in x and y axes.

In case you're interested, the navX MXP does parts (a-d). Here's a link to some Java code that does the calculations for (b-d) [step a is done in hardware on the navX MXP] so you'll get a feel for the process. In this code (see the setQuaternion() function), the gravity vector is derived from a quaternion (a representation of the orientation of the body which has fused the gyro and accelerometer data). This is then subtracted from the calibrated accelerometer, yielding linear acceleration.

The navX MXP provides this information to the RoboRio, and there are teams discussing on Chiefdelphi using the navX MXP and attempting to do step (e), what you are describing. The standard caveat is that this requires a double-integration (acceleration->velecity->distance) and small amounts of noise and error will quickly compound, so that actual distance that this will be accurate for is something still to be determined.

The next step after that would seem to be (f) fuse the encoder data w/the estimated distance vector, which if done well could potentially increase the quality of the estimate of the distance vector.