Introduction to MulticopterDesign and Control
Quan Quan , Associate [email protected]
BUAA Reliable Flight Control Group, http://rfly.buaa.edu.cn/Beihang University, China
Lesson 12 Position Control Based On Semi-Autonomous Autopilot
2016/12/25 2
Howisamulticopter controlledbasedontheSemi‐
AutonomousAutopilotstotrackagiventargetposition?
1. Problem Formulation
2. System Identification
3. Position Controller Design
4. Simulation
2016/12/25 3
Outline
2016/12/25 4
1. Problem Formulation
Semi-Autonomous Autopilot Based Development
Planning
Position control
Attitude control
Control allocation +Motor control
Position Stabilization
Attitude Stabilization
SAA
FAA
SAA: Semi-Autonomous Autopilot, FAA: Fully-Autonomous Autopilot
2016/12/25 5
Semi-Autonomous Autopilot Based Development
1. Problem Formulation
Planning
Position control
内环控制
零散硬件
Commercial Multicopterwith SDK,
Open-Source Multicopter
SAA
FAA
Attitude Stabilization
Position Stabilization
SecondaryD
evelopment
Not only can it avoid the trouble of modifying the low-level source code of autopilots, but also it can utilize commercial reliable autopilots to achieve targets.
2016/12/25 6
Open-Source Projects Web site URL
Ardupilot http://ardupilot.com
Openpilot http://www.openpilot.org/
Paparazzi http://paparazziuav.org
Pixhawk https://pixhawk.ethz.ch/
Mikrokopter http://www.mikrokopter.de
KKmulticopter http://www.kkmulticopter.kr/
Multiwii http://www.multiwii.com/
Aeroquad http://www.aeroquadstore.com/
Crazyflie https://www.bitcraze.io/category/crazyflie/
CrazePony http://www.crazepony.com/
DR. R&D http://www.etootle.com/
ANO http://www.anotc.com/
Autoquad http://autoquad.org/
MegaPirate http://megapiratex.com/index.php
Erlerobot http://erlerobotics.com/
MegaPirateNG http://code.google.com/p/megapirateng
Open-Source Projects Web site URL
Taulabs http://forum.taulabs.org/
Flexbot http://www.flexbot.cc/
Dronecode https://www.dronecode.org/
Percepto http://www.percepto.co/
Parrot API(SDK) https://projects.ardrone.org/embedded/ardrone-
api/index.html
3DR DRONEKIT(SDK) http://www.dronekit.io/
DJI DEVELOPER(SDK) http://dev.dji.com/cn
DJI MATRICE 100+ DJI
Guidance
https://developer.dji.com/cn/matrice-100/
SDK for XMission(SDK) http://www.xaircraft.cn/en/xmission/developer
EHANG GHOST SDK(SDK) http://dev.ehang.com/
Table 1.1 Major open source Projects
1. Problem Formulation
Semi-Autonomous Autopilot Based Development
2016/12/25 7
[1] How J P, et al. Real-time indoor autonomous vehicle test environment. IEEE Transaction on Control Systems, 2008, 28(2): 51-64.
Real-Time Indoor Autonomous Vehicle Test Environment,MIT
1. Problem Formulation
Semi-Autonomous Autopilot Based Development
2016/12/25 8
Sem
i-A
uton
omou
s A
utop
ilotStabilize Mode
Altitude Hold Mode
Loiter Mode
……
A position controller is designed based on the stabilize mode, which is the most basic mode among the three commonly-used modes.
1. Problem Formulation
Structure of Multicopter with SAA
2016/12/25 9
Structure of Multicopter with SAA
Tuz
uu u
Figure 12.1 The closed loop diagram based on SAA
1. Problem Formulation
Attitude Model+ SAA
(Black Box)
Position Model(Gray Box)
u
u
zu
Tu
bor xv
bor yv
ezv
RC CommandsAttitude Angles
+ Vertical VelocityPosition
+ Yaw Angle
Position Controller
e e e, , ,x y zp p p
bz
2016/12/25 10
A multicopter is divided into three channels
Altitude Channel Yaw Channel Horizontal position channel
1. Problem Formulation
Models of Three Channels
2016/12/25 11
Parameters depending
on Multicopter and
Autopilot are Unknown.
1. Problem Formulation
Models of Three Channels (Assumptions)
Altitude Channel
Yaw Channel
Horizontal position channel
e e
e ez T
z z
z v z u T
p v
v k v k u
z zz
z
z z uk k u
b
b h bb
b
b h h b hb
h h
h h h
h h
h h h h
0 11 0
g
v
Θ ω u
p R v
v K v Θ
Θ ω
K Θ K ω K uω
2016/12/25 12
Objective of Position Control
Given a reference trajectory,it consists two parts:
and
Objective is to design extra position controller satisfying
Is a ball with the center at zero and radius 。
or
1. Problem Formulation
d ( )tp d ( )t
4 1( , )0B
d( ) ( ) 0t t x x d 4 1( ) ( ) ( , )t t x x 0BTT
d d d= x pTT= x p
The purpose of the identification is to obtain the parameters of the transfer functions. This will facilitate the controller design.
2. System Identification
2016/12/25 13
Objective
Figure 12.2 System Identification Toolboxof MATLAB.
Figure 12.3 Comprehensive Identification from FrEquency Responses(CIFER) toolbox.
2016/12/25 14
1. Prior Knowledge. The priorknowledge means the existingknowledge about the characteristicsof the system, the data recordingmethods and other aspects of thesystem. This knowledge is veryhelpful to choose the candidatemodels, design the experiment, anddetermine the calculation methodand the criterion of validation.Because of the different goals ofidentifications, the required priorknowledge may be very different,even for the same system.
2. System Identification
Figure 12.4 The procedure of system identification
Procedure
2016/12/25 15
2. Experiment design. The purposeof the experiment design is to obtainthe input-output data of the system,which can reveal the systemperformance as much as possibleunder known conditions. The input-output data are sometimes recordedduring a specifically designedidentification experiment, where theuser may determine which signals tomeasure and when to measure them.
2. System Identification
Figure 12.4 The procedure of system identification
Procedure
2016/12/25 16
There are two kinds of experiments for system identification: open-loop experiments and closed-loop experiments, which are shown in Figure 12.3(a)(b), respectively.
• The excitation for open-loop experiments are richerthan that of closed-loop experiments, because any input signal can be selected in open-loop experiments. Obviously, if systems can work without controllers, open-loop experiments are the better choices.
• Some systems are unstable, and they must work with feedback controllers. As a result, the input signals are decided by the controllers.
Figure 12.5 Two kinds of experiments for system identification
Procedure
2. System Identification
2016/12/25 17
3. Data record. Observe the input-output data through the designedexperiment.4. Model selection. Choose a set ofcandidate models and determinewhich one of them is the most suitablemodel through the validation.Through the mathematical modeling,a parametric model with unknownparameters can be obtained. Then, theunknown parameters can becalculated by parametric identificationmethods.
2. System Identification
Figure 12.4 The procedure of system identification
Procedure
2016/12/25 18
5. Model calculation. The model calculation means that the unknown parameters of the candidate model are determined by appropriate optimization methods.6. Model validation. A criterion is determined to test whether the candidate model with the calculated parameters is valid for the designed purpose. In general, this criterion relies on the observation data, the prior knowledge, and the application of the identified model. If the model with the parameters passes the validation, it will be chosen as the final model. If not, the previous steps need to be revised.
Procedure
2. System Identification
Figure 12.4 The procedure of system identification
2016/12/25 19
A trade-off open-loop system identification
The modification can be usedfor identification on newsystems. But, the originalsystem is still unknown.
1. Altitude Channel
2. System IdentificationModel used in System Identification
2016/12/25 20
Unstable
Candidate Model
Controller redesign
Stable
2. System Identification
1. Altitude Channel
Model used in System Identification
e e
e ez T
z z
z v z u T
p v
v k v k u
e e
e eT z e z T z
z z
z u p z v z u p
p v
v k k p k v k u
ez zT p z pu k p u
e z p zzz p u pp s G s u s
2016/12/25 21
Model used in System Identification2. Yaw Channel
Unstable
Stable
Candidate Model
2. System Identification
z zz
z
z z uk k u
zu k u
zz z
z
z u z uk k k k u
us G s u s
This channel is harder to control than the other two channels because of the influence of . Thus, the horizontal velocity controller is designed in the following part.
2016/12/25 22
The horizontal position channel is identified after the other twochannels are well controlled. In this situation, under the controllerof the yaw channel, is satisfied. Then, is a constant matrix.The transfer function of horizontal position channel is expressed asfollows
2. System IdentificationModel used in System Identification
3. Horizontal Position Channel
e h hb
h hb
h h
h
1 1diag ,
1 1diag ,
s s ss s
s ss s
v u
v u
p R G u
R G u
d R
R
b h hb hh s s s
vv u vv G u
2016/12/25 23
2. System IdentificationModel used in System Identification
3. Horizontal Position ChannelFor the SAA which contains a velocity feedback controller, is reasonable. The transfer function is stable in channel and it can be identified directly. Otherwise
b h bb
b
b h h b hb
h h h
h h
h h h h
0 11 0
g
v
Θ ω u
v K v Θ
Θ = ω
Θ K ω uω K K
h b hbh h v vu K v u
b h bb
b
b h h b h h b h hb b
h h h
h h
h h h h
0 11 0
g
v
u v Θ ω u vω
v K v Θ
Θ = ω
K K v K Θ K ω K u
hvK
b h
svv uG
3. Position Controller Design
2016/12/25 24
PID Controllerdy y
ue Multicopter
with SAA
p d d d i d( )z e e z e e z e eT p z z p z z p z zu k p p k p p k p p
p d d d i dz zu k k k
1 1 1h hp h hd hd h hd hi h hd
u K R p p K R p p K R p p
Figure 12.5 PID controller
2016/12/25 25
Simple, no requiring model
PID Controller
3. Position Controller Design
Parameters need to be tunedrepeatedly especially forstrongly coupled channel, andit is difficult to achieve an idealtransient process.
2016/12/25 26
AOD:Additive Output Decomposition, DIC: Dynamic Inversion Control
Gdy 1dG y
dy1G
1. In practice, is unknown, but it can be identified as approximately
2. There have differences between and . How to deal with?G
G
DIC
1. Idea of DIC
Plant
3. Position Controller Design
AOD-Based DIC
G
G
2016/12/25 27
AOD for transfer functions:
[2] Quan Quan, Kai-Yuan Cai. Additive-Output-Decomposition-Based Dynamic Inversion Tracking Control for a Class of Uncertain Linear Time-Invariant Systems. The 51st IEEE Conference on Decision and Control, 2012, Maui, Hawaii, USA, 2866-2871.
3. Position Controller Design
AOD-Based DIC[2]
1. Idea of DIC
y Gu d s py y yp p py G u s p py Gu d G u
, ,
,
x f t x u
y h t x
p sy t y t y t
, ,
,
p p p p
p p p
x f t x u
y h t x
, ,
, ,s s s s
s s s p p
x f t x u
y h t x h t x
2016/12/25 28
Identification
AOD:
AOD-Based DIC
3. Position Controller Design
2. Altitude channel
el
ˆ ˆ( )z z p zzp z p u pd p s G s u s
e z p zzz p u pp s G s u s
epˆ
z p zzz p u p pp s G s u s
es
ˆz p z z p zz zz p u p p u p pp s G s u s G s u s
ep
ˆz p zzz p u p pp s G s u s
lˆ
z z p z p zz zp p u p u pd G s G s u s
e
e e
p
p l
ˆz p zz
z
z p u p
z z p
p s G s u s
p s p s d s
2016/12/25 29
AOD-Based DIC:
is realizable,and
AOD-Based DIC
Unrealizable ,so a low-pass filter is adopted
2. Altitude channel
3. Position Controller Design
z pzp uG s
ˆz pzp uG s
ezp
epzp
lzpd
edzp
z pzp uQ s 1ˆz pzp uG s
Figure 12.6 The closed-loop system based on AOD-based DIC
e
1d l
ˆz z p zzp p u z pu s G s p s d s
e
1d l
ˆz z p z p zz zp p u p u z pu s Q s G s p s d s
z pzp uQ s
1ˆz p z pz zp u p uQ s G s 0 1
z pzp uQ
2016/12/25 30
Stability analysis,theorem:
Suppose (1) is minimum phase;
(2) and are stable, and ;
(3) ;
(4) is constant。
Then is bounded and as ,where
3. Position Controller Design AOD-Based DIC2. Altitude channel
Quan Quan, Kai-Yuan Cai. Additive-Output-Decomposition-Based Dynamic Inversion Tracking Control for a Class of Uncertain Linear Time-Invariant Systems. The 51st IEEE Conference on Decision and Control, 2012, Maui, Hawaii, USA, 2866-2871.
ˆz pzp uG s
z pzp uQ s
z pzp uG s 0 1z pzp uQ
edzp
zpu t ( ) 0zpe t
e edzp z ze p p
1ˆsup 1 1z p z p z pz z zp u p u p uG j G j Q j
2016/12/25 31
3. Position Controller Design AOD-Based DIC3. Other two channels
and
h h v h v b hb h b h
1h d l
ˆs s s s s v v u v u vu Q G v d
h b h v hb hl h v
ˆs s s s v v ud v G u
1d l
ˆu uu s Q s G s s d s
lˆ
ud s s G s u s
2016/12/25 32
3. Position Controller Design AOD-Based DIC3. Other two channels
The desired horizontal velocity needs to be transformed from the desired horizontal position. First, the transient process is expected as follows
where . If so, then . Since
the desired horizontal velocity should satisfy
Then
hh hd h hd( ) pp p K p p h hdlim ( ) ( ) 0
tt t
p p
bh hp R v
b hh d hd h hd( ). pR v p K p p
b h
1h d h hd( ).
pv R K p p
h
2 2 pK D P
2016/12/25 33
Transient process is nice,the tuned parameters are
less,less experiments
Design is somewhat complicated
AOD-Based DIC
3. Position Controller Design
4. Simulation
2016/12/25 34
Plant
Zander J (2013) AR Drone Simulink. https://github.com/justynazander/AR Drone Simulink. Accessed 20 Jan 2016
2016/12/25 35
System Identification
4. Simulation
Figure 12.7 The simulation for the system identification
hu,
zTu u
b eh , ,zp vh
,zpu uvu
Inputs
Outputs Measured Outputs
Actual Outputs
: AR. Drone with Stabilizing ControllerG
2016/12/25 36
1. Prior Knowledge
System Identification
4. Simulation
The models of multicopters are established above are the prior knowledge.
Altitude Channel
Yaw Channel
Horizontal position channel
e e
e ez T
z z
z v z u T
p v
v k v k u
z zz
z
z z uk k u
b
b h bb
b
b h h b hb
h h
h h h
h h
h h h h
0 11 0
g
v
Θ ω u
p R v
v K v Θ
Θ ω
K Θ K ω K uω
2016/12/25 37
2. Experiment design
4. Simulation
System Identification
Before the identification, stabilizing controllers will be employed if needed. The controllers of the altitude channel (also called as z channel) and the yaw channel (also called as ψ channel) are designed as
Here, the desired yaw angle is set to 0, so under the well-designed yaw channel controller and the horizontal position channel is decoupled into x channel and y channel. This means that controls velocity through θ, and controls the velocity through . Then
b h hb hh
( ) ( )( ) G ( ) ( )
( ) ( )x v xx
y v yy
v u v
v u v
G s u ss s s
G s u s
vv u vv u
ez zT p z pu k p u z
u k u
d
eyv exv
yvuxvu
0 5 10 150
0.5
1
1.5
0 5 10 150
0.2
0.4
0.6
0 5 10 150
0.2
0.4
0.6
0 5 10 150
0.5
1
1.5
channelx channely
channelz yaw channelTime (s)
Time (s)
Time (s)
Time (s)
(m/s
)bxv
()
rad
(m)
ezp
(m/s
)byv
Setting Time 3.71s
Setting Time 3.52s Setting Time 2.76s
Setting Time 5.85s
2016/12/25 38
Settling time is long!
System Identification
4. Simulation
2. Experiment design
Figure 12.8 The actual step response of each channel under PID controller
2016/12/25 39
4. Simulation
System Identification3. Data record
Pseudo-Random Binary Signals (PRBS) are generated to stimulate these channels.
2016/12/25 40
4. Simulation
System Identification3. Data record
Figure 12.9 The input output data of the four channels used in the identification (The output dataare the measured outputs)
2016/12/25 41
The principle of choosing model is
(1) The identification results can pass the model validation and the fitness is used as the criterion.
(2) The identification results are supposed to be minimum-phase systems, because nonminimum phase systems are hard to tackle for most controller designs.
(3) While the previous two principles are guaranteed, the order of the transfer function needs to be chosen as small as possible.
4. Simulation
System Identification4. Model selection
2016/12/25 42
5.Model calculation
Time Domain Data
Remove means
Transfer Function Models
4. Simulation
System Identification
The math works. System identification toolbox. http://www.mathworks.com/help/ident/index.html. Accessed 20 Jan 2016
2016/12/25 43
5. Model calculation
4. Simulation
System Identification
The fitness of the four channels are: 98.56%, 98.08%, 95.84%, 90.40%
3 2
2
2
15.48 29.94.642 16.09 10.91
7.086 17.024.742 15.49 7.063
6.257.077 6.249
1.277 3.5064.045 3.522
x vx
y vy
z pz
v u
v u
p u
u
sG ss s s
sG ss s s
G ss s
sG ss s
G
2016/12/25 44
AOD-Based DIC
4. Simulation
h hb 2 2
2
2
1 1( ) diag ,( 1) ( 1)
1( )( 1)
1( 1)
z pz
x y
p uz
u
ss s
Q ss
Qs
v vQ u
0.2, 0.2, 0.3, 0.3x y z
Figure 12.10 The simulation structure for the AOD-based DIC controller
Gˆ -1G Q
G
2016/12/25 45
AOD-Based DIC
Simple P/PD Stabilizing Controller AOD-Based DIC
Tracking performanceis improved
4. Simulation
0 5 10 150
0.5
1
1.5
0 5 10 150
0.2
0.4
0.6
0 5 10 150
0.2
0.4
0.6
0 5 10 150
0.5
1
1.5
channelx channely
channelz yaw channelTime (s)
Time (s)
Time (s)
Time (s)
(m/s
)bxv
()
rad
(m)
ezp
(m/s
)byv
Setting Time 3.71s
Setting Time 3.52s Setting Time 2.76s
Setting Time 5.85s(s)
(m/s
)bxv
()
rad
(m)
ezp
(m/s
)byv
3.71s
3.52s 2.76s
5.85s
0 5 10 150
0.2
0.4
0.6
0 5 10 150
0.2
0.4
0.6
0 5 10 150
0.2
0.4
0.6
0 5 10 150
0.2
0.4
0.6
Settling Time 1.10s Settling Time 1.01s
Settling Time 1.60s Settling Time1.02s
channelx channely
channelz yaw channelTime (s) Time (s)
Time (s) Time (s)
2016/12/25 46
Figure 12.11. The comparison of the horizontal position tracking performance
DIC controller has an excellenttracking performance. However,the tracking trajectories ofthe DIC controller lag behindthose of the PD controller alittle. The reason is the gainKph is not large enough. IfKph increases, the time delay ofthe tracking will decrease, butthe transient process will getworse, vice versa. Thus, usersneed to adjust Kph of the DICcontroller according to actualrequirements.
AOD-Based DIC
4. Simulation
-0.6 -0.4 -0.2 0 0.2 0.4-0.6
-0.4
-0.2
0
0.2
0.4
(m)
eyp
(m)exp
Actual Trajectory (AOD-based DIC)Actual Trajectory (PD Controller)Desired Trajectory
5. Summery
2016/12/25 47
1. More and more multicopter companies open the SDKs of their
autopilots. Based on these autopilots, tasks can be accomplished by
secondary developments.
2. Practice method is proposed for secondary developments.
3. In the future, one possibility of the development on multicopters is
that some companies provide a stable and reliable multicopter platform
or operating systems, based on which new applications can be
developed easily.
2016/12/25 48
Acknowledgement
Deep thanks go to
for material preparation.Jing Zhang Zi-Bo Wei Xunhua Dai
2016/12/25 49
Thank you!All course PPTs and resources can be downloaded at
http://rfly.buaa.edu.cn/course
For more detailed content, please refer to the textbook: Quan, Quan. Introduction to Multicopter Design and Control. Springer,
2017. ISBN: 978-981-10-3382-7.It is available now, please visit http://
www.springer.com/us/book/9789811033810