I’m currently working on a wall-following program for Colin, my mobile robot. True to form, I’ve added an extra complication to the problem: Colin won’t respond directly to his sensor inputs. Instead, he’ll use his sensor inputs to create a simple model of his environment. Then he’ll decide how to move based on this model.

In order to follow a wall, Colin only needs to know the position of the wall relative to himself. To do this Colin will calculate the (x, y) coordinates of each obstacle detected by his rangefinders. Then he’ll fit a line to those points and assume that line represents the wall he’s supposed to be following.

I’ll discuss the actual wall following algorithm in detail in my next post. I’m laying out my method for fitting lines to Colin’s sensor readings in a separate post so I don’t get bogged down in the underlying maths when I’m presenting the wall-following algorithm. If you’re not interested in the maths feel free to skip this post; you can use my line fitting classes without understanding their inner workings. If you want to see what’s going on under the hood, read on.

Interpreting Range Readings

The first thing Colin does is take the range readings from his sonar sensors and convert them into (x, y) points in his local coordinate system. To do this Colin combines his the range readings from his sensors with the orientations of those sensors relative to his heading, which are constant. This turns his range readings into points in a polar coordinate system defined by (r, θ). Then converting these polar points to Cartesian coordinates is pretty easy. Colin calculates the x and y coordinates of each point as follows:

$x_{i}=r_{i}cos(\theta_{i})$

$y_{i}=r_{i}sin(\theta_{i})$

In the above two equations, $r_{i}$ is the range reading from sensor i and $\theta_{i}$ is the angle of sensor i relative to Colin’s heading.

My Point data structure does this conversion automatically. Its constructor takes the polar coordinates of the point as arguments and automatically calculates the Cartesian coordinates. It also stores r, as this is used to assign weight to the point later in the program.

Line Fitting

After converting his sensor readings to an array of Cartesian points, Colin fits a line to them. One of the simplest ways to do this is with least-squares linear regression, which works as follows. Assume the following equation describes all of the points:

$y_{i}=a+bx_{i}+\epsilon_{i}$

Where $y_{i}$ and $x_{i}$ are coordinates of point i, a and b are the slope and intercept of the fitted line, and $\epsilon_{i}$ is the error between the line and point i. Least squares regression determines coefficients a and b such that the sum of squares of the errors, $\sum_{i=0}^{n}\epsilon_{i}^{2}$ is minimized.

In terms of linear algebra, the line equation is represented by $Ax=B$ where A is an n by 2 matrix containing the x values of Colin’s points:

$A=\begin{bmatrix} 1 & x_{0} \\ 1 & x_{1} \\ 1 & x_{2} \\ ... & ... \\ 1 & x_{n} \\ \end{bmatrix}$

B is an n by 1 vector containing the y values of Colin’s points:

$B=\begin{bmatrix} y_{0} \\ y_{1} \\ y_{2} \\ ... \\ y_{n} \\ \end{bmatrix}$

and x is a 2 by 1 vector containing the y intercept and slope of the fitted line.

$x=\begin{bmatrix} a \\ b \\ \end{bmatrix}$

Unfortunately, there is usually no x vector that satisfies $Ax=B$ for every entry in the A and B matrices because all of the points will not lie on the same line. What we want, then, is a vector $\hat{x}$ that minimizes the sum of squares of the errors for each entry. The equation for $\hat{x}$ is as follows:

$A^{T}A\hat{x}=A^{T}B$

This equation can be solved pretty simply by multiplying both sides by the inverse of $A^{T}A$ :

$\hat{x}=(A^{T}A)^{-1}A^{T}B$

The above equation calculates the slope and y intercept of the line that best fits all of Colin’s sensor readings. That brings us a step closer to our goal!

Also, the math presented above is only a very high-level overview. If you’d like to go more in-depth I’d recommend this very thorough textbook chapter from MIT.

Weighting the Points

You may have noticed a problem with the idea of using simple linear regression to model walls. Most of Colin’s sensors will either detect no obstacle or they will detect a far-away obstacle that is not part of the wall Colin is following. This means that most of the points are not relevant. At most three or four of these points will actually represent a wall.

Very distant points are likely to represent failed readings or obstacles that aren’t relevant to Colin’s decision making. So I decided that points that are more distant from Colin should be less significant and closer points should be more significant. Using this criteria, Colin uses a simple weighting function to assign significance to his points:

$w_{i}=\exp(-\frac{d_{i}^{2}}{c})$

where $w_{i}$ is the weight assigned to point i, $d_{i}$ is the distance between Colin and point i, and c is a constant. The weighting function’s graph looks like this:

The function assigns a weight of 1 for a distance of 0. Varying the value of c adjusts the distance at which the weight decays to 0.

Weighted Linear Regression

Colin uses the above weighting function to create a weights matrix, W:

$W=\begin{bmatrix} w_{0} & 0 & ... & 0 \\ 0 & w_{1} & ... & 0 \\ ... & ... & ... & ... \\ 0 & 0 & ... & w_{n} \\ \end{bmatrix}$

As you can see, W is a diagonal matrix of the weights assigned to each point. It fits into the linear regression equation as follows:

$A^{T}WA\hat{x}=A^{T}WB$

Similar to the unweighted regression, this is solved for $\hat{x}$ by multiplying both sides by the inverse of $A^{T}WA$ :

$\hat{x}=(A^{T}WA)^{-1}A^{T}WB$

Using the above function Colin calculates the equation of a line fitted to the obstacles that are closest to him and therefore most likely to be relevant to his decision making.

My class, LineFitter, stores an array of Points and automatically fits a line to those points using the above method for weighted linear regression. This makes it very easy to use in my wall following program, which I will present in my next post.

Alternate Methods

A more sophisticated method would incorporate RANSAC to determine which points actually represent the wall and then fit a line to those points, ignoring the others. It would be very interesting to try to implement this method and compare its performance to my simple weighted linear regression. However, I don’t believe Colin would benefit much from using the more sophisticated method in his current configuration.

This is because Colin is working with a very small data set of only 8 points. This means the both my method and RANSAC could easily find spurious relationships in the data. Further, RANSAC is probably overkill; a brute-force search for relationships between Colin’s points would probably be more efficient on such a small data set. Since both are workable and weighted regression is much easier to implement, that’s where I’m starting. This is a good excuse to do further research on RANSAC, though, and I may do a post on that topic in the future.

Bot Blog

building my robot army one sketch at a time

Weighted Linear Regression for Wall Following

Interpreting Range Readings

Line Fitting

Weighting the Points

Weighted Linear Regression

Alternate Methods