# 2D path following robot, converting XY axis path to input on wheels

at the moment I am creating an android program, that will steer my simple, 3 wheel (2 motors, 1 for balance) robot to move online following the path drawn by user on his screen. The robot is operated through WiFi and has 2 motors that will react on any input signals.

Imagine user drawing a path for this robot on smartphone screen. It has aquired all the points on XY axis, every time beginning with (0,0). Still I have no idea, how to somehow "convert" just points, into voltage input to both motors. Signals will be sent in approx. 60Hz connection, so quite fast. Maybe not every single axis point will be taken into consideration, there will be surely some skips, but that is irrelevant, since this path does not have to be done perfectly by the robot, just in reasonable error scale.

Do you have any idea on how to make the robot follow defined axis points that overall create a path?

Edit 10.01:

The voltage will be computed by the robot, so input on both is between -255 and 255 and the velocity should increase or decrease lineary in those borders. Additionaly, I would like to solve it as if there were perfect conditions, I don't need any feedback crazy models. Let's assume that all the data is true, no sensors and additional devices. Just XY axis path and required input (ommit wheel slide too).

You're attempting to move a robot along a predefined path without the aid of sensors, so really we just need to convert the list of points into a list of pre-scripted actions.

1. Convert input points to $({\Delta}x, {\Delta}y)$ pairs
2. Convert $({\Delta}x, {\Delta}y)$ pairs to $(\Delta\theta, d)$ pairs

3. Convert $(\Delta\theta, d)$ pairs to $(V_{left}, V_{right}, {\Delta}t)$ tuples

The first step is easy -- simple subtraction.

The second step is also fairly straightforward: using the pythagorean theorem for the distance and the atan2 function for the angle:

(Then just keep track of the last $\theta$ so you can calculate $\Delta\theta$).

The last step is a little tricky. You want to convert that set of angles and distances to the left and right wheel voltages, and the time to run them. This will actually give you two $(V_{left}, V_{right}, {\Delta}t)$ tuples for every $(\Delta\theta, d)$ pair: one to change the angle, and one to travel the distance.

For a given width $w$ between the wheels, the change in the angle (radians) will be based on the movements of the right and left wheel: $$\Delta \theta = \frac{1}{w} \left( {\Delta}d_{left} - {\Delta}d_{right}\right)$$ You'll have to decide what voltages and $\Delta t$ will produce that change in distance for your robot.

Next, you'll do the same calculation for $d$. Both motors will use the same voltage to (hopefully) travel the same distance.

That should produce a list of times and voltages that approximate the line you drew.

• That is exacly what I am going to do, thx for making it really clear! Jan 10 '14 at 18:55
• Hey mate, there is one problem with atan2 function. It only makes specyfic angles. Take a look on my linklink. Over there I have 4 possibilities in what direction will my robot go. atan2 will always tell me different angle. Though I have previous one, I don't know how to thell the robot what is the degree it has to turn from current position... Jan 12 '14 at 18:20
• Hmmm, I am thinking about Law of cosines, I will just have to have 2 lengths - previous distance and current distance and 3rd will be length between previous point and next point! Jan 12 '14 at 18:45

There are two ways you can approach this problem, open-loop and closed-loop. Open-loop approach should be easier although it will be highly inaccurate. The closed-loop is more complex but it should improve the accuracy. The decision depends on your application and how much time you can spend.

The robot has to follow a requested path drawn by the user. To do that, you first need to track the position and orientation (state) of the robot. In the open-loop approach this is done by blindly predicting the state of the robot based on the input. So for example if you know that the robot is positioned at (0,0) facing at 0 degrees, and the input in the wheels is the same (i.e. drives "straight") at 1 meter per second, you would expect that after a second, the state of the robot will be (1,0) and still facing at 0 degrees (in reality this wont be the case because of uncertainties but it is just a rough estimation). This is usually done based on a mathematical model of the robot. For the close-loop approach you also use feedback from the robot. One way to do this is with odometry, i.e. measuring the distance traveled by each wheel and therefore estimating its state using a model, for example the Kinematic Model. State estimation is improved, since you have the model (as in the open-loop) to "predict" the state and then you have the feedback to correct that prediction. For later reference lets define the state estimation as $\hat{x}$.

