drhastings
Posts: 113
Joined: Wed Feb 06, 2013 11:38 pm

Balancing robot with camera

Sat Jul 27, 2013 3:25 am

Hello,

Just wanted to share my current project, here is a fairly poor video. http://www.youtube.com/watch?v=KxqTbxvpaxw

Basically its just a Pi with an added board carrying a motor driver, power regulation circuitry and an mpu6050 accel/gyro package. I've also wired in some wheel encoders to enable it to stay in one spot. As you can see the PID constants need a little tuning but it can stand up finally. I also need to finish figuring out what remote control scheme is best and then implement it. I'm thinking just plugging an old usb xbox 360 controller I have into another pi that I have running as a media center and then using the tv as a display and controller to explore/harass my cats.

You may have noticed the camera on the pan/tilt bracket attached to the top. I've got that streaming video over wifi and I've had some success with tracking faces and using the servos to keep the camera pointed at a face (Not while balancing yet). I'm hopeful that I might be able get some kind of automated behavior out of it as well, probably start with line following then move to attempting to station keep around an object detected by the camera.

Most of the software is written in c, with a little bit of c++ to get to all the opencv goodies.

Thanks to Joan for your pipio library which I'm using for the servos and pwm.
Thanks to rgh for porting the i2cdevlib mpu 6050 code to the linux i2c system (I'm using the int pin though).
Thanks to Pierre Raufast whose magic mirror project was helpful in getting opencv installed and getting the build environment set up.

I'll post more videos as I get more functionality on line if anyone is interested.

Edit: Another video. http://www.youtube.com/watch?v=2NiU6QBj8L0
Edit: New Chassis Video. https://www.youtube.com/watch?v=YTfwhKtGmlo
Edit: Camera feed opengl trickery. https://www.youtube.com/watch?v=y9BnMib3P3I
Edit: Color blob tracking implemented on the gpu. https://www.youtube.com/watch?v=_BYPBhzZD6E
Edit: New camera mount. https://www.youtube.com/watch?v=rx2B6ifhy70
Edit: Robot point of view video. https://www.youtube.com/watch?v=TQRH5pjSjKc
Last edited by drhastings on Fri Feb 21, 2014 6:24 am, edited 6 times in total.
http://www.dansrobotprojects.com/

CraigR8806
Posts: 1
Joined: Sat Jul 27, 2013 4:27 am

Re: Balancing robot with camera

Sat Jul 27, 2013 4:30 am

Dude that is soooo awesome!!! I saw a TedTalks where they had a ball that was what made contact with the ground and three wheels in a triangle formation were what assisted in its balancing. Just an idea for you. Keep the videos comin tho dog!!

User avatar
joan
Posts: 14472
Joined: Thu Jul 05, 2012 5:09 pm
Location: UK

Re: Balancing robot with camera

Sat Jul 27, 2013 7:50 am

That's a great job.

What would be really, really useful would be some auto-tune PID software.

drhastings
Posts: 113
Joined: Wed Feb 06, 2013 11:38 pm

Re: Balancing robot with camera

Sat Jul 27, 2013 5:20 pm

joan wrote:What would be really, really useful would be some auto-tune PID software.
Thanks for the encouragement.

I agree this would be incredibly useful especially considering the fact that the motors are seeing raw battery voltage. The last iteration of this had a micro-controller in the mix so I was able to use an adc to sense drooping voltage and apply a linear compensation factor to the power I was feeding the motors which worked decently but I can't do that with this round of hardware.

I'm not quite sure how to go about auto-tuning the PID constants. I've never formally studied control theory. Come up with some cost function and then have the program vary the constants until it finds the minimum? I'm also essentially running three control loops that all feed into the output power to each wheel each cycle. A PD loop looking at the angle which pushes it upright, a PD loop looking at the position of the wheels keeping it still, and a P loop that keeps it from rotating while it does everything else. I'm not sure how that would effect things for trying to auto tune.

Auto tuning is a bit down the road though as far as things to work on though. I want to complete the basic functionality first.
http://www.dansrobotprojects.com/

Ravenous
Posts: 1956
Joined: Fri Feb 24, 2012 1:01 pm
Location: UK

Re: Balancing robot with camera

Mon Jul 29, 2013 1:02 pm

My own control theory was decades ago, and I doubt even now auto tuning works for all kinds of system. The ziegler/nicholls method was shown to me as the "standard" though I don't think it works while the system's running. It's more of a way to set the system up based on a few tests at the start, if I remember correctly.

Anyway I really barged in to say - auto tuning is probably a good idea for a balancer like this! If one's made to carry varying cargoes around on its head, the change in weight (and balance position!) will change the system response a lot. This makes me wonder how commercial designs like the Segway work - they balance from no one on board to a person heavier than the machine. Whether or not it's using PID, it might be doing some sort of compensation, at least when the user climbs on.

drhastings
Posts: 113
Joined: Wed Feb 06, 2013 11:38 pm

Re: Balancing robot with camera

Tue Jul 30, 2013 5:57 am

Ravenous wrote:My own control theory was decades ago, and I doubt even now auto tuning works for all kinds of system. The ziegler/nicholls method was shown to me as the "standard" though I don't think it works while the system's running. It's more of a way to set the system up based on a few tests at the start, if I remember correctly.

Anyway I really barged in to say - auto tuning is probably a good idea for a balancer like this! If one's made to carry varying cargoes around on its head, the change in weight (and balance position!) will change the system response a lot. This makes me wonder how commercial designs like the Segway work - they balance from no one on board to a person heavier than the machine. Whether or not it's using PID, it might be doing some sort of compensation, at least when the user climbs on.
Thanks Ravenous. Yes auto-tuning would be handy but I've come to realize a good deal of the oscillations I'm seeing are due to the variability in latency between when a new set of sensors readings becoming available and when a new pwm is set for the motors. Given the small masses I'm moving and the torque available from these motors I've found that I've been able to get much more stable operation just by cranking up the proportional pretty high when I was using an avr for the control, even under slightly varying loads. I'm slowly moving more of my sensor reading into kernel space in an attempt to reduce said latency but I think at least part of the loop is going to have to run in userspace. I don't really want to figure out floating point in kernel and I'd also rather not switch to integer math with the FPU available.

That aside, zeigler-nichols is where I started to get the constants I have and I imagine one could base an auto-tuning algorithm on it although I would probably have to code in lower and upper bounds for the various constants to keep it from throwing itself on the floor while exploring for better numbers.

Currently I've written a kernel module that reads the encoders, to get the value you read from a device file. I think what I'm going to do is move the mpu i2c read into the kernel as well. The mpu will signal new a new set of data, the kernel module will then trigger the read in kernel space. Then I can have any read of the device file block until the next set of mpu data is available and have the read return all the sensor data. Then I can hopefully use the blocking read on a high priority thread as the timing method used by the control loop. Timing will never be as tight as on the avr but I'm hoping I can reduce the timing jitter I'm seeing now.

I think I'm going to ditch pigpio eventually as well as I think it is doing a number of things I don't need and I think I can probably speed up writing a new pwm value if I concentrate on my particular use case as opposed to the general case as it looks like Joan has.
http://www.dansrobotprojects.com/

User avatar
joan
Posts: 14472
Joined: Thu Jul 05, 2012 5:09 pm
Location: UK

Re: Balancing robot with camera

Tue Jul 30, 2013 7:08 am

That's bad news. From the video it looked like the PID needed tuning. I suppose this is another example of the last 10% of the work takes 90% of the time.

mikerr
Posts: 2789
Joined: Thu Jan 12, 2012 12:46 pm
Location: UK
Contact: Website

Re: Balancing robot with camera

Tue Jul 30, 2013 9:46 am

Moving more weight (battery) up to the top will help with oscillating as well.
Its tempting to get drawn into faster sensors, but instead you can slow the fall rate.

erigr222
Posts: 10
Joined: Tue Apr 30, 2013 8:01 am

Re: Balancing robot with camera

Tue Jul 30, 2013 12:50 pm

Are you actually using the camera for stabilization? Im working on a similar project, but I find the RPi to be way too slow for any real time image processing.


/Eric

drhastings
Posts: 113
Joined: Wed Feb 06, 2013 11:38 pm

Re: Balancing robot with camera

Tue Jul 30, 2013 3:48 pm

@Joan, yes certainly needed tuning from that video. https://www.youtube.com/watch?v=exTqdS_RtiQ This link shows how stable I was able to get it using the same motors, sensors etc using an avr for the control. The main difference I'm seeing now is that periodically it looks like I get latency spikes that end up shoving the thing off center. I was also able to run my sensor at 200khz with the avr and keep up with it but I keep seeing fifo overflows on the sensor. I'll experiment and see what I can do but it might be a bit.

@mikerr, true but then again, that statement will always be true and the center of gravity is higher on this one than on the other robot linked to from this post so I'm optimistic I can improve what I've got. I find the programming more fun than the construction anyway.

@erigr222, no the camera doesn't help with the orientation. I agree with your statement about the pi being slow for image processing but I've been able to get ~7 frames a second or so of lbp cascade matching (not while balancing). That's one hell of a lot better than I've ever done with an avr ;) so I'm sure I can figure out something fun to do with that.
http://www.dansrobotprojects.com/

