DPRG
DPRG List  



[DPRG] Odometry and backlash

Subject: [DPRG] Odometry and backlash
From: Markus Lampert markus at bibi.ca
Date: Tue Jan 6 15:17:35 CST 2015

Hi Doug,

on startup I set: 
  MDR0(0x08) = 0x01; // x1 quadrature mode, free running, no index
  MDR1(0x10) = 0x02; // 2-byte counter mode, enabled, no flags used

both chips are on the same SPI bus, so a assert both /SS lines and configure them in parallel. On retrieval I use the same method to "mark" the current counter value and then I read out each counter value independently.

At this point, once I set the speed the PWM is HW defined/controlled and I took the PID controller out of the loop as well. Having been scratching my head for the past few days I will not be surprised with - whatever I'll find ;).

I'll try to get access to a big hall this evening so I can let the robot run and see if I observe any of the behaviour you describe. Free running the wheels I thought I heard 2 distinct (different) motor pitches when running backwards, however letting it actually run on the floor (the short stretch I have available) did not confirm that, definitely not at the ratio the encoder values suggest.

Failing access to the hall I will probably mark the wheels and build an interim, external wheel revolution counter.

Thanks,
Markus


On Tue, 6 Jan 2015 14:53:57 -0600
"Paradug" <paradug at gmail.com> wrote:

