Picture Pages of the new robot
Here are some PRO/E Wildfire rendered pictures of the robot. The robot is first designed in Pro/E mainly to make sure all of the components could fit together and then to get the dimensions of the parts to make is easier to fabricate them.
Here are some pictures of the exterior.
Here are some pictures with the frame and suspension hidden so its easier to see the interior.
Here is a close up of the suspension.
Here is a picture of the PCB panel that PCB Express made for me
Here is two pictures of the test setup before the PCBs are placed into the robot, four of the board I made (1 watt speaker amp board, 12-5volt with 6 drivers board, dual h-bridge board, IO board), with the 167 Kitcon board purchased from Phytech.
testingrobothardware.mov is a video showing the testing of the assembled boards. In it I am streaming video (from a desktop, although it was tested using the via board). The first thing I show is the logitech orbit camera being streamed to the laptop which has a game controller which controls the camera and the motors. (the robot could be controlled from either from either computer). I also show the shaft encoder, current sensor, servo motor and light. The video is being streamed with directshow from my Masters thesis. It uses motion jpeg instead of mpeg-4 since the via board cannot handle the mpeg 4 compression, although the bandwidth used is higher.
One of the goals of the project is to have hardware vision processing. As a good learning project I decided to implement the sobel filter in hardware. To do this I used the Altera Nios2 platform mainly because it has Ethernet built right in. Here is a picture of the test setup and the video. In the video the top left window is directly from the web cam. The top right is doing software sobel filtering. The bottom right filter is having it done in hardware. The software and hardware look identical that is good. Although the hardware could run at 500 times a second on a 50mhz processor (it takes around 100000 clocks to process a 320x240 image, the Ethernet only allows up to around 700 Kbytes/sec to be transferred, even less when the laptop is loaded down. However it is visible from the video that although the frame rate of the hardware is less it looks identical.
Quicktime file of the sobel filter in action, low quality though.mov
ICP (Iterative closest point) Simulation
Here is some video of my latest simulation
EM Algorithm Simulation
I first made a nice little test bench that generates random planes and places points randomly in them. I got the basics of the EM algorithm to work, the red is the generated plane and the blue is the matched plane.
So I tried some more representative planes and profiled it and I got it working at a pretty fast speed. I also tested it with noise planes and it seemed to mostly work. The noise introduced a few issues such as when two planes where close to each other or touching they would often steal points from each other.
Now that the algorithm works in the test bench I converted the code to a plug in, and placed it into the simulation
I needed to change some of the simulation constants such as the maximum plane distance error and how far a point could be from the current bounds of a plane so it could join it. After changing a few of these values it did work well. One thing is the point density really mattered in terms of the performance. The floor plane has a lower density and it is often missed. To make it worse its edge points are often stolen by the other planes. It shows how density alters the performance of the algorithm.
There are two versions of the video, one with no noise and one with noise. The with without noise still has a few plane errors mostly due to the initial guesses heavily influence the results of the algorithm. The one with noise, the noise regularly splits up planes because using 2 even though it is wrong would mathematically be a better fit. Note that sometimes a plane isn't quite rendered correctly, in particular the floor plane when the robot is rotating. This is just a rendering flaw because it uses axis aligned bounding boxes so depending how a plane is rotated sometimes errors occur.
My conclusion is that EM does a decent job but it is somewhat dependant on high point density and the preset constants.
I made a version of ICP that works with the output of the EM. It does not work as well as the point version as it should not however it works half decent. This video also uses an improved version of EM. Note that there is no version of ICP on EM with noise because the noise used makes the planes so inconsistent that they cannot be matched from one frame to the next.
This is the next step to the above. I tried to add plane mapping. I decided to start by first "cheating" by knowing the robots true coordinates and using that to map the planes to see if that would work. It did work, the red planes seen in the video are the mapped planes. I also add some ICP to that, with the hope that the ICP could be then used to replace the "cheating" position and solve the mapping problem. Unfortunately that was not the case (especially with noise) and I decided to look at other methods to map planes. It would be a while before I would solve the mapping problem and come back to the EM part.
It is interesting to note that even though this demo is not even close to the final algorithm some of the class structures are first created here and are the ones that evolve to versions uses in the working algorithm.
I was playing around with (extended) Kalman filters to get more insight into how they work. For one reason or another I could not find a decent matlab version of it that was easy to read, as the few demos I did find had many files which made the variables confusing as they changed name in each file due to using local function names! I eventually decided to make my own. It probably is not perfect and needs to be tuned but it is contained in 2 files and it is very easy to change the parameters. My main conclusion after implementing it and understanding it a bit more is that it is a bit too heavy weight of an algorithm (I know that now people use particles filters/fastslam which does have the order n^4 matrix thing going on).
This is a kalman like algorithm however, it is not a kalman filter. The underlying mechanism is similar as it uses the delta correction mechanism and simply average the corrections over the 4 planes. The only difference is the kalman gain is set manually at a fixed number rather then calculating the covariance matrix however it does a good job in keeping close to the actual position. It does need a bit of smoothing though, that hopefully I will add to a later algorithm
This video shows a kalman like filter with 4 planes. The start to the full mapping thing. Notice it is not 100% accurate however it does not having an open loop divergent error.
The next simulation uses more of an EM style algorithm to place the plane. It works pretty well as it stores past readings and uses it to update future readings. The black represents the noisy plane readings, the blue represents the calculated plane which is pretty much stable. The red is the real position and the blue is the calculated position. The calculated position is only used to initially match the planes and it not used other then that which makes the fact that it is noisy tolerable. It is noisy simply because it is the last reading, and it is unsmoothed so it represents the noise from the last reading.
There is no given Position or Rotation information, everything is calculated which makes this a pure slam. However it is unrealistic to have planes that are always the same size and to be able to see all of them. The next step is to add the ability to figure out what kind of information a plane is giving and to smooth the results.
After a bit of time elsewhere I took a look at all the stuff I did and I realized it had some pretty big flaws. Some of it algorithmic some of it how I am approaching the problem. I realized the main problem to solve was not the noise but rather planes appearing, disappearing and fading out. An example is going down a hallway and having the a side wall disappear out of vision behind you.
So I switched to a particle filter approach with the main difference of each corner of a plane being one particle. When a plane is shifting or getting bigger or smaller some of the particles will be correct some won't. So I have to filter out of the wrong ones to remove the error.
The other problem with my approach is that I was looking at solving a plane and then using that to figure out location. The better way is to solve location and then use that to solve a plane. This made a whole bunch of small changes that really improved how it worked.
pftbnocorrection.avi I am adding some noise particles that skew the robot to go one way in both rotation and translation. Since there is no correction the robot goes way off. There is some noise to the planes here.
pftbcorrection.avi I am using the particle filter to correct for the noise particle. It is not perfect because if the noise from the planes aligns with the noise from the particle it can count for movement
pftbnonoiseparticles.avi This is without the noise particles just to show that it does not cause any extra errors
I found about a book by Thrun called Probabilistic Robots which was his textbook version of the past few years of papers, many of which I read. It contained a lot more details then his papers.
During it I realized he wasn't quite using particles the way I thought they were, in that they were more using it for computational efficiency to do with the Kalman Filter. In any case this changed my thinking of particles filters and the whole thing with reweighing them to minimize the error. Most of Thruns work (especially since pass) is an online algorithm where nothing is stored. Most of his offline has to deal with finding loops, and in those he mentioned mostly he does hill climbing. I realized that since I am doing a hybrid mabey hill climbing would be better since I could hill climb my way out of incorrect guesses rather then store particles to represent different guesses.
In any case I tried this and made quite a few improvements from above
ptbnocorrectfullnoise.avi: This one contains no particle injection and only zero mean noise. Notice that it doesn't converge. The reason is the rotation noise. I didn't realize this earlier but translationmovement = translationmovement*rotation and when there is noise in the rotation the translation will be wrong. It does not occur when there is just translation noise and not rotation noise. There is a way to potentionly fix this by guessing what is noise and what is real by smoothing the rotation over time and then feeding that back into the translation. But plane locking (simular to smoothing) would do that same thing so I left that it. This isn't really a problem I would want to solve anyway since this really is just another form of noise.
ptbfullproblemnocorrection.avi: This has both noise and particle injection. One of the particles was a rotation error the other was a translation but during a rotation movement the same particle injections occur, that is why it goes well off during rotations. This is what happens with 0 correction
ptbfullnoparticleinjection.avi: This is no particle inject but noise with the correction. Notice it is pretty accurate except for small errors, probably the noise creates some long term minimization problems which is why there is more then 0 drift although it isn't too bad.
ptbfullproblemnonoise.avi: This contains no noise but has particle injection. Notice there is no errors are there shouldn't be any because there is no particle injection in the long term readings just the short term ones.
ptbfullproblem.avi: Solves both noise and particle injection. Slightly less accurate then the version without the injection since I guess sometimes noise is assumed to be a correct reading, but not bad otherwise
ptbfullwlocking.avi: This is the same as above except I lock the readings after 50 iterations. The end result is there being no real drift. I suppose 0 drift might be impossible due to noise not being zero mean or math float point errors (occurred a lot so far). The whole point of the algorithm is just to get the best medium term result and use that to lock in as at that point the error is really small, since the drift takes a long to to occur. The issue is when to lock in? that is when is the error the smallest. I am using uniform noise so far so any period is about the same but generally there will be more noise with higher range. However then there is a problem that using a correct plane but noisy long range data will cause errors. I am probably going to put in some rules which use the most accurate readings first if possible and if not really on less accurate ones.
Special thanks goes out to PCB Express for making the PCBs for me