User avatar
joan
Posts: 14472
Joined: Thu Jul 05, 2012 5:09 pm
Location: UK

Re: Balancing robot with camera

Tue Jul 30, 2013 4:38 pm

The system on both videos appears stable. The system needs a little instability to work. I'd be proud of either achievement.

drhastings
Posts: 113
Joined: Wed Feb 06, 2013 11:38 pm

Re: Balancing robot with camera

Tue Jul 30, 2013 5:27 pm

joan wrote:The system on both videos appears stable. The system needs a little instability to work. I'd be proud of either achievement.
Thanks Joan and don't worry about that as I am! Doesn't mean I'm about to put this down though as I'm finally at the point its turning into a toy instead of a pile of parts. :D
http://www.dansrobotprojects.com/

drhastings
Posts: 113
Joined: Wed Feb 06, 2013 11:38 pm

Re: Balancing robot with camera

Thu Aug 01, 2013 11:29 pm

As I anticipated moving the sensor reading code into the kernel has helped with the control loop by both reducing the magnitude and the variability in latency between a new orientation solution from the mpu and getting a new pwm value written to the motors.

After doubling the frequency of the control loop to 200hz and about 5 minutes of parameter tuning I was able to arrive at this. https://www.youtube.com/watch?v=tI3ENpd267w The quality of this video is pretty poor, I'll get a better one when it can do something a little more interesting.

