I am in the process of creating a robot absolute positioning system, such as stangPS, and was wondering if any teams or programmers could help me with a few concepts.
I am keeping track of robot x and y values (relative to the origin at bottom left of field, field would be quadrant 1 on a Cartesian plane) in integer variables, and trying to keep track of the robots 2d position. I understand how I can increment these value by keeping track of distance traveled with shaft encoders and a yaw gyro. But, my theory only works with straight line movement. Heres why:
When the robot moves straight from its starting position, I could increase the x variable with the distance traveled. Then if I turn and drive a distance I could find the y and x value of change by taking the sin and cosine of the angle I turned, multiplied by the distance of that line traveled (hypotenuse of a right triangle).
But, I am totally stuck on how to code this for arcing robot movement. The robot does not always travel in a linear fashion, and I am not sure how to compensate for this.
Has anyone ever used the same method as I am, and know what to do to solve my problem?
I’ve worked with a similar positioning system.
I haven’t worked in a situation where a robot arcs, but I have an idea about how you might approach the problem. If you think about it, in 26.2 ms, your robot won’t really make that much of an arc. In that time frame, you can assume that your robot has moved in a straight path.
If you take the initial heading (angle) of your robot and the final heading of the robot (before/after 26.2 ms), and take an average, you should get the approximate heading of what the robot is traveling on. I’m not quite sure, but I have a suspicion that this will (along with all the other trig i’m sure you’ll figure out) generate pretty accurate results about how much x and y distance you have covered.
And of course, there are better ways to “average” the heading of the robot, by sampling the gyro many times (interrupt) and taking a more significant average.
probizzle is correct, except for two things. I think the gyro does not output changes fast enough to sample more than 26.2ms, or even that fast. The other thing is that averaging to find the values for integration may be overkill. It may just be possible to use the first or last value for the interval and still be accurate enough.
We’ve been doing this for a couple of years with good results. As others have said already, all you really need to do is for each loop, read your delta from the encoder wheel and read the most recent angle measured from your gyro and just pretend that you travelled in a straight line since the last time you did the calculation. This happens so fast that you end up approximating the arc with a series of very small straight lines which works well enough for what you’re trying to do.
How many encoders are you using? You only need 1 when you use the gyro with it. If you’re using more than one you might just be doing unnecessary calculations.
The encoders are giving you a nice actual value of position(like an accelerometer that’s already integrated twice, in a way), so those won’t have this problem.
The gyro is giving you a rate, though, which is where the problem you are describing occurs. Enter Euler’s Method: given rate data at every point and a starting position, you can approximate a solution by using very small straight lines, as everyone else has mentioned. The 26.2 ms loop assures that the step size will be very small. Even if you have one of the gyros that can measure 150 degrees/sec (the BEI one from two years ago is 75 degrees/second I think), 150 degrees/second * 0.026 second/loop = 4 degrees/loop before your gyro reaches its limit and the data is useless anyway. So for any given loop, you can assume the robot has not deviated from a straight path by more than 4 degrees, a pretty good approximation.
If you are looking for more accuracy, you can average two rates as probizzle suggested. Through some mathematical property which I can’t explain, error in these numerical integration methods is approximately proportional the step size raised to the power of the number of slopes sampled per step. So if you have a small step size and you sample two slopes, you will have a very accurate approximation.
For the purposes of FIRST, you are more likely to have error due to not being in the exact starting position or getting bumped by another robot and sending the gyro above its yaw rate limit, so averaging slopes may be unneccesary. However, it doesn’t eat computing time and it’s a cool project anyway. Have you thought about adding an accelerometer to get position data that isn’t dependent on the wheels not slipping (but does have the disadvantage of having an acceleration limit of something like 1.5g, which can be easily broken by running into something)?