Guiding a Quadrotor Towards a Target

Question

I am working on a quadrotor. I know its position -- $a$, where I would like to go -- target position $b$, and from that I calculate a vector $c$ -- a unit vector that will take me to my target:

c = b - a
c = normalize(c)

Since a quadrotor can move in any direction without rotation, what I have tried to do is

1. rotate $c$ by the robots yaw angle
2. split it into its $x, y$ components
3. pass them to the robot as roll and pitch angles.

The problem is that if the yaw is 0° ±5 then this works, but if the yaw is near +90 or -90 it fails and steers to wrong directions. My question is am I missing something obvious here?

Answer 1

Re-implementing your solution, I get this:

Angle Between Vectors

First, you want the angle between points $A$ and $B$ -- not specifically the unit vector.

(via Fx Programming): $\theta = math.atan2(B_{x}-A_{x}, B_{y}-A_{y})$

Vehicle Yaw Angle

Next (and I suspect this is your problem), you need to subtract the vehicle's yaw angle $\psi$ from your calculated $\theta$.

Heading vs Yaw

If you're using a compass for the "yaw angle" of your vehicle, this could also be your mistake; heading and yaw are not the same. Compass heading is zero along the positive $y$ axis, increasing as it turns clockwise:

Yaw is zero along the positive $x$ axis, increasing as it turns counterclockwise:

The 90 degree overlap between these measurements, combined with adding (instead of subtracting) the vehicle yaw from the desired yaw, may be why things worked when your target was within ±5° and behaved badly at ±90°.

Conversion to Component X and Y

From there, you say that you are converting this result $(\theta-\psi)$ into its $x$ and $y$ components, passing them to the robot as the roll and pitch angles. With the above corrections, you should get the desired result at this point. However, directly mapping these components to tilt angles might be problematic since you are only considering the difference in position, and not the velocity (really, the momentum) of the vehicle.

PID Control

You may be best served by using PID control loops for the roll and pitch of the vehicle. That is, once you fix your code and are able to hit your target, my guess is that you'll start overshooting it instead -- oscillating back and forth. A correctly-tuned PID will prevent that from happening while still allowing you to approach the target quickly.

Instead of plugging your $x$ and $y$ into roll and pitch, consider them to be the error values that the roll and pitch PIDs accept as input.

Answer 2

I'll assume you're talking about a 3D vector here. Can you just generalize normalize() like that? Is it that common (i've never seen it so if it is, then news to me). Otherwise, obvious compass wrap issues apply to each of the X and Y components. Why not call them roll and/or pitch and/or yaw? (mixing 3D and 2D nomenclature confuses the question).

My 2D normalize looks something like this;

int Pilot_QuickestTurnTo(int hdgNow, int hdgNew)
{
    hdgNow = Pilot_Hdg360(hdgNow);
    hdgNew = Pilot_Hdg360(hdgNew);
    if (hdgNow < hdgNew)
        hdgNow += 360;
    int left = hdgNow - hdgNew;
        return (left < 181 ? -left : 360 - left);
}

If it is indeed a quad, I assume your X and Y components are really YAW, Altitude ( (X, Y) & Z). You'll need to handle the YAW(X, Y) in 2D, and simply drop or gain altitude for Z (and again that's why I suspect normalize is more than you have it as).