2 Quad
2 Quad
2 Quad
(https://charlestytler.com/)
A blog on autonomy and dynamics which tends to diverge on tangents
HOME (HTTP://CHARLESTYTLER.COM)
QUADCOPTER (HTTPS://CHARLESTYTLER.COM/CATEGORY/QUADCOPTER/)
ABOUT ME (HTTPS://CHARLESTYTLER.COM/ABOUT-ME/)
Modeling Vehicle Dynamics – Quadcopter
Equations of Motion
JUN
CHARLIE (HTTPS://CHARLESTYTLER.COM/AUTHOR/ADMIN/)
7
In this post we will see how we can describe motion of the quadcopter – or any
vehicle – as a set of differential equations.
The contents of this post will build on the concepts of multiple reference frames
and Euler angles. If you aren’t familiar with those, you may want to read the first
post in this series.
Describing Motion
Before going into any details about equations of motion, we should identify what
we are trying to describe. There are two questions we should answer:
Therefore we will need variables which represent both the linear and angular
velocity of the vehicle. The linear velocity in the body-frame of the vehicle is
defined as:
The angular velocity of the rotating body-fixed reference frame is defined as:
We also need to know the forces which influence motion of the vehicle. Again we
define this for both linear translation force and angular moment:
Finally, some influences may be in the inertial frame (such as gravity) while some
may be attached to the body (such as the vehicle’s propulsion). Therefore, the
Euler angles must also be tracked in order to convert between the inertial and
body frames.
What additional variables are useful?
The above variables are sufficient to describe all of the dynamics of the quadcopter
vehicle, however they do not specify the actual position in space of the vehicle. This
is because the dynamics in one location should be the same as any other location
(things that change with location such as aerodynamics and gravity we will
consider constant, and only enter the equations through the force and moment
variables).
This is desirable since we expect the quadcopter to behave the same no matter
where it starts from. But given a starting point, the equations of motion will allow
us to propagate that starting location forward in time to a new position if we know
all of the “state” variables defined above.
With that being said, sometimes it is useful to include additional state variables to
observe. For navigation or motion planning, it is generally necessary to track the
inertial position as well as it changes over time. Therefore we will define three
more variables for position in the inertial reference frame:
Sign Conventions
All dynamics equations will use right-handed coordinate frames. In flight controls it
is standard for the X component to be aligned with the forward direction of the
vehicle. And the forward direction is assumed to be at a heading of 0 degrees
when it is aligned with North in the inertial frame. This results in the somewhat
odd convention of [X, Y, Z] being a [North, East, Down] reference frame, in order
to maintain right-handedness.
However, for navigation it makes sense to define height as positive in the up
direction. Therefore, our navigation frame, which we will refer to with (E) for Earth,
in relation to the inertial (n) frame will have the convention:
The body frame is useful though, because our vehicle sensors are attached to it, as
well as our propulsion (the propellers). Therefore, the equations of motion we
derive will be inertial motion (change in position and orientation with respect to
the ground), but expressed within the coordinate system of our body axes
.
You should notice once we derive the final set of equations however, that inertial
position is not necessary for these equations of dynamics. The equations of motion
only represent the change in these values from the vehicle’s point of view. In that
sense, they are very useful as they may be applied to solve for the future motion of
a vehicle from any inertial starting point.
This equation defines the change in linear momentum, however we can also
express the rotational analogue. Crossing both sides of the equation with a
position vector from some origin we get the rotational law of motion about
the origin . We can simplify the equation to moment equals the change in
angular momentum , or for systems with constant mass distribution, moment
equals moment of inertia times angular acceleration — the derivative of
angular velocity .
We will use both the linear and rotational forms of this law to derive the total
vehicle equations of motion. But first, a note on what reference frame we will be
working in.
Translation Rotation
Using the chain rule for a rotating body Using the chain rule for a rotating body
frame, the law of motion becomes: frame, the rotational law of motion
becomes:
Propeller Thrust
For this post we will not go into propeller dynamics, but simply assume each of the
four quadcopter propellers provides a thrust force perpendicular to the propeller
and a torque about the center of gravity of the vehicle.
Note that since our body frame convention has positive in the down direction,
thrust from the propellers is negative.
The moments are represented with components L, M, N for rotations about the
axes, respectively. As a moment is measured by force times distance, we’ll
need the motor distances from the center of gravity, as shown in the image above.
For pitch and roll, half the motors provide positive moment, and the other half
provide a negative moment, depending on which side of the axis they are located.
For yaw, the quadcopter takes advantage of its four rotating propellers. Half of the
propellers are installed to rotate clockwise (#3 and #4) and the other half counter-
clockwise (#1 and #2). Each propeller as it pushes against the air will also apply a
torque about the cg of the quadcopter in the direction of its rotation as a function
of its thrust, propeller radius, and distance from the cg. We will define this torque
as a function , and can express the yaw moment of the vehicle as:
Gravity Force in Body Coordinates
Gravity always acts towards the center of the Earth, and is expressed in the inertial
frame as:
Where g = 9.81 m/s2, the gravitational constant. Remember that our z-inertial
coordinate is in the Down direction, so gravity is a positive acceleration.
But all of our other forces are defined within the body frame of the vehicle. To
include gravity in our equations of motion we must convert it into body frame
components as well. Using our Euler angle transformation matrix
(http://charlestytler.com/modeling-vehicle-dynamics-euler-angles/) from inertial to
body frame, , this is easy:
This means that every point of mass in the body is multiplied by the square of its
distance from the x-axis — just as in the scalar form I = mr2. So the further the
mass is away from the axis, the greater the angular inertia is about that axis.
Symmetry
The products of inertia can be thought of as the cross-coupling of different axes’
inertias on each other. But, if the vehicle is symmetric about an axis, then we will
see that some of the products of inertia will equal zero, and make our equations
easier!
If we again think of them in the discrete form, the product of inertia is the sum
of all mass points :
With the moments of inertia the distance from the axis is squared, and therefore
always results in a positive value. However here we can have both positive and
negative values, since the distances and are from the center of the vehicle.
If the vehicle is symmetric across an axis, that would mean all the mass one side is
equal to the other side and all at the same distance. For example, if the vehicle’s
mass distribution is symmetric about the x-axis, then the sum of mass points in
the negative direction equals the sum of mass points in the positive direction, and
the positive and negative x distances all cancel out.
If the quadcopter is built so it is symmetric about both the x and y axes, then the
inertia matrix would become:
Acceleration
The final component we must express to complete our equations of motion is the
acceleration. As acceleration is the time derivative of velocity, we will represent
acceleration as the derivative of our defined inertial velocity terms for linear and
rotational motion.
Linear Acceleration
We have already learned that we must use Coriolis’ Theorem to represent the
inertial velocity derivative in the rotating body frame. And we came up with the
formula:
Translational Motion
We have now derived representations for all components of our system’s force,
mass, and acceleration in translation. Plugging these into Newton’s formula
provides us with our first set of equations of motion.
But if we assume our quadcopter is symmetric about both the x and y axes of the
body frame, then the products of inertia all go to zero and this becomes:
Additional Coordinate Frame
Transformations
Angular Velocity vs. Euler Angle Rates
We now have an equation for how the angular velocity changes over time.
These are the values which a gyroscope sensor would measure on-board a
quadcopter. One caveat though, is that the angular velocity about the body-fixed
frame is not the same as the rate of change of the Euler angles
(http://charlestytler.com/modeling-vehicle-dynamics-euler-angles/) .
This is because the Euler angles represent a sequence of rotations each in their
own intermediate frame of reference, whereas the angular velocity is the
instantaneous angular rate about each of the body-fixed axes. We can use
coordinate transformations to express the angular velocity as a function of the
Euler angle derivatives by transforming them all into the body frame.
To help make this clear I’ve labeled the intermediate reference frames in the
sequence of Euler rotations below as b” and b’. So if we want to know how
would be represented in the body frame (b), we must transform it from its
intermediate frame. Here we see is the rotation rate about unit vector in the
b” frame, so we will refer to it as . However in the final body-fixed frame, b, that
unit vector has been rotated by the second and third Euler angles in the sequence.
Therefore, the rotation rates measured in the body-fixed frame due to the
rate of change of the first Euler angle will equal the rotation transformed by the
and rotations. (Since its rotation angle is also parallel with the inertial axis ,
you could get the same answer by multiplying it by all three Euler angle rotations
with the inertial-to-body coordinate transformation derived in Modeling Vehicle
Dynamics – Euler Angles (http://charlestytler.com/modeling-vehicle-dynamics-
euler-angles/)).
The second Euler rotation, is treated similarly, and needs no rotation as the
unit vector it rotates around, , is a unit vector in the body frame, b.
Sequence of Euler Angle Rotations (Body 3-2-1) from inertial to body-fixed reference frames, with
intermediate reference frames b” and b’
The relationship between measured gyro rates and Euler angle rates can be
expressed in scalar form multiplied by unit vectors as:
To make things a little easier to read and fit on the page, I’m going to use a
shorthand for the trigonometric functions: and . The above
expression can be simplified by multiplying the matrices.
Up next
The above set of equations describes the motion of the quadcopter as a set of
nonlinear differential equations. Still needed are models for the propeller force
and torque functions and values plugged in for the mass and moment of inertia
variables. But simple estimates and approximations would be good enough to start
simulating vehicle dynamics.
Additional References:
[1] Nelson, Robert, “Flight Stability and Automatic Control”
(https://books.google.com/books?
id=Uzs8PgAACAAJ&dq=robert+nelson+dynamics&hl=en&sa=X&ved=0ahUKEwjL
gqi2167UAhUF7mMKHVUZDzgQ6AEIJDAA)
A good starting point with straight-forward text on deriving the basic rigid body
equations of motion for an airplane. Books by Etkin and Roskam are also common
resources for this.
This author provides very clear formulations of the equations and also provides
useful information on the numerical integration of them. Also includes model
dynamics for various lift and thrust sources, including propellers through actuator
disk theory.
[4] Lewis, Frank L., and Stevens, Brian L., “Aircraft Control and Simulation”
(https://books.google.com/books?
id=XIabCgAAQBAJ&printsec=frontcover&dq=isbn:1118870999&hl=en&sa=X&ve
d=0ahUKEwiRjbvP167UAhVK82MKHXQGAZwQ6AEIKDAA)
This book is a great resource for practical examples of the equations in use for
simulation of vehicles.
Share this:
(https://charlestytler.com/quadcopter-equations-motion/?share=twitter&nb=1)
(https://charlestytler.com/quadcopter-equations-motion/?share=facebook&nb=1)
motion/#comment-27)
Nice work :). I’ve spotted a small error though, in the rotation matrix from the
inertial- to the body reference frame.
The term in the second row of the first column should be: -cos(phi) * sin(psi) +
sin(phi)*sin(theta)*cos(psi)
This mistake is repeated in the rotation matrix from body to inertial in the
x_dot, y_dot, z_dot (navigation coordinates) step, but then it occurs in the first
row of the second column (due to the transpose operation).
Also, I have a question regarding the u, v, w values in your work. You state these
are the velocities as seen from the Inertial frame, in body-frame coordinates.
However, in the calculation of v_dot you actually throw away part of (vdot)
_inertial by subtracting the cross-product of omega and v, which leaves you with
vdot as seen from the body-frame and not the inertial frame, so (vdot)_body.
This means that the [u,v,w] vector, which is simply the integral of vdot, is
actually a non-inertial velocity. You subsequently express the [u,v,w] vector in
inertial reference frame coordinates to obtain your navigation coordinates [x, y,
z]. I would argue these navigation coordinates should be obtained by
integrating the velocities w.r.t. the inertial frame (vdot)_inertial, and not
integrating velocity w.r.t. the body frame (vdot)_body, as is happening now.
What are your thoughts on this?
Kind regards,
Lenard
Reply
motion/#comment-26)
hi , it is a great explanation I ever seeing in web
if introduce a nonlinar control , that will be great job
Reply
Dominic (https://charlestytler.com/quadcopter-equations- October 12, 2019
motion/#comment-25)
Hi Charlie!
Is there anyway you could help me modify the code in order to implement its
own control?
(i.e. make a pre-determined trajectory and give it control law to follow the
trajectory?)
Thank you!!
Reply
motion/#comment-24)
I think sign of L and M (Rolling moment and pitching moment) should be
changed.
F1 and F4 generates roll left moment (-), and F2 and F3 generates roll right
moment (+).
and F1 and F3 generates pitch up moment (+), and F2 and F4 generates pirch
down moment (-).
but in this article, sign of L and M is opposite to my opinion.
Reply
equations-motion/#comment-23)
I have a doubt as why you transformed forces in inertial frame where F= ma is
valid for inertial frame . Here x, xdot , xdouble dot are defined wrt to inertial
frame then shouldn’t the forced be transformed to the inertial frame.
Reply
Metehan (https://charlestytler.com/quadcopter-equations- August 25, 2019
motion/#comment-21)
Nicely summarized. I really enjoyed while reading and I have one or two
questions if you don’t mind. In Yaw moment calculation you said, ” Each
propeller as it pushes against the air will also apply a torque about the cg of the
quadcopter in the direction of its rotation as a function of its thrust, propeller
radius, and distance from the cg” but for yaw motion, propeller of quadrotor
uses Counter angular momentum that will try to change the quadrotor’s
attitude in the opposite direction of propeller rotation. Am I wrong?
For Moment Calculations, why did you represent the L and M like this
L= F1d1y-F2d2y-F3d3y+F4d4y
M=-F1d1x+F2d2x-F3d3x+F4d4x , is it because of the forces in z direction taken
as minus ? Otherwise It would have been like this when we consider the BFF
and Right Hand Rule
L= -F1d1y+F2d2y+F3d3y-F4d4y
M=+F1d1x-F2d2x+F3d3x-F4d4x
Final Question
Which Body Fixed Frame representation is correct ? The one you showed here
or here: https://www.researchgate.net/figure/Inertial-and-body-fixed-frame-of-
quadrotor_fig1_270163654 (https://www.researchgate.net/figure/Inertial-and-
body-fixed-frame-of-quadrotor_fig1_270163654)
Because Moment equations will change when you apply second BFF
coordination.
Reply
motion/#comment-13)
Outstanding work! The combination of skillful, patient description and runnable
Python code are going to make these pages the standard reference for teaching
and research with quadcopters and other multiotor vehicles. Kudos, and thanks.
Reply
motion/#comment-11)
Hi there! Is there a small mistake just above the list of equations of motion in
the “Navigation Coordinates” section? The matrix should be time derivatives of
x_e, y_e, -z_e
Reply
motion/#comment-4)
Really wonderfull article sir. It inspired tremendously and made easy for me to
understand core concepts. Please broadcast series of videos on Youtube about
quadrotors.
Reply
motion/#comment-3)
Thanks Charlie! I like your blog. I just started a blog along these lines: flexing the
old, analytical muscles by studying and (eventually) building a quadrotor. My
blog is here http://www.mtwallets.com (http://www.mtwallets.com). If you look
at the “quadcopter” posts you’ll see I got into the motor-propeller dynamics,
“lift” concepts, and now I’m working on (but haven’t posted) my platform
dynamic model. I have some good papers on the topic and I’m wrestling to
derive the equations of motion using the Lagrangian…it feels close but I’m still
not getting to the “final” equations I see in some of the papers. I’m going to
study yours a bit and see if I can get, “unstuck”. Thanks again!
Reply
motion/#comment-2)
The Moment of Inertia Matrix term (3,2) should be -I_yz not -Ixz.
Reply
Home (https://charlestytler.com/)
About Me (https://charlestytler.com/about-me/)
Theme Designed by InkHive.com (http://inkhive.com/). Unless otherwise stated, all content and images are
created by Charles Tytler.