The closed-loop approach is certainly more accurate but it mainly depends on what sensors you use. An odometry based estimation will be acceptable for short periods but after that it will diverge and be useless. As always, this depends on your application and tolerance for accuracy.

The second step is to decide what input is required to send to the motor. This decision is made based on where the robot is i.e. $\hat{x}$ and where it needs to go $x_r$. For this you need a controller that constantly tries to minimize the error $e=\hat{x}-x_r$. So for your example, $x_r$ holds the coordinates of the next point that has to be visited by the robot. A simple controller would be one that:

1. Rotates the robot until it faces the point $x_r$
2. Moves the robot in a straight line until it reaches the point
3. Load the next point in $x_r$ and go to the first step

There are of course more sophisticated controllers which have certain properties but the above should work for your application.

• Yes, yes! That is an idea I was also considering but I thought its not very good because the robot will do a lot of spinning around... but now that I think of it, the frequency of signals will be so high, that it will be barely noticable! And about first point with direction: The robot will always start in (0,0) facing 0 degrees, I will measure angle to second point, rotate, go to it, and do the same with the path from second point to third but! I will simply take into consideration previous angle and substract new one with it, so it will face mentioned third point! Jan 10 '14 at 14:42
• I am not sure what you mean by your comment "the robot will do a lot of spinning around". To minimize the uncertainties it would be good idea to take some measurements to figure out how the robot reacts to specific inputs and use those. For example, for going straight use input 100 for both wheels and try to figure out the velocity by measuring the time it takes to travel 1 meter. Do this for rotation as well. Then use those specific inputs to drive the robot around. Jan 10 '14 at 14:53
• I meant like this: the path will be very accurate, like every pixel on the screen, It all depends on how accurate will I do in on the ground for robot. If its like 1px = 1cm the movement will be like: spin in direction, move ~1cm, spin, move, spin, move. I was afraid of it for a moment, but I realised it will be executed really fast so for small degrees it will be barely noticable. And yes, practical tests will be really important, to determine like... everything. Jan 10 '14 at 15:27
• @Sullson yes I think you should go to a practical test and so you get the idea of what these errors are going to show up. If your ground has a small difference or obstacle that the robot can pass, a wheel has some difference from the other, putting 100% of voltage on but motor wouldn't result in a strain line, and this will accumulates the error. With practical test you will get that, and how much it will affect the end position of the path. Jan 10 '14 at 20:14
• Yes, input equal to 100% is just stupid, I will manipulate it, so it can meet the expectation of frequency of inputng strings from smartphone, and preferably I will make the time between inputs longer, depending on the length of the distance it hs to pass! Jan 10 '14 at 20:24

# for the hardware

You don't say what type of motor you use. If it's a DC-brushed type, putting more or less voltage will not necessary linearly alter the speed, in fact if you increase the voltage the relative amount the load on the motor increase the speed won't change. To do a good control you need a feed-back of the motor or wheel speed, or an angle sensor that you can derivate the speed. If you use this information, and correct it according to the speed you want.

# for the software

What you presumable want to navigation is Dead reckoning, but if one wheel slips, the position information will drift and accumulates to the point it turns unusable. So, don't expect the best results with only this.

One more simple solution is to use an electronic compass so you know to what direction relative to north (deviations should not be a problem), your robot is pointing. This should help at the turns and more straight path, even if some wheel slip occurs, it will close the loop by that. Wheel slip will still affect the distance traveled.

• Hey, thx for ideas. You pointed out some good issues. I made some edits in original question about that, as I mentioned, I am not seeking for perfect model with feedback and feedforward and everything, first, I want to discover a simple algorythm, assuming all the data is good and there will be no errors. Jan 10 '14 at 12:42
• Indoor robot navigation is an actively research topic, this suggestion is the basic when is far from perfect (from the point of accuracy). The suggested feedback only compensates for motor speed vs voltage, as the wheel can slip without being detected. The suggestion of using an electronic compass will compensate for wheel slip but just in the turns. Anyway, you don't want any type of feedback. Jan 10 '14 at 20:05