> Markus,
>     I re-read your last message and saw that you are you are using a
> LS7366. Does your code setup the registers in the chip? What register
> settings are you using?
> Regards,
> Doug P.
> 
> -----Original Message----- 
> From: Paradug
> Sent: Tuesday, January 06, 2015 12:43 PM
> To: Markus Lampert ; David Anderson
> Cc: dprglist at dprg.org
> Subject: Re: [DPRG] Odometry and backlash
> 
> Markus,
>      When you run the motors backwards, can you see the speed
> difference? With a 43K/64K ratio on the encoders the 43K count motor
> should be going roughly 2/3 the speed of the other motor. If the
> motors appear to be about the same speed, that would suggest a coding
> issue in the encoder routine (ex: not using interrupts (missing
> counts), not handling count subtraction properly when the motor backs
> up,  etc). If they are going different speeds that means the encoders
> are most likely correct and the issue is either the motor (I have
> never seen one off this much) or the motor control code.
> 
> Regards,
> Doug P.
> 
> -----Original Message----- 
> From: Markus Lampert
> Sent: Tuesday, January 06, 2015 12:16 AM
> To: David Anderson
> Cc: dprglist at dprg.org
> Subject: Re: [DPRG] Odometry and backlash
> 
> Hi David,
> 
> thanks for writing this up. It seems each time you do you add more
> details which are a tremendous help!
> 
> Unfortunately I didn't get very far today. Actually I got stuck at 1.
> "Be sure the encoders are working properly"
> 
> I did a) and b), the manual back and forth of both wheels for 10
> turns and the wheels get to 3012 and 3013 counts respectively, which
> is also close to what it should be, 3 counts per motor revolution and
> a 100:1 gearbox. I put the 12/13 over-counts to my eyesight ;).
> Turning them back both end up at 1. So far so good.
> 
> For c) I let the motors spin until the quad encoders were about to
> roll over (I configured them for 16bits since that is what my
> processor is using). Spinning both forward I end up with:
> right:  63943
>   left:  62182
> I assume that is "close enough"?
> 
> However, running backwards I end up with
> right: 63556
>   left: 42986
> There's definitely something off. Running in reverse the motors sound
> differently too, more like 2 motors (whereas running forward sounds
> more like "more of the same"). So I sat the robot on the ground and
> repeated the exercise. Unfortunately I can't let it run for very long
> before it finds an obstacle (old house syndrome again). It does seem
> to do a slight arch when running either way, each way a different
> motor "leading" - although I did not find a significant difference in
> the arch radius going forward vs. backward. I guess the motors do
> have a //preferred// direction and since one always has to go against
> that it is a bit slower than the other.
> 
> Anyway, it seems I am losing counts on the left motor when going
> backwards. That is a rather surprising result considering the quad
> decoding is done by a dedicated chip with signal conditioning for
> each wheel (LS7366). I'll have a look at the output from the wheel
> encoders tomorrow and see what's going on there.
> 
> Thanks a bunch!
> Markus
> 
> 
> On Mon, 5 Jan 2015 15:30:48 -0600
> David Anderson <davida at smu.edu> wrote:
> 
> > Markus,
> >
> > My SR04 robot, which has very good odometery, also has a laser
> > pointer and can be wiggled back and forth within the backlash of
> > the gear train, about +- 1 degree.  It's not a show stopper, and
> > may not be the cause of the errors you are seeing.
> >
> > I'm a little suspicious of the measurement method.  Seems like it
> > assumes the robot is pivoting perfectly around the un-driven wheel,
> > which may not be the case, and that's a bit hard to control for.
> >
> > In my experience odometery can be made to work very well but it
> > takes a lot of tuning of the robot.   For what it's worth, here's
> > the method that works for me for a differentially steered robot:
> >
> > 1.    Be sure the encoders are working properly.
> >
> > That is, not dropping any ticks, and working correctly forwards and
> > backwards.  Care taken that the encoder hardware and software are
> > working correctly and robustly will save lots of headaches later on.
> >
> >      a.  Make a mark on the wheel(s) that lines up with something
> > on the robot, set the encoder count to 0, and turn the wheel
> > forward by hand 10 turns.  Write down the number of encoder counts,
> > then turn the wheel backward by hand 10 turns.  Be sure you get
> > back close to 0.  Zero itself can be hard to get depending on the
> > resolution of the encoders, but this is so you will know you are
> > not dropping ticks forward or reverse.
> >
> >      b.  Do the same with the other wheel, and verify that the
> > numbers are about the same for both wheels.
> >
> >      c.  Run the motors forward at full speed for a time and be sure
> > that both encoders show about the same number of total counts.  They
> > won't be identical because the motors are not really running at the
> > same speed, open-loop.  But they should be close. Do the same in
> > reverse. This helps determine if you are dropping any ticks at high
> > speed.
> >
> >
> > 2. Calibrate ticks-per-inch.  (or mm or whatever)
> >
> > Set the encoders to 0 and run both motors forward at a medium
> > speed, in more-or-less a straight line, for 10 feet, and stop.
> > Write down the total number of counts for each encoder.  Then the
> > robot's ticks-per-inch constant is:
> >
> > #define TICKS_PER_INCH = ((left_encoder+right_encoder)/2)/120.
> >
> > You can layout 10 feet with a tape measure on any convenient
> > surface, kitchen floor, sidewalk, etc.    Alternately, lots of
> > buildings have handy 1 foot square tile floors and a hallway makes
> > a nice piece of built-in graph paper for encoder calibration.
> >
> >
> > 3.  Manually measure the robot's wheel base.
> >
> > For a differential two wheel drive robot, lay a ruler across the
> > bottom of the robot from wheel to wheel, and record the distance
> > from the center of one wheel to the center of the other.   This is
> > the starting point for calibration of accurate turning and angles.
> >
> > #define WHEEL_BASE = hand measurement.
> >
> >
> > 4.  Run a series of left and right hand squares.
> >
> > This is really the only way to separate out the various sources of
> > errors.  It doesn't have to be 10 feet square, just use whatever
> > room you have available.  The larger the square, the more apparent
> > the errors.  But a smaller square works fine.  Also on carpet, if
> > that's all you have available.
> >
> > The procedure is to drive around a square and stop, and mark the
> > distance in X and Y from the stopping point to the starting
> > point.   Do it several times in one direction, say clock-wise, and
> > average together the errors.   A little like sighting in a rifle
> > scope.  Then do the same thing in the opposite direction,
> > counter-clockwise in this case, and average together those errors.
> >
> > The left and right hand squares will reveal two sources of errors.
> > Borenstein's UMBmark paper goes into the math in detail.  But more
> > basically the errors will be symmetrical and asymmetrical.    If the
> > clockwise and counter-clockwise position errors are symmetrical, it
> > means the robot is turning either too far or not far enough in the
> > corners.  That's a wheel base error.
> >
> > The larger the wheel base value, the fewer degrees the robot will
> > turn for the same number of encoder ticks, and vice-versa.  So if
> > the robot arrives short of the goal in both directions, it is not
> > turning enough, and the wheel base value needs to be reduced.  If
> > it overshoots the origin in both directions, it is turning too
> > much, and the wheelbase value needs to be increased.
> >
> > On the other hand, if the errors are not symmetrical around the
> > origin (they won't be) then the robot has two wheels that are not
> > exactly the same size.  This is common.   The correction is to add
> > a small value to the TICKS_PER_INCH for one wheel, and subtract it
> > from the other wheel.
> >
> > #define LEFT_TICKS_PER_INCH = TICKS_PER_INCH + WHEEL_SIZE_ERROR
> > #define RIGHT_TICKS_PER)INCH = TICKS_PER_INCH - WHEEL_SIZE_ERROR
> >
> > This corrects the odometery for the error in wheel size without
> > effecting the previously measure ticks per inch.
> >
> > 5.  Now adjust the WHEEL_SIZE_ERROR.
> >
> > Rerun the clockwise and counter-clockwise squares, adjusting the
> > WHEEL_SIZE_ERROR until the position errors become symmetrical.
> > You'll probably have to do this a few times.  This will cancel out
> > the wheel size errors.
> >
> > 6.  Finally adjust the WHEEL_BASE value.
> >
> > Increase or reduce the WHEEL_BASE constant until the position
> > errors, now symmetrical, cluster around 0, for both clockwise and
> > counter-clockwise runs.   This is the fine tuning of the WHEEL_BASE,
> > which is in turn the accuracy of the robot's angles. Once all the
> > errors are clustered symmetrically around the origin at 0,0, then
> > that's about as good as it's going to get.
> >
> > Just as a reference, the odometery code that uses these constants
> > looks like this:
> >
> > void odometery()
> > {
> >
> >      /* calculate elapsed encoder counts and distance since last
> > cycle */
> >
> >      left_inches = (float)left_velocity / LEFT_TICKS_PER_INCH;
> >      right_inches = (float)right_velocity / RIGHT_TICKS_PER_INCH;
> >      inches = (left_inches + right_inches) / 2.0;
> >
> >      /* calculate angle of rotation since last cycle */
> >
> >      theta += (left_inches - right_inches) / WHEEL_BASE;
> >
> >      /* And wrap theta at two pi */
> >
> >      theta -= (float)((int)(theta/TWOPI))*TWOPI;
> >
> >      /* calculate robot's current position in X and Y */
> >
> >      X_pos += inches *  sin(theta);
> >      Y_pos += inches *  cos(theta);
> > }
> >
> > This code runs at 25 Hz and maintains three global values: X, Y, and
> > theta, which are the robot's current pose and location.  Those
> > values are read by the navigation code that drives the robot to a
> > target way point location.
> >
> > Whoa, little windier than I intended, interesting how "simple"
> > things are sometimes more complex than is apparent.
> >
> > Anyway, hope this is helpful,
> > dpa
> >
> >
> >
> > On 01/05/2015 12:12 PM, Markus Lampert wrote:
> > > Hey Rud, Steve,
> > > eventually I will want to incorporate visual processing. For now
> > > though I want to keep things as simple as possible and see how
> > > far they go.
> > >
> > > Hi David,
> > >
> > > those are good questions.
> > >
> > > First off, I am currently not using UMBMark, mainly because I
> > > don't have access to a level and smooth 10' square - the virtues
> > > of living in an old house and our office building is all carpet.
> > >
> > > What I did was I mounted a laser pointer on my robot and mark the
> > > spot on the wall where it hits. I then turn on a single wheel to
> > > the lowest PWM signal that still makes the robot move reliably. I
> > > count the encoder ticks until I should have completed a full
> > > circle and measure where the laser pointer ends up in relation to
> > > where it started off. The intent was to do that in both
> > > directions and thereby get the accurate ratios of right wheel
> > > radius to wheel base and left wheel radius to wheel base. Which I
> > > then can use to calculate theta (or phi)....
> > >
> > > What happens though is that with the same settings I get
> > > differences of almost 3 degrees. The observed relation is that I
> > > should get about 4.7 encoder ticks per degree (if only one wheel
> > > is turning). In other words, with exactly the same number of
> > > encoder ticks the resulting turn is between 360 and 362.8 degrees.
> > >
> > > I have repeated the tests with different turning angles. The
> > > number of turns doesn't seem to make a difference. If I turn 10
> > > times around I end up with the same variance as I do for a single
> > > turn. So the steady state seems to be working OK and the error is
> > > solely introduced by starting and stopping.
> > >
> > > By placing the robot on the floor, again turning on the laser
> > > pointer I can 'wiggle' the robot by ~1 degree before the gearbox
> > > engages and I get the first encoder tick (2cm laser pointer
> > > travel when the wall is 150cm away). Note that this is 1 degree
> > > in total, not 1 degree in each direction.
> > >
> > > After sending my email yesterday I spent the rest of the day
> > > rebuilt my robot to get the battery pack (currently by far the
> > > heaviest piece of the robot) closer to the wheels. Will redo my
> > > tests tonight and see if it makes a difference. I also thought of
> > > repeating the tests without battery in order to eliminate
> > > slippage and inertia as much as possible (although that might
> > > help me understand what is going on it will not help the robot).
> > >
> > > Any help appreciated, thanks,
> > > Markus
> > >
> > >
> > > On Mon, 5 Jan 2015 00:11:24 -0600
> > > David Anderson<davida at smu.edu>  wrote:
> > >
> > >> Hi Markus,
> > >>
> > >> It would be good to know exactly where the problem lies.   How
> > >> have you determined the rather large variance in calibration
> > >> runs?  Was this done using the UMBMark?   Large left-hand and
> > >> right-hand squares?
> > >>
> > >> Is the large variance observed in  the stopping point of a
> > >> series of similar, say, left-hand runs?   Or is it between the
> > >> left-hand and right-hand runs?  Those are different problems.
> > >>
> > >> ~1 degree slop in the gear train is probably manageable,
> > >> depending on how you are doing the navigation.   If it is a
> > >> single calculation at the start of a run or line segment, then
> > >> ~1 degree theta error at the start will throw the robot way off
> > >> in X,Y at the end.   If instead the odometry navigation is a
> > >> continuously running calculation as the robot travels, then ~1
> > >> degree of error becomes less and less significant as the robot
> > >> approaches its target, and basically insignificant when the
> > >> robot is at the target.
> > >>
> > >>
> > >> Don't give up on your drive-train yet. :)
> > >>
> > >> dpa
> > >>
> > >>
> > >>
> > >>
> > >>
> > >> On 01/04/2015 02:21 PM, Markus Lampert wrote:
> > >>> Hi,
> > >>>
> > >>> I am adding odometry to one of my robots and found a rather
> > >>> large variance in my calibration runs. As it turns out my gear
> > >>> motor has some backlash resulting in ~1 degree of heading
> > >>> "wiggle room". The quad encoder sits on the motor shaft, not on
> > >>> the wheel itself which means they don't pick up on the backlash
> > >>> at all.
> > >>>
> > >>> I could not find any methods for backlash compensation other
> > >>> than for CNC machines, which doesn't seem to translate to
> > >>> mobile robots.
> > >>>
> > >>> Is there a method to compensate for it or am I in the business
> > >>> of finding a 'better' drive train for my robot?
> > >>>
> > >>> Thanks in advance,
> > >>> Markus
> > >>>
> > >>> _______________________________________________
> > >>> DPRGlist mailing list
> > >>> DPRGlist at dprg.org
> > >>> http://list.dprg.org/mailman/listinfo/dprglist
> > >> _______________________________________________
> > >> DPRGlist mailing list
> > >> DPRGlist at dprg.org
> > >> http://list.dprg.org/mailman/listinfo/dprglist
> >
> 
> _______________________________________________
> DPRGlist mailing list
> DPRGlist at dprg.org
> http://list.dprg.org/mailman/listinfo/dprglist
> 

More information about the DPRG mailing list