How to determine if the robot has arrived at the desired position

[mstacho]

Hi all,

It's time to put my thinking cap on.  Here's the idea:

I have a robot hand working with 6 motors in 3 fingers.  I need to "pre-shape" the hand -- essentially, set the motors to a desired position *before* grasping takes place.  In order to do this properly, I'll need to know when the motors have arrived at the pre-shape position, so that I can then proceed to grasp the object.  Pre-shaping is done without contact, so I can't use force sensors.

However, since controllers are never 100% accurate, each motor will have a small error.  Thus, I can't just say:

if ALL_6_MOTORS are at CORRECT_POSITION
   PROCEED_TO_GRASP

I've tried using a simple threshold on abs(MOTOR_POSITION  - DESIRED_POSITION) for all 6 motors, but it's proving to be a bit of a pain, and isn't reliable at all (even though my threshold is absurdly large, at about 15 degrees of error).  To make matters a bit worse, I'll need to make sure that the motors are at that position for a given amount of time (say 0.2 seconds), to avoid any false positives that might result from, say, overshooting the desired position.

So the question is: how would I do this robustly?  It has to work 95% of the time at least.  The PC is handling this part, not my Axon, so I have more computational power than I need, but I can't seem to get it to work!

MIKE

[richiereynolds]

So is the problem that the fingers really aren't at the correct position, or that you're having trouble in the software determining that they're at the correct position?

[mstacho]

The problem is sort of both haha.  I can get the fingers to CLOSE to the exact position.  In fact, they come close enough for it not to be an issue (like within 1%).  But since they aren't exact, the encoder reading of the motors and the desired encoder reading are subtly different, and will most likely be different for each motor (so motor 1 might be off by 1%, while motor 5 might be off by 0.7%).  My control laws are pretty good, but I'll never be able to get the error to 0 all the time.

So I need the software to figure out that the motors are close enough for it not to matter.

Some extra caveats: using the derivative (when the motor stops, you're there) won't work on the real robot since it will be moving, and hence will bounce the encoders around a bit and cause noise haha.

Now, I did manage to get the threshold working somewhat, but it's really not reliable -- if the motor happens to have a larger error, say 2%, once every ten runs, the entire thing will fail.  This happens, and is quite irritating :-P

I should point out that this seems to be a relatively common problem that is often solved with just a threshold.  Problem is, this hand has to work as my research project, so I can't let it just fail 10% of the time and move on...

[waltr]

Is there a way you can have the code do a position calibration? Say close completely using the force sensors to determine the hand is closed then re-set the position variable.

The only other way I can think of at the moment is get the position feed-back sensors working more accurately and be more noise immune.

[mstacho]

Yup, the original position is completely calibrated and it works quite well.  There are hard mechanical limits that the motors just move to, and we know they are at the zero position.  As I said before, the issue is knowing you've reached some nonzero position unambiguously.

I'll look into getting the encoders working better, or maybe just make sure my control laws are much more up to snuff.  The problem, of course, is that the gains that work for large motions might drastically overshoot for smaller ones...

I have a feeling my only solution is just thresholding and having a human operator supervise it.  Once it works, we can always just buy better equipment and rebuild it :-P

MIKE

[Maltaeron]

Have you tried/do you think it might work that if you looked at your noisy encoder values and if after .2 seconds or whatever, they aren't getting any closer, you just call it good enough and proceed to grasp? I'm not sure if I understand your problem very well, but it seems to me that your motors essentially get to the right position, bug being so noisy it is hard to tell that they are there.This approach should hopefully function as a threshold-as-large-as-it-needs-to-be because it will accept an infinitely bad error so long as it doesn't see it getting any better.

[mstacho]

Your idea is a good one, but it still rests on the fundamental problem: how is it that I can tell the computer that the error is "good enough", or that the encoders aren't getting any close?  I'm left with the same problem.

At the moment, my solution is to just enlarge the threshold :-P I've made it to 36 encoder ticks (out of a possible 1300) as the threshold, but am keeping the motors stationary for about 0.1s.  Since the motions are always complete in under 1s, this seems to work fairly well.  There are still some silly problems, but they'll be rare.  For instance, what happens if I only want to move a small angle, say 40 encoder ticks?  But unless I do something like fuzzy logic that would require me to spend WAY more time on this than needed (and to put aside my mathematical reservations of fuzzy logic :-P ) this will have to be good enough!

Now to just wait for the force sensors and the hand will be ready to go!

MIKE

[Maltaeron]

The "good enough" is the judgement that you decide by choosing this method, because when the encoder errors stop improving, you go on regardless of however bad they may be. Maybe some pseudocode will help show what I mean.

Code: [Select]

accumulativeError = MAXIMUMVALUE
do
{
  bestAccumulativeError  = accumulativeError
  accumulativeError = 0
  while (less than .025 seconds since last iteration)
    foreach encoder, accumulativeError += encoder error
  }
} while (accumulativeError < bestAccumulativeError)
// The error has not gotten better over the last iteration
DoGrasp()

