Bug Toy - PID - Visual Tracking and Moon Delay

GroG · April 16, 2011, 2:27am

Update 2011.4.20

First hacked attempt at a P - control.. it got a little complex being multi-threaded - the feedback on one thread, the commands on another. The result is in the video (at the bottom of this post). I think I have to boost the kp constant a little..among other things

Things I noticed:

dwindling battery power affects everything - once there is more feedback as to how "successful" any move was - I might have error change power levels too (versus length of time as it does now)
there is a +200 ms in estimating times because anything under this value has really no effect
there are two forms of feedback 1 is the angle difference "headingDelta" and feedback of position in expected time.. right now an error is only assessed at the end of the move.. it was my plan at some point to have this thread monitor the error value associated with the command which was saved 700 (lag time) ms ago in some array (this has not happened yet)
there should be a way to derive lag time dynamically
there should be a way of saving output even in spread sheet form so that some real analysis can come out of it...

       while (isRunning)
       {
         
           try {
             
             if (headingCurrent ==  headingTarget)
             {
               synchronized (lock) {
               lock.wait(); // WAIT for a heading to change to
               }
             }
                         
             // save power command, target heading, and time into array (round-robin the array)
             
             // compute PID of (800ms target) with - current feedback 
             // Output = kp * error + ki * errSum + kd * dErr;
             
             // speed at the moment is unsafe - browns out at 50% power
             estimatedTime = Math.abs(headingDelta) * 10 + 200; 
             
             // if turn time > 800 ms - lag time - i can check and adjust while turning - if not i have to pause to correct
             // send corrections - am I moving.. am I not moving ?
             beginMotion = System.currentTimeMillis();
             if (headingDelta > 0)
             {
               // correct right - don't block
               right.moveFor(-0.13f, estimatedTime);
               left.moveFor(0.16f, estimatedTime);
               
             } else if (headingDelta < 0)
             {
               // correct left - don't block
               right.moveFor(0.13f, estimatedTime);
               left.moveFor(-0.16f, estimatedTime);
             }
                         
             estimatedTime += 700; // add lag time
                         
             Thread.sleep(estimatedTime); // wait
             // wait for estimated time + lag time
             // check feedback for correction 
             
             // combine with current command
             // feedback = waitForHeadingChange();
             
             
             
           } catch (InterruptedException e) {
             LOG.warn("duration thread interrupted");
           }
           
         
         }
     }

Update 2011.4.18

Thanks for the feedback. Being that I understand things better in a visual medium, I have asked my elves to draw up the problem (and possible solution) in graph form. They have come up with the following graph.

The issue is a 1500 ms lag time of video feedback for position control. I was interested in the possibilities of using P, PD, or PID in order to provide accurate controls for tracking or positioning Bug Toy. Tele-fox and JIP (lag/lead) suggested a possible solution would be to "save" power levels, therefore it can be thrown back into a regular PID request. Warning psuedo-psuedo-code follows.

Loop :

save the current power level - so it can be used in an error calculation in 1500 ms
get the current feedback and use it in PID with saved power level from 1500 ms ago - derive error
add error and adjust current power level

With 70 ms sampling rate there will be a set of about 22 samples saved in the 1500 ms lag. Its a queue of applied power levels, push the current power level on, pop the last one off to use it in PID. This way the power profile and feedback follow one another, even though the feedback is 1.5 seconds out of phase.

Did the elves do OK? Now, hopefully I can get them little guys to write the actual code.

Also, I've been reading about how I & D are difficult if the sampling rate is not constant. The video frames are AROUND 70ms but can vary 50ms to 110ms.. How problematic will this be? I was going to tell the elves to start with a queue and just P-roportional ... since they've been working so hard lately.

Hello !

I've started a few tests with Bug Toy and discovered I needed to create a "Differential Drive Service" for MRL. The idea was if a few variables were supplied regarding the footprint of any Bot (not just Bug Toy) an accurate model / map could be constructed and all the calculations and processing would be available from the service.

Additionally any sensor which provided absolute or relative positioning could be attached to the service to provide more SLAM data.