It oscillates a lot less and I know I'm not really optimal on my constants but over all I'm quite pleased with the improvement. As a plus side, running the camera, at least for streaming video, is no longer interfering with the control loop timing so I'm now able to stream to another pi I have attached to my tv, you can sort of see it towards the end of the video.

Now I just need to finish the remote control software, tune up my constants and I can start roaming about.

If anyone is interested I'll share the software but it is pretty heavily in flux right now.
http://www.dansrobotprojects.com/

User avatar
elektrknight
Posts: 140
Joined: Sat Mar 02, 2013 1:25 pm

Re: Balancing robot with camera

Fri Aug 02, 2013 6:30 am

drhastings wrote: Yes auto-tuning would be handy but I've come to realize a good deal of the oscillations I'm seeing are due to the variability in latency between when a new set of sensors readings becoming available and when a new pwm is set for the motors.
That is critical observation that points to the culprit. You have phase shift in your control loop, the closer it gets to 180 degree
the more oscillations you will see if the first order approximation holds for your system. In any case you probably need to
increase phase margin in your feedback loop to stabilize your bot.

And to do that you will need a real-time response which is why I need to mention Machinoid real-time distro http://www.raspberrypi.org/phpBB3/viewt ... 3&p=397551
Placek Malinowy to jest to!

drhastings
Posts: 113
Joined: Wed Feb 06, 2013 11:38 pm

Re: Balancing robot with camera

