# How path following works analytically for way points?

First of all sorry for the confusing question title. I am also confused about the concept.

I have implemented a quadcopter and its controller. Controller finds the rotor speeds based on the position and yaw angle references.

The thing that I don't understand is, let's say I want the vehicle to climb 5m up and then 5m left. At this point, I think I need to create a vector containing the reference values. By the way, the model is discretized by some deltaT time interval so the reference vector. This does not coincide and well behave according to the dynamics of the vehicle. Let's say, the reference input for altitude is 5m until 5sec and 0 for [5,10]sec. But it is not guaranteed that the vehicle will reach to 5m altitude in 5sec. Thus, my intention is, that reference vector shouldn't rely on the time. Therefore, my perception is to use some if condition to check if the vehicle is reached for the first waypoint and then, register the next one.

Which is a simple if-else statement? This makes me think what is the mathematical or analytical background in this. Is it just the following of the line between two waypoints by geometrical analysis like Line of Sight guidance law things.

Can you give me some insight about the concept which makes me confused?

You have a waypoint, which is some $<x,y,z>_\mbox{waypoint}$ coordinate.

You should have some internal position estimate, $<x,y,z>_\mbox{quadcopter}$.

If the waypoint coordinates are $x_w,y_w,z_w$, and the quadcopter coordinates are $x_q,y_q,z_q$, then you can calculate your distance to the waypoint as:

$$r = \sqrt{(x_q-x_w)^2 + (y_q-y_w)^2 + (z_q-z_w)^2}\\$$

Now you have a singular value that indicates how close you are to the waypoint. You can also check speeds by looking at the quadcopter's speed on each axis -

$$\dot{s} = \sqrt{\dot{x}_q^2 + \dot{y}_q^2 + \dot{z}_q^2} \\$$

You can check now that you're at target along two criteria. For example, maybe your position tracking has some degree of overshoot, and you'd like to be sure that the quadcopter has stabilized at the waypoint before continuing on to the next point. Maybe the first waypoint is at the entrance to a duct or pipe, and the second waypoint is in the duct/pipe.

So, then you'd want to make sure that your distance to the waypoint, $r$, is small, and that your vehicle speed $\dot{s}$ is also small. If your position error $r$ is big but your speed $\dot{s}$ is small, then maybe you're at the previous waypoint and about to start moving.

If your position error $r$ is small but your speed $\dot{s}$ is big, then you're close to your waypoint but you're moving quickly and are about to overshoot it. You might not care, or you might care a lot. It depends on the type of maneuver you're trying to complete and the allowable deviation from your waypoint-to-waypoint path is. If you overshoot the first waypoint by a lot and then immediately proceed to the second, then you're not actually taking the desired path to the second waypoint.

Last piece of advice I could offer is to make sure that your waypoint coordinates and quadcopter coordinates are in the same frame of reference. You wouldn't want a quadcopter in real GPS coordinates trying to track a relative waypoint coordinate - your quadcopter might autonomously take off for the intersection of the prime meridian and the equator.

• I am still not sure how to target and follow one waypoint after another one. You are just saying how I can find my state based on the position and velocity error. Am I wrong? Apr 11 '17 at 18:58
• @freezer - Maybe I'm not understanding your problem correctly. I thought your problem was that you had no way of knowing if you had arrived at the current waypoint. You said you were using a timing system that was prone to errors; you said, Therefore, my perception is to use some if condition to check if the vehicle is reached for the first waypoint. The condition you check to see if you've reached a waypoint is proximity, and that's the formula I've given: the straight-line distance from your quadcopter to the waypoint. I cautioned that you may want to check approach speed, too. Apr 11 '17 at 20:20
• @freezer - If your question isn't "am I at the first waypoint" but instead, "how do I navigate from waypoint A to waypoint B", then that's a difference question altogether. Maybe you're asking about sequencing? You can store the sequence of waypoints in a list and "pop" off the next waypoint in the list when your current position is equal to the current waypoint (as defined by an $r$ value at or below some threshold you set). Again, though, I'm not really sure what you're asking about at this point. Apr 11 '17 at 20:23
• I am actually asking both. Actually, I think there may be a way to connect all the points and fly over them continuously. In waypoint checks, I need to check what you offer in the answer and need to use if conditions whether the conditions are satisfied or not. Right? Then I need to register the next wp when proximity is enough? Sorry for the confusion, I am really confused too and have lots of questions. I am seeking to find a book or article to clarify this actually. Apr 12 '17 at 20:34

Most people just create a queue of waypoints on the path and go through one at a time. There are more complex path execution schemes that are more continuous but your application I doubt this is required. If your interested in looking for more complex schemes you can use B-Splines(https://en.wikipedia.org/wiki/B-spline) between waypoints. You hit the nail on the head when you brought up that you might not be at the target location when you think you will be there. Usually people get around that by having a good controller and recomputing the path as needed.