Encoders, odometers, even wii camera on the bot could provide relative positioning. A video camera can provide absolute positioning.

I'm working on the video position currently. I've learned a few things. Like, white electrical tape provides extremely good marker points for Lucas Kanade Optical Tracking

One of the benefits of working with video in this way is you get the whole map at once. There is no scanning from side to side. It is absolute. But here's the trade off.

The lag between "real life" to display can be 1.5 seconds ! Although the "processing time" of each frame is only 70 ms when tracking Bug Toy. It's a stream packed full of data every 70 ms, but running 1.5 seconds behind.

1.5 seconds would happen to be the minimal amount of lag for controlling something on the moon since its 1.5 light seconds away. It gave me new appreciation for "very remote" control. It also made me think about how some moon rover software might work. Immediate obstacles would have to be dealt with by the rover, but finding areas of interest or best paths to known destinations would be done remotely. The immediate obstacle avoidance would have the ability to repress the best path objective when needed. But when an obstacle was not present the best path behavior would be dominant (subsumption architecture).

Jeeze, where's the question already? It's coming...

I started looking into PID control for the Differential Drive Service. I have never implemented a PID control, but at this point I have looked at Big Face's tutorial and I am trying to digest several others examples on the internet.

The question is: Would PID be applicable with such a large lag, when lag will so grossly affect Error?

I have started a "calibration" routine which attempts to determine the exact amount of time between starting a command (turn right) and the feedback. Once this is done, the system can further calibrate by testing power levels and look at their results in the video stream at the expected lag time. This calibration seemed like a good idea, but now I'm debating whether it could all be implemented in an appropriate PID control?

Things that make you go Hmmmmm....

Oh, I just remembered... this is how I would expect the SoccerBots control system to work, where there is an immediate/localized behavior which is quick/tactical and a remote strategic behavior with laggy commands and feedback for the players controlling the SoccaBots.

https://www.youtube.com/watch?v=WwDbVX8au2s?hl=en

jip · April 16, 2011, 3:38am

I’m not sure if this will

I’m not sure if this will help you in any way with the kind of lag you have in your setup, but I’ve read briefly about lag-lead compensators used to help cure the problem, that when using a PID it usually takes non-zero time to measure the error and thus introduces a small amount of lag.

GroG · April 16, 2011, 10:01am

Thanks for the lead

Thanks for the lead Jip

http://en.wikipedia.org/wiki/Lead-lag_compensator

I read it… can’t say I really understand it…

I’ll try banging my head at it for an extended period of time… ouch… ouch… ouch…

GroG · April 16, 2011, 3:04pm

Making some progress

I really liked this article who’s intent is to explain it to kids…

http://www.inpharmix.com/jps/PID_Controller_For_Lego_Mindstorms_Robots.html

Turn = Kp(error) + Ki(integral) + Kd*(derivative)

He steps through each part P, I and D… unfortunately he goes through and has some caveats about the feedback being “immediate”

Which in this system is what is giving me a headache…

GroG · April 16, 2011, 3:17pm

CTC and I were shouting about it

He mentioned move very slowly to compensate…

I was thinking I prefer fast and a little jerky…

Like this

Predict where to turn
Send turn command (possibly full speed)
1.5 seconds later you would get the result
compensate for error -> go to Predict

add to this the complexity but possible benefit of not actually waiting 1.5 seconds, but checking all the frames of video for correctness as you go

axbri · April 16, 2011, 4:50pm

PID regulation

You asked for my opinion, but my “expertise” is very limited. I belive that it is hard to make a PID regulation when time lag between measured and actual value is to big. Especially the D-part can easily cause oscillations. Maby you can run your robot slower…

telefox · April 17, 2011, 12:55am

Direct PID is designed for

Direct PID is designed for ‘online’ systems, with control loops operating in real time (or close to it). Your system is more like an offline system, where the feedback is received after the motion is carried out, rather than during it.

Instead of using a PID loop to directly control the output to Bug Toy’s motors, you could use a PID loop to tune a set of variables used in generating a ‘move profile’ which is then used to control Bug Toy.