Fri Aug 02, 2013 7:48 am

elektrknight wrote:
drhastings wrote: Yes auto-tuning would be handy but I've come to realize a good deal of the oscillations I'm seeing are due to the variability in latency between when a new set of sensors readings becoming available and when a new pwm is set for the motors.
That is critical observation that points to the culprit. You have phase shift in your control loop, the closer it gets to 180 degree
the more oscillations you will see if the first order approximation holds for your system. In any case you probably need to
increase phase margin in your feedback loop to stabilize your bot.

And to do that you will need a real-time response which is why I need to mention Machinoid real-time distro http://www.raspberrypi.org/phpBB3/viewt ... 3&p=397551
Although I could certainly nail down timing even further if I installed a real time os, I've solved the problem adequately for my needs. I'm running a loop at 200 hz and according to gettimeofday I'm only 1.7% off nominal on average in timing which ends up being 85 usec by some quick calcs, even with opencv eating the rest of the cycles. Miles better than I was seeing before and I think much more work in this direction will just be gilding the lily.
http://www.dansrobotprojects.com/

User avatar
elektrknight
Posts: 140
Joined: Sat Mar 02, 2013 1:25 pm

Re: Balancing robot with camera

Fri Aug 02, 2013 1:04 pm

drhastings wrote:I'm running a loop at 200 hz and according to gettimeofday I'm only 1.7% off nominal on average in timing which ends up being 85 usec by some quick calcs, even with opencv eating the rest of the cycles.
Keyword is on average, running on Machinoid would give you a much much better guarantee.
BTW Machinoid is fully compatible with Raspbian so you can just take all of your binaries and run on it.
The only exception being your kernel module, you would need to recompile it as Machinoid kernel is 3.5.7 based
all build tools are included.

I love your videos! Any chance you could share your code?

Another way of dealing with these oscillation would be to increase them and turn your robot into sort of Kapitza's pendulum
on wheels. But that could be way too much shaking :-)
Placek Malinowy to jest to!

drhastings
Posts: 113
Joined: Wed Feb 06, 2013 11:38 pm

Re: Balancing robot with camera

Fri Aug 02, 2013 4:19 pm

Rather, keywords "adequately solved". I've seen no numbers on guaranteed response times for that system and any jitter less than the pwm period I'm using (400 hz currently) is just going to fall out in the wash since a new value isn't going to affect the output until it enters a new period anyway. I'm still not seeing a great benefit to going to the trouble. If I was trying to drive steppers I would probably be more concerned.

I'll put some code up but it will likely be next week as I'm heading out of town for the weekend.
http://www.dansrobotprojects.com/

Ravenous
Posts: 1956
Joined: Fri Feb 24, 2012 1:01 pm
Location: UK

Re: Balancing robot with camera

Fri Aug 02, 2013 4:44 pm

Agreed, I think a system like that should run fine with a few hundred hertz sample rate! I mean if you didn't control it at all it would take quite a few cycles to start to tip. Any wobbles in there at the moment are probably a matter of tuning, if you need it to serve drinks at a party or something. Certainly good enough for now.

drhastings
Posts: 113
Joined: Wed Feb 06, 2013 11:38 pm

Re: Balancing robot with camera

Fri Aug 02, 2013 5:36 pm

Ravenous wrote:Agreed, I think a system like that should run fine with a few hundred hertz sample rate! I mean if you didn't control it at all it would take quite a few cycles to start to tip. Any wobbles in there at the moment are probably a matter of tuning, if you need it to serve drinks at a party or something. Certainly good enough for now.
Well tuning and the fact that the encoder hard ware isn't exactly 100%. The encoder wheels can wobble a little bit which leads to missed counts and if it wanders near a strong ir source, like a bright incandescent bulb it can lose or gain events. If it drives into direct sunlight it pretty much tries to commit suicide as it thinks the wheel are running away.
http://www.dansrobotprojects.com/

drhastings
Posts: 113
Joined: Wed Feb 06, 2013 11:38 pm

Re: Balancing robot with camera

Tue Aug 06, 2013 7:35 am

