0
$\begingroup$

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.

$\endgroup$
  • $\begingroup$ Basically you have a bunch of points, say $A$, $B$, $C$, etc. You travel from $A$ to $B$ along some heading, turn at $B$ and head towards $C$, turn at $C$ and head towards $D$, etc. and you want to know how much to turn at each point. Is that the idea? Are your points on a flat plane? A sphere? In 3D? $\endgroup$ – EuYu Feb 7 '14 at 2:43
  • $\begingroup$ No, no, I will edit more information in, thanks for making my information clearly lacking. I see where I should have been more clear. $\endgroup$ – CamelopardalisRex Feb 7 '14 at 2:50
  • $\begingroup$ Your question is still unclear to me. There are an infinite number of curved paths your robot can follow to get from point to point. Are your paths prescribed? Are you trying to minimize something? Your problem seems to have no restrictions. Or are you simply trying to find a way to move from point to point? $\endgroup$ – EuYu Feb 7 '14 at 3:04
  • $\begingroup$ I am simply trying to get from point to point. Ideally, I would end my journey at each point set up to easily head to the next point. I don't really think I have any restrictions, except for a maximum turn angle, which I haven't discovered yet. I know there are an infinite number of curves, how do I find any of them? I'd like to turn at a steady, consistent angle the whole trip, if that helps. Otherwise, it would also be nice to have a tighter angle, I suppose. $\endgroup$ – CamelopardalisRex Feb 7 '14 at 3:11
  • $\begingroup$ One simple way is to use circular arcs. Given a starting point, a heading at that point and a target point, there is a unique circular arc from the initial to target point in that heading. Of course, if your turn angle is limited, then arcs of high curvature wouldn't work, but otherwise would this be an acceptable solution? $\endgroup$ – EuYu Feb 7 '14 at 3:14
0
$\begingroup$

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$$

$\endgroup$
  • $\begingroup$ Ok, I've re-read this a half dozen times, and I haven't quite been able to pen down any kind of formula that takes in point A, point B, t and outputs the needed angle to turn, in order to follow the circular arc. $\endgroup$ – CamelopardalisRex Feb 7 '14 at 7:49
  • $\begingroup$ You wanted a rate of turning, I've given it to you in terms of the radius of the circles: $1/r$ radians/unit arclength. I'm not sure exactly what you want. $\endgroup$ – EuYu Feb 7 '14 at 15:55
  • $\begingroup$ I think I understand. I am going to test out what I think I understand tomorrow and we'll see. Maybe your little blurb here made it make sense, or maybe sleeping let me think better, but I think I've got it now. Thanks. $\endgroup$ – CamelopardalisRex Feb 8 '14 at 0:27
  • $\begingroup$ I discovered an error which I have corrected. I still can't be sure I haven't made any algebra errors so let me know if it works out. $\endgroup$ – EuYu Feb 8 '14 at 3:13

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.