Depending on how quickly the encoders can give new values and the stability you get, you can experiment with the .025 seconds value to balance the speed and stability. Do this make more sense?

[mstacho]

Hm, yeah, that's actually a really neat way to do this.  When I get to work tomorrow I'll see if I can get it working. 

MIKE

[growler]

Can you describe your motors? Sounds like 1300 encoder ticks, how far apart are the mechanical limits you mentioned?

I'm working on some similar position servo like control of encoder equipped dc motors for a bit.

At what frequency does your control loop run and what does the basic structure look like? Do you have any other information from the motors other than the encoder? Things like the voltage and current across the motor leads at any given time or motor supply voltage.

[mstacho]

The motors: http://www.robotshop.com/ca/solarbotics-gm17-gear-motor.html

The encoders: http://solarbotics.com/products/51510/

The mechanical limits are on the backs of the fingers, and it requires me to just bring the entire finger all the way back.  I usually calibrate by hand, though.  Control loop runs at about 100Hz, and it's basic PID: get error, generate output, toss into PWM.

I don't have any more information other than encoder information, except maybe PWM on-time. 

What's your project?

MIKE

[growler]

I'm just working on algorithms that help reduce position error in order to help with odometry and path calculation.

Mainly right now I'm spinning motors on the bench with various loads and graphing the results and getting in a bit over my head with some math.

What processor are you using? What else is the processor doing? How are you reading the encoders, I assume you are polling them? Do you control for position or velocity?

Sorry if that is a lot of questions, but I bet you can improve results by either increasing loop frequency, or adding in a feed-forward component to your control loop. I'm not sure if you are running into processor constraints or not. The first would likely be easier if it will work and your loop doesn't start running into itself. The second requires some modeling of your system but you basically estimate the PWM value needed to move the desired amount needed by the next loop based on your model and use that along with your PID feed-back to generate the PWM value you actually feed to the motors.

[mstacho]

That sounds neat, although you're probably dealing with the possibility that your wheels slip, right?  I'd be interested in knowing how you're going to compensate for that (if at all)?  What's wrong with the math?  I know an equation or two, I might be able to help out.