A simple move profile might involve a turn move followed by a straight-line move.
(!!!WARNING!!! Flakey pseudo-code ahead!)

The turn part is calculated as:
IF(turnAngle > 0) THEN                                   //turnAngle is the relative turn angle, CW = positive
     leftMotorPower = 100%
     rightMotorPower = -100%
ELSE IF(turnAngle < 0) THEN
     leftMotorPower = -100%
     rightMotorPower = 100%
ENDIF
turnTime = turnAngleturnControlFactor            //turnControlFactor is the value derived from one of your PID loops, it converts
                                                                     //the turn angle into the time required to make the turn

The straight-line part is calculated as:
IF(moveDistance > 0) THEN                                   //moveDistance is the relative move distance, forward = positive
     leftMotorPower = 100%
     rightMotorPower = 100%
ELSE IF(moveDistance < 0) THEN
     leftMotorPower = -100%
     rightMotorPower = -100%
ENDIF
moveTime = moveDistancemoveControlFactor //moveControlFactor is the value derived from one of your PID loops, it converts
                                                                     //the move distance into the time required to execute the move

After both parts of the move profile have been calculated, you can have Audrey issue the commands, i.e. tell Bug Toy to run both motors in opposition for turnTime (ms), then tell Bug Toy to run both motors together for moveTime (ms), then tell Bug Toy to stop. Upon receiving the results of the move, you can calculate the actual turn angle and actual move distance, and compare them to the intended angle and distance, to get two errors for feeding into the PID loops.
Note that if turnTime + moveTime exceeds the lag between starting the profile and getting the feedback you’ll have to factor in a partially completed move when using the intended turn/move values in the PID error calcs.
The result of the PID loop iterations will give you updated values for turnControlFactor and moveControlFactor, which are then used to build the next move profile.

In my example, both motors run full tilt all the time, but you may want to scale them back for short turns/moves, and instead increase the turn/move time to compensate. If you want to get real fancy you can improve the performance by using a non-linear calculation for turn/move time, and introduce a whole range of other control factors, but I don’t think you’d really see much benefit.

Perhaps one of the more interesting features of this method is that you can have multiple ‘sets’ of turnControlFactor and moveControlFactor that are applied under different circumstances. Maybe when Bug Toy is carrying an object you switch over to a different set? Maybe there’s a set that’s used when travelling over a certain type of terrain?
By breaking the control factors into sets you can have the system tune itself for multiple scenarios, without ‘unlearning’ how to work well in areas that have already been tuned.

GroG · April 17, 2011, 5:23pm

Thanks for the details

Thanks for the details explanation, suggestions, and pseudo code

I have thrown your nuggets of wisdom to my elves… some exploded, but others appear are beginning to digest them… I’ll update later with their reactions…

GroG · April 18, 2011, 1:32pm

Is the update “sort of” like

Is the update “sort of” like your solution?

telefox · April 18, 2011, 6:45pm

Yes, the principle looks

Yes, the principle looks about right. In my head I imagined a series of short moves (under 2s each) that were constantly being queued up, but there is nothing wrong with planning a entire A-to-B sort of move and then tracking how well Bug Toy is following the intended profile.

Starting with just the proportional factor is a good idea. Once you’ve got a good foothold you can try increasing Kp and using Kd to ‘dampen’ the response. You might be surprised at how many industrial PID controllers have Ki set to 0 =)

GroG · April 20, 2011, 9:55pm

It will take more tinkering

It will take more tinkering for sure ! I have half a “calibration” routine… just to get funky and bust some moves, and record the values.

Initially I thought that calibration was going away from PID control, but now I understand just enough to see that calibration could be used to figure out what all the constant factors are in the PID.

Calibration is just purposeful feedback time…

telefox · April 20, 2011, 10:08pm

Nothing wrong with

Nothing wrong with calibration… even mammals have to calibrate themselves in the beginning =)

Looks like you’ve identified the ‘zero-offset’ value with your minimum estimatedTime of 200ms. That might be a value that also has to vary with motor power if you really need the accuracy further down the track.

The moveFor() functions seem like a convenient way to control the motors, especially for this application.