Basic Euclidean Geometry, Circle Arc

Question

So, here is my problem explained as best as I can. I'm working on some navigation logic for a wheeled vehicle, but I've not the foggiest idea of how to do much path finding, really.

So, my basic idea is that I have 2-3 points (starting and ending, but I also have data for where the NEXT ending point is, if that's useful) and my current heading and I want to know how much I should be turning at any given point.

Note, I will be running this as a program, so I can constantly check my current heading and location, if need be and once I arrive at my next location, I will receive another end point; in other words I should always have my current location and the next two locations unless I am arriving at my final destination on my next movement.

But as far as the mathematics go, I am unsure how to find how much I need to be turning to follow the arc I want to travel along to reach my next point (and hopefully be set up to easily reach the point after)

The end use of this is for a wheeled robot to follow a path, and it cannot turn in place, it must turn as it goes. We can assume this takes place in 2-D.

So, the robot must move at it's current heading, but turn as it goes inorder to reach the next location. So, if it starts at point A, it must drive along a curved path to B, and then from thee drive along another curved path to C, and so on until it reaches the last waypoint on it's path. The waypoints dynamically change as the sensor aboard notice obstacles.

The robot has a maximum turning angle, but that value is unknown as of yet. I need the preliminary code set up to test it.

Answer 1

Suppose you are given a starting point $A=(x_0,y_0)$ and a point $B=(x_1,y_1)$. Suppose that you have a vector $\mathbf{t} = (x', y')$. Then there exists a unique circle tangent to $\mathbf{t}$ at $A$ through $B$. You don't really need the equation of the circle, but rather its radius. The full equations are rather horrendous, but it shouldn't be too bad for a computer implementation. You should probably check the below for errors.

The line through $A$ and $B$ is given by $$y = \frac{y_1-y_0}{x_1-x_0}x + y_0 -\frac{y_1-y_0}{x_1-x_0}x_0$$ The perpendicular bisector of segment $AB$ is therefore $$y = -\frac{x_1^2-x_0^2}{2(y_1-y_0)}x +\frac{(y_1^2 - y_0^2) + (x_1^2 - x_0^2)}{2(y_1-y_0)}\tag{*}$$ The line tangent to the circle at $(x_0, y_0)$ is given by $$y = -\frac{x'}{y'}x + y_0 -\frac{x'}{y'}x_0\tag{**}$$ The intersection of the two lines $(*)$ and $(**)$ gives the center. The $x$ coordinate of the center is given by $$x = \frac{-y'(y_1^2 - y_0^2 + x_1^2 - x_0^2) - 2(y_1-y_0)(x'x_0-y'y_0)}{2(y_1-y_0)x' -y'(x_1^2 - x_0^2)}$$ And $y$ coordinate is given by substituting the above into the expressions for either lines $(*)$ or $(**)$. This gives the coordinates of your center $C=(x,y)$. The radius of the circle is then just the distance from $A$ to $C$ (or equivalently $B$ to $C$) $$r = \sqrt{(x-x_0)^2 + (y-y_0)^2}$$ The circle turns through an angle of $\theta=2\pi$ through arc-length $s=2\pi r$. Therefore the rate of angle change per unit arc-length is given by $$\frac{d\theta}{ds} = \frac{1}{r}$$ The direction of turning depends on the location of $A$, $B$ and $\mathbf{t}$. The sense of the rotation is counter-clockwise if and only if $$x'(y_1-y_2) - y'(x_1-x_2) > 0$$