-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathch3.tex
2010 lines (2010 loc) · 183 KB
/
ch3.tex
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
%====================================================
% CHAPTER 3 - Dynamics
%====================================================
\chapter{Kinematics and Dynamics}
\label{ch:dynamics}
%====================================================
The following generally applicable rigid body dynamics are first developed with respect to generalized net forces and torques acting on a rigid vehicle. Following that, dynamics are extended to the nonlinear multibody case wherein constrained relative rotational actuation between interconnected bodies is incorporated, representing the actuator action which the prototype can undergo. Propeller aerodynamic effects are subsequently included into the actuation input model. Finally a consolidated quaternion-based model is presented which is used for the controller development next in Ch:\ref{ch:control}.
%====================================================
\section{Rigid Body Dynamics}
\label{sec:dynamics.rigidbody}
%====================================================
\subsection{Lagrange Derivation}
\label{subsec:dynamics.rigidbody.lagrange}
%====================================================
Fundamentally any body, rigid or otherwise, can undergo two kinds of motion; namely rotational and translational movement. Often a Lagrangian approach for combined angular and translational movements is used to derive the differential equations of motion for each degree of freedom, \cite{classicaldynamics}. The Lagrangian principle ensures that (translational and rotational) energies are conserved throughout the system's state progression. When combined with Euler-Rotation equations, the Euler-Lagrangian formulation from \cite{lagrange-formalism} fully defines the aerospace 6-DOF equations of motion.
\par
Lagrangian formulation is regarded as especially useful in non-Cartesian (\emph{spherical etc\ldots}) coordinate frames and with multibody systems. With that being said, Cartesian coordinates were already defined in Sec:\ref{subsec:proto.conventions.motoraxis} for the plant. Alternatively, relative coordinates could be used for implicit Euler-based dynamics as in \cite{autonomousrobotseuler}. Rigid body dynamics in Cartesian coordinates do lend themselves to Newtonian mechanics. Both Newton-Euler and Euler-Lagrange formulations produce the same resultant differential equations of motion, but follow conceptually different derivations. The Lagrangian operator $\mathcal{L}$ is a scalar term defined as the difference between a trajectory's kinetic and potential energies, $T$ and $U$ respectively. Considering some generalized path trajectory $\vec{\mathbf{r}}(t)$ for a body, with both position $\vec{\xi}$ and attitude $\vec{H}$ states:
\begin{equation}\label{eq:generalpath}
\vec{\mathbf{r}}(t)\triangleq\begin{bmatrix}
\vec{\xi} & \vec{H}
\end{bmatrix}^T~~~~\in\mathcal{F}^{a}
\end{equation}
Coordinates in Eq:\ref{eq:generalpath} are \emph{generalized} and taken with respect to some hypothetical shared frame $\mathcal{F}^{a}$. The generalized coordinates are later refined to Cartesian body coordinates with respect to the inertial frame. The Lagrangian is the difference of the trajectory's kinetic and potential energies, by definition:
\begin{subequations}
\vspace{-6pt}
\begin{equation}\label{eq:lagrangian.a}
\mathcal{L}\big(\vec{\mathbf{r}},\dot{\vec{\mathbf{r}}},t\hspace{1pt}\big)\triangleq T\big(\vec{\mathbf{r}},\dot{\vec{\mathbf{r}}}\hspace{1pt}\big)-U\big(\vec{\mathbf{r}},\dot{\vec{\mathbf{r}}}\hspace{1pt}\big)
\end{equation}
where the trajectory's kinetic and potential energy functions are $T$ and $U$ respectively, introducing a rigid body's general (translational and rotational) kinetic and potential energies, both defined with respect to that shared reference frame $\mathcal{F}^a$.
\par
Noting first that there is no attitude contribution for stored potential energy, so $U\big(\vec{\mathbf{r}},\dot{\vec{\mathbf{r}}}\hspace{1pt}\big)$ consists entirely of gravitational potential energy. The gravitational acceleration vector in the inertial frame $\mathcal{F}^I$ is:
\begin{equation}\label{eq:grav}
\vec{G}_I=\begin{bmatrix} 0&0&-9.81 \end{bmatrix}^T~~~~[\text{m.s}^{-2}],~\in\mathcal{F}^I
\end{equation}
where $\vec{G}_I$ acts in the negative $\hat{Z}_I$, downward, direction. Substituting translational kinetic and potential energies into the Lagrangian yields the following scalar term:
\begin{equation}\label{eq:lagrangian.b}
\mathcal{L}(\vec{\mathbf{r}},\dot{\vec{\mathbf{r}}},t)=\frac{1}{2}\dot{\vec{\xi}}^{~T}(m_b)\dot{\vec{\xi}}+\frac{1}{2}\dot{\vec{H}}^{~T}(J_b)\dot{\vec{H}}-m_b\vec{G}_{a}(h\cdot\hat{Z}_I)
\end{equation}
\end{subequations}
The vehicle's mass is $m_b$ and its generalized inertia matrix is similarly $J_b$, aligned and translated with respect to the common frame $\mathcal{F}^{a}$. The Euler-Lagrange formulation equates partial derivatives of the Lagrangian to any generalized forces $\vec{\mathbf{R}}$ acting on the system in frame $a$. In the rigid body motion case those \emph{generalized forces} are net forces $\vec{F}_{\mu}$ and net torques $\vec{\tau}_{\mu}$ in the shared frame $\in\mathcal{F}^{a}$.
\begin{equation}\label{eq:euler-lagrange}
\frac{d}{dt}\bigg(\frac{\partial \mathcal{L}}{\partial \dot{\vec{\mathbf{r}}}}\bigg)-\frac{\partial \mathcal{L}}{\partial \vec{\mathbf{r}}} = \vec{\mathbf{R}} = \begin{bmatrix}
\vec{F}_{\mu}\\
\vec{\tau}_{\mu}
\end{bmatrix}~~~~\in\mathcal{F}^{\Lambda}
\end{equation}
Evaluating symbolic partial derivatives of Eq:\ref{eq:lagrangian.b} with respect to the path coordinates $\vec{\mathbf{r}}(t)$ and path rates $\dot{\vec{\mathbf{r}}}(t)$ respectively produces the two following equations:
\begin{subequations}
\begin{equation}\label{eq:partial.a}
\frac{\partial \mathcal{L}}{\partial \vec{\mathbf{r}}}=\begin{bmatrix}
m_b\vec{G}_a\\
0
\end{bmatrix}~~~~\in\mathcal{F}^{a}
\end{equation}
\vspace{-5pt}
\begin{equation}\label{eq:partial.b}
\frac{d}{dt}\bigg(\frac{\partial \mathcal{L}}{\partial \dot{\vec{\mathbf{r}}}}\bigg)=\bigg[
\frac{d}{dt}m_b\dot{\vec{\xi}} ~~~ \frac{d}{dt}J_b\dot{\vec{H}}\bigg]^T~~~~\in\mathcal{F}^{a}
\end{equation}
\end{subequations}
where $\vec{G}_a$ in the above is the gravitation force transformed to the common frame $\mathcal{F}^a$ which $\mathcal{L}\big(\vec{\mathbf{r}},\dot{\vec{\mathbf{r}}}\hspace{1pt}\big)$ is defined with respect to. The body mass $m_b$ and inertia $J_b$ could potentially have some non-zero time derivative, but for now are regarded as constants. Time varying inertias are later defined in Sec:\ref{sec:proto.inertia} and introduced to the dynamics subsequently in Sec:\ref{subsec:dynamics.nonlinearities.gyrotorques}. Here only the general rigid body case is considered. Any vector in some non-Newtonian rotating reference frame $\mathcal{F}^{a}$ has a time derivative, relative to another frame $\mathcal{F}^{b}$ with an angular velocity $\vec{\omega}_{a/b}$, as per Rotating Reference Frame or Reynolds Transportation Theorems\cite{reynolds}:
\begin{equation}\label{eq:reynolds}
\frac{d\vec{f}_b}{dt_a}=\frac{d\vec{f}_b}{dt_b}+\vec{\omega}_{a/b}\times\vec{f}_b~~~~\in\mathcal{F}^b
\end{equation}
Applying Eq:\ref{eq:reynolds} to those partial derivatives in Eq:\ref{eq:partial.b} and further defining the generalized coordinates $\big[\vec{\xi},~\vec{H}\hspace{1pt}\big]^T$ as 6-DOF Cartesian body coordinates with respect to the inertial frame $\mathcal{F}^I$ or the body frame $\mathcal{F}^b$ described in Sec:\ref{sec:proto.conventions}.
\par
The angular orientations $\vec{H}$ are with respect to a \emph{common frame} $\mathcal{F}^{a}$, unlike Euler angles $\vec{\eta}\in\mathcal{F}^{v2,v1,I}$. Recalling the definition of an attitude in a shared frame $\vec{\eta}_b$ from Eq:\ref{eq:angular-rates.b}, where $\vec{\omega}_b\equiv\dot{\vec{\eta}}_b$ and $\vec{\eta}_b\in\mathcal{F}^{b}$, the trajectory's definition for $\vec{\mathbf{r}}$ is refined:
\begin{subequations}\label{eq:path-def}
\begin{equation}\label{eq:path-def.a}
\vec{\mathbf{r}}(t)=\begin{bmatrix}
\vec{\xi}&
\vec{H}
\end{bmatrix}^T
\triangleq
\begin{bmatrix}
\vec{\mathcal{E}_b}\\
\vec{\eta}_{b}
\end{bmatrix}~~~~\in\mathcal{F}^b
\end{equation}
Note that the position $\vec{\mathcal{E}}_b$ in Eq:\ref{eq:path-def.a} is the position in the \emph{body frame}, unlike $\vec{\mathcal{E}}_I\in\mathcal{F}^{I}$ from Eq:\ref{eq:position-frame}. The path rate $\dot{\vec{\mathbf{r}}}(t)$ is the defined as:
\begin{equation}
\dot{\vec{\mathbf{r}}}(t)=
\begin{bmatrix}
\dot{\vec{\xi}} & \dot{\vec{H}}
\end{bmatrix}^T
\triangleq
\frac{d}{dt}
\begin{bmatrix}
{\vec{\mathcal{E}}}_b\\
\vec{\eta}_b
\end{bmatrix}
\equiv
\begin{bmatrix}
\vec{v}_b\\
\vec{\omega}_b
\end{bmatrix}~~~~
\in \mathcal{F}^b
\end{equation}
\end{subequations}
Substituting those changed path coordinates from Eq:\ref{eq:path-def} into the Lagrangian Eq:\ref{eq:lagrangian.b} yields a familiar Lagrangian scalar for a vehicle's energies for $\mathcal{F}^b$ relative to $\mathcal{F}^I$:
\begin{equation}\label{eq:3.7a}
\mathcal{L}=\frac{1}{2}\vec{v}_b^{~T}(m_b)\vec{v}_b + \frac{1}{2}\vec{\omega}_b^{~T}(J_b)\vec{\omega}_b
-m_b\vec{G}_b z_I
\end{equation}
where $\vec{G}_b$ is the gravitational force vector from Eq:\ref{eq:grav} transformed to $\mathcal{F}^b$ and $z_I$ is the vertical height of the vehicle \emph{in the inertial frame}. The time derivative of the substituted path coordinates in the partial derivative Eq:\ref{eq:partial.b} is then:
\begin{subequations}
\begin{equation}
\frac{d}{dt}\bigg(\frac{\partial \mathcal{L}}{\partial \dot{\vec{\mathbf{r}}}}\bigg)=\bigg[
m_b\frac{d}{dt}\vec{v}_b ~~~ J_b\frac{d}{dt}\vec{\omega}_b\bigg]^T
\end{equation}
With respective time derivatives of body frame vectors relative to the inertial frame, using the Reynolds transportation theorem:
\begin{equation}
m_b\frac{d}{dt}\vec{v}_b=m_b\dot{\vec{v}}_b+\vec{\omega}_{b/I}\times m_b\vec{v}_b~~~~\in\mathcal{F}^b
\end{equation}
\vspace{-8pt}
\begin{equation}
J_b \frac{d}{dt}\vec{\omega}_b=J_b\dot{\vec{\omega}}_b+\vec{\omega}_{b/I}\times J_b\vec{\omega}_b~~~~\in\mathcal{F}^b
\end{equation}
\end{subequations}
which, when substituted back into the Euler-Lagrange formulation in Eq:\ref{eq:euler-lagrange}, yields familiar Newton-Euler rigid body differential equations of translational and rotational motion for generalized net force and torque inputs; $\vec{F}_\mu$ and $\vec{\tau}_\mu$ respectively.
\begin{subequations}\label{eq:newton}
\begin{equation}
\begin{bmatrix}
m_b\dot{\vec{v}}_b+\vec{\omega}_{b/I}\times m_b\vec{v}_b\\
J_b\dot{\vec{\omega}}_b+\vec{\omega}_{b/I}\times J_b\vec{\omega}_b
\end{bmatrix}
-
\begin{bmatrix}
m_b\vec{G}_b\\
0
\end{bmatrix}
=
\vec{\mathbf{R}}
=
\begin{bmatrix}
\vec{F}_{\mu}\\
\vec{\tau}_{\mu}
\end{bmatrix}~~~~\in\mathcal{F}^{b}
\end{equation}
\vspace{-10pt}
\begin{equation}\label{eq:newton.a}
\therefore\vec{F}_{\mu}=m_b\dot{\vec{v}}_b+\vec{\omega}_b\times m_b \vec{v}_b - m_bR_I^b(-\eta) \vec{G}_I
\end{equation}
\vspace{-12pt}
\begin{equation}\label{eq:newton.b}
\therefore\vec{\tau}_{\mu}=J_b\dot{\vec{\omega}}_b+\vec{\omega}_b\times J_b\vec{\omega}_b
\end{equation}
\end{subequations}
It is important to stress that $\vec{\eta}_b\not=\vec{\eta}$~~because each Euler Angle is defined in sequentially rotated reference frames $\in\mathcal{F}^{v_2,v_1,I}$. Four separate equations are then needed to completely describe a body's position and attitude states:
\begin{subequations}\label{eq:states}
\begin{equation}\label{eq:states.a}
\dot{\vec{\mathcal{E}}}_I=R_b^I(-\eta)\vec{v}_b~~~~\in\mathcal{F}^I
\end{equation}
\vspace{-10pt}
\begin{equation}\label{eq:states.b}
\vec{F}_{\mu}=m_b\dot{\vec{v}}_b+\vec{\omega}_b\times m_b\vec{v}_b -m_b \vec{G}_b ~~~~\in\mathcal{F}^b
\end{equation}
\vspace{-8pt}
\begin{equation}\label{eq:states.c}
\dot{\vec{\eta}}=\Phi(\eta)\vec{\omega}_b~~~~\in\mathcal{F}^{v2,v1,I}
\end{equation}
\vspace{-8pt}
\begin{equation}\label{eq:states.d}
\vec{\tau}_{\mu}=J_b\dot{\vec{\omega}}_b+\vec{\omega}_b\times J_b\vec{\omega}_b~~~~\in\mathcal{F}^b
\end{equation}
\end{subequations}
where $\Phi(\eta)$ is the Euler matrix which relates Euler rates $\dot{\vec{\eta}}$ and angular velocity $\vec{\omega}_b$, defined previously in Eq:\ref{eq:angular-rates.c}. State differentials from Eq:\ref{eq:states} can be simplified to a pair of equations defined entirely in the reference frames of the variables which they represent. The nonlinear form of those equations substitutes $d\vec{\eta}/dt=\Phi(\eta)\vec{\omega}_b$ into the Lagrangian derivative in Eq:\ref{eq:partial.b}.
\begin{equation}\label{eq:3.11}
\frac{d}{dt}\bigg(\frac{\delta \mathcal{L}}{\delta \dot{\mathbf{r}}}\bigg)=\bigg[m_b\frac{d}{dt}\vec{v}_b~~~J_b\frac{d}{dt}\dot{\vec{\eta}}_b\bigg]^T\Rightarrow\bigg[m_b\frac{d}{dt}\vec{v}_b~~~J_b\frac{d}{dt}\Phi(\eta)\vec{\omega}_b\bigg]^T
\end{equation}
which only affects the angular component because the two kinetic energies are independent of one another. Applying the differential chain rule to the angular component of Eq:\ref{eq:3.11} yields:
\begin{equation}
J_b\frac{d}{dt}\Phi(\eta)\vec{\omega}_b=J_b\big(\dot{\Phi}(\eta)\vec{\omega}_b+\Phi(\eta)\dot{\vec{\omega}}_b \big)
\end{equation}
Drawing from \cite{autonomousrobotseuler} and recognizing that $J_b$ must be transformed to the shared intermediate Euler axes, $J\triangleq\Psi(\eta)^TJ_b\Psi(\eta)$. The state differential for the Euler angle acceleration counterpart of Eq:\ref{eq:newton.b}, defined in intermediate (non-inertial) Euler frames for each respective Euler angle, then becomes:
\begin{subequations}\label{eq:nonlinear}
\begin{equation}\label{eq:nonlinear.a}
M(\eta)\ddot{\vec{\eta}}+C(\eta,\dot{\eta})\dot{\vec{\eta}}=\Psi(\eta)\vec{\tau}_{\mu}~~~~\in\mathcal{F}^{v2,v1,I}
\end{equation}
\vspace{-15pt}
\begin{equation}\label{eq:nonlinear.b}
M(\eta)=\Psi(\eta)^TJ_b\Psi(\eta)
\end{equation}
\vspace{-10pt}
\begin{equation}\label{eq:nonlinear.c}
C(\eta,\dot{\eta})=-\Psi(\eta)J_b\dot{\Psi}(\eta)+\Psi(\eta)^T \big[\Psi(\eta)\dot{\vec{\eta}}\hspace{2pt}\big]_{\times}J_b\Psi(\eta)
\end{equation}
\end{subequations}
The relationship $\dot{\Psi}\equiv\Psi\dot{\Phi}\Psi$ was used to simplify Eq:\ref{eq:nonlinear}, the singularity present in $\Phi$ remains. The equation in Eq:\ref{eq:nonlinear.a} completely describes the state derivative $\ddot{\vec{\eta}}$ in its own reference frame(s) $\in\mathcal{F}^{v2,v1,I}$. The two differential equations which fully describe the entire body's 6-DOF motion are:
\begin{subequations}\label{eq:rigid-frame}
\begin{equation}\label{eq:rigid-frame.a}
\vec{F}_{\mu}=m_b\dot{\vec{\mathcal{E}}}_I+R_b^I(-\eta)\vec{\omega}_b \times m_b \dot{\vec{\mathcal{E}}}_I-m_b\vec{G}_I~~~~\in\mathcal{F}^I
\end{equation}
\vspace{-10pt}
\begin{equation}\label{eq:rigid-frame.b}
\vec{\tau}_{\mu}=\Psi(\eta)^{-1}M(\eta)\ddot{\vec{\eta}}+\Psi(\eta)^{-1}C(\eta,\dot{\eta})~~~~\in\mathcal{F}^{v2,v1,I}
\end{equation}
\end{subequations}
In most cases the body frame counterparts in Eq:\ref{eq:states} are used rather than Eq:\ref{eq:rigid-frame} when describing states. Eq:\ref{eq:rigid-frame} is superfluous when considering that inputs $\vec{F}_\mu$ and $\vec{\tau}_\mu$ both act in the body frame $\mathcal{F}^b$. Irrespective of the differential equation used, some singular transformation will still be performed by either $\Psi(\eta)$ from Eq:\ref{eq:angular-rates.d} or $\Phi(\eta)$ from Eq:\ref{eq:angular-rates.f}.
\par
The generalized input forces and torques $\vec{F}_{\mu}$ and $\vec{\tau}_{\mu}$ respectively are produced by the system's controllable inputs but could include any external disturbances acting on the body. Those control inputs are directly affected by the vehicle's actuators. How actuator action produces the control inputs depends on the actuator's associated \emph{effectiveness} function. In the general case, which is expanded in Sec:\ref{sec:dynamics.aero}, the control inputs for a regular quadrotor (Fig:\ref{fig:net-force}) are as follows:
\begin{figure}[hbtp]
\vspace{-6pt}
\centering
\includegraphics[width=0.8\textwidth]{figs/net-force}
\vspace{-10pt}
\caption{Generalized quadrotor net forces and torques}
\label{fig:net-force}
\vspace{-18pt}
\end{figure}
\par
Typically $\vec{F}_{\mu}$ is the net heave force acting on the center of motion $\vec{\mathbf{O}}_b$. The net heave is the sum of all thrust forces produced by rotating propellers, as some function of those rotational speeds, $\vec{T}(\Omega_i)$.
\begin{subequations}\label{eq:generalized-net-forces}
\begin{equation}
\vec{F}_{\mu} = \sum_{i=1}^4 \vec{T}(\Omega_i)\cdot\hat{Z}_b~~~~\in\mathcal{F}^b
\end{equation}
Similarly net torque $\vec{\tau}_{\mu}$ is the sum of all \emph{differential} torques produced from opposing propeller thrust vectors. Each torque arm $\vec{L}_{[1:4]}$ is the thrust's orthogonal displacement relative to the origin of \emph{motion}.
\begin{equation}
\vec{\tau}_{\mu} = \sum_{i=1}^4 \vec{L}_i \times \vec{T}(\Omega_i)\cdot\hat{Z}_b~~~~\in\mathcal{F}^{b}
\end{equation}
\end{subequations}
In Eq:\ref{eq:generalized-net-forces}, the thrust vector $\vec{T}(\Omega_i)$ is a function of the $i^{th}$ motor's rotational velocity $\Omega_i$, fixed in the $\hat{Z}_b$ direction. Each thrust vector could potentially be $\in\mathbb{R}^3$ such as the redirected vector from Eq:\ref{eq:motor-module-force-redirect}. All of the above equations are still applicable to any 6-DOF body but common simplifications applied to the system for quadrotor control are explored in App:\ref{app:equations.standard}. Aerodynamic components pertinent for thrust and torque generation relative to Eq:\ref{eq:generalized-net-forces} are now introduced and obviously the contextual focus is on quadrotor with dual-tilting axis actuators.
%====================================================
\section{Aerodynamics}
\label{sec:dynamics.aero}
%====================================================
Aerodynamic effects detailed here and subsequent nonlinear multibody responses in Sec:\ref{sec:dynamics.nonlinearities} both affect the generalized forces and torques acting on the body. The relationship between a propeller's rotational speed $\Omega_i$, in revolutions per second or [\emph{RPS}], and its perpendicular thrust vector $\vec{T}(\Omega_i)$ is more complicated than the quadratic simplification taken at static conditions which some papers assume (e.g \cite{x4flyer} etc\ldots). Produced thrust is mostly dependent on the incident air stream flowing through the propeller's rotational plane, typically being the body velocity's component normal to that propeller's plane. Fluid flowing \emph{tangentially} across the propeller's plane contributes toward in-plane aerodynamic drag and hence torque.
\par
The combination of aerodynamic blade-element\cite{bem,forwarddescent} and fluid-dynamics momentum or \emph{disc actuator} theories equate an integral term generated across the propeller's length with the produced thrust or torque. A schedule of all aerodynamic effects encountered by a quadrotor's propellers is thoroughly detailed in both \cite{bladesforquadrotors} and \cite{nonlineardynamics}. The following is a review of pertinent aerodynamic theories. Vortex ring state and parasitic drag effects are omitted as they will be approximately negligible given the aircraft's proposed flight envelope with low translational velocities.
%====================================================
\subsection{Propeller Torque and Thrust}
\label{subsec:dynamics.aero.bem}
%====================================================
\emph{\color{Gray} A possible situation which the prototype could encounter is where an upstream propeller provides the incident fluid flow to another downstream propeller. Such a situation presents a complicated fluid dynamics and vortex wake effect problem. Propeller overlapping effects are discussed in \cite{configurationpropulsion} but remain open to further research in the context of the aircraft considered here.}
\par
To expedite the system identification process some simplifications are made on the aerodynamics to construct an approximate model, specifically using coefficients in place of complete local chord and pitch based integrals. Such an assumption holds true given that twisted, fixed pitch propellers are used (Fig:\ref{fig:fixed-pitch}) and not variable pitch swash-plate actuated propellers (Fig:\ref{fig:variable-pitch}).
\begin{figure}[htbp]
\vspace{-8pt}
\centering
\begin{subfigure}{0.49\textwidth}
\centering
\includegraphics[width=0.8\textwidth]{figs/fixed-pitch}
\vspace{-4pt}
\caption{Twisted, fixed pitch}
\label{fig:fixed-pitch}
\end{subfigure}
\begin{subfigure}{0.49\textwidth}
\centering
\includegraphics[width=0.8\textwidth]{figs/variable-pitch}
\vspace{-4pt}
\caption{Swash-plate variable pitch; \cite{variablepitch}}
\label{fig:variable-pitch}
\end{subfigure}
\vspace{-10pt}
\caption{Propeller types}
\label{fig:props}
\vspace{-18pt}
\end{figure}
\par
A propeller's profile applies a perpendicular scalar thrust force $T$ onto the fluid in which it rotates. To build the following theoretical explanation propellers are first considered in terms of momentum theory and only perpendicular fluid flow through the propeller's plane is regarded. That fluid stream (Fig:\ref{fig:bem-flow}) has an incident upstream velocity $v_\infty$ and a resultant slip velocity $v_s$ downstream relative to the rotational plane. The change of fluid flow as a result of the propeller's rotation can be given as:
\begin{equation}
v_s = \Delta v + v_\infty
\end{equation}
where $\Delta v$ is the net change in fluid velocity caused by the propeller blade's rotating aerofoil profile. The propeller induces a velocity directly in front of its rotational plane $v_i$, such that the net fluid flow into the plane is $v_b=v_i+v_\infty$. That induced inflowing fluid velocity is different to the net velocity contribution of the propeller; $v_i\not=\Delta v$.
\par
\begin{figure}[htbp]
\centering
\includegraphics[width=0.85\textwidth]{figs/bem-flow}
\vspace{-12pt}
\caption{Disc Actuator Propeller Planar Flow}
\label{fig:bem-flow}
\vspace{-15pt}
\end{figure}
It is shown in \cite{bladesforquadrotors} that at static conditions, using Bernoulli's pressure theorem, the net fluid flow through the propeller's plane is:
\begin{equation}\label{eq:bernoulli}
v_b = \frac{1}{2} ( v_s - v_{\infty} ) = \frac{1}{2} \Delta v = \frac{1}{2} v_s \big|_{v_\infty=0}
\end{equation}
Stemming from classical disc actuator, or fluid \emph{momentum} theory \cite{fluidmomentum,propellers}, the scalar force $T(\Omega_i)$ acting on the fluid is calculated as a function of mass flow rate with respect to the change in fluid velocity or \emph{pressure differential}:
\begin{equation}\label{eq:prop-mass}
T=(A_b v_b)\Delta v = \rho \pi R_b^2v_b \Delta v = \rho \pi R_b^2(v_i+v_\infty)\Delta v = \frac{1}{2} \rho \pi R_b^2 \Delta v^2
\end{equation}
where $R_b$ is the disc (propeller) radius in $\text{m}$ for the fluid stream under consideration, $A_b$ is the swept area of that propeller disc. The fluid density of that stream $\rho$ is typically $1.225~[\text{kg.m}^{-3}]$ for air at standard temperature and pressure (\emph{stp}). However, the desired form of thrust generated is as a function of propeller rotational velocity, $T(\Omega_i)$ in $[\text{RPS}]$, so Eq:\ref{eq:prop-mass} is not yet satisfactory.
\par
Eq:\ref{eq:prop-mass} could be solved from the aerodynamic propulsive power expended using $\Delta P=T\Delta v$. The relationship between rotational kinetic energy of a propeller and its transferred propulsive power is difficult to quantify, compound parasitic losses deteriorate the efficiency of the propeller and motor. Furthermore, the fluid velocity through the propeller's plane is not purely normal but is in fact a vector.
\par
Fluid flow induced by the propeller's rotation $v_i$ directly in front of its plane of rotation has both axial and tangential induced components, termed $a$ and $a'$ respectively. Induced fluid velocity components are abstracted to induction factors which are dependent on the incident fluid velocity entering the propeller's plane of rotation:
\begin{subequations}\label{eq:induction-factors}
\vspace{-5pt}
\begin{equation}\label{eq:induction-axial}
v_i=a v_\infty~~\text{in the axial direction}
\end{equation}
\vspace{-16pt}
\begin{equation}\label{eq:induction-tangential}
v_\theta=a' \Omega_i R_b~~\text{in the tangential direction}
\end{equation}
\end{subequations}
Using the induction factors to rewrite the fluid's through velocity $v_b$ and its slip stream velocity $v_\infty$:
\begin{subequations}
\vspace{-5pt}
\begin{equation}
v_b=(1+a)v_\infty
\end{equation}
\vspace{-18pt}
\begin{equation}
v_s=(1+2a)v_\infty
\end{equation}
\end{subequations}
A consequence of the tangential fluid flow is that an angular momentum flow rate exists across the propeller plane. This produces a fluid-momentum torque opposing the rotational motion about the propeller's axis, analogous but perpendicular to Eq:\ref{eq:prop-mass}:
\begin{equation}\label{eq:prop-moment}
H=\rho\pi R_b^3 (v_\theta-v_\infty) v_b
\end{equation}
\par
Together, Eq:\ref{eq:prop-mass} and Eq:\ref{eq:prop-moment} comprise propeller momentum theory but cannot be solved on their own. Blade-element theory analyses incremental aerofoil sections of width $dr$ of the propeller profile at some radius $r$, the sectional view of which is illustrated in Fig:\ref{fig:bem-profile}. Each aerofoil element has a net local fluid velocity $\vec{U}$ across its profile, calculated as:
\begin{equation}
\vec{U}=\sqrt{(v_\infty+v_i)^2+(v_\Omega+v_\theta)^2}
\end{equation}
where each profile has a chord length $c$ and an inclination (or \emph{pitch}) $\theta$ of the aerofoil \emph{zero-lift line} relative to the horizontal. Local fluid velocities incident to the propeller profile (Fig:\ref{fig:bem-profile}) make their own angle of attack $\phi$ such that a true effective angle of attack $\alpha_{eff}$ is encountered:
\begin{equation}
\phi=\theta-\alpha_{eff}
\end{equation}
That local angle of attack varies with the incident fluid flow magnitude $v_\infty$ and the induced axial velocity $v_i$. The trigonometric ratio between the two is given as:
\begin{equation}
\phi=tan^{-1}\bigg(\frac{v_\infty+v_i}{v_\Omega+v_\theta}\bigg)=tan^{-1}\bigg(\frac{v_\infty(1+a)}{\Omega r(1+a')}\bigg)
\end{equation}
\par
\begin{figure}[hbtp]
\vspace{-15pt}
\centering
\includegraphics[width=\textwidth]{figs/bem-profile}
\caption{Blade element profile at radius r}
\label{fig:bem-profile}
\end{figure}
In-plane fluid flow $\vec{U}(r,\phi)$, for an element at radius $r$ with a local angle of attack $\phi$, then contributes towards elemental lift and drag forces as a function of the aerofoil's dimensionless lift, $C_L$, and drag, $C_D$, coefficients. Those coefficients are determined by the aerofoil's characteristics, but would be constant across the length of a variable pitch, hinged and untwisted flat propeller (Fig:\ref{fig:variable-pitch}).
\begin{subequations}\label{eq:3.25a}
\begin{equation}
\Delta L=\frac{1}{2}\rho \vec{U}(r,\phi)^2 c C_L
\end{equation}
\vspace{-10pt}
\begin{equation}
\Delta D=\frac{1}{2}\rho \vec{U}(r,\phi)^2 c C_D
\end{equation}
\end{subequations}
Where air density $\rho$ in Eq:\ref{eq:3.25a} is taken at \emph{stp}. Lift and drag forces, when taken parallel and perpendicular to the plane of rotation, are thrust $T$ and torque $F_H$ forces (Fig:\ref{fig:bem-profile}). The in-plane force applies an aerodynamic torque $H$ at the propeller's hub because the force $F_H$ acts at a radius $r$, \cite{starmac}.
\begin{subequations}
\begin{equation}\label{eq:element-thrust}
dT=\frac{1}{2}\rho\vec{U}(r,\phi)^2c\big(C_L cos(\theta)+C_D sin(\theta)\big).dr
\end{equation}
\vspace{-5pt}
\begin{equation}\label{eq:element-drag}
dF_H=\frac{1}{2}\rho\vec{U}(r,\phi)^2c\big(C_L sin(\theta)+C_D cos(\theta)\big).dr
\end{equation}
\vspace{-5pt}
\begin{equation}\label{eq:element-torque}
\therefore dH = \frac{1}{2}\rho\vec{U}(r,\phi)^2c\big(C_L sin(\theta)+C_D cos(\theta)\big)r.dr
\end{equation}
\vspace{-10pt}
\begin{equation}\label{eq:element-power}
\therefore dP = \Omega r dF_H .dr
\end{equation}
\end{subequations}
\par
Rotational power expended is a product of angular velocity and the opposing in-plane torque, Eq:\ref{eq:element-power}. Power is mostly used instead of torque or drag terms in Eq:\ref{eq:element-torque} or Eq:\ref{eq:element-drag} respectively. Calculating forces and power terms as per momentum theory for each element, in terms of axial and tangential induction factors:
\begin{subequations}\label{eq:moment-thrust-element}
\begin{equation}
dT=\rho 4 \pi r^2 v_\infty(1+a)a.dr
\end{equation}
\vspace{-16pt}
\begin{equation}
dP=\rho 4 \pi r^2 v_\infty(1+a)\Omega r (1+a').dr
\end{equation}
\end{subequations}
Equating momentum and element terms produces the blade-element momentum equation(s) for aerodynamic thrust and power from a propeller. Following a few assumptions, most importantly that the lift coefficient $C_L$ is a linear function of the effective angle of attack $\alpha_{eff}$ is typically characterized as:
\begin{equation}
C_L=a_L(\theta-\phi)
\end{equation}
Firstly the lift coefficient curve gradient $a_L$ is shown in \cite{aerodynamicsforengineering} for an ideally twisted blade, like the fixed pitch propellers under consideration, to be $2\pi$. An ideal lift coefficient is then a function:
\begin{equation}\label{eq:lift-curve-gradient}
C_L=2\pi(\theta-\phi)
\end{equation}
Secondly, assuming tangentially induced velocities $v_\theta$ are small when compared to the propeller's translational speed at radius $r$, $v(\Omega_i)=\Omega_i r$. The tangential induction factor $a'$ is then the ratio:
\begin{equation}
a'=\frac{v_\theta}{\Omega_i r}<<1
\end{equation}
Small angle approximations then apply to Eq:\ref{eq:element-thrust}-\ref{eq:element-torque}; $cos(\phi+\alpha_{eff})\approx 1$ and $sin(\phi+\alpha_{eff})\approx \phi+\alpha_{eff}$. Similarly net inflow and axial velocities are $(v_\infty + v_i)<<\Omega_i r$, the following integrals are then found:
\begin{subequations}
\begin{equation}\label{eq:bem-thrust}
T(\Omega_i)=\int_{r=0}^R \frac{1}{2} a_L b c \rho (\Omega_i r)^2 \bigg[\theta-\frac{v_\infty+v_i}{\Omega_i r}\bigg].dr
\end{equation}
\begin{equation}\label{eq:bem-power}
P(\Omega_i)=\int_{r=0}^R \frac{1}{2}a_L b c \rho (\Omega_i r)^3\bigg[\big(\theta-\frac{v_\infty+v_i}{\Omega_i r}\big)\big(\frac{v_\infty+v_i}{\Omega_i r}\big) + C_d\bigg].dr
\end{equation}
\end{subequations}
where $b$ is the number of blades the propeller has. In practice, knowing exact pitch and chord values as a function of $r/R$ is difficult and calculating integrals at each process step is cumbersome. Both Eq:\ref{eq:bem-thrust} and Eq:\ref{eq:bem-power} can be solved by equating element and momentum terms, a full solution of which is given in App:\ref{app:equations.bem}. Often dimensionless thrust and power coefficients are defined across the entire blade's length:
\begin{subequations}\label{eq:coefficients}
\begin{equation}\label{eq:thrust-coefficient}
C_T(J)\triangleq\frac{T}{\rho \Omega_i\hspace{-2pt}^2 D^4}
\end{equation}
\vspace{-6pt}
\begin{equation}\label{eq:power-coefficient}
C_P(J)\triangleq\frac{P}{\rho \Omega_i\hspace{-2pt}^3 D^5}
\end{equation}
\end{subequations}
where the propeller's diameter is $D$ in $[\text{m}]$, then $\Omega_i$ is the propeller's rotational speed in \emph{revolutions per second} [\emph{RPS}] and different from other inertial equations like Eq:\ref{eq:angular-rot}, with units $[\text{rad.s}^{-1}]$. For fixed pitch propellers the thrust and power coefficients are easily determined and remain consistent. Both Eq:\ref{eq:thrust-coefficient} and Eq:\ref{eq:power-coefficient} vary as a function of the dimensionless \emph{advance ratio} $J$.
\begin{equation}\label{eq:advance}
J\triangleq\frac{v_\infty}{\Omega_i R}
\end{equation}
Typically the net upstream velocity $v_\infty$ in Eq:\ref{eq:advance} is simply the perpendicular component (projected onto the plane's normal vector $\hat{n}$, shown later in Eq:\ref{eq:normal-fluid}) of the vehicle's translational velocity in the body frame; $\vec{v}_b\perp\hat{n}$. For the case of a zero advance ratio $J=0$, the conditions are regarded as static. Static thrust and power coefficients are nominal in their values.
\par
Propeller databases like \cite{UIUC} provide comprehensive coefficient values for a range of small and medium diameter propeller types at different advance ratios. Included in the database are blade profiles, pitch angles and chord lengths. All the results are outcomes of the investigation \cite{lowreynolds}.
\par
The introduction of those coefficients drastically reduces thrust estimation complexity. For a typical $6\times 4.5$ inch propeller the following coefficients were linearly interpolated from similar pitched database results in \cite{UIUC} to match subsequent physical test values. Static thrust and power coefficients determined from tests subsequently in Fig:\ref{fig:thrust-plot} and Fig:\ref{fig:torque-plot} are respectively:
\begin{subequations}
\begin{equation}
{\color{blue}C_{T0}}=0.191
\end{equation}
\vspace{-15pt}
\begin{equation}
{\color{red}C_{P0}}=0.0877
\end{equation}
\end{subequations}
Fig:\ref{fig:coeffs-plot} plots interpolated coefficients for thrust {$\color{Blue}C_{T}$} and power {$\color{Red}C_{P}$} as a function of the advance ratio $J$. As the incident upstream fluid velocity $v_\infty$ increases, the thrust coefficient decreases. So too does the power coefficient and hence the aerodynamic torque. The thrust and power coefficients can be assumed constant for low advance ratios, or in the case considered here, translational velocities.
\begin{figure}[htpb]
\centering
\includegraphics[width=0.74\textwidth]{graphs/coeffs-plot}
\vspace{-4pt}
\caption{Thrust and power coefficients}
\vspace{-18pt}
\label{fig:coeffs-plot}
\end{figure}
\par
Static thrust and torque tests were respectively performed on test rigs in Fig:\ref{fig:thrust-rig} and Fig:\ref{fig:torque-rig}. Measured values for each test are plotted; {\color{Red}$T(\Omega)$} in Fig:\ref{fig:thrust-plot} for thrust and {\color{Red}$H(\Omega)$} in Fig:\ref{fig:torque-plot} for torque. The physically tested values are fitted with quadratic trend-lines and plotted against static coefficient estimates using Eq:\ref{eq:thrust-coefficient} for thrust {\color{LimeGreen}$\hat{T}C_t(\Omega)$} and Eq:\ref{eq:power-coefficient} for calculated torque {\color{LimeGreen}$\hat{H}C_p(\Omega)$}. Results from Fig:\ref{fig:coeffs-plot} are used as a lookup table and values from Eq:\ref{eq:coefficients} are calculated. Induced propeller thrust and torques can be accurately modelled quadratically, power is cubic with respect to rotational velocity.
\begin{figure}[htbp]
\centering
\begin{subfigure}{0.49\textwidth}
\centering
\includegraphics[width=\textwidth]{figs/thrust-rig}
\vspace{-10pt}
\caption{Propeller thrust test rig}
\label{fig:thrust-rig}
\end{subfigure}
\begin{subfigure}{0.49\textwidth}
\centering
\includegraphics[width=\textwidth]{graphs/thrust-plot}
\vspace{-10pt}
\caption{Static lift force results}
\label{fig:thrust-plot}
\end{subfigure}
\vspace{-4pt}
\caption{Propeller thrust tests}
\label{fig:thrusts}
\end{figure}
\par
\begin{figure}[hbtp]
\begin{subfigure}{0.5\textwidth}
\centering
\includegraphics[width=\textwidth]{figs/torque-rig}
\vspace{-10pt}
\caption{Aerodynamic drag torque test rig}
\label{fig:torque-rig}
\end{subfigure}
\begin{subfigure}{0.5\textwidth}
\centering
\includegraphics[width=\textwidth]{graphs/torque-plot}
\vspace{-10pt}
\caption{Torque plot}
\label{fig:torque-plot}
\end{subfigure}
\vspace{-4pt}
\caption{Static induced torque results}
\label{fig:torques}
\vspace{-16pt}
\end{figure}
\par
Advance ratios, Eq:\ref{eq:advance}, or rather the propeller incident fluid flows are dependent on the vehicle's net translational and angular velocity. The fluid velocity's normal component to the propeller plane is given by:
\begin{equation}\label{eq:normal-fluid}
v_\infty = (\vec{v}_b\hspace{-1pt}' + \vec{L}_{arm}\times \vec{\omega}_b\hspace{-1pt}')\cdot \hat{n}(\lambda_i,\alpha_i)~~~~\in\mathcal{F}^{M_i}
\end{equation}
where $\vec{v}_b\hspace{-1pt}'$ in $[\text{m.s}^{-1}]$ is the body's translational velocity and $\vec{\omega}_b\hspace{-1pt}'$ in $[\text{rad.s}^{-1}]$ is the body's angular velocity, both transformed to the propeller's frame, $\in\mathcal{F}^{M_i}$. Furthermore $\hat{n}(\lambda_i,\alpha_i)$ is the unit vector normal to the propeller's rotational plane, relative to the body velocity. Then $J$ is calculated from Eq:\ref{eq:advance}.
\par
{\color{Gray}\emph{It is worth reiterating that the above static coefficients are indeed calculated from physical static tests, but advance ratio coefficient dependencies are linearly interpolated from the closest available matching data (APC Thin-Electric 8X6 propellers) cited from \cite{UIUC}}.}
\par
Clockwise and anti-clockwise propellers and rotations were used for both thrust and torque tests. Despite both test rigs (Fig:\ref{fig:thrust-rig} and Fig:\ref{fig:torque-rig} respectively) having been designed to specifically isolate each response, results from opposing directional tests were averaged in the hopes that stray opposing effects would cancel each other out. Both clockwise and anti-clockwise rotational testing results for thrust and torque measurements are included in App:\ref{app:thrust-torque}
\par
{\color{Gray}\emph{Discrepancies which exist between the model or coefficient values derived can be accounted for with lumped uncertainty disturbance terms. Model uncertainty compensation can easily be incorporated into adaptive backstepping or $H_\infty$ control algorithms. The deviation of the modelled thrust or torques from their true values would be simple to incorporate into a plant dependent Lyapunov candidate function; Sec:\ref{subsubsec:control.attitude.nonlinear.adaptivebackstep}.}}
%====================================================
\subsection{Hinged Propeller Conning and Flapping}
\label{subsec:dynamics.aero.flap}
%====================================================
Aerodynamics which adversely affect a propeller's performance have all been well documented in their own right, mostly in the context of helicopter aerodynamic and propeller fields\cite{basichelicopter,bramwell}. Typically such effects are more pronounced when observing hinged variable pitch propellers (Fig:\ref{fig:variable-pitch}), fixed pitch propellers with small radii have a diminished effect. Moreover, low translational velocities suppress such responses but they're worth mentioning.
\par
Conning and flapping are the two most significant aerodynamic effects encountered by a propeller. Other phenomena like cyclic vortex ring states are deemed to be inapplicable here and fall outside the scope of the investigation.
\par
In translational flight, for a propeller without shrouding or a ducting, each blade encounters varying incident fluid flow throughout its cycle. The advancing blade relative to the body's translational direction encounters a greater fluid flow than the retreating blade, constructive and destructive interference from the body's translational velocity adds to local fluid flows. The effective local angles of attack, sectional view in Fig:\ref{fig:bem-profile}, for advancing and retreating propeller blades are then asymmetrical. Unbalanced angles of attack produce a dissymmetry of lift across the propeller blade's surface.
\par
\begin{figure}[htbp]
\centering
\includegraphics[width=\textwidth]{figs/prop-flap}
\caption{Propeller blade flapping; \cite{starmac}}
\label{fig:prop-flap}
\end{figure}
\par
Throughout each rotation the blade is forced up and down as it cycles through a varying fluid velocity field, applying a torque moment about the propeller's hub. That torque's magnitude is a function of the body's net translational velocity and the propeller material's stiffness and hence its susceptibility to deflection. The flapping pitches the effective propeller plane or \emph{tip-path plane}, and hence the thrust vector line, away from its principle axis; shown in Fig:\ref{fig:prop-flap}.
\par
The propeller's resultant thrust vector is pitched away from its perpendicular normal by some deflection angle, $\alpha_{1s}$ in Fig:\ref{fig:prop-flap}, toward the direction of translational movement or wind disturbance. Propeller flapping is diminished at low translational velocities with small wind disturbances relative to propeller rotational speed. As such flapping is not applicable to the feasible flight envelope envisaged for the prototype here.
\par
\begin{figure}[hbtp]
\centering
\includegraphics[width=0.95\textwidth]{figs/prop-coning}
\caption{Propeller coning}
\label{fig:prop-coning}
\end{figure}
Coning is another form of propeller deflection, illustrated in Fig:\ref{fig:prop-coning}, which again is dependent on the blade material's stiffness. Coning causes both advancing and retreating propeller blades to both deflect upward. Distributed loading on the propeller surface from supporting a body's weight causes the upward deflection. The coning reduces the effective propeller disc's radius, adversely affecting thrust produced, Eq:\ref{eq:bem-thrust}. Increased loading accentuates the coning angle experienced by the propellers and as such reduces the tip-path plane.
\par
Both aerodynamic propeller deflections can be quantified numerically. Their derivation and resultant equations are cumbersome. In practice, both effects on the produced prototype are not significant enough to affect the derived plant model. The frame could potentially be affected in more adverse ways given certain flight conditions with higher translational velocities or incident wind and fluid flow disturbances.
%====================================================
\subsection{Drag}
\label{subsec:dynamics.aero.drag}
%====================================================
For any solid body with some non-zero relative translational velocity through a fluid, that fluid has a second-order damping response opposing the body's movement. Net drag $\vec{D}_{net}$ is locally dependent on individual component cross-sections. For a vehicle's velocity $\vec{v}_b=[u~v~w]^T$ in $\mathcal{F}^b$, the drag force is:
\begin{equation}\label{eq:distrubance}
\vec{D}_{net}(\vec{v}_b)=\begin{bmatrix}
D_{ii} & D_{ij} & D_{ik}\\
D_{ji} & D_{jj} & D_{jk}\\
D_{ki} & D_{kj} & C_{kk}
\end{bmatrix}
\begin{bmatrix}
u\\
v\\
w
\end{bmatrix}^2
~~~~\in\mathcal{F}^b
\end{equation}
Each drag coefficient's subscript $\hat{\imath},\hat{\jmath}$ and $\hat{k}$ is dependent on the body's directional cross-section area for each $\hat{X}_b,\hat{Y}_b,\hat{Z}_b$ axis respectively. Given a well designed and symmetrical frame, it can be assumed the off-diagonal elements are of little or no consequence and as such the drag equation can be simplified to the diagonal:
\begin{equation}
\vec{D}_{net}(\vec{v}_b)\approx diag\big(D_{ii},~D_{jj},~D_{kk}\big)\vec{v}_b^{\hspace{2pt}2}~~~~\in\mathcal{F}^b
\end{equation}
Due to the second-order degree of translational velocity on the drag force, such terms can be relegated to a lumped disturbance term which is compensated for in the control loop, Sec:\ref{subsubsec:control.attitude.nonlinear.adaptivebackstep}. The time scale separation between velocity and wind drag effects within the control loop accommodates such an assumption. Analogous rotational drag-like effects opposing angular rates exist but, for the intents and purposes of most practical flight envelopes, can be disregarded.
\par
In simulation if the plant has sufficient disturbance rejection then the drag term in Eq:\ref{eq:distrubance} would be easily accounted for in an adaptive backstepping algorithm. Drag, much like wind turbulence, is shown later in Sec:\ref{sec:simulation.disturbance} to be not consequentially destabilizing. Furthermore, it is possible to physically test for the drag coefficients to attain a higher-certainty model but, given the flight conditions proposed for this research, such effects will be small, if not negligible. As such those tests are outside the scope of investigation here.
%====================================================
\section{Quaternion Attitude}
%====================================================
\subsection{Rotation Matrix Singularity}\label{subsec:dynamics.rigidbody.singularity}
%====================================================
The singularity inherent to Euler angle parametrization is often mentioned but far less common is the mathematical demonstration of how that singularity manifests itself. In general, a singularity occurs for some matrix $A$ in $\vec{y}=A\vec{x}$ when the matrix has a zero determinant, losing rank and hence differentiability of $\vec{y}$ in terms of $\vec{x}$. The combined rotation matrix from the inertial frame $\mathcal{F}^{I}$ to the body frame $\mathcal{F}^{b}$ is the singular component of an Euler parametrized sequence.
\par
Consider the case of a rotational 3-axis gimbal system, illustrated in Fig:\ref{fig:gimbal}, which mimics the sequential nature of the Euler set. When the intermediary sequenced rotational angle is at $\pi/2$ rad, the remaining two axes become co-linear, Fig:\ref{fig:gimbal-lock}. In a ZYX rotation sequence, as adopted in this work, the singularity occurs from the pitching angle $\theta$ about the $\hat{Y}$ axis. Both the roll $\phi$ and yaw $\psi$ rotations will subsequently have the same rotational effect. Such a situation results in a loss of a degree of freedom.
\newpage
\begin{figure}[htbp]
\begin{subfigure}{0.5\textwidth}
\centering
\includegraphics[clip, trim=16.5cm 8.5cm 16.5cm 6.7cm, width=0.93\textwidth]{pdfpages/gimbal}
\caption{3-Axis gimbal}
\label{fig:gimbal}
\end{subfigure}
\begin{subfigure}{0.5\textwidth}
\centering
\includegraphics[clip, trim=16.5cm 8.5cm 16.5cm 6.7cm, width=0.93\textwidth]{pdfpages/gimbal-lock}
\caption{Locked gimbal with loss of DOF}
\label{fig:gimbal-lock}
\end{subfigure}
\caption{Mechanical gimbal lock}
\vspace{-10pt}
\end{figure}
\par
What is clear physically is not necessarily as obvious mathematically. A loss of rank occurs in the Euler matrix $\Psi(\eta)$, defined previously in Eq:\ref{eq:angular-rates.e} from Sec:\ref{subsec:proto.conventions.frames}. That relation between angular velocity, in the inertial frame or inversely in the body frame, and the angular rates of the Euler Angles has a determinant:
\begin{equation}\label{eq:euler-derivative}
\begin{bmatrix}
\dot{\phi}\\
\dot{\theta}\\
\dot{\psi}
\end{bmatrix}
=\begin{bmatrix}
1 & sin(\phi)tan(\theta) & cos(\phi)tan(\theta)\\
0 & cos(\phi) & -sin(\phi)\\
0 & sin(\phi)sec(\theta) & cos(\phi)sec(\theta)\\
\end{bmatrix}
\begin{bmatrix}
p\\
q\\
r
\end{bmatrix}
=\Phi(\eta)\omega_b~~~~\in\mathcal{F}^{v_1,v_2,I}
\end{equation}
\vspace{-2pt}
\begin{equation}
det\big(\Phi(\eta)\big)=cos(\phi)\big(cos(\phi)sec(\theta)\big)+sin(\phi)\big(\sin(\phi)sec(\theta)\big)=sec(\theta)
\end{equation}
\vspace{-6pt}
\begin{equation}
\therefore \underset{{\theta \rightarrow \pi /2}}{lim}|\Phi(\eta)|=sec(\theta)\rightarrow \infty
\end{equation}
The Euler matrix $\Phi(\eta)$ loses rank as $\theta\rightarrow\pi/2$ rad, losing differentiability as well. The physical consequence of this is the loss of a degree of freedom. More specifically, considering how the ZYX rotation or transformation matrices are formulated, from Eq:\ref{eq:rotationmatrix}:
\begin{subequations}
\begin{equation}
R_I^b(\eta)\triangleq R_z(\psi)R_y(\theta)R_x(\phi)=\begin{bmatrix}
c_\psi & -s_\psi & 0\\
s_\psi & c_\psi & 0\\
0 & 0 & 1
\end{bmatrix}
\begin{bmatrix}
c_\theta & 0 & s_\theta\\
0 & 1 & 0\\
-s_\theta & 0 & c_\theta
\end{bmatrix}
\begin{bmatrix}
1 & 0 & 0\\
0 & c_\phi & -s_\phi\\
0 & s_\phi & c_\phi
\end{bmatrix}
\end{equation}
\vspace{-3pt}
\begin{equation}
\therefore R_I^b(\eta)=\begin{bmatrix}
c_\psi c_\theta & c_\psi s_\theta s_\phi - s_\psi c_\phi & c_\psi s_\theta c_\phi + s_\psi s_\phi\\
s_\psi c_\theta & s_\psi s_\theta s_\phi + c_\psi c_\phi & s_\psi s_\theta c_\phi - c_\psi s_\phi\\
-s_\theta & c_\theta s_\phi & c_\phi c_\theta\\
\end{bmatrix}
\end{equation}
In the case where $\theta=\pi/2$ rad, and using trigonometric double angles, the following can be reduced:
\begin{equation}
R_I^b(\eta)=\begin{bmatrix}
0 & c_\psi s_\phi - s_\psi c_\phi & c_\psi c_\phi + s_\psi s_\phi\\
0 & s_\psi s_\phi + c_\psi c_\phi & s_\psi c_\phi - c_\psi s_\phi\\
-1 & 0 & 0\\
\end{bmatrix}\Bigg|_{\theta=\pi/2}
\end{equation}
\vspace{-3pt}
\begin{equation}
=
\begin{bmatrix}
0 & s(\phi - \psi) & c(\phi - \psi)\\
0 & c(\phi - \psi) & s(\phi - \psi)\\
-1 & 0 & 0
\end{bmatrix}
\end{equation}
\vspace{-4pt}
\begin{equation}\label{eq:gimbal}
\therefore R_I^b(\eta)\big|_{\theta=\pi/2}\equiv R_{x'}(\phi-\psi)
\end{equation}
\end{subequations}
where the resultant in Eq:\ref{eq:gimbal} represents an $\hat{X}'$-axis rotation in a new intermediate frame, following a $\pi/2$ rotation about the $\hat{Y}$-axis. Through trigonometric double angles, a degree of freedom is lost at $\theta=\pi/2$ when both $\phi$ and $\psi$ affect the same angle.
%====================================================
\subsection{Quaternion Dynamics}
\label{subsec:dynamics.rigidbody.quaternion}
%====================================================
An algorithm proposed in \cite{euleranglesingularity} suggested a solution to avoid Euler Angle singularities. The heuristic proposed involved switching between sequence conventions (ZYX,ZYZ etc\ldots there are 12 in total) such that the singularity is always avoided. However the implementation of such an algorithm is cumbersome and compulationally exhaustive. Far more elegant is the use of \emph{quaternion} attitude representations in $\mathbb{R}^4$, used in \cite{rotationsequences,spacecraftattitutdequaternions} amongst other, but most notably made popular by \cite{shoemake} for use in animation.
\par
A quaternion is analogous to a rotation matrix in that it represents an attitude difference between two reference frames. An $\mathbb{R}^3$ attitude is paramterized as one rotation $\theta$ about a single unit \emph{Euler} axis $\hat{u}$, demonstrated using the Rodriguez Formula in \cite{unwinding}. In brief, a quaternion consists of a scalar component $q_0$ and complex vector component $\vec{q}\in \mathbb{C}^3$ such that:
\begin{equation}
Q\triangleq
\begin{bmatrix}
q_0 \\
\vec{q}
\end{bmatrix}
~~\in\mathbb{R}^4
\end{equation}
The relationship between an Euler angle rotation matrix $R_I^b(\eta)$ and a quaternion attitude $Q_b$ is given by the Rodriguez formula:
\begin{equation}\label{eq:rodriguez}
R_I^b(\eta)\equiv R(Q_b)\triangleq \mathbb{I}_{3\times 3}+2q_0[\vec{q}\hspace{2pt}]_\times+2[\vec{q}\hspace{2pt}]_\times\text{}^2
\end{equation}
where $[.]_\times$ is the cross-product matrix, defined previously in Eq:\ref{eq:cross-product-matrix}, and $
\mathbb{I}_{3\times 3}$ is an identity matrix as per convention. All quaternions, unless otherwise specified, are unit quaternions $Q\in\mathbb{Q}_u$. Quaternions with a unity magnitude ensure that rotational operations maintain the vector operand's magnitude. A unit quaternion is defined as follows:
\begin{equation}
\norm{Q}\triangleq\sqrt{{q_0}^2+\vec{q}\text{}\hspace{2pt}^2}=1
\end{equation}
Quaternion multiplication is distributive and associative, but not commutative. Specifically a quaternion multiplication operator is equivalent to the Hamilton product. For two quaternions, $Q$ and $P$:
\begin{subequations}
\begin{equation}
Q\otimes P = \begin{bmatrix}
q_0 \\
\vec{q}
\end{bmatrix}
\otimes
\begin{bmatrix}
p_0 \\
\vec{p}
\end{bmatrix}
\end{equation}
\begin{equation}\label{eq:quaternion-operator}
\triangleq\begin{bmatrix}
q_0p_0-\vec{q}\cdot\vec{p}\\
q_0\vec{p}+p_0\vec{q}+\vec{q}\times\vec{p}
\end{bmatrix}
\end{equation}
\begin{equation}\label{eq:quaternion-product}
=\underbrace{q_0 p_0 - \vec{q}\cdot \vec{p}}_{scalar}+\underbrace{p_0 \vec{q} + q_0 \vec{p} + \vec{q}\times\vec{p}}_{vector}
\end{equation}
\end{subequations}
Because the vector component of a quaternion is complex valued, it is natural that a quaternion complex conjugate $Q^*$ exists, defined:
\begin{equation}\label{eq:quaternion-conjugate}
Q^*\triangleq\begin{bmatrix}
q_0 \\
-\vec{q}~
\end{bmatrix}
\end{equation}
It follows that the fundamental quaternion identity is:
\begin{equation}
Q\otimes Q^* = \mathbb{I}_{4\times 4}
\end{equation}
A right handed quaternion rotation applied to some vector $\vec{\nu} \in\mathbb{R}^3$ involves multiplication by two unit quaternions $Q$ and its conjugate $Q^*$.
\begin{equation}
\begin{bmatrix}
0 \\
\vec{\nu}\hspace{2pt}'
\end{bmatrix}
=Q\otimes
\begin{bmatrix}
0 \\
\vec{\nu}
\end{bmatrix}
\otimes Q^*
\end{equation}
Mostly, the zero scalar components are omitted in a rotation (\emph{or transformation}) operation, it is implied that vector operands are substituted with zero scalar quaternions.
\begin{equation}\label{eq:quaternion-rotation}
\vec{\nu}\hspace{2pt}'=Q \otimes (\vec{\nu}\hspace{2pt}) \otimes Q^*
\end{equation}
In the case of rigid body attitude parametrization using quaternions, $Q_b$ is the quaternion which represents the difference between body and inertial frames $\mathcal{F}^b$ and $\mathcal{F}^I$ respectively. A quaternion operator is equivalent to a rotation matrix operation, for some vector $\vec{\nu}_I\in\mathcal{F}^I$:
\begin{equation}
\vec{\nu}_b=R_I^b(\eta)\vec{\nu}_I \underset{Q}{\iff} Q_b \otimes (\vec{\nu}_I) \otimes Q_b^*~~~~\in\mathcal{F}^b
\end{equation}
Since quaternions are non-commutative, the construction of a body quaternion $Q_b$ from an Euler angle set $\vec{\eta}$ is \emph{sequence dependent}. Euler angles, despite being singular, are conceptually simpler for describing a body's orientation. A ZYX sequenced body quaternion $Q_b$ relative to the inertial frame can be constructed from its Euler angle counterparts using:
\begin{equation}\label{eq:quaternion-sequence}
Q_b\triangleq Q_z\otimes Q_y\otimes Qx=\begin{bmatrix}
cos(\psi/2)\\
0\\
0\\
sin(\psi/2)
\end{bmatrix}
\otimes
\begin{bmatrix}
cos(\theta/2)\\
0\\
sin(\theta/2)\\
0
\end{bmatrix}
\otimes
\begin{bmatrix}
cos(\phi/2)\\
sin(\phi/2)\\
0\\
0
\end{bmatrix}
\end{equation}
A quaternion's time derivative, defined in \cite{fullquaternion}, with $Q_\omega$ being a quaternion with a vector component equal to angular velocity $\vec{\omega}_{b/I}$ and a zero scalar component, is:
\begin{subequations}\label{eq:quaternion-deriv}
\begin{equation}
\frac{d}{dt}Q_b\triangleq\frac{1}{2}Q_b\otimes Q_{\omega}=\frac{1}{2}Q_b\otimes\vec{\omega}_b
\end{equation}
\vspace{-12pt}
\begin{equation}
=\begin{bmatrix}
-\frac{1}{2}\vec{q}\hspace{2pt}^{T} \vec{\omega}_b\\
\frac{1}{2}\big([\vec{q}\hspace{2pt}]_\times+q_0\mathbb{I}\big)\vec{\omega}_b
\end{bmatrix}
\end{equation}
\end{subequations}
Using quaternions to represent attitudes negates the need for an Euler Matrix, $\Phi(\eta)$ from Eq:\ref{eq:angular-rates.f}, to represent attitudes and their rates. A body quaternion is fully defined in the inertial frame with respect to the body frame or inversely so. The first quaternion time derivative replaces angular velocity rate differentials in Eq:\ref{eq:states.a} and Eq:\ref{eq:states.c} respectively:
\begin{subequations}
\begin{equation}
\dot{\mathcal{E}}=R_b^I(-\eta)\vec{v}_b~~\in\mathcal{F}^I\underset{Q}{\iff}Q_b(-\eta)\otimes\vec{v}_b\otimes Q_b^*(-\eta)=Q_b^*\otimes \vec{v}_b \otimes Q_b
\end{equation}
\vspace{-14pt}
\begin{equation}
\dot{\eta}\triangleq\Phi(\eta)\vec{\omega}_b~~\in\mathcal{F}^{v2,v1,I}\underset{Q}{\iff}\dot{Q}_b=\frac{1}{2}Q_b\otimes \vec{\omega}_b
\end{equation}
\end{subequations}
Second order time derivatives for quaternion acceleration are not as useful as their higher order, velocity counterparts. The second order derivative is provided here for the sake of completeness. If at all possible, quaternion accelerations are avoided due to their complexity. The quaternion analogue for angular acceleration Eq:\ref{eq:rigid-frame.b}, dependent on net torque acting on a body $\vec{\tau}_{\mu}$, is given by:
\begin{equation}
\ddot{Q}\big(\dot{Q},Q,t)\triangleq\dot{Q}\otimes Q^* \otimes \dot{Q}+\frac{1}{2}Q\otimes \big[J_b^{-1}(\vec{\tau}_{\mu}-4(Q^*\otimes \dot{Q})\times(J_b(Q^*\otimes \dot{Q}))\big]
\end{equation}
An Euler angle attitude error state, used for control input, is defined as the subtracted error between a desired and an existing attitude orientation, $\vec{\eta}_d\in\mathcal{F}^d$ and $\vec{\eta}_b\in\mathcal{F}^b$ respectively, where $\vec{\eta}_d$ is some attitude setpoint produced from a trajectory generator and \emph{both Euler sets are in shared frames}.
\begin{equation}\label{eq:euler-error}
\vec{\eta}_e\triangleq\vec{\eta}_d-\vec{\eta}_b
\end{equation}
Quaternion attitude control and its stability goals are expanded upon subsequently in Sec:\ref{subsec:control.attitude.problem}. In contrast with Eq:\ref{eq:euler-error}, a quaternion attitude error is a multiplicative term defined as the difference between two quaternions $Q_d$ and $Q_b$:
\begin{equation}\label{eq:quaternion-error}
Q_e\triangleq Q_b^*\otimes Q_d
\end{equation}
%====================================================
\subsection{Quaternion Unwinding}
\label{subsec:dynamics.rigidbody.unwinding}
%====================================================
Although quaternions are indeed better than their Euler angle attitude counterparts and lack the associated singularity, they do contain one caveat. Because a quaternion $Q=[q_0~\vec{q}\hspace{2pt}]^T$ represents a body's attitude in $\mathbb{R}^3$ using $\mathbb{R}^4$, there is an infinite coverage of attitude states \cite{unwinding}.
\par
Each unit quaternion, stemming from Euler-Rodriguez theorem, represents a single Euler-axis rotation of $\theta$ about a unit axis $\hat{u}$ such that:
\begin{equation}\label{eq:quaternion-euler-axis}
Q=\begin{bmatrix}
q_0\\
\vec{q}
\end{bmatrix}\triangleq
\begin{bmatrix}
cos(\theta/2)\\
sin(\theta/2)\hat{u}
\end{bmatrix}
\end{equation}
That rotation is applied with a quaternion operator, Eq:\ref{eq:quaternion-rotation}. For every attitude state in 3-D there exist two unique quaternions which correspond to the same orientation, differing by their rotational direction about the Euler-axis. The rotation angle $\theta$ about the Euler-axis $\hat{u}$ is reciprocal in that $\theta=\theta + 2k\pi,~k\in\mathbb{N}$. There are then two definitions for $Q_b$:
\begin{subequations}
\begin{equation}
Q_b =
\begin{bmatrix}
cos(\theta/2)\\
sin(\theta/2)\hat{u}
\end{bmatrix}
\end{equation}
\vspace{-6pt}
\begin{equation}
Q_b=\begin{bmatrix}
cos(\pi - \theta/2)\\
sin(\pi - \theta/2)\hat{u}
\end{bmatrix}
=
\begin{bmatrix}
-cos(\theta/2)\\
sin(\theta/2)\hat{u}
\end{bmatrix}
\end{equation}
\vspace{-4pt}
\begin{equation}\label{eq:euler-quaternion}
\therefore\vec{\eta}\in\mathbb{R}^3\underset{Q}{\iff}\begin{bmatrix}
\pm q_0\\
\vec{q}
\end{bmatrix}
\in\mathbb{R}^4
\end{equation}
\end{subequations}
Eq:\ref{eq:euler-quaternion} asserts that for each attitude in $\mathbb{R}^3$ there are \emph{two} corresponding quaternions in $\mathbb{R}^4$, $[\pm q_0~\vec{q}~]^T$. A consequence of this is that two possible error state trajectories exist for every attitude difference. Both a clockwise $+\theta$ and an anticlockwise $2\pi-\theta$ rotation point to the same quaternion attitude error state. This could lead to an erroneous and unnecessary ``unwinding" of a complete counter revolution. So for attitude controllers, the requirement is that for positive and negative quaternion scalars the control input is consistent. That requirement is:
\begin{equation}
\vec{\tau}_d=h([q_0~\vec{q}\hspace{2pt}]^T,t)\equiv h([-q_0~\vec{q}\hspace{2pt}]^T,t)
\end{equation}
or more simply that $Q_e\triangleq[|q_0|~\vec{q}\hspace{2pt}]^T$. The simplest solution adhering to that constraint, which is often used, is to neglect the quaternion scalar component altogether. Using a reduced error state, only the quaternion error vector as an argument for the control law $h(\vec{q}_e,t)$. Such a solution is an oversimplification and would only ever be locally stable.
\par
An alternative is to use only the absolute quaternion scalar, which ensures the error state represents a right-handed (clockwise) rotation and not necessarily the shortest path. If the resolution of trajectory coordinates generated is sufficiently fine, the control plant will not encounter a problem.
\par
One proposal presented in \cite{nonlinearquadcopter} suggested using a \emph{signum} operator to design the controller coefficient sign for the desired virtual angular velocity, $\vec{\omega}_d$ control plant input.
\begin{subequations}\label{eq:signum-unwinding}
\begin{equation}
\vec{\omega}_d=\frac{2}{\Gamma_1}sgn(q_0)\vec{q}
\end{equation}
with $\Gamma_1$ being a proportional error coefficient and signum defining the operator's sign:
\begin{equation}
sgn(q_0)\triangleq
\begin{cases}\begin{array}{ll}
1 & ~~q_0\geq 0\\
-1 & ~~q_0< 0\\
\end{array}
\end{cases}
\end{equation}
\end{subequations}
Eq:\ref{eq:signum-unwinding} was shown to be asymptotically stable but only locally in the case where the Euler-axis angle is constrained ($\theta\leq \pm\pi$). That control law would still need the control torques to be calculated from that angular velocity $\vec{\omega}_d$ setpoint using Eq:\ref{eq:states.d}.
\par
In \cite{intelligentbackstep}, the authors used a backstepping controller with a trajectory using the absolute quaternion scalar. The resultant was a global asymptotically stable control law which tracked quaternion setpoints for a satellite's attitude. That satellite's stability proof was difficult given the hybrid nature of the resulting equations. Controllers presented in Sec:\ref{subsec:control.attitude.nonlinear} all incorporate \emph{signed} quaternion scalars into the control law, the trajectory generation is assumed to specify the preferred quaternion rotational sense.
\newpage
%====================================================
\section{Multibody Nonlinearities}
\label{sec:dynamics.nonlinearities}
%====================================================
The unique component of the prototype's design which facilitates redirection of a propeller's thrust vector (Eq:\ref{eq:motor-module-force-redirect} and Sec:\ref{subsec:proto.design.actuation}) is also what makes finding the complete equations of motion drastically more complex. The relative (revolute) motion within the multibody system results in torque responses opposing those angular accelerations. Such induced responses, if left unmodelled, would almost definitely destabilize the attitude of the plant. Unmodelled inertia rate responses are shown to be destabilizing in \cite{inertiaspin}. Typically, multibody dynamics are solved and simulated as a series of interacting torque and force constraints. There are different schools of thought on the subject, each proposing methodologies for stepping through the systems dynamics (\emph{e.g}. Implicit Euler integration\cite{physicallybased,multibodydynamics}).
\par
The prototype investigated here is a multibody system connected with revolute joints, which permit a single degree of relative rotation between each connected rigid body. There are no translational degrees of freedom between each body. Opposed to the angular accelerating actuator action on a body are \emph{gyroscopic} and \emph{inertia} Newtonian torque responses. The responses from each body are solved independently and those excitation induced torque constraints are introduced as additive external torques to the dynamic model derived in Sec:\ref{subsec:dynamics.rigidbody.lagrange}. A distinction must be made between torque responses here and those in Eq:\ref{eq:states.d}. Recalling the classical differential equation of angular motion already derived:
\begin{equation}\label{eq:angular-multibody}
\dot{\vec{\omega}}_b=J_b^{-1}\big(-\vec{\omega}_b\times J_b\vec{\omega}_b+\vec{\tau}_\mu\big)~~~~\in\mathcal{F}^b
\end{equation}
Eq:\ref{eq:angular-multibody} treats the entire body as rigid, included terms are as a result of the entire multibody's \emph{collective motion}. What follows is an extension of that attitude state to incorporate relative movements between each connected body. The objective here is to model the multibody dynamic system with clear responses induced from servo rotations of inner and middle ring bodies, $\Delta\lambda_i$ and $\Delta\alpha_i$ respectively. The subsequent derivations are Lagrangian analytical dynamics applied to the multibody system under consideration. For the purposes of this derivation it is assumed that no potential energy can be stored within the structure from material flexure. The only potential energy contribution is as a result of gravitational potential energy. Moreover, each connected body is first solved as a closed-energy system to ascertain the relative rotational response which is then incorporated in the overall multibody system.
\par
Alternatively, the net dynamics could indeed be derived from a Lagrangian for the \emph{entire} 13 body dynamic system. Those connected bodies are; four rotor/propeller bodies (Fig:\ref{fig:inertia-prop}), four inner ring bodies (Fig:\ref{fig:inertia-inner}), four middle ring bodies (Fig:\ref{fig:inertia-middle}), and the frame structure (Fig:\ref{fig:inertia-frame}) and each with six degrees of freedom. Constraints on the assembly's joints would eventually reduce the degrees of freedom and simplify solving for net responses. The purpose here is to model the body's response to changes in the actuation servos' positions $\Delta\lambda_i$ and $\Delta\alpha_i$ so independent bodies are analyzed first. The final result is, in fact, a Lagrangian for those collective thirteen bodies, whose partial derivative with respect to the net angular velocity relative to the inertial frame $\partial\vec{\omega}_b$ produces the net torque acting on the system.
%====================================================
\subsection{Relative Rotational Gyroscopic and Inertia Torques}
\label{subsec:dynamics.nonlinearities.gyrotorques}
%====================================================
\emph{\color{gray}Rotation matrices are used in the following derivations owing to the fact that induced torque responses are dependent on transformed rotational inertias. Quaternions, as mentioned in Sec:\ref{sec:proto.inertia}, are ill-suited to inertia transformations.}
\par
Each of the four motor modules are symmetrical and so the induced torque response characteristics from one module can be extrapolated simply through a $\hat{Z}_b$ reference frame rotation. Each motor module is positioned relative to the body frame's center of motion $\vec{\mathbf{O}}_b$, as in Fig:\ref{fig:body-frame}. Because each relative rotation from the actuator set $\vec{u}\in\mathbb{U}$ is actuated separately and upon a different body, their responses are calculated independently too.
\par
Drawing again from Lagrangian theory and considering only the angular energy component for the inner ring assembly attached to frame $\mathcal{F}^{M_i}$, there is no relative translational motion between each connected body and the origin of \emph{motion} $\vec{\mathbf{O}}_b$, the center of the body frame $\mathcal{F}^b$ from Fig:\ref{fig:body-frame} and Fig:\ref{fig:inertia-center}. Relative velocity of each body's center of gravity with respect to $\vec{\mathbf{O}}_b$, from each rotational actuation, is small in relation to the net vehicle's translational velocity. So translational kinetic energy for each module is treated as an extension of the body's net kinetic energy in Eq:\ref{eq:3.7a} and assumed to be independent of any actuator's position. The following is with regards to the $i^{th}$ motor module, numerical subscripts are implied.
\begin{figure}[htbp]
\vspace{-10pt}
\centering
\includegraphics[width=0.8\textwidth]{figs/response-inner}
\vspace{-8pt}
\caption{Exploded inner ring inertial bodies for $\vec{\tau}_\lambda(\lambda_i)$}
\label{fig:response-inner}
\vspace{-16pt}
\end{figure}
\par
Deriving dynamic responses for changes in the $\lambda_i$ servo, acting on the inner ring frame $\mathcal{F}^{M_i}$ relative to the middle ring frame $\mathcal{F}^{M_i'}$, requires a relative path coordinate to be defined. The only path variable between the two frames is that servo's rotational position $\lambda_i$ about the $\hat{X}_{M_i}$ axis. Path coordinates $\vec{\mathbf{u}}(t)=\begin{bmatrix}\lambda_i&0&0\end{bmatrix}^T$ are then used to construct the inner ring's closed energy system Lagrangian with respect to the middle ring frame $\mathcal{L}_{n/m}\in\mathcal{F}^{M_i'}$.
\par
The inner ring assembly consists of two separate bodies, exploded in Fig:\ref{fig:response-inner}. Each body has a relative rotational motion and independent kinetic energies. Those bodies are; the rotor assembly with an inertia $J_{r}$ defined earlier in Eq:\ref{eq:prop-inertia}, and the inner ring which has an inertia $J_{ir}$ \emph{without including} the rotor assembly. Reiterating that the rotor's inertia $J_r$ is constant with respect to the propeller's rotational velocity $\Omega_i$, that inner ring inertia is given by:
\begin{equation}
J_{ir}\triangleq J_{n}-J_{r}
\end{equation}
where $J_n$ is the net inertia for the inner ring assembly, explicitly defined in Eq:\ref{eq:inertia.inner}. The rotor assembly has an angular velocity $\vec{\omega}_{r/m}$ relative to the middle ring frame $\mathcal{F}^{M_i'}$ due to the BLDC motor's rotation $\Omega_i$ and the inner ring's servo rate $\dot{\lambda}_i$:
\begin{equation}\label{eq:angular-rot}
\vec{\omega}_{r/m}\triangleq R_x(\lambda_i)\vec{\Omega}_i+\dot{\vec{\lambda}}_i~~~~\in\mathcal{F}^{M_i'}
\end{equation}
with the propeller's angular velocity vector about the inner ring frame's $\hat{Z}_{M_i}$ axis $\vec{\Omega}_i\triangleq\begin{bmatrix}0 & 0 & \Omega_i\end{bmatrix}^T\in\mathcal{F}^{M_i}$ and measured in $[\text{rad.s}^{-1}]$ not in \emph{revolutions per second}. The servo position is defined as a vector in the $\hat{X}_{M_i'}$ axis projected as $\vec{\lambda}_i\triangleq \lambda_i\cdot\hat{X}_{M_i'}=\begin{bmatrix}\lambda_i & 0 & 0\end{bmatrix}^T$ measured in $[\text{rad}]$. Next, the inner ring's angular velocity $\vec{\omega}_{n/m}$ relative to the middle ring $\mathcal{F}^{M_i'}$ is only as a result of $\dot{\lambda}_i$:
\begin{equation}\label{eq:angular-inner}
\vec{\omega}_{n/m}\triangleq\frac{d}{dt}(\vec{\lambda}_i)=\dot{\vec{\lambda}}_i~~~~\in\mathcal{F}^{M_i'}
\end{equation}
\par
The Lagrangian for the inner ring's closed system energy $\mathcal{L}_{n/m}$, in the middle ring frame $\mathcal{F}^{M_i'}$, consists purely of rotational kinetic energy from angular velocities described in Eq:\ref{eq:angular-rot} and Eq:\ref{eq:angular-inner}. Stored gravitational potential energy as a result of the rotated center of mass for the inner ring is omitted here as it is already included in Eq:\ref{eq:grav-torque} and is shown to simplify out subsequently in Eq:\ref{eq:3.109} when considering the entire system as a whole. The inner ring's Lagrangian is:
\begin{subequations}\label{eq:lagrange-inner}
\begin{equation}\label{eq:lagrange-inner.a}
\mathcal{L}_{n/m}=\frac{1}{2}\vec{\omega}_{r/m}\text{}^T\big(J_{r}'\big)\vec{\omega}_{r/m}+\frac{1}{2}\vec{\omega}_{n/m}\text{}^T\big(J_{ir}'\big)\vec{\omega}_{n/m}
\end{equation}
Both inertias for the rotor and inner ring bodies, $J_r$ and $J_{ir}$ respectively, are transformed to align with the middle ring frame $\mathcal{F}^{M_i'}$ using an $R_x(\lambda_i)$ rotation to align with the middle ring's frame $\mathcal{F}^{M_i'}$.
\begin{equation}
J_r'= R_x(\lambda_i)\big(J_r\big)R_x^{-1}(\lambda_i)~~\text{and}~~J_{ir}'= R_x(\lambda_i)\big(J_{ir}\big)R_x^{-1}(\lambda_i)
\end{equation}
Then expanding the Lagrangian $\mathcal{L}_{n/m}$ in Eq:\ref{eq:lagrange-inner.a} with the above definitions for transformed inertias and relative angular velocities $\vec{\omega}_{r/m}$ and $\vec{\omega}_{n/m}$ yields:
\begin{multline}\label{eq:lagrange-inner.b}
\mathcal{L}_{n/m}=\frac{1}{2}\Big(R_x(\lambda_i)\vec{\Omega}_i+\dot{\vec{\lambda}}_i\Big)^T\big(R_x(\lambda_i)\big(J_{r}\big)R_x^{-1}(\lambda_i)\big)\Big(R_x(\lambda_i)\vec{\Omega}_i+\dot{\vec{\lambda}}_i\Big)\\+\frac{1}{2}\dot{\vec{\lambda}}_i^{\hspace{2pt}T}\big(R_x(\lambda_i)\big(J_{ir}\big)R_x^{-1}(\lambda_i)\big)\dot{\vec{\lambda}}_i
\end{multline}
\end{subequations}
Reiterating $J_{ir}$ is the inner ring's inertia independent of the rotor assembly $J_{r}$. Recalling the Euler-Lagrange formulation from Eq:\ref{eq:euler-lagrange} using path coordinates $\vec{\mathbf{u}}(t)$ for the inner ring frame $\mathcal{F}^{M_i}$ relative to the middle ring frame $\mathcal{F}^{M_i'}$, the generalized (torque) forces $\vec{\mathbf{U}}$ acting on the middle ring are then:
\begin{equation}\label{eq:euler-lagrange-inner}
\vec{\mathbf{U}}(\lambda_i)=\frac{d}{dt}\bigg(\frac{\partial \mathcal{L}_{n/m}}{\partial \dot{\vec{\mathbf{u}}}}\bigg)-\frac{\partial \mathcal{L}_{n/m}}{\partial \vec{\mathbf{u}}}~~~~\in\mathcal{F}^{M_i'}
\end{equation}
Consider first the partial derivative of the Lagrangian $\mathcal{L}_{n/m}$ with respect to the generalized path coordinates $\partial\vec{\mathbf{u}}$. The latter part of the Euler-Lagrange formulation with respect to the inner ring in Eq:\ref{eq:euler-lagrange-inner} then expands from the differential product rule:
\begin{multline}\label{eq:euler-lagrange-inner-partial}
\frac{\partial\mathcal{L}_{n/m}}{\partial\vec{\mathbf{u}}}=\frac{1}{2}\Bigg[\bigg(\frac{\partial}{\partial\vec{\lambda}_i}R_x(\lambda_i)\vec{\Omega}_i\bigg)^T\big(J_r'\big)\Big(R_x(\lambda_i)\vec{\Omega}_i+\dot{\vec{\lambda}}_i\Big)+\Big(R_x(\lambda_i)\vec{\Omega}_i+\dot{\vec{\lambda}}_i\Big)^T\bigg(\frac{\partial}{\partial\vec{\lambda}_i}J_r'\bigg)\Big(R_x(\lambda_i)\vec{\Omega}_i+\dot{\vec{\lambda}}_i\Big)
\\
+\Big(R_x(\lambda_i)\vec{\Omega}_i+\dot{\vec{\lambda}}_i\Big)\big(J_r'\big)\bigg(\frac{\partial}{\partial\vec{\lambda}_i}R_x(\lambda_i)\bigg)\Bigg]+\frac{1}{2}\dot{\vec{\lambda}}_i^{\hspace{2pt}T}\bigg(\frac{\partial}{\partial\vec{\lambda}_i}J_{ir}'\bigg)\dot{\vec{\lambda}}_i
\end{multline}
Partial derivatives with respect to the chosen path variable, $\partial\vec{\mathbf{u}}=\partial\vec{\lambda}_i$ in Eq:\ref{eq:euler-lagrange-inner-partial}, only act on rotation matrices $R_x(\lambda_i)$. For the partial derivative of a generalized rotation matrix $R_{\hat{u}}(\theta)$, whose equation is given by Eq:\ref{eq:genrotationmatrix}, derived with respect to its angular operand $\partial\theta$:
\begin{subequations}\label{eq:partial-rotation}
\begin{equation}\label{eq:rotation-partial}
\frac{\partial}{\partial\theta} R_{\hat{u}}(\theta)\triangleq [\hat{u}]_\times R_{\hat{u}}(\theta)
\end{equation}
where $[\hat{u}]_\times$ in Eq:\ref{eq:rotation-partial} is the cross product matrix from Eq:\ref{eq:cross-product-matrix} of the unit vector $\hat{u}$ in the direction of the axis about which the rotation matrix applies its rotation. In the case of an $\hat{X}$ axis rotation matrix, as in Eq:\ref{eq:euler-lagrange-inner-partial}, the expanded partial derivative is:
\begin{equation}\label{eq:rotation-xaxis-partial}
\frac{\partial}{\partial\vec{\lambda}_i}R_x(\lambda_i)\triangleq [\hspace{2pt}\hat{\i}\hspace{2pt}]_\times R_x(\lambda_i) =
\begin{bmatrix}
0 & 0 & 0\\
0 & 0 & -1\\
0 & 1 & 0
\end{bmatrix}R_x(\lambda_i)
\end{equation}
Similarly, the same partial derivative of a rotation matrix transpose $R_x^T(\lambda_i)$ as follows:
\begin{equation}\label{eq:rotation-xaxis-transpose-partial}
\frac{\partial}{\partial\vec{\lambda}_i}R_x^T(\lambda_i)\triangleq -[\hspace{2pt}\hat{\i}\hspace{2pt}]_\times R_x^T(\lambda_i)=\begin{bmatrix}
0 & 0 & 0\\
0 & 0 & 1\\
0 & -1 & 0
\end{bmatrix} R_x^T(\lambda_i)
\end{equation}
\end{subequations}
Applying Eq:\ref{eq:partial-rotation} to the Lagrangian's partial derivative in Eq:\ref{eq:euler-lagrange-inner-partial} reduces and simplifies:
\begin{multline}\label{eq:euler-lagrange-inner-partial}
\frac{\partial\mathcal{L}_{n/m}}{\partial\vec{\mathbf{u}}}=\frac{1}{2}\bigg[\Big([\hspace{2pt}\hat{\i}\hspace{2pt}]_\times R_x(\lambda_i)\vec{\Omega}_i\Big)^T\big(R_x(\lambda_i)\big(J_{r}\big)R_x^{-1}(\lambda_i)\big)\Big(R_x(\lambda_i)\vec{\Omega}_i+\dot{\vec{\lambda}}_i\Big)
\\
+\Big(R_x(\lambda_i)\vec{\Omega}_i+\dot{\vec{\lambda}}_i\Big)^T\big([\hspace{2pt}\hat{\i}\hspace{2pt}]_\times R_x(\lambda_i)\big(J_r\big)R_x^{-1}(\lambda_i)-R_x(\lambda_i)\big(J_r\big)[\hspace{2pt}\hat{\i}\hspace{2pt}]_\times R_x^{-1}(\lambda_i)\big)\Big(R_x(\lambda_i)\vec{\Omega}_i+\dot{\vec{\lambda}}\Big)
\\
+\Big(R_x(\lambda_i)\vec{\Omega}_i+\dot{\vec{\lambda}}\Big)^T\big(R_x(\lambda_i)\big(J_r\big)R_x^{-1}(\lambda_i)\big)\Big([\hspace{2pt}\hat{\i}\hspace{2pt}]_\times R_x(\lambda_i)\vec{\Omega}_i\Big)\bigg]
\\
+\frac{1}{2}\dot{\vec{\lambda}}_i^{\hspace{2pt}T}\big([\hspace{2pt}\hat{i}\hspace{2pt}]_\times R_x(\lambda_i)\big(J_{ir}\big)R_x^{-1}(\lambda_i)-R_x(\lambda_i)\big(J_r\big)[\hspace{2pt}\hat{\i}\hspace{2pt}]_\times R_x(\lambda_i)\big)\dot{\vec{\lambda}}_i=0
\end{multline}
Fortunately the quadratic form of kinetic energies included in Eq:\ref{eq:lagrange-inner.b} resulted in symmetrical partial derivatives of rotation matrices in Eq:\ref{eq:euler-lagrange-inner-partial} simplify out. After some mathematics it follows that partial derivatives of the Lagrangian in Eq:\ref{eq:lagrange-inner} with respect to $\vec{\mathbf{u}}$ are negligible, or that $\partial\mathcal{L}_{n/m}/\partial\vec{\mathbf{u}}= 0$. Only the partial derivatives with respect to the path rate $\dot{\vec{\mathbf{u}}}$ remain:
\begin{equation}\label{eq:3.42b}
\vec{\mathbf{U}}(\lambda_i)=\frac{d}{dt}\bigg(\frac{\partial \mathcal{L}_{n/m}}{\partial \dot{\vec{\mathbf{u}}}}\bigg)=\frac{d}{dt}\bigg(\big(J_{r}'\big)\Big(R_x(\lambda_i)\vec{\Omega}_i+\dot{\vec{\lambda}}_i\Big)+\big(J_{ir}'\big)\dot{\vec{\lambda}}_i\bigg)
\end{equation}
Transformed rates of change for inertias $\dot{J}_r'$ and $\dot{J}_{ir}'$ must first be defined before evaluating the simplified Lagrangian derivative in Eq:\ref{eq:3.42b}. Derivatives of those inertias cannot be separated by time scale from the remainder of Eq:\ref{eq:3.42b} given that $\dot{\lambda}_i$ determines both inertia rates of change $\dot{J}_r'$ and $\dot{J}_{ir}'$, but is also a component of the kinetic energy in Eq:\ref{eq:lagrange-inner.b}.
\par
In general for some transformed inertia $J$ to be aligned relative to a frame $\mathcal{F}^b$ where the inertia is originally defined with respect to a frame $\mathcal{F}^a$, if the two frames differ by some rotation angle $\theta$ about an Euler axis $\hat{u}$, the generalized rotation matrix from frame $\mathcal{F}^a$ to $\mathcal{F}^b$ is given by $R_{\hat{u}}(\theta)$ from Eq:\ref{eq:rotationoperator}. The transformed inertia is then calculated as:
\begin{subequations}
\begin{equation}
J'=R_{\hat{u}}(\theta)\big(J\big)R_{\hat{u}}^{-1}(\theta)
\end{equation}
which, from the product rule and the rotation matrix time derivative definition previously in Eq:\ref{eq:rotation-matrix-derivative}, has a rate of change as a result of the angular velocity $\dot{\theta}$:
\begin{equation}
\dot{J}'=\frac{d}{dt}\Big(R_{\hat{u}}(\theta)\big(J\big)R_{\hat{u}}^{-1}(\theta)\Big)
\end{equation}
\vspace{-10pt}
\begin{equation}
=\frac{d}{dt}\Big(R_{\hat{u}}(\theta)\Big)\big(J\big)R_{\hat{u}}^{-1}(\theta)+R_{\hat{u}}(\theta)\Big(\frac{d}{dt}\big(J\big)\Big)R_{\hat{u}}^{-1}(\theta)+R_{\hat{u}}(\theta)\big(J\big)\frac{d}{dt}\Big(R_{\hat{u}}^{-1}(\theta)\Big)
\end{equation}
\vspace{-6pt}
\begin{equation}\label{eq:inertial-rate-def}
=[\dot{\vec{\theta}}\hspace{2pt}]_\times R_{\hat{u}}(\theta)\big(J\big)R_{\hat{u}}^{-1}(\theta)+R_{\hat{u}}(\theta)\big(\dot{J}\big)R_{\hat{u}}^{-1}(\theta)-R_{\hat{u}}(\theta)\big(J)[\dot{\vec{\theta}}\hspace{2pt}]_\times R_{\hat{u}}^{-1}(\theta)
\end{equation}
\end{subequations}
where $\dot{\vec{\theta}}\triangleq\dot{\theta}\cdot\hat{u}$ is the projcted angular velocity vector between the two frames. In most cases, the inertia will not be changing in its principle frame, or rather that $\dot{J}=0$. Both the rotor assembly and inner ring inertias are constant in their principle frames. The transformed inertias then have the following derivatives; first for the rotor assembly:
\begin{subequations}\label{eq:rotor-deriv}
\begin{equation}
\dot{J}_r'=\frac{d}{dt}\Big(R_x(\lambda_i)\big(J_r\big)R_x^{-1}(\lambda_i)\Big)
\end{equation}
\vspace{-10pt}
\begin{equation}
=[\dot{\vec{\lambda}}_i]_\times R_x(\lambda_i)\big(J_r\big)R_x^{-1}(\lambda_i)-R_x(\lambda_i)\big(J_r\big)[\dot{\vec{\lambda}}_i]_\times R_x^{-1}(\lambda_i)
\end{equation}
\end{subequations}
Similarly for the inner ring's transformed inertia rate $\dot{J}_{ir}'$ (again without the rotor's contribution):
\begin{subequations}\label{eq:inner-deriv}
\begin{equation}
\dot{J}_{ir}'=\frac{d}{dt}\Big(R_x(\lambda_i)\big(J_{ir}\big)R_x^{-1}(\lambda_i)\Big)
\end{equation}
\vspace{-12pt}
\begin{equation}
=[\dot{\vec{\lambda}}_i]_\times R_x(\lambda_i)\big(J_{ir}\big)R_x^{-1}(\lambda_i)-R_x(\lambda_i)\big(J_{ir}\big)[\dot{\vec{\lambda}}_i]_\times R_x^{-1}(\lambda_i)
\end{equation}