How Do You Convert Drone Position Errors into Roll and Pitch Angles?

4 replies · 3K views · last reply
Young_kids
Messages
3
Reaction score
0
TL;DR
computing remaining 3D angles from x, y and the angle in between them
Hi guys
We have a problem we we are building a flight controller for a drone, but fail to understand how to construct the position controller.
It takes the x, y positions and the angle in between them which we call yaw. How to compute the angles roll and pitch for the desired x and y positions. We have an idea of the following
Mathematica-style display of a yaw rotation matrix R_yaw with cos(ψ) and sin(ψ) entries, and the product R_yaw(x; y; z) giving cos(ψ)x − sin(ψ)y, sin(ψ)x + cos(ψ)y, z.

Can someone explain what formula that converts the x error and y error as well as the angle yaw to the desired roll and pitch angles.

We found a formula

Two equations rotating position errors into body frame: v_xerr = x_err cos(phi) + y_err sin(phi), and v_yerr = y_err cos(phi) - x_err sin(phi).

and
Two equations: theta_d = k_p1 v_xerr - k_d1 v_xerr dot, and phi_d = k_d2 v_yerr dot - k_p2 v_yerr, shown as displayed math with theta_d and phi_d as desired pitch and roll angles.

But we don't understand why, hope you can help
 
Attachments
  • Two rotation matrix equations: R_yaw is a 3x3 matrix with cos(psi), -sin(psi), sin(psi), cos(psi) and a 1 in the lower right; R_roll(phi) applied to (x, y, z) gives x, cos(phi)y - sin(phi)z, sin(phi)y + cos(phi)z.
    1580113087821.png
    2.2 KB · Views: 288
Young_kids said:
It takes the x, y positions and the angle in between them which we call yaw. How to compute the angles roll and pitch for the desired x and y positions.
If the direction is just horizontal (x & y), why do you need the other angles?
 
A.T. said:
If the direction is just horizontal (x & y), why do you need the other angles?

Because we are making a cascaded control setup, the inner loop controls, pitch, roll, yaw, z. As this is an underactuated system, the desired pitch and roll angles are obtained by cascaded controllers, which make up the outer loop. This outer loop takes the x_error, y_error and measured yaw, and should compute the desired pitch and roll angles in order to achieve the desired position. We are unsure how to compute these desired desired pitch and roll angles, can you give us a hint please
 
Young_kids said:
Because we are making a cascaded control setup,
Before looking at specific equations, the overall plan for cascaded loops must make sense. Can you post a block diagram of the entire control scheme from sensors to actuators?
 
anorlunda said:
Before looking at specific equations, the overall plan for cascaded loops must make sense. Can you post a block diagram of the entire control scheme from sensors to actuators?

Hi anorlunda
Thanks for replying. Our block diagram is as follows overall
Block diagram of a drone cascaded control system: a position controller with World to Body conversion and roll and pitch PIDs as outer loop, feeding inner loop PIDs for roll, pitch, yaw and thrust into a mixer, then the minidrone, with sensing and estimation feeding back system states.

We are unsure of how do the world to body conversion, where the x_error, y_error and yaw are used to compute the desired roll and pitch angle