zeta = 0.5912
MEC4127F: Introduction to Robotics
Chapter 10: Attitude control design for quadcopters
Table of Contents
MEC4127F: Introduction to Robotics
Chapter 10: Attitude control design for quadcopters
10.1 Introduction
10.2 Control allocation
10.3 Analysis of attitude dynamics
10.4 Frequency domain design
10.4.1 Proportional-plus-integral control design
10.4.1.1 Definition
10.4.1.2 Design methodology
10.4.2 Proportional-plus-derivative control design
10.4.2.1 Definition
10.4.2.2 Design methodology
10.4.3 Proportional-integral-derivative control design
10.4.2.1 Definition
10.4.2.2 Design methodology
10.5 Determining orientation error
10.5.1 Quaternion-based error formulation
10.5.2 Thrust vectoring
10.6 Attitude control design
10.6.1 Decoupled rotational dynamics
10.6.2 Roll/pitch control design
Example: PD control design for roll channel
Example: PID control design for roll channel
10.6.3 Yaw-rate control
Example: Yaw-rate PI control design
10.7 Implementation of nonlinear control scheme
Example: Attitude control
10.1 Introduction
Chapter 9 provided an overview of the dynamic model of an idealised quadcopter. This included defining the rotational and translational dynamics of the UAV. Chapter 10 will now make use of the rotational dynamic models to inform control design of the body-frame roll, pitch, and yaw channels.
In the case of classical control theory, it is common practice to design two separate controllers — one for the rotational system, and one for the translational system. Because the quadcopter is only able to traverse in the 
-
plane as a result of a roll or pitch motion, the attitude controller can be thought of an inner loop, with the translational controller considered as the outer loop. This is often referred to as a cascaded control scheme. Figure 10.1 gives a high-level illustration of this concept.
- plane as a result of a roll or pitch motion, the attitude controller can be thought of an inner loop, with the translational controller considered as the outer loop. This is often referred to as a cascaded control scheme. Figure 10.1 gives a high-level illustration of this concept.
Figure 10.1: High-level block diagram of cascaded control scheme.
We will work towards implementing the control strategy depicted in the figure above over the course of Chapter 10 and Chapter 11, but we first have to define several key elements to the control system, namely:
- control allocation — mapping desired thrust and torque vectors to the rotor speeds;
- dynamic analysis — assessing the dynamics of the system in order to inform an appropriate flight envelope;
- attitude control design — design a controller that can stabilise the orientation of the quadcopter whilst also meeting the prescribed specifications;
- thrust vectoring — relate the position control command to the orientation error in order to dictate orientation changes required;
- position control design — design a controller that can stabilise the translation of the quadcopter whilst also meeting the prescribed specifications;
10.2 Control allocation
Recall that the controllable torque vector was derived to be
where l is the distance from the centre of mass to each motor shaft, 
is the thrust coefficient, and 
is the angular velocity of rotor i. Additionally, the net thrust produced by all four rotors was shown to be
is the thrust coefficient, and is the angular velocity of rotor i. Additionally, the net thrust produced by all four rotors was shown to be
As we will show later in this Chapter, the torque vector will be used to control the orientation of the quadcopter. The net thrust will be used to control the heave of the platform, which we will make use of in Chapter 11. Together, these two quantities will also be used to dictate translational motion of the quadcopter. For this reason, 
and 
will be thought of as virtual control inputs, similar to how 
were virtual control signals when handling the differentially-steered robot.
and will be thought of as virtual control inputs, similar to how were virtual control signals when handling the differentially-steered robot.As with the wheeled robot control, we also need to be able to map from the virtual control signals to the low-level actuator signals — that being the motor speeds. In other words, we require a mapping: 
. Combining the two equations above into a single matrix yields
. Combining the two equations above into a single matrix yields
Assuming our controller prescribes desired torque and thrust values of 
, we can invert the relationship above to determine the desired rotor speeds, which is given by
, we can invert the relationship above to determine the desired rotor speeds, which is given by
syms kf kt l positive
A = [kf kf kf kf;
0 -l*kf 0 l*kf;
-l*kf 0 l*kf 0;
kt -kt kt -kt];
inv(A)
The matrix mapping above will be very useful when we begin to define our controllers and need to determine the rotor speeds required to produce the required rigid-body thrust and torque. Notably, the matrix operation above returns the squared rotor speeds that are required. Naturally, we would have to square-root each element to obtain the desired rotors speeds. We must also not forget that are we are assuming that the desired and actual rotor speeds are related by
where 
is a fixed delay period.
is a fixed delay period.10.3 Analysis of attitude dynamics
In Chapter 9 we presented the rigid-body orientation dynamics of the quadcopter as
where 
is a diagonal inertia matrix given by
is a diagonal inertia matrix given by
describes the angular velocity of the rigid body in 
, and 
is the applied torque vector in . The second term in the dynamic equation above, namely 
, is referred to as the centripetal moment and arises because we are describing the dynamics in , which is a non-inertial reference frame. We can further assess this term by writing it out in its explicit form:
where
syms w_x w_y w_z J_x J_y J_z positive
w = [w_x w_y w_z]';
J = diag([J_x,J_y,J_z]);
m_cent = cross(w,J*w)
The implication of this term is that if there is rotational motion in at least two channels, such as 
about and 
about 
, then this will result in a centripetal coupling moment about as a result of 
. This type of effect is undesirable, as we ideally do not want any coupling between any two axes, as it makes for a more challenging control design procedure. We therefore want to ideally make design choices such that 
.
about and about , then this will result in a centripetal coupling moment about as a result of . This type of effect is undesirable, as we ideally do not want any coupling between any two axes, as it makes for a more challenging control design procedure. We therefore want to ideally make design choices such that .Quadcopters are usually designed such that 
, which suggests that the centripetal moment reduces to
, which suggests that the centripetal moment reduces to
Additionally, if we are able to regulate the rotation of the quadcopter body about such that 
, this would then imply that
such that , this would then imply that
which would remove the centripetal coupling from the attitude dynamics, thereby leaving the simplified form of
As is a diagonal matrix, each channel would then behave independently from each other in theory.
is a diagonal matrix, each channel would then behave independently from each other in theory.The design choice of enforcing may seem at first like a drastic condition that would hinder the flight envelope of the quadcopter. However, there are actually a few good reasons to proceed with this:
may seem at first like a drastic condition that would hinder the flight envelope of the quadcopter. However, there are actually a few good reasons to proceed with this:- If we consider the task of pointing our thrust vector, which is parallel to , in the required direction, then it becomes immediately apparent that , acting about , will have no effect on the thrust vector's direction.
- The mass moment of inertia about ,
is usually larger than 
and 
, owing to the location of the motors on the extremities of the quadcopter body. This means that a larger moment is required about to create comparable motion. - The torque coefficient,
, is typically much smaller than 
, which implies that significantly smaller moments can be created about when compared to the moments about and .
In summary, the approach outlined about has no negative effect on the quadcopter's ability to vector thrust, and additionally, motion about will require the most energy and also be the slowest when compared to the other two principle axes. We must, however, bear in mind that we will need to regulate the rotational velocity about such that , which necessitates the appropriate rate controller.
will require the most energy and also be the slowest when compared to the other two principle axes. We must, however, bear in mind that we will need to regulate the rotational velocity about such that , which necessitates the appropriate rate controller.10.4 Frequency domain design
This Section will provide a refresher of frequency domain design using log-polar plots, which would have been covered in MEC3079S (or an equivalent course). For more information, see Chapter 11 and Chapter 12 of the Control Systems notes.
10.4.1 Proportional-plus-integral control design
10.4.1.1 Definition
The PI controller is given by
,where Kand 
are real-valued parameters that we seek to design. The name stems from the fact that a PI controller is made up by summing a proportional and integral controller, namely
are real-valued parameters that we seek to design. The name stems from the fact that a PI controller is made up by summing a proportional and integral controller, namely
.The PI controller contains a single pole, located at 
(at the origin in the s-plane), and a single zero, located at 
.
(at the origin in the s-plane), and a single zero, located at .Using the two-term representation above, and noting that the control action is given by 
, the relationship between tracking error and control action in the time-domain is obtained as
, the relationship between tracking error and control action in the time-domain is obtained as
.The PI controller therefore makes use of an integrator, which continually increases control action 
, via the term 
, until such time that 
. The term is known as the integral time constant, as decreasing increases the gain of the integrator term, 
.
, via the term , until such time that . The term is known as the integral time constant, as decreasing increases the gain of the integrator term, .10.4.1.2 Design methodology
Recall that we were able to design the PI controller by placing above and below bounds on the closed-loop transfer function, as shown in Figure 10.2.
Figure 10.2: Above and below bounds enforced for magnitude response of closed-loop system.
The all-frequency stay-below bound is calculated using
,which we previously determined as the peak magnitude response of a second-order low-pass filter, as shown in Figure 10.3.
Figure 10.3: Magnitude response of a second-order underdamped system.
If a percentage overshoot specification, 
, is given, then we can use
, is given, then we can use
and then substitute the resulting ζ into the previous equation to determine 
, which sets the stay-below boundary.
, which sets the stay-below boundary.Owing to the fact that a PI controller resembles that of a P controller of the form 
at sufficiently high frequencies (
), we can begin by designing our controller under the assumption that it is simply a proportional controller. One way of doing this is to initially set the integral time constant, , to a sufficiently large number, thereby making the zero frequency, 
, very low. We can then design in order to meet the required stability and/or transient tracking requirements. Once we have met those requirements, we can then begin to decrease . Importantly, we still need to be sufficiently small, otherwise the integral action will take a very long time to reduce the tracking error to zero. On the other hand, very small values results in increased phase lag across a wider frequency range, which makes satisfying stability conditions more tricky. We therefore want a small enough value that also does not interfere with the tracking and stability design specifications.
at sufficiently high frequencies (), we can begin by designing our controller under the assumption that it is simply a proportional controller. One way of doing this is to initially set the integral time constant, , to a sufficiently large number, thereby making the zero frequency, , very low. We can then design in order to meet the required stability and/or transient tracking requirements. Once we have met those requirements, we can then begin to decrease . Importantly, we still need to be sufficiently small, otherwise the integral action will take a very long time to reduce the tracking error to zero. On the other hand, very small values results in increased phase lag across a wider frequency range, which makes satisfying stability conditions more tricky. We therefore want a small enough value that also does not interfere with the tracking and stability design specifications.In order to ensure that 
approaches unity as 
, we set our stay-above bandwidth constraint such that the lower bound represents 
for the desired frequency bandwidth. In order to not be too overly constraining on the closed-loop gain, we use a lower bound magnitude of 
(or approximately 
dB) with a frequency range of 
, where 
is the peak frequency of a second-order system (see Figure 10.3), which we previously defined as
approaches unity as , we set our stay-above bandwidth constraint such that the lower bound represents for the desired frequency bandwidth. In order to not be too overly constraining on the closed-loop gain, we use a lower bound magnitude of (or approximately dB) with a frequency range of , where is the peak frequency of a second-order system (see Figure 10.3), which we previously defined as
.The stay-above boundary can be seen in Figure 10.2. Note that the choice of is largely heuristic — we can choose a range of values sufficiently close to (but also less than) unity.
is largely heuristic — we can choose a range of values sufficiently close to (but also less than) unity. Once we have defined 
, the bounds on can be converted to bounds on 
using the procedure detailed in Chapter 11 of Control Systems.
, the bounds on can be converted to bounds on using the procedure detailed in Chapter 11 of Control Systems.10.4.2 Proportional-plus-derivative control design
10.4.2.1 Definition
The PD controller is given by
,where α and β is the sole zero and pole of 
, respectively, and K is the DC gain, based on
, respectively, and K is the DC gain, based on
.Similarly, the high-frequency gain of is determined by
is determined by
.We also require that 
.
.10.4.2.2 Design methodology
When defining suitable tracking boundaries in the frequency domain from time-domain specifications, namely 
, we can make use of the approach previously outlined for proportional-plus-integral controllers. Recall that we define as
, we can make use of the approach previously outlined for proportional-plus-integral controllers. Recall that we define as 
where the damping ratio is obtained from the overshoot specification using
.As with the PI control design, we specified the stay-above constraint as 
, where
, where ,and
,using the same ζ from the stay-below constraint.
Using is quite convenient, as we expect to be the approximate frequency that results in the peak response of , which implies that 
is going to end up being (approximately) the closest point to the overshoot boundary from 
. By this logic, is likely to require the greatest phase lead when meeting overshoot requirements, which suggests that 
is a suitable choice.
is quite convenient, as we expect to be the approximate frequency that results in the peak response of , which implies that is going to end up being (approximately) the closest point to the overshoot boundary from . By this logic, is likely to require the greatest phase lead when meeting overshoot requirements, which suggests that is a suitable choice.Following the boundary formulation, we begin the design by first setting 
, which effectively converts our lead controller into a proportional controller based on
, which effectively converts our lead controller into a proportional controller based on
.We then initiialy set 
and then identify on the log-polar plot which frequency requires peak phase lead (i.e. 
), and what amount of phase is required. As previously defined, the maximum phase response of a lead filter is given by
and then identify on the log-polar plot which frequency requires peak phase lead (i.e. ), and what amount of phase is required. As previously defined, the maximum phase response of a lead filter is given by
,where 
. We usually choose as an educated starting guess.
. We usually choose as an educated starting guess.Assuming we can identify the desired amount of phase lead required, 
, and frequency point which will experience this maximum phase lead, 
, we can then determine the required zero location, α, by rearranging the above equation as
, and frequency point which will experience this maximum phase lead, , we can then determine the required zero location, α, by rearranging the above equation as
,where is measured in radians. Once we have determined α, we can then determine the pole location using
is measured in radians. Once we have determined α, we can then determine the pole location using
.We should also be cognizant of the fact that at there will also be a gain increase, so the frequency point that we identify needing phase lead will end up moving both rightwards and upwards in the log-polar plane. As such, we will need to adjust K after updating α and β using the equations above.
there will also be a gain increase, so the frequency point that we identify needing phase lead will end up moving both rightwards and upwards in the log-polar plane. As such, we will need to adjust K after updating α and β using the equations above.10.4.3 Proportional-integral-derivative control design
10.4.2.1 Definition
The PID controller is given by
,where α and β is the zero and pole of from the PD component, respectively, is integral time constant, andK is a gain applied at all frequencies. In simple terms, the PID controller can be thought of as a combination of a PD and PI controller.
from the PD component, respectively, is integral time constant, andK is a gain applied at all frequencies. In simple terms, the PID controller can be thought of as a combination of a PD and PI controller.10.4.2.2 Design methodology
Recall that a PI controller is designed based on first setting 
and then choosing K based on the fact that the PI controller behaves like a P controller under this condition. Once K is chosen, is then reduced until the frequency response locus approaches the stability bounds. Analogous to this, we can design a PID controller in two stages:
and then choosing K based on the fact that the PI controller behaves like a P controller under this condition. Once K is chosen, is then reduced until the frequency response locus approaches the stability bounds. Analogous to this, we can design a PID controller in two stages:- Stage 1: Set , which implies that
. Then select 
based on design methodology detailed in Section 10.4.2. - Stage 2: After selecting that meets design requirements, continually reduce value until stability design boundaries are close to limit (with some minor safety margin).
10.5 Determining orientation error
10.5.1 Quaternion-based error formulation
Recall from Chapter 8 that the orientation error can be derived using rotation matrices or quaternions. When using quaternions, the error quaternion in is given by
is given by
which encodes the rotation required to rotate from the current orientation (captured by frame ) to the desired orientation (captured by frame 
). Importantly, 
describes the rotation in , which means that the exponential coordinates can be extracted and used as part of feedback control strategies.
) to the desired orientation (captured by frame ). Importantly, describes the rotation in , which means that the exponential coordinates can be extracted and used as part of feedback control strategies.Given the standard unit quaternion structure, our error quaternion can be written as
where 
corresponds to the rotation angle required to correct the orientation of the robot, and 
refers to the rotation axis. Combined, the corresponding orientation error in is given by
corresponds to the rotation angle required to correct the orientation of the robot, and refers to the rotation axis. Combined, the corresponding orientation error in is given by
which gives a direct quantification of the amount of rotation required in each channel in order to reorient the rigid body as required. We will make use of this result in the following subsections. Note that we could also use the rotation matrix formulation to extract the same exponential coordinates. However, the quaternion approach is more computationally efficient and remains the approach of choice.
The error vector can be used in an attitude control scheme to inform rotational corrections about all three axes. The block diagram structure is shown Figure 10.4.
Figure 10.4: High-level block diagram of attitude control scheme using quaternion-based orientation error.
10.5.2 Thrust vectoring
An alternative approach to determining the orientation error of the quadcopter is to formulate the incremental orientation required in based on the direction in which we desire to point. Given that describes the desired frame, we can then determine the description of 
in , which is expressed as 
. The incremental orientation required to rotate 
to 
is described by
based on the direction in which we desire to point. Given that describes the desired frame, we can then determine the description of in , which is expressed as . The incremental orientation required to rotate to is described by
Recall from Chapter 3 that we can determine the exponential coordinates corresponding to the rotational mapping between two (normalised) vectors using the dot and cross product. Based on this, we can determine the exponential coordinates above using
where
The resulting exponential coordinates, 
, can be used as the tracking error for the quadcopter's roll and pitch controllers. What makes this approach different to that of Section 10.5.1 is that no error is imposed about (the yaw channel). This is because the cross product operation above takes in as the first argument, meaning that the rotation vector will always be perpendicular to . This is useful when we do not want to intentionally create motion about , as exemplified in Section 10.3. For this reason, we will make use of the thrust vectoring approach when deriving our orientation error.
, can be used as the tracking error for the quadcopter's roll and pitch controllers. What makes this approach different to that of Section 10.5.1 is that no error is imposed about (the yaw channel). This is because the cross product operation above takes in as the first argument, meaning that the rotation vector will always be perpendicular to . This is useful when we do not want to intentionally create motion about , as exemplified in Section 10.3. For this reason, we will make use of the thrust vectoring approach when deriving our orientation error. The block diagram representation of the thrust vectoring orientation control approach is shown in Figure 10.5.
Figure 10.5: High-level block diagram of attitude control scheme using thrust vectoring approach.
10.6 Attitude control design
10.6.1 Decoupled rotational dynamics
With the assumption that , the attitude dynamics simplify to
, the attitude dynamics simplify to Given that is a diagonal matrix given by
is a diagonal matrix given byattitude dynamics be thought of as three independent rotational systems, which are given by
where . We can now proceed to design three independent control laws for the roll, pitch, yaw dynamics presented above. However, we must not forget about the rotor speed model, which is given by
. We can now proceed to design three independent control laws for the roll, pitch, yaw dynamics presented above. However, we must not forget about the rotor speed model, which is given by
Given that the rotor speeds algebraically relate to the torque vectors, it follows that the desired and applied torque vector relationship is of the form
By extension, the three independent rotational systems above can be described as a function of the desired torque vector as
where indicates the fixed actuation delay, which is common across all four motors.
indicates the fixed actuation delay, which is common across all four motors.10.6.2 Roll/pitch control design
We will begin by designing the body-frame roll and pitch controllers. Given that , the design for both channels will be identical for symmetrical flight operations (when the expectations placed on rotational motion about and are equivalent. We will therefore perform the design based on information about and then use the resulting controller for both body-frame roll and pitch control.
, the design for both channels will be identical for symmetrical flight operations (when the expectations placed on rotational motion about and are equivalent. We will therefore perform the design based on information about and then use the resulting controller for both body-frame roll and pitch control.
The system above is linear time-invariant, so we can represent the system in transfer function form as
where 
is the input to the open-loop system (representing the desired torque about ) and 
is the corresponding angular velocity output. We are ultimately interested in controlling the orientation of the quadcopter, which requires adaptation of our plant model. While the relationship between the angular velocity in and the orientation in 
is naturally nonlinear, we will abstract the body-frame incremental orientation behaviour in a way similar to how we dealt with the translational and rotational control loops of the wheeled robots in Chapter 8. It follows that the incremental angular position about is given by
is the input to the open-loop system (representing the desired torque about ) and is the corresponding angular velocity output. We are ultimately interested in controlling the orientation of the quadcopter, which requires adaptation of our plant model. While the relationship between the angular velocity in and the orientation in is naturally nonlinear, we will abstract the body-frame incremental orientation behaviour in a way similar to how we dealt with the translational and rotational control loops of the wheeled robots in Chapter 8. It follows that the incremental angular position about is given by
where 
describes the incremental roll angle as a result of integrating the angular velocity about . Note that our plant model has obtained an additional integrator as a result of relating a moment to an angular position instead of an angular velocity.
describes the incremental roll angle as a result of integrating the angular velocity about . Note that our plant model has obtained an additional integrator as a result of relating a moment to an angular position instead of an angular velocity. Recalling Control Systems, the corresponding closed-loop system above will be unstable, as evidenced by the double integrators contributing 
of phase lag at all frequencies, as well as the delay time also contributing phase lag that monotonically increases with frequency. We therefore require a controller that can provide phase lead to stabilise the closed-loop system. Under the assumption that there are no external disturbances acting on our orientation system, we can get away with using a PD controller and rely on the two integrators to provide the sufficient type number to track step and ramp references.
of phase lag at all frequencies, as well as the delay time also contributing phase lag that monotonically increases with frequency. We therefore require a controller that can provide phase lead to stabilise the closed-loop system. Under the assumption that there are no external disturbances acting on our orientation system, we can get away with using a PD controller and rely on the two integrators to provide the sufficient type number to track step and ramp references.
,for our roll channel if we define a percent overshoot specification, , as well as a settling time requirement, 
. However, if we are expecting any disturbances to act on the roll/pitch channels, such as centripetal coupling, then we would require integral action. This can be facilitated using a PID controller of the form
, as well as a settling time requirement, . However, if we are expecting any disturbances to act on the roll/pitch channels, such as centripetal coupling, then we would require integral action. This can be facilitated using a PID controller of the form
which follows the design methodology described in Section 10.4.3. Examples of PD and PID designs are provided below, respectively.
Example: PD control design for roll channel
The example below provides the PD design framework for the system
where 
kgm
and 
seconds. The desired specifications are:
kgm and seconds. The desired specifications are:- a desired
settling time requirement of 
second and, - an allowed percentage overshoot of
.
is determined using
OS = 10;
zeta = -log(OS/100)/sqrt( pi^2+log(OS/100)^2 )
Ma = 1/( 2*zeta*sqrt(1-zeta^2) )
For the stay-above boundary we set , and then apply this stay-above boundary for all frequencies up until the peak frequency, which is defined as
, and then apply this stay-above boundary for all frequencies up until the peak frequency, which is defined as
rad/s.Ts = 1;
wn = 4/zeta/Ts;
wp = wn*sqrt(1-2*zeta^2)
We then begin by trying to design a P controller of the form 
, by setting .
, by setting .The code below generates the design boundaries in the log-polar plane and also plots the frequency response locus of
. Frequency point 
is also highlighted on the 
locus, as we require all frequencies up to to be above the enclosed green region.
is also highlighted on the locus, as we require all frequencies up to to be above the enclosed green region.clear
s = tf('s');
P = 1000/(s^2)*exp(-s*.01);
K = 0.005;
alpha = 3.43;
beta = 42;
G = K*(1+s/alpha)/(1+s/beta);
L = P*G;
Ts = 1;
OS = 10;
zeta = -log(OS/100)/sqrt( pi^2+log(OS/100)^2 );
wn = 4/zeta/Ts;
wp = wn*sqrt(1-2*zeta^2);
Ma = 1/( 2*zeta*sqrt(1-zeta^2) ); %overbound
Mb = .98; %underbound
figure(1),clf,hold on
M = [Ma Mb];
for k = 1:length(M)
phi = linspace(0,2*pi,1e4);
c = -1;
r = 1/M(k);
K = c + r*exp(1i*phi);
mag_K = 20*log10( abs(K) );
phi_K = angle( K )*180/pi;
for i=1:length(phi_K)
if( phi_K(i) > 0), phi_K(i) = phi_K(i)-360; end
end
if( k==1 )
plot( phi_K(2:end-1),-mag_K(2:end-1),'--b',LineWidth=2 )
else
plot( phi_K(2:end-1),-mag_K(2:end-1),'-g',LineWidth=2 )
Lw = squeeze( freqresp(L,wp) );
ang_Lw = angle(Lw)*180/pi;
if( ang_Lw > 0 )
ang_Lw = ang_Lw-360;
end
plot(ang_Lw,20*log10(abs(Lw)),'o',MarkerFaceColor='g',MarkerEdgeColor='k')
end
end
[mag,phase] = nichols(L);
for i=1:length(phase)
if( phase(i) > 0 )
phase(i) = phase(i)-360;
end
end
plot(squeeze(phase),squeeze(20*log10(mag)))
plot(-180,0,'ro',MarkerFaceColor='r')
legend('$|T(j\omega)|\leq M_A$','$|T(j\omega)|\geq M_B$',Interpreter="latex")
xlim([-360,0])
title('Loop transfer function of roll/pitch system in log-polar plane')
For 
and , we see that we are satisfying the (green) 
requirement, but not the (blue) requirement. We can now make use of our lead filter to provide the much needed phase lead so that we can avoid entering the blue circle. We choose as the frequency point at which our lead filter will supply the maximum phase lead (owing to the fact that this point will need to move right, and also end up moving upwards from the gain increase), and then measure that we need approximately 
, or 
radians, of phase lead in order to get out the circle.
and , we see that we are satisfying the (green) requirement, but not the (blue) requirement. We can now make use of our lead filter to provide the much needed phase lead so that we can avoid entering the blue circle. We choose as the frequency point at which our lead filter will supply the maximum phase lead (owing to the fact that this point will need to move right, and also end up moving upwards from the gain increase), and then measure that we need approximately , or radians, of phase lead in order to get out the circle. Using 
rad/s, we can then determine the required zero location of our controller using
rad/s, we can then determine the required zero location of our controller using,as determined using the code below.
w_max = wp;
phi_max = 80*pi/180;
alpha = w_max/tan( (phi_max+pi/2)/2 )
beta = w_max^2/alpha
After updating the α and β values in the code block above with a gain of , we see that we violating the stay-out boundary. We can either increase our value to provide phase lead at higher frequencies, or reduce the gain term, K. The latter is easiest, and a gain of 
is sufficient to meet both specifications in the frequency domain.
, we see that we violating the stay-out boundary. We can either increase our value to provide phase lead at higher frequencies, or reduce the gain term, K. The latter is easiest, and a gain of is sufficient to meet both specifications in the frequency domain.Checking the time-domain step response shows that we meet both user specifications in the time domain too.
T = feedback(L,1);
figure, hold on
step(T,Ts+2)
yline(1+OS/100,'--b',['$\% OS=$',num2str(OS),'$\%$'],'Interpreter','latex')
xline(Ts,'--r',['$t_s=$',num2str(Ts),' s'],'Interpreter','latex','LabelVerticalAlignment','bottom')
ylim([0 1+1.5*OS/100])
title('Simulated step response of closed-loop roll/pitch system')
Our designed feedback controller, which is applicable to both the roll and pitch channels, is given by
Example: PID control design for roll channel
We will use the same system from the previous example, namely,
where kgm and seconds. The desired specifications are also identical:
kgm and seconds. The desired specifications are also identical:- a desired settling time requirement of second and,
- an allowed percentage overshoot of .
This results in the same design boundaries and design frequency:
rad/s.According to the design methodology detailed in Section 10.4.3, we first design set 
and then the design as if its a PD controller. We will therefore make use of the solution provided in the previous example to simulate the first design stage where is selected.
and then the design as if its a PD controller. We will therefore make use of the solution provided in the previous example to simulate the first design stage where is selected.The code block below allows for design of the PID controller, as well as the necessary visualisation for purposes of synthesis.
clear
s = tf('s');
P = 1000/(s^2)*exp(-s*.01);
K = 0.005;
alpha = 0.33;
beta = 42;
Ti =5;
G = K*(1+s/alpha)/(1+s/beta)*(1+Ti*s)/(Ti*s);
L = P*G;
Ts = 1;
OS = 10;
zeta = -log(OS/100)/sqrt( pi^2+log(OS/100)^2 );
wn = 4/zeta/Ts;
wp = wn*sqrt(1-2*zeta^2);
Ma = 1/( 2*zeta*sqrt(1-zeta^2) ); %overbound
Mb = .98; %underbound
figure(1),clf,hold on
M = [Ma Mb];
for k = 1:length(M)
phi = linspace(0,2*pi,1e4);
c = -1;
r = 1/M(k);
K = c + r*exp(1i*phi);
mag_K = 20*log10( abs(K) );
phi_K = angle( K )*180/pi;
for i=1:length(phi_K)
if( phi_K(i) > 0), phi_K(i) = phi_K(i)-360; end
end
if( k==1 )
plot( phi_K(2:end-1),-mag_K(2:end-1),'--b',LineWidth=2 )
else
plot( phi_K(2:end-1),-mag_K(2:end-1),'-g',LineWidth=2 )
Lw = squeeze( freqresp(L,wp) );
ang_Lw = angle(Lw)*180/pi;
if( ang_Lw > 0 )
ang_Lw = ang_Lw-360;
end
plot(ang_Lw,20*log10(abs(Lw)),'o',MarkerFaceColor='g',MarkerEdgeColor='k')
end
end
[mag,phase] = nichols(L);
for i=1:length(phase)
if( phase(i) > 0 )
phase(i) = phase(i)-360;
end
end
plot(squeeze(phase),squeeze(20*log10(mag)))
plot(-180,0,'ro',MarkerFaceColor='r')
legend('$|T(j\omega)|\leq M_A$','$|T(j\omega)|\geq M_B$',Interpreter="latex")
xlim([-360,0])
title('Loop transfer function of roll/pitch system in log-polar plane')
With 
, for example, our PID controller largely resembles a PD controller, with all boundary conditions met. We can then reduce until we hit a design limit, which is usually the stability boundary. 
suffices to still meet all design boundaries, whilst also sufficiently reducing the integral time constant.
, for example, our PID controller largely resembles a PD controller, with all boundary conditions met. We can then reduce until we hit a design limit, which is usually the stability boundary. suffices to still meet all design boundaries, whilst also sufficiently reducing the integral time constant.Checking the time-domain step response shows that we meet both user specifications in the time domain too.
T = feedback(L,1);
figure, hold on
step(T,Ts+2)
yline(1+OS/100,'--b',['$\% OS=$',num2str(OS),'$\%$'],'Interpreter','latex')
xline(Ts,'--r',['$t_s=$',num2str(Ts),' s'],'Interpreter','latex','LabelVerticalAlignment','bottom')
ylim([0 1+1.5*OS/100])
title('Simulated step response of closed-loop roll/pitch system')
Our designed PID controller, which is applicable to both the roll and pitch channels, is given by
Note that in certain cases when designing a PID controller, we may need to update the values after choosing , as well as the peak frequency value (where the largest phase lead is applied).
values after choosing , as well as the peak frequency value (where the largest phase lead is applied). 10.6.3 Yaw-rate control
As stated in Section 10.3, we need to enforce in order reap the benefits of the simplified, decoupled attitude dynamics. The yaw dynamics in is given by
in order reap the benefits of the simplified, decoupled attitude dynamics. The yaw dynamics in is given by
The system above is linear time-invariant, so we can represent the system in transfer function form as
where 
is the input to the open-loop system (indicating the desired torque about ) and 
is the corresponding angular velocity output. Under the assumption that the yaw controller must act as a regulator (having a zero velocity setpoint), we can design either a P or PI controller, depending on the disturbance requirements. Given that there will be small but non-negligible centripetal coupling on yaw channel, based on Section 10.3, which is described by 
. Given this information, and recalling the type number requirements for a control loop with external disturbances, we will require an integrator in our controller in order to reject this effective disturbance fully at steady state. For this reason, we will use a modified version of the PI control design framework from Section 10.4.1, which has the form
is the input to the open-loop system (indicating the desired torque about ) and is the corresponding angular velocity output. Under the assumption that the yaw controller must act as a regulator (having a zero velocity setpoint), we can design either a P or PI controller, depending on the disturbance requirements. Given that there will be small but non-negligible centripetal coupling on yaw channel, based on Section 10.3, which is described by . Given this information, and recalling the type number requirements for a control loop with external disturbances, we will require an integrator in our controller in order to reject this effective disturbance fully at steady state. For this reason, we will use a modified version of the PI control design framework from Section 10.4.1, which has the form 
Specifically, while we should actually be designing the yaw loop based on sensitivity requirements of 
, we will keep things simple and use the approach outlined in Section 10.4.1, based on the fact that sensitivity and complementary sensitivity are related by
, we will keep things simple and use the approach outlined in Section 10.4.1, based on the fact that sensitivity and complementary sensitivity are related by
As a rule of thumb, we will double the bandwidth of the yaw-rate controller, relative to the roll/pitch controllers, in order to ensure that the term does not manifest large values, which would in turn result in coupling in the roll and pitch channels for 
, based on
does not manifest large values, which would in turn result in coupling in the roll and pitch channels for , based on 
The design of the yaw-rate PI controller follows based on Section 10.4.1, with an example given below.
Example: Yaw-rate PI control design
The example below provides the PI design routine for the yaw-rate system
where 
kgm and seconds. The performance specifications are:
kgm and seconds. The performance specifications are: - a desired settling time requirement of
seconds (based on the bandwidth of the yaw-rate controller needing to be at least twice as high as that of the roll/pitch controllers from the previous example), and - an allowed percentage overshoot of (we could relax this a bit if need be but we also do not want large overshoot spikes to result in coupling).
is determined usingOS = 10;
zeta = -log(OS/100)/sqrt( pi^2+log(OS/100)^2 )
Ma = 1/( 2*zeta*sqrt(1-zeta^2) )
For the stay-above boundary we set , and then apply this stay-above boundary for all frequencies up until the peak frequency, which is defined as
, and then apply this stay-above boundary for all frequencies up until the peak frequency, which is defined as
rad/s.Ts = 0.5;
wn = 4/zeta/Ts;
wp = wn*sqrt(1-2*zeta^2)
We then begin by trying to design a P controller of the form 
, by setting , which makes the PI controller appear like a P controller.
, by setting , which makes the PI controller appear like a P controller.The code below generates the design boundaries in the log-polar plane and also plots the frequency response locus of
. Frequency point is also highlighted on the 
locus, as we require all frequencies up to to be above the enclosed green region.
is also highlighted on the locus, as we require all frequencies up to to be above the enclosed green region.clear
s = tf('s');
P = 500/s*exp(-s*.01);
K = 0.06;
Ti = 0.8;
G = K*(1+s*Ti)/(s*Ti);
L = P*G;
Ts = 0.5;
OS = 10;
zeta = -log(OS/100)/sqrt( pi^2+log(OS/100)^2 );
wn = 4/zeta/Ts;
wp = wn*sqrt(1-2*zeta^2);
Ma = 1/( 2*zeta*sqrt(1-zeta^2) ); %overbound
Mb = .98; %underbound
figure(1),clf,hold on
M = [Ma Mb];
for k = 1:length(M)
phi = linspace(0,2*pi,1e4);
c = -1;
r = 1/M(k);
K = c + r*exp(1i*phi);
mag_K = 20*log10( abs(K) );
phi_K = angle( K )*180/pi;
for i=1:length(phi_K)
if( phi_K(i) > 0), phi_K(i) = phi_K(i)-360; end
end
if( k==1 )
plot( phi_K(2:end-1),-mag_K(2:end-1),'--b',LineWidth=2 )
else
plot( phi_K(2:end-1),-mag_K(2:end-1),'-g',LineWidth=2 )
Lw = squeeze( freqresp(L,wp) );
ang_Lw = angle(Lw)*180/pi;
if( ang_Lw > 0 )
ang_Lw = ang_Lw-360;
end
plot(ang_Lw,20*log10(abs(Lw)),'o',MarkerFaceColor='g',MarkerEdgeColor='k')
end
end
[mag,phase] = nichols(L,{0.01,500});
for i=1:length(phase)
if( phase(i) > 0 )
phase(i) = phase(i)-360;
end
end
plot(squeeze(phase),squeeze(20*log10(mag)))
plot(-180,0,'ro',MarkerFaceColor='r')
legend('$|T(j\omega)|\leq M_A$','$|T(j\omega)|\geq M_B$',Interpreter="latex")
xlim([-360,0]),ylim([-20 40])
Starting with , we see that a gain of 
is sufficient to meet both specifications. We then incrementally decrease as far as possible until we approach the stay-out boundary. A value of 
brings the frequency response locus close to the stay-out boundary without violating it.
, we see that a gain of is sufficient to meet both specifications. We then incrementally decrease as far as possible until we approach the stay-out boundary. A value of brings the frequency response locus close to the stay-out boundary without violating it.Checking the time-domain step response shows that we meet both user specifications.
T = feedback(L,1);
figure, hold on
step(T,Ts+2)
yline(1+OS/100,'--b',['$\% OS=$',num2str(OS),'$\%$'],'Interpreter','latex')
xline(Ts,'--r',['$t_s=$',num2str(Ts),' s'],'Interpreter','latex','LabelVerticalAlignment','bottom')
ylim([0 1+1.5*OS/100])
Our designed yaw-rate feedback controller is given by
10.7 Implementation of nonlinear control scheme
Following the design of the roll/pitch and yaw controllers, we can integrate all elements and apply for the control solution to the full nonlinear dynamics of the orientation plant. This naturally requires a feedback mechanism, which in this case is facilitated using the thrust vectoring approach outlined in Section 10.5.2. The block diagram in Figure 10.6 describes the nonlinear control scheme when using a yaw-rate PI controller, a roll/pitch PD controller.
Figure 10.6: Attitude control scheme using thrust vectoring.
The control scheme above in theory allows us to specify a direction that we want to point towards, namely , which subsequently dictates the orientation change required by the quadcopter to make this possible. While the specification of 
is currently somewhat arbitrarily, we will ultimately determine this quantity using our translational position controller, which will be covered in Chapter 11.
to point towards, namely , which subsequently dictates the orientation change required by the quadcopter to make this possible. While the specification of is currently somewhat arbitrarily, we will ultimately determine this quantity using our translational position controller, which will be covered in Chapter 11.Example: Attitude control
The code block below provides a nonlinear 3D simulation of the quadcopter using the yaw-rate PI controller and roll/pitch PD controllers. The controllers are implemented using pseudo-continuous-time difference equations.
You should notice that:
- When
, the system behaves as expected, with settling times equivalent to that of the linear simulations. - When
, the unmodelled dynamics causes some differences between the linear simulations and nonlinear counterpart, depending on the difference between and . - When , centripetal coupling results in a disturbance torque on the yaw channel, which can cause a resulting disturbance torque on the roll/pitch channels. As a result, the PD controller (lacking an integrator) will result in a non-zero steady state error in the short term.
- As the body-frame yaw-rate is being regulated and not the yaw angle, the yaw angle can drift slowly over time when . However, this does not affect the direction of the thrust vector.
Jxx = 1e-3;
Jyy = 1e-3;
Jzz = 2e-3;
J = diag( [Jxx,Jyy,Jzz] );
dT = 0.01;
%initial conditions
w = [0 0 0]';
q = [1 0 0 0];
u = [0 0 0]';
%desired direction of z axis
z_des = [1 1 0]';
z_des = z_des/norm(z_des);
%desired quaternion
z = [0 0 1]';
n = cross(z,z_des);
alpha = acos( dot(z,z_des));
v = n/sin(alpha);
q_des = [cos(alpha/2) sin(alpha/2)*v'];
R_des = quat2rotm(q_des);
e_past = zeros(3,1);
u_past = zeros(3,1);
desiredMotorSpeeds_past = zeros(4,1);
u_dot = zeros(3,1);
e_dot = zeros(3,1);
w_dot = zeros(3,1);
kf = 1e-5;
kt = 1e-8;
l = 0.1;
A = [kf kf kf kf;
0 -l*kf 0 l*kf;
-l*kf 0 l*kf 0;
kt -kt kt -kt];
invA = inv(A);
q_array = [];
time = [];
for i=1:0.5/dT
%orientation error
R = quat2rotm(q);
z = [0 0 1]';
z_des = R'*R_des*[0 0 1]';
n = cross(z,z_des);
alpha_e = acos( dot(z,z_des));
v_e = n/sin(alpha_e);
e = alpha_e * v_e;
%derivatives
e_dot = (e-e_past)/dT;
%roll/pitch PD controller
K = 0.005;
a = 0.33;
b = 42;
u_dot(1) = -b*u(1)+b*K*e(1)+b*K/a*e_dot(1);
u(1) = u(1)+dT*u_dot(1);
u_dot(2) = -b*u(2)+b*K*e(2)+b*K/a*e_dot(2);
u(2) = u(2)+dT*u_dot(2);
%yaw PI controller
K = 0.06;
Ti = 0.8;
u_dot(3) = K/Ti*(-w(3))+K*(-w_dot(3));
u(3) = u(3)+dT*u_dot(3);
%control allocation
desiredMotorSpeeds = invA*[0; u];
%dynamics
motorSpeeds = desiredMotorSpeeds_past;
m = A(2:4,:)*motorSpeeds;
w_dot = J\( m-cross(w,J*w) );
w = w+dT*w_dot;
alpha = norm(w)*dT;
if( alpha==0 )
v = [0 0 0]';
else
v = w/norm(w);
end
dq = [cos(alpha/2) sin(alpha/2)*v'];
q = quatmultiply(q,dq);
q = q/norm(q);
%update past state
e_past = e;
desiredMotorSpeeds_past = desiredMotorSpeeds;
%plotting
% if( mod(i,5)==0 )
% figure(1),clf
% plotTransforms([0 0 0],q,"MeshFilePath",'multirotor.stl',"MeshColor","red","FrameAxisLabels","on","FrameLabel","B")
% xlim([-2 2]),ylim([-2 2]),zlim([-1 3])
% drawnow
% end
q_array = [q_array; q];
time = [time dT*i];
end
%determine zb wrt {w} and zd wrt {w}
z_b2w = quatrotate(q_array,[0 0 1]);
z_d2w = quatrotate(q_des,[0 0 1]);
%plots
figure(3),clf
subplot(3,1,1),plot(time,z_b2w(:,1)),hold on, line([0 0.5],[z_d2w(1) z_d2w(1)])
xlabel('time'),ylabel('x-axis'),legend('${^W\hat{\bf z}_B}$','${^W\hat{\bf z}_D}$',"interpreter","latex")
subplot(3,1,2),plot(time,z_b2w(:,2)),hold on, line([0 0.5],[z_d2w(2) z_d2w(2)])
xlabel('time'),ylabel('y-axis'),legend('${^W\hat{\bf z}_B}$','${^W\hat{\bf z}_D}$',"interpreter","latex")
subplot(3,1,3),plot(time,z_b2w(:,3)),hold on, line([0 0.5],[z_d2w(3) z_d2w(3)])
xlabel('time'),ylabel('z-axis'),legend('${^W\hat{\bf z}_B}$','${^W\hat{\bf z}_D}$',"interpreter","latex")
