# Guiding a Quadrotor Towards a Target

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?

• How are you calculating the yaw angle? Also, what controller are you using and how do you send it data? – DaemonMaker Dec 24 '12 at 20:35
• Interesting question @Hamza, welcome to Robotics. – Mark Booth Dec 25 '12 at 1:02
• @Hamza, What language and system are you using? Im also working on a quadcopter with Atmega328 and Ada programming language. If you have a blog about the project please share. – user697 Jan 3 '13 at 8:31
• You are correct @MarkBooth, I had two tabs open and was intending to mark the other post as a duplicate. I marked this one by mistake and saw no way to undo it. Given that it takes more than one vote to close it I figured this wouldn't be a problem. I did not realize however that it posted a comment on my behalf. – DaemonMaker Jul 9 '13 at 15:35
• No problem @DaemonMaker these things happen. Close as duplicate votes now automatically post comments, which I think is a useful feature as it prompts people to review the other question before casting a close vote themselves. – Mark Booth Jul 10 '13 at 10:00

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

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.

• That's some edit there, over 90% and totally changes the answer (from PID to ATAN2). But your flashy graphic formula skills are beast! – Spiked3 Dec 27 '12 at 17:42
• I still recommend the PID (it's there at the bottom), I just worked through the beginning part of the question to make sure that my assumptions were correct. The graphic formulas are part of Latex formatting, which is worth checking out. – Ian Dec 27 '12 at 17:51
• "compass heading starts from the positive y axis and increases clockwise, while yaw starts from the positive x axis and increases counterclockwise" reference? "converting the x component of (θ−ψ) to roll and the y component to pitch" I don't get that at all - more explanation please (i'm missing something). – Spiked3 Dec 27 '12 at 18:46
• The original question mentioned "passing [the x and y components] to the robot as roll and pitch angles", which to me indicates that the quadcopter moves side to side by changing the roll angle, and forward and backward by changing the pitch angle. I'll add some clarity. – Ian Dec 27 '12 at 20:30
• neat. I've never seen it done that way. I have seen, and do myself, just flip cos/sin to get the same results. I'll have to think on the roll/pitch some more. Yes, that would cause motion, but i'm not sure how that has anything to do with where you are and where your going, other than the speed at which you get there. Thanks. – Spiked3 Dec 27 '12 at 22:38

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).