The processor is an ATMega640, on the Axon, and it is also responsible for handling communications to the hand through UART, monitoring at least 6 force sensors (although I just came up with a way this morning to increase this number absurdly without increasing the number of addressing pins I'm using ), and then also regulating the forces.

The thing is, accurate positioning isn't really important, since the actual grasping is being handled by the force sensors -- I'm never going to know with 100% certainty where the fingers will contact the object, especially at the back of the object, so I am using force sensors to adapt once contact is made.  The main issue is the pre-shaping of the hand, which in theory will make the grasp take place faster since the fingers will already be "close enough" before grasping takes place.

However, I'm thinking that I'll just accept the errors I have now and make the threshold larger.  You make a good case for feed-forward control.  In fact, I did my masters in feed forward control and this is just such a situation where it would be useful if I were extremely interested in low errors. 

Problem with a robot hand is that it's not a static system, so the load vs. torque would change for the motors as they move the links above them.  So I'd have to rig up some kind of simple adaptive system (enter my masters project  ), which I'm relatively certain will cause the Axon some pain.  I guess eventually I'll toss this onto a bigger processor, but for now, it works haha.

Good suggestions, though!

MIKE

[growler]

Yeah, six motors with one micro is not looking that doable with any of the more interesting "tricks", at least at a higher loop frequency. Wasn't sure if you had any fast processors in the loop.

You do make a good point about the system not being static which is something I had not really considered before posting. Not that its theoretically a show stopper but that a lot of arithmetic every cycle. As you say bigger processor.

My last practical questions are, are you using floats? What determines the loop timing, is it at a set frequency or is it just looping as fast as possible? What kind of interrupts are you using?

I was able to increase my loop frequency by an order of magnitude by switching everything to fixed point math. Again something that could be fixed with a bigger processor.

As for my project, wheel slippage is in the works although it isn't a terribly large concern at the scale I'm working at right now, internal sources of friction are the biggest influences and the mass isn't very big. The idea however is to monitor for jumps in velocity that do not correlate to changes in the velocity setting, then do something with that info . I honestly haven't given it a ton of thought as of yet, been working on other aspects and I would have to change the little mobile platform I actually have moving to be able to make it reliably slip for testing.

The main problem with the math is I find myself going back on my promise to myself to never make myself mess with differential equations again after that undergrad course years course. I find I know an equation or two, they just aren't the right ones and I'm having to reteach myself quite a bit. I've got some free body diagrams of the mechanical and electrical systems and some equations but I've been having trouble with the transform to something useful. I'm not too confident in some of my parameter measurements as well, such as motor inductance and frictional coefficients. Oh well, part of the "fun".

Sounds like you know more about the topic than I do anyway, good luck!

One last question I guess, the grasping is taken care of by the force sensors. What tells the hand where to grasp? By that I mean what determines the correct pre-shape as you put it? Just curious.

[mstacho]

Haha in engineering one can never avoid differential equations!  But luckily, no decent human being ever sits down to solve them by hand, so get yourself a good suite of simulation software and go nuts :-P

Feedforward control doesn't have to be nearly as advanced as people make it look.  Yeah, you'll need a good guess at the parameters, but as long as you're not TOO terribly far off the feedback should correct for the rest.  In fact, the role of feedback is to remove effects of modelling errors!  So if your FF controller provides a reasonable guess as to what the motor needs, and the FB controller handles the rest, you should still get much better operation than the FB controller alone.  So don't worry too much about your parameter identification on the motors.  An easy way to test if it's working is to try the control with and without the FF term on.  If the error is lower, you're golden!  Then, just tune the FF controller parameters by hand until it works (or learn to do adaptive stuff, it's actually not too tough for simple DC motors, and I can give some guidance...as long as your processor can handle it  )

I'm using floats, but the processor is just looping as fast as it can with a 10ms delay between loops.  But the encoders are being read outside the loop and the PID is taking place there as well, based entirely on interrupts.  Ultimately, it's just a prototype, so I can probably handle the errors.  Hell, if I wanted it FAST I'd just do it on an FPGA and turn the entire code into machine code :-P

The main grasp loop will go like this:

Use some 3D vision to estimate the center of mass of the object (with a lot of uncertainty, given the fact that the object will only be seen from the front) and give a basic guess as to the places we can put the fingers (this is done from an algorithm I found in a paper that searches for grasps that will be stable, given what we know about the object).  The pre-shape comes from these two best guesses.  As the fingers close, they will make contact, maybe not at the same place as predicted.  If this happens, the algorithm will evaluate whether the new grasping points will still make a stable grasp, based on any updated information about the object's center of mass and mass properties.

That's the theory, anyway.  In all honesty, I'll be dollars to donuts that just making a reactive force controller and some ad-hoc grasping location guesser (something like: put the center of the hand over the object's center and go for it) will handle a LARGE array of objects without any high level planning.  That's why I'm going to test the simpler solution first haha.

MIKE

[growler]

Good stuff. The difference in work involved of the two approaches does make a simple guess coupled with a robust reactive controller look attractive, as you would need that part for either approach I imagine given the uncertainties involved even with the pre-shape guesses. How delicate are the object you are grabbing?

I assumed as much with the parameter inaccuracy, after all I'm completely guessing on the PID terms already and that works well enough and I know at least with the motors there are ways to attempt to calculate that as well. Always good to hear someone else agrees, thanks for that.

I mean I already have improved things somewhat by including an equation of the form I think I should arrive at with terms including distance desired before next loop, the velocity and a static term and just plugging in constants that I guessed. I've got more work to do before I feel like I understand it though.

Any suggestions on good simulation software? I'm currently working on figuring out Octave cause I'm cheap but I'm not positive it is the right tool for the job. Its probably been ten years since I've done any real math and its been interesting trying to figure out what I remember how to do and relearning what I don't.

[mstacho]

Yeah, one fundamental assumption I'm making is that the hand is not strong enough to break the object.  This can be relaxed later, since the algorithms will learn, but I don't really want to have to clean egg guts off the hand a thousand times before it figures out the right forces :-P  It is, afterall, just a prototype!

Hm, good simulation software...well, Octave is free so it's got that going for it.  As long as it has an ODE solver I don't think you'll end up getting much better, unless you go to a graphical software package that'd cost some money, like Simulink.  I'm not sure if there is a free version of Simulink out there, but it's a tool developed to solve differential equations numerically and it's really easy to use...but it's like $1000 :-(

Your basic motor equation is:

J*a + C*v + K(x) = Torque

where J is the rotational inertia, C the damping ("friction") and K the static friction (note that it's K(x), not Kx, so you don't multiply by x here, it's just a function.  This handles the whole "static friction depends on the direction you're moving" thing), a is the acceleration, v the speed.

If you're controlling the position, I wouldn't even bother dealing with the electrical components at all, and my control law would be:

u(t) = KP*err + KD*errDot + (C*v - K(x))

Where that last term is the feedforward term, and the first two terms are just PD control.  What this does is turn your system into a pure acceleration (F = ma) system by cancelling out the other terms.  Since K(x) is, for all intents and purposes, constant, and C is just a "small damping term", you just get to play around with some parameter guesses until your motor works like a charm :-P

That's basically what you're doing, sounds like to me, so you should be rockin'.  How does it work?

MIKE

[growler]

Thanks for the equations and advice, I looked at simulink and its not really an option, oh well. I can deal well enough with cli's.

The equations are pretty much what I arrived at for my control law. Works like a charm for one motor now but of course of other problems abound.

I'm in the process of going from a program that kind of works to one where I've verified the output.  As something of a masochist I decided to write my encoder processing routine in assembly to see if I could. I thought things were just a bit choppy due to other factors but finally tracked down some bugs in the assembly routine that was double counting on one state transition and skipping another (kind of, I'm amazed it was working as well as it did before the fix), which lead to the speed oscillations I had been seeing. Anyway I think that bug was masking the fact that one of my home made encoders is has something wrong with it, either that or the encoder problem just decided to develop at the same time.

Anyway I now have relatively smooth velocity regulated motion at low speeds on the working motor which was the original problem that sent me down this road in the first place.  My encoders are quit low resolution as well.  This is the first time I'm approaching most of these problems so everything is taking a while. Things are still somewhat oscillatory and I haven't spent much time try to tune anything so I'm thinking things will work out well enough for my needs anyway. I'll likely take a stab at the actual modeling since I've taken the time to make some measurements anyway though.

Thanks for the help, and in your thread no less, lol.