I've got some remote control working along with the a remote camera feed. The software isn't quite calibrated for the kinds of moves the robot is actually capable of yet and the pid constants need attention still.

I think the position profiling I'm using for the motion control will be improved significantly by adding in some consideration for acceleration.

http://www.youtube.com/watch?v=2NiU6QBj8L0
http://www.dansrobotprojects.com/

drhastings
Posts: 113
Joined: Wed Feb 06, 2013 11:38 pm

Re: Balancing robot with camera

Thu Feb 13, 2014 8:06 am

I've made a new chassis out of threaded steel rods and plexiglass. I'm still playing with the new constants for the PD controllers but they seem pretty good.

Video of new chassis

The frame is much more rigid which is nice. I still need to make a mount for the camera assembly. I've got the math worked out for the servos and a bunch of openmax and opengl doing various tasks with the camera feed. For example, the robot can calculate a "roll" angle that corrects the video stream to world up in real time based on the robots orientation and the angles of the servos holding the camera.

I've also had some preliminary success with color threshholding based blob detection through opengl shaders. The final step of the pipe line is a 1x1 pixel framebuffer whose color values encode the blob centroid. Its not perfect yet, I think I need to read a little bit more about color spaces. I'm currently doing the threshholding off the u and v values in yuv but I think I'll get better results if I use HSV instead. I have a couple of more stages to add to the pipeline, effectively a multi-stage render, that I think should improve the results but it woks on a 640x640 video stream at 25 fps with minimal load on the cpu and very little data transferred between the arm and the gpu.

I'll get a video together of that in action once I've got it working a little bit better.

Another idea I've had is using opengl shaders to find the brightest point in the image which should be the laser pointer then using trig to get the range to what the laser is striking. I don't know if I will bother though as I can think of better uses for the gpu cycles and there is already a sonar mounted next to the camera which gives some range info.
http://www.dansrobotprojects.com/

drhastings
Posts: 113
Joined: Wed Feb 06, 2013 11:38 pm

Re: Balancing robot with camera

Fri Feb 14, 2014 1:10 am

As mentioned, a brief video of the blob tracking in action.

This is from the robots perspective as it tries to keep a red handkerchief centered in the camera. The centering logic is very crude at the moment but I'm sure it can be improved upon. I think the blob tracking will work better if I add a blurring stage and perhaps erode the scene a bit after thresholding and before finding the centroid.

The most exciting part is that this takes minimal cpu time as the the first time I bring anything to the cpu it is just the coordinate value for the centroid.
http://www.dansrobotprojects.com/

Ravenous
Posts: 1956
Joined: Fri Feb 24, 2012 1:01 pm
Location: UK

Re: Balancing robot with camera

Fri Feb 14, 2014 12:13 pm

The blob tracking is interesting, especially if it doesn't slow down the rest of the robot control.

Is that with a pan & tilt mechanism on the camera, or is the chassis rotating itself for the pan? (Just curious.)

drhastings
Posts: 113
Joined: Wed Feb 06, 2013 11:38 pm

Re: Balancing robot with camera

Fri Feb 14, 2014 4:28 pm

Ravenous wrote:The blob tracking is interesting, especially if it doesn't slow down the rest of the robot control.

Is that with a pan & tilt mechanism on the camera, or is the chassis rotating itself for the pan? (Just curious.)
This video is just using a pan tilt mechanism. I haven't had a chance to integrate it with the movement logic yet. I also need to realign and attach the encoders before it moves much anyway.

I need to improve the camera tracking logic as well. Currently its as simple as centroid left of center, nudge left, right of center, nudge right, etc... I can think of a couple of ways to improve on that when the time comes but I think my time is better spent on the shaders at this point.
http://www.dansrobotprojects.com/

drhastings
Posts: 113
Joined: Wed Feb 06, 2013 11:38 pm

Re: Balancing robot with camera

Tue Feb 18, 2014 1:17 am

Another video, The new chassis is working out well. https://www.youtube.com/watch?v=rx2B6ifhy70
http://www.dansrobotprojects.com/

Return to “Automation, sensing and robotics”