I'm taking an experimental robotics class this semester and it is by far the most fun of my classes. It's actually not much of a class, but more of a group project with teacher guidance. Our three hour class is just a time to meet up with our group and work together while also getting feedback/advice from the professors.
Even the projects that the various groups are working on were decided by us. I spearheaded the campaign to make a group to develop a person following robot, and fortunately there were a few others that were interested in such a project.
So the last month or so, we've been making our little Pioneer robot follow us around, and it's doing quite well! At the moment, it detects people by using a laser scanner range finder to look for legs in the vicinity. If it finds any pairs of legs, it stores them in a constantly updated world model which keeps track of the person the robot is trying to follow.
Then the navigation part gets the location of the person from the world model and actually drives the wheel motors toward the person.
Our plan is to also incorporate the onboard camera as an additional sensor for detecting people. That way if the laser can't find a person, the camera still can.
I wrote the laser leg detect using an algorithm I found described in a research paper. I also wrote the navigation code which is getting more and more complex every day! My team mates are working on the world model code (pretty complicated because it has to match up new leg detects with old leg detects even as the robot is moving) and the camera detection code.
The craziest part of this navigation code is how much trigonometry I've had to code up. I'm going to attempt to describe what I've been working on lately, because it's quite insane.
The laser scanner returns locations of people in polar coordinates relative from the robot location. That is, it doesn't return an (x,y) of the person (these are called Cartesian coordinates), but instead returns an angle (where 90 degrees is straight ahead) and a distance in meters to the person. Polar coordinates are usually written as (r,t) where r is the distance, and t is the angle.
So, I have the relative (r,t) but I need to convert that to an (x,y) in an absolute coordinate space where (0,0) is the initial starting place of the robot. To do this, I have to do a lot of math... First I get the current robot position and also the angle the robot is facing (0 degrees means it's facing along the positive x axis).
Then I convert the relative angle to the person, to an angle in the absolute space with the following equation:
absoluteAngle = relativeAngle - 90 + angleOfRobot
With the absolute angle, the current robot location, and the distance to the person, I can determine where in the absolute coordinate space the person is at with the following:
personX = distance*cos(absoluteAngle) + robotX
personY = distance*sin(absoluteAngle) + robotY
Now, at some point the robot might no longer be able to see a pair of legs. In that situation, I want the robot to go to the last known location of the person. To do that, I get the current robot location including its angle, and basically work backwards from the personX and personY that I calculated above...
First I find the absolute angle and distance from the robot to the person as follows:
distance = sqrt((robotX-personX)^2 + (robotY-personY)^2)
absoluteAngle = arctan((robotY-personY)/(robotX-personX))
Then I must convert the absolute angle into a relative angle from the robot (again where 90 degrees is straight ahead).
relativeAngle = absoluteAngle - angleOfRobot + 90
And at this point the robot has enough information to drive itself toward the last known spot of the person.
Last night I tested my first attempt at this code, and it didn't work very well. Actually it wasn't doing the steps I described above, I was doing the relative to absolute angle translation wrong which meant the robot would act crazy once it lost the legs. I had to really think hard about what was happening, and I came up with the plan as I just described. I've coded it up but I haven't been able to test it on the robot yet. I'm planning on going into the robot lab later today to try it out!
