Controller Design and Implementation of a New Quadrotor Manipulation System

TL;DR

This paper presents the design, modeling, and implementation of a novel quadrotor with a 2-DOF manipulator capable of tracking 6-DOF trajectories, maximizing payload and mission time through a unique topology.

Contribution

It introduces a new quadrotor manipulation system with a minimal actuator design, along with an algorithm for approximate inverse kinematics solutions.

Findings

System parameters identified through experimental procedures.

Feasibility validated via numerical and experimental results.

Proposed algorithm effectively approximates complex inverse kinematics.

Abstract

The previously introduced aerial manipulation systems suffer from either limited end-effector DOF or small payload capacity. In this dissertation, a quadrotor with a 2-DOF manipulator is investigated that has a unique topology to enable the end-effector to track 6-DOF trajectory with the minimum possible number of actuators/links and hence, maximize the payload and/or mission time. The proposed system is designed, modeled, and constructed. An identification process is carried out to find the system parameters. An experimental setup is proposed with a 6-DOF state measurement and estimation scheme. The system feasibility is validated via numerical and experimental results. The inverse kinematics require a solution of complicated algebraic-differential equations. Therefore, an algorithm is developed to get an approximate solution of these equations.

Tables

Table 1. Table 3.1: System Parameters

Par.	Value	Unit	Par.	Value	Unit
$m$	$1$	$kg$	$l_2$	$85\times {10}^{-3}$	$m$
$d_q$	$223.5\times {10}^{-3}$	$m$	$m_0$	$30\times {10}^{-3}$	$kg$
$I_x$	$13.215\times {10}^{-3}$	$N.m.s^2$	$m_1$	$55\times {10}^{-3}$	$kg$
$I_y$	$12.522\times {10}^{-3}$	$N.m.s^2$	$m_2$	$112\times {10}^{-3}$	$kg$
$I_z$	$23.527\times {10}^{-3}$	$N.m.s^2$	$I_r$	$33.216\times {10}^{-6}$	$N.m.s^2$
$l_0$	$30\times {10}^{-3}$	$m$	$l_1$	$70\times {10}^{-3}$	$m$
$K_{F_1}$	$1.667\times {10}^{-5}$	$kg.m.ra{d}^{-2}$	$K_{F_2}$	$1.285\times {10}^{-5}$	$kg.m.ra{d}^{-2}$
$K_{F_3}$	$1.711\times {10}^{-5}$	$kg.m.ra{d}^{-2}$	$K_{F_4}$	$1.556\times {10}^{-5}$	$kg.m.ra{d}^{-2}$
$K_{M_1}$	$3.965\times {10}^{-7}$	$kg.m^2.ra{d}^{-2}$	$K_{M_2}$	$2.847\times {10}^{-7}$	$kg.m^2.ra{d}^{-2}$
$K_{M_3}$	$4.404\times {10}^{-7}$	$kg.m^2.ra{d}^{-2}$	$K_{M_4}$	$3.170\times {10}^{-7}$	$kg.m^2.ra{d}^{-2}$

Table 2. Table 3.2: Controller Parameters in Simulation

Parameter	Value	Parameter	Value
$K_{p_{x,y}}$	$2$	$K_{d_{x,y}}$	$7$
$K_{i_{x,y}}$	$0.5$	$K_{p_z}$	$10$
$K_{d_z}$	$10$	$K_{i_z}$	$5$

Table 3. Table 3.3: Experimental Controller Parameters

Parameter	Value	Parameter	Value
$K_{p_{x,y}}$	$5$	$K_{d_{x,y}}$	$12$
$K_{i_{x,y}}$	$2$	$K_{p_z}$	$15$
$K_{d_z}$	$15$	$K_{i_z}$	$4$
$L_{ob{s}_{x,y}}$	${\left[\mathrm{18.01\; 45.18\; 0.45}\right]}^T$	$L_{ob{s}_z}$	${\left[\mathrm{18.01\; 45.18\; 0.45}\right]}^T$

Table 4. Table 5.1: PD-DOb parameters

Par.	Value	Par.	Value
$M_{n_{\zeta }}$	$diag\{1.2,1.2,1.2,0.02,0.004,0.0009\}$	$M_{n_{\sigma }}$	$diag\{0.02,0.02\}$
$K_{P_{\zeta }}$	$diag\{3,3,10,3,3,3\}$	$K_{P_{\sigma }}$	$diag\{10,10\}$
$K_{D_{\zeta }}$	$diag\{1.5,1.5,5,3,3,3\}$	$K_{D_{\sigma }}$	$diag\{7,7\}$
$P$	${\left[\mathrm{10\; 10\; 10\; 100\; 100\; 100\; 100\; 100}\right]}^T$

Table 5. Table 5.2: MPC-DOb controller parameters

Parameter	Value
$Q_{mp{c}_{\zeta }}$	$C^{T}C$
$R_{mp{c}_{\zeta }}$	$I_6$
$Q_{mp{c}_{\sigma }}$	$2C^{T}C$
$R_{mp{c}_{\sigma }}$	$0.2I_2$
$u^{max}$	${\left[\mathrm{6\; 6\; 6\; 6\; 0.7\; 0.4}\right]}^T$
$u^{min}$	${\left[\mathrm{0\; 0\; 0\; 0}-0.7-0.4\right]}^T$
$s_{u_{\zeta }}$	$2$
$s_{u_{\sigma }}$	$10$
$n_{u_{\zeta }}$	$3$
$n_{u_{\sigma }}$	$6$
$h$	$0.01$
$k_{max}$	$38$
${\tau }_x^s$, ${\tau }_y^s$	0.5

Table 6. Table 5.3: Comparison study: MPC-DOb vs PD-DOb

RMSE	PD-DOb	MPC-DOb
$x_e$ [m]	0.0390	0.0377
$y_e$ [m]	0.0370	0.0306
$z_e$ [m]	0.0227	0.0031
${\psi }_e$ [deg]	0.4469	0.0250
${\theta }_e$ [deg]	1.0199	0.0309
${\varphi }_e$ [deg]	0.2876	0.8113
Control Effort
$\sum T$ [N]	748.3817	645.4135
$\sum {\tau }_{m_1}$ [N.m]	11.6674	11.6460
$\sum {\tau }_{m_2}$ [N.m]	4.6629	4.6355

Table 7. Table 5.4: FTRLS-DOB simulation parameters

Parameter	Value
$A_{e_1}$	$0.16$
$S_{c,d}$	$diag\{20,20,20,50,50,50\}$
$D_c$	$diag\{0,0,10,0,0,0\}$
$M_{c,d}$	$2I_6$
$D_{c,d}$	$diag\{15,15,15,100,100,100\}$
$A_{e_2}$	$0.032$
${\eta }_h^{min}$	$0.8$
${\gamma }_g$	$5$
$S_c$	$diag\{0,0,20,0,0,0\}$
$z_0$	$1$
$K_{p_{\sigma }}$, $K_{d_{\sigma }}$	$20I_2$
$K_{P,e}$	$diag\{25,25,150,200,200,500\}$
$K_{D,e}$	$diag\{15,15,100,100,500,100\}$

Table 8. Table A.1: AscTec Pelican, technical Data [ 1 ]

Dimensions	65.1 $\times$ 65.1 $\times$ 18.8 cm
Propeller size	10 inch
Motors	4 $\times$ 160 W electrical, brushless (sensorless)
Max. thrust	36 N
Max. payload	650 g
Max. total weight	1650 g
Max. airspeed	16 m/s
Max. flight time	30 mins (without payload)
Range	$1\mathrm{km}$
Battery	6250 mAh (LiPo)

Table 9. Table A.2: AscTec Atomboard technical specifications [ 1 ]

CPU	Intel® Atom™ Processor Z530
CPU L2 Cache	512k
Number of cores	1
Clock Speed	1.6GHz
RAM	1GB DDR2
GPU	Intel® GMA500
Hard disk	8GB SD Card
Display connector	LVDS
USB2.0	7x mini-USB A/B
UART	2x

Table 10. Table E.1: Laser Range Finder, URG-04LX, specifications [ 2 , 3 ]

Model No.	URG-04LX
Power source	5VDC$\pm$5%. Sensor will not operate with USB bus power. Prepare power source separately.
Current consumption	500mA or less(800mA when start-up)
Measuring area	60 to 4095mm(white paper with 70mm) ${240}^{\circ }$
Accuracy	is $\pm$ 10 mm for distances of less than 1 m. For greater distances, the error is quoted as $\pm$2%
Repeatability	60 to 1,000mm : $\pm$ 10mm
Angular resolution	Step angle : approx. 0.36°(360°/1,024 steps)
Light source	Semiconductor laser diode (wave length =785nm)
Scanning time	100ms/scan
Noise	25dB or less
Interface	USB, RS-232C(19.2k, 57.6k, 115.2k, 250k, 500k, 750kbps)
Ambient temperature/humidity	-10 to +50 degrees C, 85% or less(Not condensing, not icing)
Vibration resistance	10 to 55Hz, double amplitude 1.5mm Each 2 hour in X, Y and Z directions
Impact resistance	196m/$s^2$, Each 10 time in X, Y and Z directions
Weight	Approx. 160g
Accessory	Cable for power communication/input-output(1.5m), D-sub connector with 9 pins

Table 11. Table E.2: Laser Range Finder, URG-04LX, Pins of the interface connector [ 2 ]

Pin No.	Signals	Colors
1	N.C.	Red
2	N.C.	White
3	OUTPUT(Synchronous output)	Black
4	GND(5th pin of 9-pin, D-sub connector)	Purple
5	RxD(3rd pin of 9-pin, D-sub connector)	Yellow
6	TxD(2nd pin of 9-pin, D-sub connector)	Green
7	0V	Blue
8	DC5V	Brown

Table 12. Table E.3: Laser Range Finder, URG-04LX, the USB connector

Connector Type

USB-miniB (5pin)

Table 13. Table F.1: Ultrasonic, SRF04, technical specifications [ 4 ]

Sensor type	Reflective Ultrasonic
Sender	N1076, Receiver N1081
Voltage	5v only required
Current	30mA Typical, 50mA Max.
Frequency	40kHz
Range	3cm - 3m
Sensitivity	Detect 3cm diameter
Input Trigger	10uS Min. TTL (Transistor Transistor Logic) level pulse
Echo Pulse	Positive TTL level signal, width proportional to range
I/O required	two digital lines, 1 output, 1 input
Small Size	43mm x 20mm x 17mm height
Weight	11.33 gm

Table 14. Table I.1: Pin Connections of the 6-DOF torque/force sensor [ 5 ]

PIN	SIGNAL
1	$F_x$
2	$F_y$
3	$F_z$
4	$M_x$
5	$M_y$
6	$M_z$
7	+ power, Sensor excitation: $\pm$5.0V
8	- power
9	power and signal common

Equations

$$R_b=\left[\begin{array}{ccc}{C}_{\psi }{C}_{\theta }& S_{\varphi }{S}_{\theta }{C}_{\psi }-S_{\psi }{C}_{\varphi }& S_{\psi }{S}_{\varphi }+C_{\psi }{S}_{\theta }{C}_{\varphi }\\ S_{\psi }{C}_{\theta }& C_{\psi }{C}_{\varphi }+S_{\psi }{S}_{\theta }{S}_{\varphi }& S_{\psi }{S}_{\theta }{C}_{\varphi }-C_{\psi }{S}_{\varphi }\\ -S_{\theta }& C_{\theta }{S}_{\varphi }& C_{\theta }{C}_{\varphi }\end{array}\right],$$

$$A_e=A_b{A}_0^b{A}_1^0{A}_2^1.$$

$$A_e^b=A_2^b=A_0^b{A}_1^0{A}_2^1$$

$$=\left[\begin{array}{cc}{R}_e^b& p_{eb}^b\\ O_{1\times 3}& 1\end{array}\right],$$

$$A_0^b=\left[\begin{array}{cccc}0& 0& 1& 0\\ -1& 0& 0& 0\\ 0& -1& 0& -l_0\\ 0& 0& 0& 1\end{array}\right],$$

$$A_1^0=\left[\begin{array}{cccc}{C}_{{\theta }_1}& 0& S_{{\theta }_1}& l_1{C}_{{\theta }_1}\\ S_{{\theta }_1}& 0& -C_{{\theta }_1}& l_1{S}_{{\theta }_1}\\ 0& 1& 0& 0\\ 0& 0& 0& 1\end{array}\right],$$

$$A_2^1=\left[\begin{array}{cccc}{C}_{{\theta }_2}& -S_{{\theta }_2}& 0& l_2{C}_{{\theta }_2}\\ S_{{\theta }_2}& C_{{\theta }_2}& 0& l_2{S}_{{\theta }_2}\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right].$$

$$p_e=p_b+R_b{p}_{eb}^b.$$

$$R_e=R_b{R}_e^b,$$

$${\dot{p}}_e={\dot{p}}_b-Skew(R_b{p}_{eb}^b){\omega }_b+R_b{\dot{p}}_{eb}^b,$$

$${\omega }_e={\omega }_b+R_b{\omega }_{eb}^b,$$

$$v_{eb}^b=J_{eb}^b\dot{\Theta },$$

$$J_{eb}^b=\left[\begin{array}{cc}0& l_2{C}_{{\theta }_2}\\ S_{{\theta }_1}(l_1+l_2{C}_{{\theta }_2})& l_2{C}_{{\theta }_1}{S}_{{\theta }_2}\\ -C_{{\theta }_1}(l_1+l_2{C}_{{\theta }_2})& l_2{S}_{{\theta }_1}{S}_{{\theta }_2}\\ 1& 0\\ 0& -S_{{\theta }_1}\\ 0& C_{{\theta }_1}\end{array}\right].$$

$$v_e=J_b{v}_b+J_{eb}\dot{\Theta },$$

$$v_e=J_b{Q}_b{\dot{\chi }}_b+J_{eb}\dot{\Theta },$$

$$T_b({\Phi }_b)=\left[\begin{array}{ccc}0& -S_{\psi }& C_{\psi }{C}_{\theta }\\ 0& C_{\psi }& S_{\psi }{C}_{\theta }\\ 1& 0& -S_{\theta }\end{array}\right].$$

$$v_e=J\dot{q},$$

$$J=\left[\begin{array}{cccccccc}1& 0& 0& J_{14}& J_{15}& J_{16}& J_{17}& J_{18}\\ 0& 1& 0& J_{24}& J_{25}& J_{26}& J_{27}& J_{28}\\ 0& 0& 1& J_{34}& J_{35}& 0& -C_{(\varphi +{\theta }_1)}{C}_{\theta }(l_1+l_2{C}_{{\theta }_2})& J_{38}\\ 0& 0& 0& C_{\psi }{C}_{\theta }& -S_{\psi }& 0& C_{\psi }{C}_{\theta }& J_{48}\\ 0& 0& 0& C_{\theta }{S}_{\psi }& C_{\psi }& 0& C_{\theta }{S}_{\psi }& J_{58}\\ 0& 0& 0& -S_{\theta }& 0& 1& -S_{\theta }& C_{(\varphi +{\theta }_1)}{C}_{\theta }\end{array}\right],$$

$$J_{14}=S_{\theta }((C_{\psi }{S}_{\varphi }-C_{\varphi }{S}_{\psi }{S}_{\theta })(l_0+l_1{S}_{{\theta }_1}+l_2{C}_{{\theta }_2}{S}_{{\theta }_1})-C_{{\theta }_1}(l_1+l_2{C}_{{\theta }_2})(C_{\psi }{C}_{\varphi }+S_{\psi }{S}_{\varphi }{S}_{\theta })$$

$$+l_2{C}_{\theta }{S}_{\psi }{S}_{{\theta }_2})-C_{\theta }{S}_{\psi }(l_2{S}_{\theta }{S}_{{\theta }_2}+C_{\varphi }{C}_{\theta }(l_0+l_1{S}_{{\theta }_1}+l_2{C}_{{\theta }_2}{S}_{{\theta }_1})+C_{\theta }{C}_{{\theta }_1}{S}_{\varphi }(l_1+l_2{C}_{{\theta }_2})),$$

$$J_{15}=-C_{\psi }(l_2{S}_{\theta }{S}_{{\theta }_2}+C_{\varphi }{C}_{\theta }(l_0+l_1{S}_{{\theta }_1}+l_2{C}_{{\theta }_2}{S}_{{\theta }_1})+C_{\theta }{C}_{{\theta }_1}{S}_{\varphi }(l_1+l_2{C}_{{\theta }_2})),$$

$$J_{16}=C_{{\theta }_1}(l_1+l_2{C}_{{\theta }_2})(C_{\psi }{C}_{\varphi }+S_{\psi }{S}_{\varphi }{S}_{\theta })-(C_{\psi }{S}_{\varphi }-C_{\varphi }{S}_{\psi }{S}_{\theta })(l_0+l_1{S}_{{\theta }_1}+l_2{C}_{{\theta }_2}{S}_{{\theta }_1})-l_2{C}_{\theta }{S}_{\psi }{S}_{{\theta }_2},$$

$$J_{17}=-(l_1{S}_{{\theta }_1}+l_2{C}_{{\theta }_2}{S}_{{\theta }_1})(C_{\varphi }{S}_{\psi }-C_{\psi }{S}_{\varphi }{S}_{\theta })-(l_1{C}_{{\theta }_1}+l_2{C}_{{\theta }_1}{C}_{{\theta }_2})(S_{\psi }{S}_{\varphi }+C_{\psi }{C}_{\varphi }{S}_{\theta }),$$

$$J_{18}=l_2{C}_{\psi }{C}_{\theta }{C}_{{\theta }_2}-l_2{C}_{{\theta }_1}{S}_{{\theta }_2}(C_{\varphi }{S}_{\psi }-C_{\psi }{S}_{\varphi }{S}_{\theta })+l_2{S}_{{\theta }_1}{S}_{{\theta }_2}(S_{\psi }{S}_{\varphi }+C_{\psi }{C}_{\varphi }{S}_{\theta }),$$

$$J_{24}=C_{\psi }{C}_{\theta }(l_2{S}_{\theta }{S}_{{\theta }_2}+C_{\varphi }{C}_{\theta }(l_0+l_1{S}_{{\theta }_1}+l_2{C}_{{\theta }_2}{S}_{{\theta }_1})+C_{\theta }{C}_{{\theta }_1}{S}_{\varphi }(l_1+l_2{C}_{{\theta }_2}))-S_{\theta }(C_{{\theta }_1}$$

$$(l_1+l_2{C}_{{\theta }_2})(C_{\varphi }{S}_{\psi }-C_{\psi }{S}_{\varphi }{S}_{\theta })-(S_{\psi }{S}_{\varphi }+C_{\psi }{C}_{\varphi }{S}_{\theta })(l_0+l_1{S}_{{\theta }_1}+l_2{C}_{{\theta }_2}{S}_{{\theta }_1})+l_2{C}_{\psi }{C}_{\theta }{S}_{{\theta }_2}),$$

$$J_{25}=-S_{\psi }(l_2{S}_{\theta }{S}_{{\theta }_2}+C_{\varphi }{C}_{\theta }(l_0+l_1{S}_{{\theta }_1}+l_2{C}_{{\theta }_2}{S}_{{\theta }_1})+C_{\theta }{C}_{{\theta }_1}{S}_{\varphi }(l_1+l_2{C}_{{\theta }_2})),$$

$$J_{26}=C_{{\theta }_1}(l_1+l_2{C}_{{\theta }_2})(C_{\varphi }{S}_{\psi }-C_{\psi }{S}_{\varphi }{S}_{\theta })-(S_{\psi }{S}_{\varphi }+C_{\psi }{C}_{\varphi }{S}_{\theta })(l_0+l_1{S}_{{\theta }_1}+l_2{C}_{{\theta }_2}{S}_{{\theta }_1})+l_2{C}_{\psi }{C}_{\theta }{S}_{{\theta }_2},$$

$$J_{27}=(l_1{S}_{{\theta }_1}+l_2{C}_{{\theta }_2}{S}_{{\theta }_1})(C_{\psi }{C}_{\varphi }+S_{\psi }{S}_{\varphi }{S}_{\theta })+(l_1{C}_{{\theta }_1}+l_2{C}_{{\theta }_1}{C}_{{\theta }_2})(C_{\psi }{S}_{\varphi }-C_{\varphi }{S}_{\psi }{S}_{\theta }),$$

$$J_{28}=l_2{C}_{\theta }{C}_{{\theta }_2}{S}_{\psi }+l_2{C}_{{\theta }_1}{S}_{{\theta }_2}(C_{\psi }{C}_{\varphi }+S_{\psi }{S}_{\varphi }{S}_{\theta })-l_2{S}_{{\theta }_1}{S}_{{\theta }_2}(C_{\psi }{S}_{\varphi }-C_{\varphi }{S}_{\psi }{S}_{\theta }),$$

$$J_{34}=-C_{\theta }((l_2{C}_{(\varphi +{\theta }_1-{\theta }_2)})/2+l_1{C}_{(\varphi +{\theta }_1)}-l_0{S}_{\varphi }+(l_2{C}_{(\varphi +{\theta }_1+{\theta }_2)})/2),$$

$$J_{35}=l_0{C}_{\varphi }{S}_{\theta }-l_2{C}_{\theta }{S}_{{\theta }_2}+l_1{C}_{\varphi }{S}_{\theta }{S}_{{\theta }_1}+l_1{C}_{{\theta }_1}{S}_{\varphi }{S}_{\theta }+l_2{C}_{\varphi }{C}_{{\theta }_2}{S}_{\theta }{S}_{{\theta }_1}+l_2{C}_{{\theta }_1}{C}_{{\theta }_2}{S}_{\varphi }{S}_{\theta },$$

Full text

Controller Design and Implementation of a New Quadrotor Manipulation System

By Ahmed Mohammed Elsayed Khalifa

For The Degree

of

Doctor of Philosophy

in

Mechatronics and Robotics Engineering

Supervision Committee

Name, Title

Affiliation

Signature, Date

Prof. Mohamed A. Fanni

Mechatronics and Robotics

Engineering, E-JUST

…………

Prof. Toru Namerikawa

Systems and Control

Engineering, Keio University

…………

Examination Committee

Name, Title

Affiliation

Approved, Date

Prof. Sohair F. Rezeka

Mechanical Engineering,

Alexandria University

…………

Prof. Khaled A. El-Metwally

Automatic Control

Engineering, Cairo University

…………

Prof. Mohamed A. Fanni

Mechatronics and Robotics

Engineering, E-JUST

…………

{copyrightKhalifa}

Controller Design and Implementation of a New Quadrotor Manipulation System

by Ahmed Khalifa

Copyright © 2016 Ahmed Mohammed Elsayed Khalifa. All rights reserved. This document was created with the document preparation system LaTeX.

{declaration} I certify that in the preparation of this thesis, I have observed the provisions of E-JUST Code of Ethics dated 8 September 2013. Further; I certify that this work is free of plagiarism and all materials appearing in this thesis have been properly quoted and attributed. I certify that all copyrighted material incorporated into this thesis is in compliance with the international copyright law and that I have received written permission from the copyright owners for my use of their work, which is beyond the scope of the law. I agree to indemnify and save harmless E-JUST from any and all claims that may be asserted or that may arise from any copyright violation. I hereby certify that the research work in this thesis is my original work and it does not include any copied parts without the appropriate citation.

Alexandria, Egypt, March 11, 2024Ahmed Mohammed Elsayed Khalifa,

{dedication} This thesis is dedicated to my father 1958 $\textendash$ 2002, and to both Prof. Ahmed Abo-Ismail 1945 $\textendash$ 2015 and Dr. Ahmed Ramadan 1973 $\textendash$ 2015 who never got to read the final draft. You are the determination in every page.

{acknowledgment}

First of all, countless thanks to ALLAH the almighty

While my name may be alone on the front cover of this thesis, I am by no means its sole contributor. Rather, there are a number of people behind this piece of work who deserve to be both acknowledged and thanked here: My academic supervisors; Prof. Mohamed Fanni, late Prof. Ahmed Abo-Ismail, late Dr. Ahmed Ramadan, and Prof. Toru Namerikawa. I would like to express my sincere condolences for the loss of Prof. Ahmed Abo-Ismail and Dr. Ahmed Ramadan. Prof. Ahmed Abo-Ismail is the founder of the Mechatronics and Robotics Engineering Department and he is the former Vice President of Education and Academic Affairs and one of E-JUST founders and pillars. Dr. Ahmed Ramadan made an extraordinary effort to build and establish most of the Labs in the Department of Mechatronics and Robotics Engineering at E-JUST. He was really thoroughly enjoyable, impeccably honest, straightforward, positive in his attitude and really has high morals. We pray for them to rest in peace and for their families to pass these difficult times. We ask ALLAH to meet them in the Paradise. I am forever indebted to my supervisor, Assoc. Prof. Mohamed Fanni, for his novel ideas, enthusiasm, guidance, and unrelenting support throughout this research. He has routinely gone beyond his duties to fire fight my worries and concerns, and have worked to instill great confidence in both myself and my work. THANK YOU FOR MAKING ME MORE THAN I AM. I would like to show my sincere appreciation and gratitude to Prof. Abdelfatah Mohamed, the former chairperson of the Department of Mechatronics and Robotics Engineering, for his support, guidance and encouragement. I would like to convey my heartfelt thanks to my home university, Menofia University, Faculty of Electronic Engineering, and Department of Industrial Electronics and Control Engineering for their support and encouragement. I would like to show my sincere appreciation and gratitude to Prof. Toru Namerikawa, Keio University, for accepting me in his research group and providing generous support, insightful advice and kind encouragement throughout my time at Keio University to date. The Namerikawa Lab has always been a place where people are ready to offer keen criticism and advice on any and all topics, academic or otherwise. For this I thank Mr. Okawa, Mr. Mori, Mr. Komagine, Mr. Hayashi, and Mr. Shinohara. Without the open-source software generously released by several individuals around the world, much of the implementation and experimental part of the work in this thesis would have been considerably more difficult. I am therefore thankful to the authors of ROS, the ROS AscTec and Laser drivers, as well as those of Linux and an uncountable number of supporting packages for this wonderful operating system. Thanks to my beloved family, my parents, my brother, and my sisters, for their self-sacrifice, consistent love, support, understanding and encouragement. Most of all, I thank my darling wife, whose love and strength have never flagged through the many ups and downs of this journey; and my children, whose smiles upon my return from a too-long day on campus always lift my spirits. I would like to gratefully acknowledge the financial support of the Ministry of Higher Education, Government of Egypt, for my postgraduate scholarship.

Ahmed Khalifa September 2016

1. 1 INTRODUCTION

2. 1.1 Research Background

3. 1.1.1 Overview of the Current Developed Aerial Manipulators

4. 1.1.2 Experimental Setup and Measurements

5. 1.1.3 Control System

6. 1.2 Objectives of Research

7. 1.2.1 Design and Modeling of the Quadrotor Manipulation System

8. 1.2.2 Experimental Setup and State Estimation

9. 1.2.3 Inverse Kinematics

10. 1.2.4 Design of a Control Scheme

11. 1.3 Thesis Contributions

12. 1.4 Thesis Plan

13. 2 SYSTEM DESIGN AND MODELING

14. 2.1 System Description

15. 2.2 System Design

16. 2.2.1 Quadrotor

17. 2.2.2 Manipulator

18. 2.3 Forward Kinematics

19. 2.4 Dynamic Model

20. 3 SYSTEM CONSTRUCTION AND EXPERIMENTS

21. 3.1 Hardware

22. 3.2 Software

23. 3.3 Identification

24. 3.4 6-DOF State Estimation

25. 3.5 Control Design for the Experiments

26. 3.6 Simulation Results

27. 3.7 Experimental Results

28. 4 INVERSE KINEMATICS SOLUTION

29. 4.1 Novel 6-DOF Trajectory Inverse Kinematics

30. 4.2 Verification

31. 5 CONTROL SYSTEM DEVELOPMENT

32. 5.1 Control Objectives

33. 5.2 Robust Position Control

34. 5.2.1 DOb-based Controller

35. 5.2.2 Simulation Study

36. 5.3 Robust Optimal Position Control

37. 5.3.1 Linearized Model

38. 5.3.2 Continuous time MPC

39. 5.3.3 Predicted plant response

40. 5.3.4 Optimal control strategy

41. 5.3.5 Constrained control

42. 5.3.6 MPC tuning guidelines

43. 5.3.7 Simulation Results

44. 5.4 Robust Force Estimation and Impedance Control

45. 5.4.1 Modeling of the Environment Dynamics and the Wind Effect

46. 5.4.2 Fast Tracking Recursive Least Squares

47. 5.4.3 Impedance Control

48. 5.4.4 Simulation Results

49. 6 CONCLUSIONS

50. 6.1 Conclusions

51. 6.2 Future Plans

52. LIST OF PUBLICATIONS

53. A Quadrotor Platform

54. B Robotic Arm

55. C Forward Kinematics

56. D System Jacobian and Parametrization

57. E Laser Range Finder

58. F Ultrasonic Ranger

59. G System Internal Connections

60. H Robot Operating System (ROS)

61. H.1 ROS

62. H.2 C++ Node for Extracting the Euler Angles from the Quaternion

63. H.3 ROS-based Arduino Code

64. H.4 Altitude Compensation Node

65. H.5 Joystick teleoperation Node

66. I 6-DOF Torque/Force Sensor

67. J Inverse Kinematics

1. 2.1 DH parameters for the manipulator
2. 3.1 System Parameters
3. 3.2 Controller Parameters in Simulation
4. 3.3 Experimental Controller Parameters
5. 5.1 PD-DOb parameters
6. 5.2 MPC-DOb controller parameters
7. 5.3 Comparison study: MPC-DOb vs PD-DOb
8. 5.4 FTRLS-DOB simulation parameters
9. A.1 AscTec Pelican, technical Data [1]
10. A.2 AscTec Atomboard technical specifications [1]
11. E.1 Laser Range Finder, URG-04LX, specifications [2, 3]
12. E.2 Laser Range Finder, URG-04LX, Pins of the interface connector [2]
13. E.3 Laser Range Finder, URG-04LX, the USB connector
14. F.1 Ultrasonic, SRF04, technical specifications [4]
15. I.1 Pin Connections of the 6-DOF torque/force sensor [5]

1. 1.1 Examples of tasks that need robotics especially aerial robots
2. 1.2 Typical quadrotor [6]
3. 1.3 Quadrotor carrying a payload via a gripper [7]
4. 1.4 Quadrotor carrying a payload via a cable [8]
5. 1.5 Swooping quadrotor with claws [9]
6. 1.6 Quadrotor carrying a payload via three 2-DOF arms [10]
7. 1.7 Quadrotor carrying a payload via 6-DOF arm [11]
8. 1.8 Quadrotor carrying a payload via 2-DOF robotic arm in with parallel joints’ axes [12]
9. 1.9 Quadrotor carrying a payload via 2-DOF robotic arm that has unique topology [13]
10. 2.1 $3$D CAD model of the proposed system
11. 2.2 Schematic of Quadrotor Manipulation System with relevant frames
12. 2.3 Different views for the quadrotor platform extracted from a SOLIDWORKS model
13. 2.4 Manipulator’s structure analysis
14. 2.5 Experimental Validation of the system design
15. 3.1 Aerial manipulation functional block diagram
16. 3.2 Close sight on position sensors connections
17. 3.3 Experiment to estimate the rotor coefficients
18. 3.4 Relation between the generated thrust force and the quadrotor’s squared motor speed
19. 3.5 Relation between the generated drag moment and the quadrotor’s squared motor speed
20. 3.6 Block diagram of the state estimation scheme
21. 3.7 Block diagram of the control structure; Note that the circle block does the summation function, and the input with minus signal (-) means+ subtraction while the input with no signal means addition
22. 3.8 Simulation results: actual position/command velocity history of the system
23. 3.9 Pictures taken during the experiment
24. 3.10 Experimental results: measured position/command velocity history of the system
25. 4.1 Block diagram of the inverse kinematic algorithm
26. 4.2 The actual response of the end-effector pose in the case of the slower trajectories: a) $x_{e}$, b) $y_{e}$, c) $z_{e}$, d) $\phi_{e}$, e) $\theta_{e}$, and f) $\psi_{e}$
27. 4.3 End-effector 3D trajectory in the case of the slower trajectories
28. 4.4 The actual response of the end-effector pose in the case of the faster trajectories: a) $x_{e}$, b) $y_{e}$, c) $z_{e}$, d) $\phi_{e}$, e) $\theta_{e}$, and f) $\psi_{e}$
29. 4.5 End-effector 3D trajectory in the case of the faster trajectories
30. 5.1 Functional block diagram of the proposed PD-DOb control system
31. 5.2 Block diagram of the conventional DOb internal loop
32. 5.3 Block diagram of the modified DOb-based controller
33. 5.4 Detailed block diagram of the PD-DOb control system
34. 5.5 PD-DOb results: The actual response in the quadrotor/joint space; a) $x$, b) $y$, c) $z$, d) $\psi$, e) $\theta_{1}$, and f) $\theta_{2}$
35. 5.6 PD-DOb results: The actual response of the end-effector pose; a) $x_{e}$, b) $y_{e}$, c) $z_{e}$, d) $\phi_{e}$, e) $\theta_{e}$, and f) $\psi_{e}$. The inner box of each figure represents zooming on that figure from instant 0 s to instant 5 s.
36. 5.7 PD-DOb results: 3D trajectory of end-effector with fixed orientation (The marker represents the end-effector orientation; Green, Blue, and Red for x-,y, and z-axis)
37. 5.8 PD-DOb results: The required control efforts
38. 5.9 Functional block diagram of the proposed MPC-DOb control system
39. 5.10 Block diagram of the detailed MPC-DOb control system for the quadrotor manipulation system
40. 5.11 MPC-DOb results: The actual response in the quadrotor/joint space; a) $x$, b) $y$, c) $z$, d) $\psi$, e) $\theta_{1}$, and f) $\theta_{2}$
41. 5.12 MPC-DOb results: The actual response of the end-effector pose: a) $x_{e}$, b) $y_{e}$, c) $z_{e}$, d) $\phi_{e}$, e) $\theta_{e}$, and f) $\psi_{e}$
42. 5.13 MPC-DOb results: 3D task space trajectory
43. 5.14 MPC-DOb results: The required controller efforts
44. 5.15 Functional block diagram of the FTRLS-DOb control system
45. 5.16 Block diagram of the detailed FTRLS-DOb control system for the quadrotor manipulation system
46. 5.17 Profile of the wind angle
47. 5.18 Profile of the wind speed
48. 5.19 FTRLS-DOb results: Actual response of the estimation of the contact force
49. 5.20 FTRLS-DOb results: Error norm of the estimation of the contact force
50. 5.21 FTRLS-DOb results: The actual response of the end-effector pose: a) $x_{e}$, b) $y_{e}$, c) $z_{e}$, d) $\phi_{e}$, e) $\theta_{e}$, and f) $\psi_{e}$
51. 5.22 FTRLS-DOb results: 3D trajectory of the end-effector pose (The marker represents the end-effector orientation; Green, Blue, and Red for x-,y-, and z-axis, respectively)
52. 5.23 FTRLS-DOb results: The actual trajectories of the variables in the quadrotor/joint space
53. A.1 AscTec Pelican quadrotor with full options [1]
54. A.2 Mounting the propellers [1]
55. A.3 Dimensions of the Pelican quadrotor [1]
56. A.4 AscTec AutoPilot functional block diagram [1]
57. A.5 AscTec AutoPilot board pinout and connections [1]
58. A.6 AscTec Power Board pinout and connections [1]
59. A.7 Top view of the onboard computer pinout and connections [1]
60. A.8 Bottom view of the onboard computer pinout and connections [1]
61. B.1 Lightweight gripper [14]
62. B.2 Schematic diagram of the manipulator used to select the joints’ motors
63. B.3 HS-5485HB digital servo motor [14]
64. B.4 HS-5485HB dimensions [14]
65. B.5 HS-422 dimensions [14]
66. B.6 Accessories for building the manipulator: a) Aluminum Tubing, b) Servo Bracket, c) Connector Hub, and d) Long “C” Servo Bracket [14]
67. B.7 Dimensions of the accessories for building the manipulator in (mm): a) Aluminum Tubing, b) Servo Bracket, c) Connector Hub, and d) Long “C” Servo Bracket.
68. B.8 SSC-32 servo controller [14]
69. B.9 SSC32 board: Shorting bar jumpers to enable serial communication with the Arduino board
70. B.10 Arduino Mega 2560 board [15]
71. B.11 12 V to 5 V 1.5 A DC-DC converter circuit diagram [16]
72. E.1 URG-04LX Laser Range Finder [2]
73. E.2 The principle of operation of the URG-04LX [3]
74. E.3 External dimension of the Laser Range Finder URG-04LX [2]
75. F.1 Devantech SRF04 Ultrasonic Range Finder [4]
76. F.2 External dimension of the SRF04 [4]
77. F.3 Terminal Connections of the SRF04 [4]
78. F.4 Beam Pattern of the SRF04 [4]
79. F.5 Timing diagram of the SRF04 [4]
80. G.1 Power connections of the manipulator
81. I.1 The Torque Force sensor with the interface mount to rotor to be identified
82. I.2 Torque/Force sensor axis orientation from the rotor side [5]

Chapter 1 INTRODUCTION

In this chapter, the research background on the aerial manipulation systems, which induces the motivation of this research, is presented. The objectives of this research are clearly described and the outline of the thesis is briefly introduced.

1.1 Research Background

1.1.1 Overview of the Current Developed Aerial Manipulators

Recently, the evolvement of technology is much more concentrating on improving the standard of human living. Robots and mechatronic systems are expected to support human activities in daily life, industry, entertainment, service, outer space and medicine. Such activities include; inspection, maintenance, firefighting, service robot in crowded cities to deliver light stuff such as post mails or quick meals, rescue operation, surveillance, demining, performing tasks in dangerous places (e.g., Nuclear stations), or transportation in remote places (see Fig. 1.1).

There are two main categories of the robotic systems; ground robots and aerial robots. However, the ground robots have constraints and limitations to move and access all types of the environments and terrains. Recently, the aerial vehicles are utilized to overcome these constraints and they offer possibilities of speed and access to regions that are otherwise inaccessible to ground robotic vehicles. There are several types of aerial vehicles (fixed wing, helicopters, single rotor, twin rotor, quadrotor, hexacopter, octocopter, $\ldots$, etc). Quadrotor system (see Fig. 1.2) is one of the Unmanned Aerial Vehicles (UAVs) which possess certain essential characteristics, such as small size and cost, Vertical Take Off and Landing (VTOL), performing slow precise movements, and impressive maneuverability, which highlight its potential for use in vital applications. Such applications include; homeland security (e.g. Border patrol and surveillance), military surveillance, service and rescue tasks, and earth sciences (to study climate change, glacier dynamics, and volcanic activity) [21]. However, most research on UAVs has typically been limited to monitoring and surveillance applications where the objectives are limited to “look” and “search” but “do not touch”. Due to their superior mobility, much interest is given to utilize them for aerial manipulation.

Previous research on the quadrotor-based aerial manipulation systems can be divided into three approaches.

In the first approach, a gripper or a tool is installed at the bottom of an UAV to transport a payload or interact with existing structures, see Fig. 1.3. In [7, 22, 23], a quadrotor with a gripper is used for transporting blocks and to build structures. In [24], a force sensor is used to apply a normal force to a surface. Accordingly, not only the attitude of the payload/tool is restricted to the attitude of the UAV, but also the accessible range of the end-effector is confined due to the fixed configuration of the gripper/tool with respect to the UAV body and blades. Consequently, the resulting aerial system has 4 Degrees Of Freedom (DOF); three translational DOF and one rotational DOF (Yaw), i.e., the gripper/tool cannot posses pitch or roll rotation without moving horizontally.

The second approach is to suspend a payload with cables, see Fig. 1.4. In [8], an adaptive controller is presented to avoid swing excitation of a payload. In [25, 26, 27], specific attitude and position of a payload is achieved using cables connected to one or three quadrotors. However, this approach has a drawback that the movement of the payload cannot be always regulated directly because the manipulation is achieved using a cable which cannot always drive the motion of the payload as desired.

To overcome these limitations, a third approach is developed in which an aerial vehicle is equipped with a robotic manipulator that can actively interact with the environment. For example, in [28], a test bed including two 4-DOF robot arms and a crane emulating an aerial robot is proposed. By combining the mobility of the aerial vehicle with the versatility of a robotic manipulator, the utility of mobile manipulation can be maximized. When employing the robotic manipulator, the dynamics of the robotic manipulator are highly coupled with that of the aerial vehicle, which should be carefully considered in the controller design. In addition, an aerial robot needs to tolerate the reaction forces from the interactions with the object or external environment. These reaction forces may affect the stability of an aerial vehicle significantly. Very few reports exist in the literature investigate the combination of aerial vehicle with robotic manipulator. In [9], the authors propose a flying robot that can similarly swoop down and grab objects in the way that an eagle can fish with its natural claws. Their proposed system consists of a quadrotor with a 1-DOF robotic arm, see Fig. 1.5.

Kinematic and dynamic models of the quadrotor combined with arbitrary multi-DOF robot arm are derived using the Euler-Lagrangian formalism in [29]. Based on that, simulation results using the Cartesian impedance control for the combined system with 3-DOF robotic arm is presented in [30]. In addition, the effects of manipulators, two 4-DOF arms, on the quadrotor are simulated based on the dynamic model which considers a quadrotor and robotic arms separately, treating the arms as the disturbance to the quadrotor control loop. In [10], a quadrotor with light-weight manipulators, three 2-DOF arms, are tested, although the movement of the manipulators is not explicitly considered during the design of the PID controller (see Fig. 1.6).

In [31, 11, 32] a manipulator with more than two links were used. However, with these systems, the final payload of the quadrotor is decreased due to the excessive manipulator weight, see Fig. 1.7.

In [12], an aerial manipulation using a quadrotor with a 2-DOF robotic arm is presented but with certain topology that disable the system from making arbitrary position and orientation of the end-effector as shown in Fig. 1.8. In this system, the axes of the manipulator joints are parallel to each other and parallel to one in-plane axis of the quadrotor. Thus, the system cannot achieve orientation around the second in-plane axis of the quadrotor without moving horizontally.

From the above discussion, the systems that use a gripper suffer from the limited allowable DOFs of the end-effector. The other systems have a manipulator with either two DOFs but in certain topology that disables the end-effector to track arbitrary 6-DOF trajectory, or more than two DOFs which decreases greatly the possible payload carried by the system.

In [13, 33], a new aerial manipulation system is proposed that consists of 2-link manipulator, with two revolute joints whose axes are perpendicular to each other and the axis of the first joint is parallel to one in-plane axis of the quadrotor, see Fig. 1.9. Thus, the end-effector is able to reach arbitrary position and orientation without moving horizontally. However, the proof that this system can perform any desired trajectory in the task space has not been achieved yet due to the resulted complications in the inverse kinematics and control. This system is similar to a serial manipulator with eight joints, two of them are passive. In this research, a novel inverse kinematics analysis is presented to prove and test the robot capability to track a 6-DOF task space trajectories. Moreover, controller design is carried out that makes use of the inverse kinematics solution to track arbitrary 6-DOF task space trajectory. Also, experimental implementation of the system is carried out to check its capabilities which is not done before.

1.1.2 Experimental Setup and Measurements

One of the key issues in the aerial manipulation system is achieving the position holding task. To do this task, the accurate measurements and estimation of the system states must be achieved. However, all the current developed systems [9, 34, 35, 12, 36, 37] use a positioning scheme that depends on a high cost 3D Motion Capture System (e.g., VICON system) which will restrict the functions of the aerial manipulation to be within limited space indoors. However, most real applications require to do tasks in arbitrary places indoors and/or outdoors. Therefore, in this work, a proposed solution to this issue is presented and tested that is based on using onboard sensors. The experiments are carried out for point-to-point control using position holding strategy in teleoperation framework rather than trajectory tracking control. Autonomous trajectory tracking control needs high accurate sensing system which is not available.

1.1.3 Control System

The control of the quadrotor manipulation system is quite challenging because it is naturally unstable, and has strong nonlinearities. In addition, it is very susceptible to parameters variations due to adding/releasing the payload and the actuators’ losses. Moreover, it is subjected to external disturbances like wind. Furthermore, its measurements are very noisy due to the thrusters that rotate at very high speed. Also, it has fast dynamics. Therefore, there is a need to design and implement an efficient and low computational cost robust control technique to address these issues.

Furthermore, the actuators (thrusters and manipulator motors) have constraints, which may degrade the performance if it is not considered during controller design. Moreover, it is had to apply an optimal control effort to reduce the required power of the thrusters such that one can save power and thus increase the mission time. Therefore, a Model Predictive Control (MPC) based control technique is proposed to address this issue.

In the motion control of the aerial manipulator, achievement of the compliance control is very important because the compliance control makes possible to perform flexible motion of the manipulator according to a desired impedance. In addition, for the teleoperation purpose, the users may need to sense the force applied at the end-effector. This is very critical demand in applications such as demining and maintenance. Therefore, one have to design a sensorless force estimation to avoid the complicated design resulted from using force sensor and the extra weight to be used in impedance control system and/or teleoperation system.

Since there are high order nonholonomic constraints connecting the variables of the proposed non-redundant system, special treatment is needed to account for controlling the independent and dependent variables.

The control objectives for the proposed robot can be divided into three main targets; robustness, optimal performance, and force estimation & impedance control.

Robustness

Conventionally, robotic manipulators are controlled by using model based control methods such as feedback linearization, which is applied to the proposed system in [13]. However, the conventional design methods are very sensitive to modeling errors which may lead to the degradation of system performance and stability. Model-free control methods have been proposed by using intelligent based control methods; however, they have a problem in real time implementation due to high computational cost. In [33], Feedback Linearization, Direct Fuzzy Logic Control, and Fuzzy Model Reference Learning Control techniques have been applied to the proposed system. In the literature, several robust control methods have been proposed to improve the performance of robotic manipulators.

Disturbance Observer (DOb)-based controller is one of the most popular methods in the field of the robust motion control due to its simplicity and computational efficiency. The motivation for using the DOb is to provide a robustness in front of a significant class of nonlinearities/uncertainties by estimating the nonlinear terms and external disturbances. Thus, one can solve the control problem relying on a set of linearized and decoupled Single Input Single Output (SISO) systems which are not affected by nonlinear/uncertain terms. As a result, a standard linear tracking controller can be used and the resulting control scheme is characterized by a low computational load with respect to the conventional nonlinear robust solutions.

The authors in [38, 39] present the principles of DOb-based control system. In the DOb based robust motion control systems, internal and external disturbances are observed by the DOb, and the robustness is simply achieved by feeding-back the estimated disturbances in an inner-loop. Another controller is designed in an outer loop so that the performance goals are achieved without considering internal and external disturbances. In [40], the DOb-based controller is designed to realize a nominal system which can control acceleration in order to realize fast and precise servo system, even if servo system has parameter variation and suffers from disturbance. In [41, 42, 43, 44, 45, 46, 47], DOb-based control technique has been applied to robotic systems and showed an efficient performance.

However, the previous work [38, 39, 47, 46, 41, 42, 48] in the DOb requires either the acceleration measurement or the velocity estimation. In fact, this is very difficult to be achieved and has limitations due to the available sensors for flying robots. In addition, it is feasible to measure the linear accelerations and angular velocities of the quadrotor via IMU while the angular velocities of the joints can be measured by encoders. Moreover, the estimation of the disturbances based on an estimated data (e.g., velocities), as in the case of the current developed DOb technique, will make the estimation error of these disturbances to be larger. In [49], the external wrench (i.e., external forces and moments) is estimated by using a model-based method for a simple UAV. In addition, the author utilizes the IMU data for the estimation. However, this method needs to know the dynamic model and it neglects some dynamics and nonlinearities. Also, it uses a nonlinear controller and estimates only the external disturbances without estimating the system dynamics. Thus, this technique is not a good choice for the considered aerial manipulator which is a high complex dynamic and kinematic robotic system.

To cope up with these limitations, the conventional DOb is modified to be compatible and feasible with the aerial manipulation system. The conventional DOb is redesigned and reformulated to use the linear acceleration and angular velocities data, which can be measured directly via the onboard IMU and encoders, to estimate the disturbances and nonlinearities.

Optimal Performance

MPC application to robotic systems in a true industrial environment, in which disturbances affect the robotic system and the model of the robot is inevitably inaccurate, is still limited. This is due to the high nonlinearities and uncertainties of dynamics in the robotics system. In [50, 51], a nonlinear MPC is utilized to solve the problem of the control of highly nonlinear systems. In [52], MPC control design with feedback linearization and sliding mode control are utilized to achieve robust trajectory tracking of robotic manipulator. In [53], nonlinear MPC with nonlinear DOb is used to control a robotic system. However, the nonlinear control methods make the MPC to be more complex, has high computational cost, and is based on the knowledge of system model.

Based on the the linearization and robustness capabilities of the DOb, one can rely on the standard linear MPC methodology to design the controller of the outer loop of the DOb-based controller such that the system performance can be adjusted to achieve an objective tracking accuracy and speed. In addition, the actuators’ constraints can be considered with optimal control effort and a low computational load.

Sensorless Robust Force Estimation and Impedance Control

Control of the interaction between a robot manipulator and the environment is crucial for successful execution of a number of practical tasks where the robot end-effector has to manipulate an object or perform some operation on a surface. Impedance control has the ability to impose, along each direction of the task space, a desired dynamic relation between the end-effector pose and the force of interaction with the environment (i.e., desired impedance). The inclusion of force information in the control of robots increases their adaptability to uncertain environments, such as those found in assembly tasks, and provides safety against breakage due to excessive contact forces [54, 55, 56]. Impedance control is still a rather far adopted solution in aerial robotics applications. In [57, 30, 58, 59], an impedance control is designed for aerial manipulator without the need to measure/estimate the contact force. However, in such work, the authors neglect some dynamics as well as external disturbances. In addition, the proposed algorithm is model-based and it does not have a robustness capability. In [60], better results are obtained with feeding back the measured force when implementing impedance control. On the other hand, for the tele-manipulation, the use of force feedback is essential to assist the human operator in the remote handling of objects with a slave manipulator [61]. In addition, it is very critical to get the force feedback to achieve the cooperative grasping among aerial manipulation robots. If the load is heavier than the carrying capacity of a single UAV, multiple UAVs can distribute the load among them and move the object.

However, typically, forces are measured by a force sensor connected to the robot end-effector. This situation is infeasible in aerial robotics since force sensors have a lot of inherent limitations such as narrow bandwidth, sensing noise, and high cost. In addition, it needs complicated arrangements in the aerial manipulator that increase the weight which is a critical issue. Hence, to avoid a force sensor due to their limitations, the contact force has to be estimated. The estimated interaction force will be used in the impedance control algorithm and it can be utilized for tele-manipulation as well as for cooperative grasping for high payload transportation.

Identification of the environment dynamics are more severe when the environment displays sudden changes in its dynamic parameters which cannot be tracked by the identification process. In [62], it is found that in order to faithfully convey to the operator the sense of high frequency chattering of contact between the slave and hard objects, faster identification and structurally modified methods were required. Several techniques are proposed to estimate the contact force/environment dynamics. In [63], the DOb and Recursive Least Squares (RLS) are used to estimate the environment dynamics. However, in this method, two DObs are used besides the RLS, and the estimation of contact force is activated only during the instance of contacting, thus there is a need to detect the instant at which the contact occurs. This is not practical approach especially if one targets an autonomous system. In [64, 65, 66, 67, 68, 69], several techniques are proposed to achieve force control without measuring the force. However, these techniques are based on ignoring some dynamics and external disturbances, which will produce inaccurate force estimation. In [70], a scheme is proposed which allows a quadrotor to perform tracking tasks without a precise knowledge of its dynamics and under the effect of external disturbances and unmodeled aerodynamics. In addition, this scheme can estimate the external generalized forces. However, as the authors claim, this estimator can work perfectly with constant external disturbances. Moreover, the estimated forces contain many different types such as wind, payload, environment impacts, and unmodeled dynamics. Thus, it cannot isolate the end-effector force alone from the others. The authors in [49] present a model-based method to estimate the external wrench of a flying robot. However, this method assumes that there are no modeling errors and no external disturbance. Moreover, it estimates the external force as one unit and it can not distinguish between external disturbance and the end-effector force which is needed to calculate. In addition, it uses a model based control which needs a full knowledge of the model.

Therefore, in this dissertation, another scheme is proposed to overcome these limitations of the currently developed techniques. Firstly, a DOb inner loop is used to estimate both the system nonlinearities and all external forces to compensate for them, as a result, the system acts like a linear decoupled SISO systems. Secondly, a fast tracking RLS algorithm is utilized with the linearization capabilities of DOb to estimate the contact force. Thirdly, a model-free robust impedance control of the quadrotor manipulation system is implemented. The DOb is designed in the quadrotor/joint space while the impedance control is designed in the task space such that the end-effector can track the desired task space trajectories besides applying a specified environment impedance. Thus, a Jacobian based algorithm is proposed to transform the control signal from the task space to the quadrotor/joint space coordinates.

1.2 Objectives of Research

Based on the reasoning in the previous section, this research proposes the control design and implementation of a new quadrotor manipulation system which will have great impacts in the human daily activities as well as other vital and critical applications.

The objectives of this research are clearly described as follows.

1.2.1 Design and Modeling of the Quadrotor Manipulation System

It is aimed to design a quadrotor with a 2-DOF manipulator that has a unique topology to enable the end-effector to track a desired 6-DOF trajectory. It has to provide solutions to limitations found on the currently developed aerial manipulation systems by having maximum mobility with minimum weight.

Kinematic and dynamic modeling of the system are aimed to facilitate designing and simulating the control of the system.

1.2.2 Experimental Setup and State Estimation

The whole system has to be built. An identification experiment is needed to find the actuators’ coefficients. A measurement scheme is implemented to get the vehicle states (i.e., pose and velocity). Furthermore, this measurement scheme has to work in different places indoors and has potential to work outdoors as well.

Real time quadrotor/joint space control is designed, with a formulation that enables the system to work in either teleoperation or autonomous mode for pick and place operation.

An experimental test has to be carried out to demonstrate the feasibility and the effectiveness of the proposed approach with holding/releasing a payload.

1.2.3 Inverse Kinematics

The proposed aerial manipulator has complexity in its inverse kinematics. In fact, inverse kinematics of the proposed system require a solution of complicated algebraic/differential equations to prove/achieve the end-effector ability to track arbitrary 6-DOF trajectories that lay within the capability of the used actuators. Therefore, in this work, a novel inverse kinematics algorithm is proposed, that solves the complex set of nonlinear differential algebraic equations approximately. This was not done before either for redundant or non-redundant aerial manipulator in the literature. After that, its accuracy has to be verified.

1.2.4 Design of a Control Scheme

This robot has several issues in point of view of the control design, which have to be addressed and solved. In this research, a linear DOb-based control technique will be utilized as the base framework for the motion control. The DOb loop is used to enforce robust linear input/output behavior of the plant by canceling disturbances, measurement noise, and plant/model mismatch. The DOb loop has to use the measurements from the onboard sensors in order to reduce the disturbance estimation error.

To optimize the control signal, to save power consumption and thus increase mission time, and to consider the actuators’ constraints, a MPC controller is designed in the external loop to achieve the required closed-loop performance.

Furthermore, the manipulation tasks require estimating (applying) a certain force at the end-effector as well as the accurate positioning of it. Thus, in this research, a robust force estimation and impedance control scheme is proposed. The robustness will be achieved based on the DOb technique. Then, a tracking linear impedance controller will be designed. Unlike the currently developed techniques for force estimation, a technique based on linearization capabilities of DOb and a Fast Tracking Recursive Least Squares (FTRLS) algorithm is proposed, which takes into consideration all the system dynamics with no need for the environment contact indication.

1.3 Thesis Contributions

The added values of this thesis can be listed as following:

•

Developing a new non-redundant 6-DOF quadrotor-based aerial manipulation system that has superior features over the currently developed aerial manipulators.

•

Experimental implementation of the proposed robot with the development of the measurement and estimation scheme to get the full state vector of the platform. This facilitates working within arbitrary places indoors as well as outdoors.

•

Proposing a novel inverse kinematics algorithm to prove the ability of the system to track arbitrary 6-DOF task space trajectories and facilitates designing the controller in the quadrotor/joint space.

•

Development of a control system for the proposed robot as follows:

–

Development of the Disturbance Observer technique to solve the robustness issue of this robot.

–

Development of a robust optimal control based on the MPC and DOb approaches.

–

Development of a robust sensorless force estimation and impedance control via the FTRLS and DOb techniques.

1.4 Thesis Plan

The Thesis is organized in 6 chapters as follows:

Chapter 1 $\textendash$ Introduction. This chapter introduces the background and the objectives of the research. The outline of the thesis is also briefly described in this chapter.

Chapter 2 $\textendash$ System Design and Modeling. This chapter initially describes the proposed flying robot. After that, the design of this system is presented. Then, the kinematic model is described. Finally, the system dynamics are reviewed.

Chapter 3 $\textendash$ System Construction and Experiments. This chapter starts by describing the construction of the experimental system from point of view of both hardware and software. Then, it presents the identification experiment to get the rotors’ aerodynamics parameters. After that, the 6-DOF system states measurements and estimation are presented. A PID controller with gravity compensation is designed and implemented in both simulation and experimental environments. The feasibility and effectiveness of the proposed system are verified by both simulation and experimental results.

Chapter 4 $\textendash$ Inverse Kinematics Solution. This chapter presents a novel inverse kinematics analysis to get the reference quadrotor/joint space trajectories from desired 6-DOF task space trajectories. The accuracy of the proposed algorithm is enlightened via numerical results.

Chapter 5 $\textendash$ Control System Development. This chapter describes the principles of the proposed DOb-based control system with a PD control in the outer loop. After that, the stability analysis of the closed-loop is presented. Then, it presents the application of an optimal robust control based on both DOb and MPC approaches. Moreover, presents the application of a robust sensorless force estimation and impedance control based on both DOb and FTRLS approaches. The effectiveness of the proposed methods are verified by simulation results.

Chapter 6 $\textendash$ Conclusions. This chapter presents the overall conclusions of this research work and describes the future developments of the research.

Chapter 2 SYSTEM DESIGN AND MODELING

This chapter initially describes the proposed flying robot. After that, the design of this system is presented. Then, the kinematic model is described. Finally, the system dynamics are reviewed.

2.1 System Description

A 3D CAD model of the proposed system is shown in Fig. 2.1. The system consists mainly of two parts; the quadrotor and the manipulator.

Fig. 2.2 presents a sketch of the proposed system with the relevant frames, which indicates the unique topology that permits the end-effector to achieve arbitrary 6-DOF trajectory. The frames are assumed to satisfy the Denavit-Hartenberg (DH) convention [71]. The manipulator has two revolute joints. The axis of the first revolute joint ($z_{0}$), which is fixed to the quadrotor, is parallel to the body $x$-axis of the quadrotor (see Fig. 2.2). The axis of the second joint ($z_{1}$) is perpendicular to the axis of the first joint and will be parallel to the body $y$-axis of the quadrotor at home (extended) configuration. Thus, the pitching and rolling rotation of the end-effector is now possible independently from the horizontal motion of the quadrotor. Hence, with this new system, the capability of manipulating objects with arbitrary location and orientation is achieved. By this non-redundant system, the end-effector can achieve 6-DOF motion with minimum number of actuators/links, which is an important factor in flight. The proposed system is distinguished from all other previous systems in the literature by having maximum mobility with minimum weight. The resulted complexity of the inverse kinematics and control are handled latter to prove the capability of the end-effector to track the reference 6-DOF trajectory.

2.2 System Design

2.2.1 Quadrotor

The quadrotor components are selected such that it can carry an additional weight equals $650$ g (larger than the total arm weight and the maximum payload). AscTec pelican quadrotor [1] form Ascending Technologies is used as the quadrotor platform. Fig. 2.3 presents the different views, which are extracted from a SOLIDWORKS model, for the quadrotor platform.

The characteristics of this quadrotor can be listed as following:

•

Lightweight and customizable tower structure that lets one mount diverse payloads (e.g., sensors, avionics, and manipulator).

•

Carbon fiber material with mass = 1.3 kg, so it combines between the rigidity and lightweight.

•

Maximum payload = 650 g. This should include the additional sensors, avionics, manipulator and the payload to be handled.

•

Maximum thrust = 36 N.

•

Maximum airspeed = 16 m/s, and maximum climb rate = 8 m/s.

See Appendix A for more technical details about the AscTec quadrotor.

2.2.2 Manipulator

A lightweight manipulator that can carry a payload of 200 g and has a maximum reach of 22 cm is deigned. Its weight is 200 g. The arm components are selected, purchased and assembled.

The arm components are:

•

Three DC motors; HS-422 (Max torque = 0.4 N.m) for the gripper, HS-5485HB (Max torque = 0.7 N.m) for joint 1, and HS-422 (Max torque = 0.4 N.m) for joint 2 [14].

•

The manipulator structure accessories are; Aluminum Tubing - 1.50 in diameter, Aluminum Multi-Purpose Servo Bracket, Aluminum Tubing Connector Hub, and Aluminum Long ’C’ Servo Bracket with Ball Bearings [14].

•

Lightweight grasping mechanism [14].

See Appendix B for details about the manipulator’s dimensions and design.

The safety of the manipulator design, with respect to the strength and rigidity, is checked through finite element analysis using ANSYS software (see Fig. 2.4).

From these figures, the maximum deflection is about 0.6 mm, which is smaller than the allowable value which equals 1 mm. In addition, the maximum stress of the structure is 113 MPa, which is smaller than the yield strength of aluminum alloy that is 270 MPa. Also, the bearings and gripper are selected to sustain the loads. Therefore, this design is safe.

The design of the proposed system is validated via experimental test as shown in Fig. 2.5.

2.3 Forward Kinematics

In this section, the position and velocity kinematic analysis are presented. Let $\Sigma_{b}$, $O_{b}$- $x_{b}$ $y_{b}$ $z_{b}$, denotes the vehicle body-fixed reference frame with its origin at the quadrotor center of mass, see Fig. 2.2. Its position with respect to the world-fixed inertial reference frame, $\Sigma$, $O$- $x$ $y$ $z$, is given by the vector $p_{b}=[x\ y\ z]^{T}$ $\in\mathbb{R}^{3}$, while its orientation is given by the rotation matrix $R_{b}$ as

$R_{b}=\begin{bmatrix}C_{\psi}C_{\theta}&S_{\phi}S_{\theta}C_{\psi}-S_{\psi}C_{\phi}&S_{\psi}S_{\phi}+C_{\psi}S_{\theta}C_{\phi}\\ S_{\psi}C_{\theta}&C_{\psi}C_{\phi}+S_{\psi}S_{\theta}S_{\phi}&S_{\psi}S_{\theta}C_{\phi}-C_{\psi}S_{\phi}\\ -S_{\theta}&C_{\theta}S_{\phi}&C_{\theta}C_{\phi}\\ \end{bmatrix},$

(2.1)

where $\Phi_{b}$=$[\psi\ \theta\ \phi]^{T}$ $\in\mathbb{R}^{3}$ is the triple $ZYX$ yaw-pitch-roll angles. Note that $C_{.}$ and $S_{.}$ are short notations for $\cos(.)$ and $\sin(.)$, respectively. Let us consider the frame $\Sigma_{e}$, $O_{2}$- $x_{2}$ $y_{2}$ $z_{2}$, attached to the end-effector of the manipulator, see Fig. 2.2. Let us define $\Theta=[\theta_{1}\ \theta_{2}]^{T}$ $\in\mathbb{R}^{2}$ to be the vector of joint angles of the manipulator. Table 2.1 presents the DH parameters for the two link manipulator.

Table 2.1: DH parameters for the manipulator

Link($i$) $d_{m_{i}}$ $a_{m_{i}}$ $\alpha_{m_{i}}$ $\theta_{m_{i}}$

[math] $-l_{0}$ [math] $-\pi/2$ $-\pi/2$

$1$ [math] $l_{1}$ $\pi/2$ $\theta_{1}$

$2$ [math] $l_{2}$ [math] $\theta_{2}$

Transformation from $\Sigma_{e}$ to $\Sigma$ is given by

$A_{e}=A_{b}A_{0}^{b}A_{1}^{0}A_{2}^{1}.$

(2.2)

Transformation from $\Sigma_{e}$ to $\Sigma_{b}$ is given by

$\displaystyle A_{e}^{b}=A_{2}^{b}=A_{0}^{b}A_{1}^{0}A_{2}^{1}$

(2.3)

$\displaystyle\qquad\qquad=\begin{bmatrix}R^{b}_{e}&p^{b}_{eb}\\ O_{1\times 3}&1\end{bmatrix},$

where $O_{1\times 3}$ represents a $1\times 3$ zeros vector, $p^{b}_{eb}$ describes the position of $\Sigma_{e}$ with respect to $\Sigma_{b}$ expressed in $\Sigma_{b}$, and $R^{b}_{e}$ describes the orientation of $\Sigma_{e}$ w.r.t $\Sigma_{b}$. The matrices $A^{b}_{0}$, $A^{0}_{1}$, and $A^{1}_{2}$ can be obtained by

$A^{b}_{0}=\begin{bmatrix}0&0&1&0\\ -1&0&0&0\\ 0&-1&0&-l_{0}\\ 0&0&0&1\\ \end{bmatrix},$

(2.4)

$A^{0}_{1}=\begin{bmatrix}C_{\theta_{1}}&0&S_{\theta_{1}}&l_{1}C_{\theta_{1}}\\ S_{\theta_{1}}&0&-C_{\theta_{1}}&l_{1}S_{\theta_{1}}\\ 0&1&0&0\\ 0&0&0&1\\ \end{bmatrix},$

(2.5)

$A^{1}_{2}=\begin{bmatrix}C_{\theta_{2}}&-S_{\theta_{2}}&0&l_{2}C_{\theta_{2}}\\ S_{\theta_{2}}&C_{\theta_{2}}&0&l_{2}S_{\theta_{2}}\\ 0&0&1&0\\ 0&0&0&1\\ \end{bmatrix}.$

(2.6)

Thus, the position of $\Sigma_{e}$ with respect to (w.r.t) $\Sigma$ is given by

$p_{e}=p_{b}+R_{b}p^{b}_{eb}.$

(2.7)

The orientation of $\Sigma_{e}$ w.r.t $\Sigma$ can be defined by the rotation matrix

$R_{e}=R_{b}R^{b}_{e},$

(2.8)

The forward kinematics problem consists of determining the operational coordinates $\chi_{e}=[x_{e}\ y_{e}\ z_{e}\ \psi_{e}\ \theta_{e}\ \phi_{e}]^{T}$ $\in\mathbb{R}^{6}$, as a function of the quadrotor/joint space coordinates $q=[x\ y\ z\ \psi\ \theta\ \phi\ \theta_{1}\ \theta_{2}]^{T}$ $\in\mathbb{R}^{8}$. For solving the forward kinematics, the inputs are $8$ variables, $q$, and the output are $6$ variables, $\chi_{e}$, obtained from $6$ algebraic equations. The end-effector position can be found from (2.7). Euler angles of the end-effector $\Phi_{e}$ can be computed from the rotation matrix of $R_{e}$, from (2.8), see Appendix C for the coding of the forward kinematics.

In the reset of this section, the velocity kinematic analysis is discussed.

The linear velocity of $\Sigma_{e}$ in the world-fixed frame, $\dot{p}_{e}$, is obtained by the differentiation of (2.7) as

$\dot{p}_{e}=\dot{p}_{b}-Skew(R_{b}p^{b}_{eb})\omega_{b}+R_{b}\dot{p}^{b}_{eb},$

(2.9)

where $Skew(.)$ is the $(3\times 3)$ skew-symmetric matrix operator [71], while $\omega_{b}$ is the angular velocity of the quadrotor expressed in $\Sigma$. The angular velocity $\omega_{e}$ of $\Sigma_{e}$ is expressed as

$\omega_{e}=\omega_{b}+R_{b}\omega^{b}_{eb},$

(2.10)

where $\omega^{b}_{eb}$ is the angular velocity of the end-effector relative to $\Sigma_{b}$ and is expressed in $\Sigma_{b}$.

The generalized velocity of the end-effector with respect to $\Sigma_{b}$, $v^{b}_{eb}=[\dot{p}^{bT}_{eb}\ \omega^{bT}_{eb}]^{T}$ $\in\mathbb{R}^{6}$, can be expressed in terms of the joint velocities $\dot{\Theta}$ via the manipulator Jacobian, $J^{b}_{eb}$, [72] as

$v^{b}_{eb}=J^{b}_{eb}\dot{\Theta},$

(2.11)

where $J^{b}_{eb}$ is given by

$J^{b}_{eb}=\begin{bmatrix}0&l_{2}C_{\theta_{2}}\\ S_{\theta_{1}}(l_{1}+l_{2}C_{\theta_{2}})&l_{2}C_{\theta_{1}}S_{\theta_{2}}\\ -C_{\theta_{1}}(l_{1}+l_{2}C_{\theta_{2}})&l_{2}S_{\theta_{1}}S_{\theta_{2}}\\ 1&0\\ 0&-S_{\theta_{1}}\\ 0&C_{\theta_{1}}\end{bmatrix}.$

(2.12)

From (2.9) and (2.10), the generalized end-effector velocity, $v_{e}=[\dot{p}^{T}_{e}\ \omega^{T}_{e}]^{T}$, can be expressed as

$v_{e}=J_{b}v_{b}+J_{eb}\dot{\Theta},$

(2.13)

where $v_{b}=[\dot{p}^{T}_{b}\ \omega^{T}_{b}]^{T}$, $J_{b}=\begin{bmatrix}I_{3}&-Skew(R_{b}p^{b}_{eb})\\ O_{3}&I_{3}\end{bmatrix},$ $J_{eb}=\begin{bmatrix}R_{b}&O_{3}\\ O_{3}&R_{b}\end{bmatrix}J^{b}_{eb}$,

where $I_{m}$ and $O_{m}$ denote $(m\times m)$ identity and $(m\times m)$ null matrices, respectively. If the attitude of the vehicle is expressed in terms of yaw-pitch-roll angles, then (2.13) will be

[TABLE]

with $\chi_{b}=\begin{bmatrix}p_{b}\\ \Phi_{b}\end{bmatrix},$ $Q_{b}=\begin{bmatrix}I_{3}&O_{3}\\ O_{3}&T_{b}\end{bmatrix},$ where $T_{b}$ describes the transformation matrix between the angular velocity $\omega_{b}$ and the time derivative of Euler angles $\dot{\Phi}_{b}$, and it is given as

[TABLE]

Thus, the system Jacobian is

[TABLE]

where $\dot{q}=[\dot{\chi}_{b}^{T}\ \dot{\Theta}^{T}]^{T}$, and $J=[J_{b}Q_{b}\ J_{eb}]$ and it is given as

[TABLE]

with

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

See Appendix D for the programming of the system Jacobian. Since the quadrotor is an under-actuated system, i.e., only $4$ independent control inputs are available for the 6-DOF system, the position and the yaw angle are usually the controlled variables. Hence, it is worth to define the independent coordinate, $\zeta=[x\ y\ z\ \psi\ \theta_{1}\ \theta_{2}]^{T}$, as the controlled variables, and the dependent coordinates, $\sigma_{b}=[\theta\ \phi]^{T}$, as the intermediate variables. Hence, it is worth rewriting the vector $\chi_{b}$ as $\chi_{b}=\begin{bmatrix}\eta_{b}\\ \sigma_{b}\end{bmatrix},$ $\eta_{b}=\begin{bmatrix}p_{b}\\ \psi\end{bmatrix},$ $\sigma_{b}=\begin{bmatrix}\theta\\ \phi\end{bmatrix}.$ Thus, the differential kinematics (2.14) will be

[TABLE]

where $\zeta=[\eta_{b}^{T}\ \Theta^{T}]^{T}$ is the vector of the controlled variables, $J_{\eta}$ is composed by the first 4 columns of $J_{b}Q_{b}$, $J_{\sigma}$ is composed by the last 2 columns of $J_{b}Q_{b}$, and $J_{\zeta}=[J_{\eta}\ J_{eb}]$. If the end-effector orientation is expressed via a triple of Euler angles, $ZYX$, $\Phi_{e}$, the differential kinematics (2.33) can be rewritten in terms of the vector $\dot{\chi}_{e}=[\dot{p}^{T}_{e},\dot{\Phi}_{e}^{T}]^{T}$ as follows

[TABLE]

where $Q_{e}$ is the same as $Q_{b}$ but it is a function of $\Phi_{e}$ instead of $\Phi_{b}$.

2.4 Dynamic Model

For the manipulator dynamics, Recursive Newton Euler method [72] is used to derive the equations of motion. Since the quadrotor is considered to be the base of the manipulator, the initial linear and angular velocities and accelerations, used in Newton Euler algorithm, are that of the quadrotor expressed in body frame. Applying the Newton Euler algorithm to the manipulator considering that the link (with length $l_{0}$) that is fixed to the quadrotor is the base link, manipulator’s equations of motion can be obtained, in addition to, the forces and moments, from manipulator, that affect the quadrotor.

For the system structure, we assume:

Assumption 2.1**.**

The quadrotor body is rigid and symmetric. The manipulator links are rigid.

For each link $i$, ($i$ = $0,1,2$), let us define

• •

$\omega_{i}^{i}$ is the angular velocity of frame $i$ expressed in frame $i$.

• •

$\dot{\omega}_{i}^{i}$ is the angular acceleration of frame $i$ expressed in frame $i$.

• •

$v_{i}^{i}$ is the linear velocity of the origin of frame $i$ expressed in frame $i$.

• •

$\dot{v}^{i}_{c_{i}}$ is the linear acceleration of the center of mass of link $i$ expressed in frame $i$.

• •

$\dot{v}_{i}^{i}$ is the linear acceleration of the origin of frame $i$ expressed in frame $i$.

• •

$r_{i}^{i}$ is the position vector from the origin of frame $i-1$ to the origin of link $i$.

• •

$r_{c_{i}}^{i}$ is the position vector from the origin of frame $i-1$ to the center of mass of link $i$.

• •

$g$ is the vector of gravity expressed in $\Sigma$.

• •

$z_{i-1}^{i-1}$ is a unit vector pointing along the $i^{th}$ joint axis and expressed in the $(i-1)^{th}$ link coordinate system.

• •

$R_{i}^{i-1}$ is the rotation matrix from frame $i$ to frame $i-1$.

• •

$I_{i}^{i}=I_{i}\begin{bmatrix}0&0&0\\ 0&1&0\\ 0&0&1\end{bmatrix}$ is the inertia matrix of link $i$ about its center of mass coordinate frame.

• •

$f_{i,i-1}^{i}$ / $n_{i,i-1}^{i}$ are the resulting force/moment exerted on link $i$ by link $i-1$ at point $O_{i-1}$.

• •

$R^{i}$ is the rotation matrix from frame $\Sigma$ to frame $i$.

Applying a payload of mass equal to $m_{p}$ at the end-effector will change link 2’s parameters such as mass moments of inertia, total mass, and center of gravity as

[TABLE]

[TABLE]

[TABLE]

where $CG_{2}$ is the point of center of gravity of link 2, ${d}_{CG_{2}}=l_{2}/2$, and $(\grave{.})$ refers to the value of the parameter after adding the payload. Changing the point of center of gravity of link 2 will make the $r_{c_{2}}^{2}$ to be

[TABLE]

For the link [math], one calculates the following:

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

For link 1, one can calculate the following:

[TABLE]

[TABLE]

[TABLE]

[TABLE]

For link 2, one can calculate the following:

[TABLE]

[TABLE]

[TABLE]

[TABLE]

The inertial forces and moments acting on link $i$ are given by

[TABLE]

[TABLE]

The total forces and moments acting on link $i$ are given by

[TABLE]

[TABLE]

[TABLE]

[TABLE]

where

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

The gravity vector expressed in frame $i$ is

[TABLE]

where

[TABLE]

[TABLE]

and

[TABLE]

where $g_{r}$ is the gravity acceleration.

The torques acting on joints 1 and 2 are finally given by

[TABLE]

[TABLE]

where $b_{1}$ and $b_{2}$ are the friction coefficients.

The interaction forces and moments of the manipulator acting on the quadrotor expressed in $\Sigma_{b}$, $F_{m,q}^{b}$ and $M_{m,q}^{b}$ are given as

[TABLE]

[TABLE]

The interaction forces expressed in the inertial frame can be obtained by

[TABLE]

The equations of motion of the manipulator can be reformulated as

[TABLE]

[TABLE]

where $M_{1}(q)$, $M_{2}(q)$, $N_{1}(q,\dot{q},\ddot{q})$, and $N_{2}(q,\dot{q},\ddot{q})$ are nonlinear terms, and they are functions of the system states.

The Newton Euler method is used to find the equations of motion of the quadrotor after adding the forces/moments from the manipulator. They are given by (2.82-2.87).

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

where $F_{m,q_{x}}$, $F_{m,q_{y}}$, and $F_{m,q_{z}}$ are the interaction forces resulted from the manipulator and affected the quadrotor in $x$, $y$, and $z$ directions expressed in the inertial frame and $M_{m,q_{\phi}}^{b}$, $M_{m,q_{\theta}}^{b}$, and $M_{m,q_{\psi}}^{b}$ are the interaction moments from the manipulator to the quadrotor around $x_{b}$, $y_{b}$, and $z_{b}$ directions.

The variables in (2.82-2.87) are defined as follows: $m$ is the mass of the quadrotor. Each rotor $j$ has angular velocity $\Omega_{j}$ and it produces thrust force $F_{j}$ and drag moment $M_{j}$ which are given by

[TABLE]

[TABLE]

where $K_{f_{j}}$ and $K_{m_{j}}$ are the thrust and drag coefficients, respectively.

$T$ is the total thrust applied to the quadrotor from all four rotors, and it is given by

[TABLE]

$\tau_{a_{1}}$, $\tau_{a_{2}}$, and $\tau_{a_{3}}$ are the three input moments about the three body axes, $x_{b}$, $y_{b}$, $z_{b}$, and they are given as

[TABLE]

[TABLE]

[TABLE]

$d_{q}$ is the distance between the quadrotor center of mass and rotor rotational axis. $\overline{\Omega}$ is given by

[TABLE]

$I_{r}$ is the rotor inertia. $I_{f}$ is the inertia matrix of the vehicle around its body-frame , and it is given as

[TABLE]

The dynamical model of the quadrotor-manipulator system can be rewritten in a matrix form as follows

[TABLE]

where $M$ $\in\mathbb{R}^{8\times 8}$ represents the symmetric and positive definite inertia matrix of the combined system, $C$ $\in\mathbb{R}^{8\times 8}$ is the matrix of Coriolis and centrifugal terms, $G$ $\in\mathbb{R}^{8}$ is the vector of gravity terms, $d_{ex}$ $\in\mathbb{R}^{8}$ is the vector of external disturbances, $\tau$ $\in\mathbb{R}^{8}$ is the vector of the input generalized forces, $u=[F_{1}\ F_{2}\ F_{3}\ F_{4}\ \tau_{m_{1}}\ \tau_{m_{2}}]^{T}$ $\in\mathbb{R}^{6}$ is vector of the actuator inputs, and $B_{m}=H_{m}(q)\ N_{m}(K_{f_{j}},K_{m_{j}},d_{q})$ $\in\mathbb{R}^{8\times 6}$ is the input matrix which is used to generate the body forces and moments from the actuator inputs. The control matrix, $N_{m}$ $\in\mathbb{R}^{8\times 6}$, is given by

[TABLE]

where $\gamma_{j}=K_{m_{j}}/K_{f_{j}}$. $K_{{\tau}_{1}}$ and $K_{{\tau}_{2}}$ are coefficients that are used to emulate the occurring of faults in the motor’s constants of joints $1$ and $2$, respectively. $H_{m}$ $\in\mathbb{R}^{8\times 8}$ is a matrix that transforms the body input forces to be expressed in $\Sigma$ and is given by

[TABLE]

From the equations of the translation dynamics part of (2.82-2.84), one can extract the expressions of the second order nonholonomic constraints as

[TABLE]

[TABLE]

where $\ddot{x}_{f}$ = $\ddot{x}-\frac{F_{m,q_{x}}}{m}$, $\ddot{y}_{f}$ = $\ddot{y}-\frac{F_{m,q_{y}}}{m}$, and $\ddot{z}_{f}$ = $\ddot{z}+g_{r}-\frac{F_{m,q_{z}}}{m}$.

It is to be noted that the force terms in the above equations are also function of the system states and their derivatives. Equations (2.99) and (2.100) can be solved for the desired trajectories of $\phi$ and $\theta$ through substituting by the desired trajectories of the other variables. These nonholonomic constrains will be utilized later to solve both the inverse kinematics and control problems.

The next step is to build the experimental system to test the system feasibility.

Chapter 3 SYSTEM CONSTRUCTION AND EXPERIMENTS

In this chapter, the whole system is built and its parameters are identified. Furthermore, experimental setup with a solution to the problem of the state estimation of the quadrotor manipulation system is introduced taking into consideration the position of the manipulator with respect to the sensors. This solution is based on a combination of several sensors and data fusion. Quadrotor/joint space control is designed, with a formulation which enables the system to work in either teleoperation or autonomous mode, based on PID with gravity compensation technique. After that, this controller is implemented in both simulation and real time environments. An experimental test is implemented to demonstrate the feasibility and the effectiveness of the proposed system in the presence of holding and releasing a payload.

3.1 Hardware

Fig. 3.1 shows the proposed experimental implementation of the whole connected system with the user interface and measurement system.

The quadrotor is equipped with $4$ rotors with $10$ inch diameter, which allow to carry an additional weight of about $650$ g. Depending on the battery size and payload, flight times between $10$ and $20$ minutes can be achieved. In addition, the quadrotor has a Flight Control Unit (FCU) “AscTec Autopilot” as well as a suitable structure enabling one to easily mount different payloads like computer boards, onboard sensors, and the robotic arm with its avionics. The FCU has an Inertial Measurement Unit (IMU) as well as two $32$-Bit $60$ MHz ARM-7 microcontrollers used for data fusion and flight control. One of these microcontrollers, the Low Level Processor (LLP), is responsible for the hardware communication, an emergency controller, and IMU sensor data fusion. An attitude and GPS-based position controller is implemented also on this processor. The other microcontroller, High Level Processor (HLP), is dedicated for custom code. All relevant and fused IMU data is provided at an update rate of $1$ kHz via a high speed serial interface. In particular, this comprises body accelerations, body angular velocities, magnetic compass, height measurement by an air pressure sensor, and the estimated attitude of the vehicle. For the onboard expensive computations, a $1.6$ GHz Intel Atom-based embedded computer is used. This computer is equipped with 1 GB RAM, a Micro SD card slot for the operating system, a 802.11n based miniPCI Express WiFi card. See Appendix A for more technical details.

State estimation is one of the key issue to implement quadrotor manipulation system. The available onboard sensor, IMU can only provide the quadrotor’s orientation, angular rates, and linear Accelerations. However, one can not depend on the accelerations to get linear position due to large integration drift for long term motion. Thus, there is a need to an external positioning system which may be GPS, 3D motion capture system, onboard camera, or range sensors (laser or sonar). However, GPS is not a reliable service (accuracy 3 $\textendash$ 15 m) as its availability can be limited by urban canyons and is completely unavailable in indoor environments. Onboard camera is tried but the available one (Web CAM) has low resolutions and gives bad results. As a result, finally, the range sensors are used.

For measuring the horizontal position, $x$ and $y$, an Hokuyo URG-04LX Laser Range Finder is used [2], see Fig. 3.2. It is a 2D laser range finder with a range of $4$ m and a field of view of $240$ degrees. It has an update rate of $30$ Hz with resolution of 1 mm. It has weight of 160 g. It is connected to the onboard computer through USB connection such that the high computation processing of laser data can be carried out onboard. This sensor is placed on the top center of the quadrotor. See Appendix E for more technical details.

The vertical position, $z$, is measured using Ultrasonic Ranging module SRF04 [4], see Fig. 3.2. It is a 1D sonar range finder with a range of $3$ cm to $3$ m and a resolution of $3$ cm. It has an update rate of $40$ Hz. It is mounted downward under one of the rotors such that it is not affected by the movement of the manipulator, so one can avoid the sensor misreadings. See Appendix F for more technical details. The data from this sensor is acquired and processed by using an Arduino board [15] which is connected to the onboard computer by USB connection.

The 2-link manipulator’s components are: Three DC motors; HS-422 (Max torque = $0.4$ N.m) for the gripper, HS-5485HB (Max torque = $0.7$ N.m) for joint $1$, and HS-422 (Max torque = $0.4$ N.m) for joint $2$. Motor’s Driver (SSC32) which is used as an interface between the main control unit and the motors. Wireless Play Station 2 (WPS2) R/C is used to send commands to manipulator’s motors remotely. The manipulator structure components are; Aluminum Tubing - $1.50$ in diameter, Aluminum Multi-Purpose Servo Bracket, Aluminum Tubing Connector Hub, and Aluminum Long “C” Servo Bracket with Ball Bearings. The Arduino board (Mega 2560) is utilized as an interface between the low level peripherals (such as ultrasonic sensor, WPS2 receiver, and motor driver (SSC32)), and the onboard computer. See Appendix B for more technical details.

The Power management system is setup as following: A $11.1$ V Li-Po battery is used to power the motors and the electronics on board. A voltage regulator circuit is used to convert this voltage to $5$ V DC to power the manipulator motors and its avionics. The Hokuyo sensor, Ultrasonic sensor, and the Arduino board are powered by the $5$ V from the USB port of the onboard computer. A low voltage detection is implemented in the quadrotor making it lands once the battery voltage drops below $8$ V. See Appendix G for power connections.

The ground station consists of a computer, joystick, and Futuba R/C. It allows a user to tele-operate the system such as visualization, sending commands, and emergency recovery.

In this work, a position controller and the position data fusion algorithms are implemented on the HLP, based on the Hokuyo and Ultrasonic sensors input from the onboard computer and the inertial data provided by the LLP. On the LLP, the attitude controller is used as inner loop.

3.2 Software

An Ubuntu Linux 12.04, with Robot Operating System (ROS) framework (fuerte version) [73], is installed on both the onboard computer and the ground station computer.

Since multiple different computation subsystems (PC, Onboard computer, autopilot board, and Arduino board) are used, which need to communicate among each other, the ROS is used as a middle-ware, see Appendix H for more details about ROS. This also enable us to communicate with the ground station over the WiFi data link for monitoring and control purposes. In addition, the ROS framework has the essential drivers and software for processing the data from the Hokuyo sensor to find the horizontal position.

The algorithm on the HLP is implemented based on a Software Development Kit (SDK) that provides all the communication routines to allow us to send/receive the low level commands/measurements to/from the LLP. This algorithm is used to implement the data fusion algorithm and the position control. A C++ software (i.e., packages and nodes) is developed under ROS to manage and process data among the the computers systems.

A program is implemented on Arduino board based on a C++ Arduino IDE to acquire and process data from the sonar to get the vertical position. In addition, it receives the manipulator’s commands from the WPS2 and send the control signal to the motor drivers.

3.3 Identification

The aim of this section is to find the real parameters of the system to be used in the control design and to emulate a quite realistic setup during the simulation study. The identified parameters include the structure parameters (e.g., mass, geometrical parameters, and mass moment of inertia) and rotor parameters (i.e., $K_{f_{j}}$ and $K_{m_{j}}$). To calculate the structure parameters, a 3D CAD model is developed using SOLIDWORKS. To estimate the rotor parameters, an experimental setup is carried out, see Fig. 3.3.

In this experiment, the rotor is mounted on a $6$-DOF torque/force sensor (see Appendix I for more technical details) that is connected to a NI Data Acquisition Card (NI DAC). Then, the DAC is connected to a PC, running SIMULINK program as an interface, to read data from DAC. The velocity of rotor is changed gradually, and in each time, the generated thrust and drag moment is measured and recorded using SIMULINK program. By using MATLAB Curve Fitting toolbox the acquired data of thrust and moment is fitted to be in the form of (2.88 and 2.89). Thus, the thrust and moment coefficients can be obtained.

Figure 3.4 (3.5) shows the relationship between generated thrust force (drag moment) and the squared rotor speed.

To measure the goodness of data fitness, the Root Mean Squared Error (RMSE) of the curve fitting is calculated, which is 0.3378 for the thrust relation and 0.003742 for the moment relation.

The identified parameters are collected in Table 3.1. It is noted that any error in the estimated parameters will be compensated by using a robust control technique.

3.4 6-DOF State Estimation

Position holding is one of the most important factors to achieve the accurate aerial manipulation. The accurate measurements are crucial for implementing the position holding. In this section, the proposed state measurement/estimation scheme, which will be used to find the system states, is described. The state estimation system is presented in details in Fig. 3.6.

On the LLP, there is a data fusion algorithm to find the quadrotor orientation at rate of 1 kHz. In addition, the IMU itself can provide the angular rates directly. Thus, both the angular position and velocity of the quadrotor are available. The position measurements are obtained by using the laser and ultrasonic sensors. Since this process is slow (about $30$ Hz) compared to the motion of the quadrotor (quadrotor dynamics have high bandwidth 1kHz), this information is fused with inertial sensor data (body accelerations $\ddot{p}_{b}^{b}$) provided by the IMU at a rate of $1$ kHz. The outputs of that filter are finally fed into a position controller. The expensive computation processing of the laser data is run on the onboard computer at approximately $30$ Hz, while the fusion filter and the position controller are executed on the HLP at $1$ kHz. Note that, this ensures the minimum possible delays, and allows us to handle the fast movements of the quadrotor.

The horizontal position from the laser range finder sensor can be obtained as following.

First, the laser scan is projected orthogonally onto the horizontal $xy$-plane by using the roll and pitch angles of the last state. The projected data is then passed to a scan matcher [74]. The scan matcher uses the yaw angle reading from the IMU as an initial guess for the yaw angle of the quadrotor. The output of the scan matcher includes $x$, $y$, and yaw angle pose components.

The vertical position from the ultrasonic sensor is obtained as following. The ultrasonic sensor is connected to the Arduino board. The sonar signal is transmitted and the echo of it is received. By measuring the time of signal traveling, one can calculate the distance ($s_{z}$) that is the vertical height of the sensor. However, the calculated distance must be compensated due to the roll and pitch rotations of quadrotor by

[TABLE]

Therefore, the measured horizontal position and vertical position (at rate of $30$ Hz), and the angles and linear accelerations (at rate of $1$ kHz) are available now. However, the $6$-DOF measurements rate are required to be high enough to enable a high update rate in the position control loop to match the quadrotor’s system dynamics. Thus, these measurements must be fused to achieve this high rate. The filter is designed and decoupled for all the three axes $x$, $y$, $z$ in the world inertial frame. The body-fixed accelerations, $\ddot{p}_{b}^{b}$, are rotated in the global frame by the rotational matrix, $R_{b}$, based on the attitude angles provided by the LLP, as

[TABLE]

where $g_{r}$ is the acceleration due to the gravity. The filter is based on a Luenberger observer [75]. In the following, only the filter for the $x$-axis is described and and thus it can be applied for the other axes accordingly. The filter uses the position, $p_{b,x}$, speed, $\dot{p}_{b,x}$, and the acceleration sensor bias, $b_{x}$, as the states. The acceleration expressed in the world frame is the system input. The measurement is the position sensors’ readings, $p_{s,x}$, from the Laser (for $x$ and $y$) or Sonar (for $z$).

Let us define the state variable as $X_{obs}=[p_{b,x}\ \dot{p}_{b,x}\ b_{x}]^{T}$, $U_{obs}=\ddot{p}_{x}$, and $Y_{obs}=p_{s,x}$. Then, the observer state equations can be written as

[TABLE]

where $A_{obs}=\begin{bmatrix}0&1&0\\ 0&0&1\\ 0&0&0\end{bmatrix}$, $B_{obs}=\begin{bmatrix}0\\ 1\\ 0\end{bmatrix}$, $C_{obs}=\begin{bmatrix}1&0&0\end{bmatrix}$, and $L_{obs}=\begin{bmatrix}L_{obs_{1}}\\ L_{obs_{2}}\\ L_{obs_{3}}\end{bmatrix}$.

The output of this position filter is the linear position and velocity of the vehicle at update rate of $1$ kHz.

Unlike the currently developed state measurement system for aerial manipulation system, with this proposed scheme, one can measure/estimate the system states in different places indoors. In addition, one can utilize this scheme for being used outdoors by using GPS-based position holding for wide operation (quadrotor is far from the target) and using these sensors when the quadrotor is near the objects. If a high performance onboard camera is available, then the data from the laser can be fused with the camera data to obtain more accurate posing indoors and outdoors.

3.5 Control Design for the Experiments

In this section, the real time control system is presented. Fig. 3.7 shows the control structure.

Since the quadrotor is an under-actuated system, i.e., only $4$ independent control inputs are available against the $6$ DOF, the position and the yaw angle are usually the controlled variables, while the pitch and roll angles are used as intermediate control inputs for horizontal position control. For position control, a cascade structure is used. As inner loop, the well tested attitude loop provided by the LLP of the FCU is used. The outer loop is the position loop, and is implemented on the HLP based on the concept of PID control with gravity compensation as following

[TABLE]

where $G_{p}$ is the gravity term of the translational part in the dynamic equation of motion (2.96) and it can be obtained as $G(1\mathrel{\mathop{\mathchar 58\relax}}3)$. $K_{p}$, $K_{d}$, and $K_{i}$ are PID tuning parameters, and $p_{b_{r}}$ and $\dot{p}_{b_{r}}$ are the reference position and velocity, respectively.

Roll and pitch commands are generated from the outer loop as reference to the inner attitude controller in order to reach and maintain desired $y$ and $x$ position, respectively. Thrust commands are generated by the outer controller to control the height by sending desired thrust values. The desired values for the intermediate controller, $\theta_{r}$ and $\phi_{r}$, are obtained from the output of position controller through the following relation

[TABLE]

This relation is derived from (2.1) based on the small angle approximation of the roll and pitch angles.

Since it is aimed to teleoperate the system, as a first step to autonomous mode, the manipulation task can be achieved by sending velocity commands to the controller. Then, for position holding, one will send a zero velocity command and make the current position as the desired position (at the instant of sending zero command velocity). The velocity commands can be sent using the joystick, where the quadrotor does not need to reach a target position but just follows the desired velocity, allowing a smooth flight. Moreover, this control scheme can be utilized to send position commands to work in autonomous mode.

For the manipulator joints’ control, the built-in position sensor with a PID controller is used. Since the dynamics of the manipulator is simple, one use this built-in controller as a first test. If it does not provide required performance, design another advanced controller will be designed. The simplicity in the manipulator dynamics comes from the lightweight and slow motion of the manipulator parts.

The desired position can be obtained from the command velocity and measured position as in algorithm 1.

3.6 Simulation Results

This proposed control strategy is applied in simulation MATLAB/SIMULINK program to the model of the considered aerial manipulation robot. In order to make the simulation environment to be quite realistic, a normally distributed measurement noise, with mean of $10^{-3}$ and standard deviation of $5\times 10^{-3}$, has been added to the measured signals. In addition, one can use the parameter obtained from the experimental identification that is presented previously. The controller parameters are presented in Table 3.2. These parameters are tuned to get a satisfactory response.

The simulation results are presented in Fig. 3.8. In this figure, the target position is assumed to be in the direction of the $y$-axis of quadrotor, and the object (with weight of $50$ g) is attached to the end-effector gripper before starting the operation. During the vehicle taking off, by sending a positive velocity command in the $z$ direction until it is reached at suitable height, see Fig. 3.8c, a very small drift in some coordinates occurs, see Fig. 3.8, which is then quickly compensated. At this moment, a zero velocity command is given such that the vehicle is hold in its initial position due to the position holding control. To move towards the target position, in which one have to place the carried object, a positive velocity command is sent in the $y$ direction, see Fig. 3.8b. As soon as the target position is reached, a zero $y$ velocity and a negative $z$ velocity are sent to put the end-effector near to the destination point and then a zero $z$ velocity is given to hold the vehicle at this point, and a command is sent to open the gripper to release the object. After object releasing, a negative velocity is sent in the $y$ direction to go back to the starting point again. Once it is reached at this point, a zero velocities are sent to all coordinates to finish the task. Moreover, a zero velocity command to the manipulator joints is always sent , see Figs. 3.8e and 3.8f. These results show the feasibility and efficiency of the proposed system. These results will be verified experimentally in the next section.

3.7 Experimental Results

In this section, experimental results of the proposed system are presented and discussed. The objective is to transfer an object in a teleoperation mode. For the teleoperation purpose, it is assumed that one cannot know the accurate coordinates of the target position before operation. Thus, velocity commands will be sent to move the system to that position. The same scenario that done in the simulation study will be implemented except that in some situations a velocity command in the $x$-direction will be sent because it is difficult to determine the target place accurately along $y$-axis as assumed in the simulation test. Then, if the target position is reached, the position holding is activated to place the object in the target position. The target object, to be transfered to another place, has a weight of $50$ g which will be attached to the gripper at the start of system operation. Experimental controller parameters are given in Table 3.3. Fig. 3.9 presents photos from the experiment that is divided into six phases; starts from taking off (by given velocity command in the $z$ direction) with the object attached to the gripper, moving to the destination by giving velocity command in the $y$ direction and fine adjustment in the $x$ direction, then reaching at the target position, releasing the object by achieving position holding by giving zero velocity commands, moving towards the home position, and finally, reaching the home position. The real experimental data is recored in the ground station through the ROS framework and the WiFi connection. Fig. 3.10 presents the experimental results. In this figure, the velocity commands as well as the actual position for the coordinates $x$, $y$, $z$, $\psi$, $\theta_{1}$, $\theta_{2}$ are presented. The manipulator joints commands are sent using WPS2 controller assuming that the built-in controller is good enough to track accurately the sent commands. To start the mission, see Fig. 3.10c, a positive $z$ velocity is sent to take off, then position holding is activated for small period by sending a zero $z$ velocity, after that the platform is commanded to go to a suitable height at lower level by sending negative $z$ velocity until the target height is reached, then one can activate the position holding again. During the vehicle taking off, a small drift occurs in the $x$, $y$, $z$, and $\psi$ directions which is quickly recovered due to the position holding at the starting point. Fig. 3.10a shows that one have to send velocity commands in $x$ direction, as it is claimed before, to fine tune the vehicle position towards the target place accurately. After taking off, the vehicle moves in $y$ direction towards the target place, see Fig. 3.10b, till reaching the target point at which the position holding is activated. At this point, a command is sent to the gripper to open and release the object. After releasing the object, the vehicle returns back to the starting point, then a negative $z$ velocity is sent until the platform becomes at height suitable to the base station. In Fig. 3.10d, as it is planed, there is no any command in $\psi$ direction, and consequently there is almost no rotation around $z$. Since, the required task is simple, and there is no need to move the manipulator joints (the manipulator needs to be stretched, i.e., $\theta_{1}$ and $\theta_{2}$ = $pi/2$), zero velocity commands are sent to the manipulator joints, see Figs. 3.10e and 3.10f. However, for sophisticated tasks, one can command the joints to do the $6$-DOF motion based on the specified task. These results prove the feasibility and a satisfactory efficiency of the proposed system, state estimation scheme, and the control algorithm. However, they show that the control algorithm for position control needs improvements in the future work by using robust estimation control techniques. Moreover, the sonar can be replaced by more accurate sensor.

Chapter 4 INVERSE KINEMATICS SOLUTION

All the previous research in the aerial manipulation area do not provide solution to the system inverse kinematics. However, in the motion planning of such system, the task is specified in terms of a desired trajectory for the end-effector. To achieve this task, the system inverse kinematics must be available to get the reference quadrotor/joint space trajectories from desired 6-DOF task space trajectories. Then, a controller is designed to track the generated trajectories from the inverse kinematics. Therefore, in this chapter a novel inverse kinematics analysis is presented to get the reference quadrotor/joint space trajectories from desired 6-DOF task space trajectories, in addition to prove that the end-effector of the proposed robot can achieve arbitrary 6-DOF trajectory tracking. The trajectory inverse kinematics of our proposed non-redundant system is difficult because of the existence of high order nonholonomic constraints. Eight complex algebraic differential equations are needed to be solved. However, obtaining exact solution of these equations is impractical. Thus, an algorithm is developed to get approximate efficient solution of these equations. The effectiveness of the proposed algorithm is validated and enlightened via numerical results.

4.1 Novel 6-DOF Trajectory Inverse Kinematics

A task for the quadrotor manipulation system is usually specified in terms of a desired trajectory for the end-effector pose, $\chi_{e,r}(t)$ ( position, $p_{e,r}(t)$, and orientation, $\Phi_{e,r}(t)$). In this section, six algebraic kinematic equations relating $\chi_{e}$ with $q$ are derived.

For the desired task space trajectories, one can assume:

Assumption 4.1**.**

The desired trajectories for the end-effector are bounded.

To find the eight variables of $q$ from the given six variables of $\chi_{e}$, two additional equations are required. These two equations are the nonholonomic constraints (2.99 and 2.100), which are differential equations.

We propose algorithm 2 to get a solution to these eight algebraic/differential equations, see Fig. 4.1.

The validation of this algorithm will be checked at the end of this section. The six algebraic inverse kinematic equations are derived next.

Define the general form for the rotation matrix $R_{e}$ as a function of the end-effector variables $\chi_{e}$, as

[TABLE]

where the elements of $R_{e}$ are given as

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

According to the structure of $R_{e}$ from (2.8), the inverse orientation is carried out first followed by inverse position.

Let us define the end-effector orientation, $R_{e,\Phi_{e}}$, as function of the Euler angles, $\Phi_{e}=[\psi_{e}\ \theta_{e}\ \phi_{e}]^{T}$, as

[TABLE]

Equating (4.1) and (4.11) provides the inverse orientation as following. The inverse orientation has three cases based on the value of $\theta_{1}$ which can be calculated from the element $r_{33}$ as

[TABLE]

By rearranging (4.12) and solving the resulting equation for $\theta_{1}$, then

[TABLE]

where $a_{1}=C_{\phi}C_{\theta}$ and $b_{1}=-S_{\phi}C_{\theta}$.

Based on the value of $\theta_{1}$, three cases for the rotational inverse kinematics solution exist.

CASE 1: $\theta_{1}$ $\neq$ [math] and $\theta_{1}$ $\neq$ $\pi$

Inspecting $r_{13}$ and $r_{23}$, one can find value of $\psi$ as

[TABLE]

[TABLE]

where $a_{2}=C_{\phi}S_{\theta_{1}}+S_{\phi}C_{\theta_{1}}$, $b_{2}=C_{\phi}S_{\theta}C_{\theta_{1}}-S_{\phi}S_{\theta}S_{\theta_{1}}$, $c_{2}=b_{2}$, and $d_{2}=-a_{2}$.

Solving (4.14) and (4.15), will give

[TABLE]

[TABLE]

[TABLE]

By inspecting $r_{32}$ and $r_{31}$, $\theta_{2}$ can be found as

[TABLE]

[TABLE]

where $a_{3}=C_{\phi}C_{\theta}S_{\theta_{1}}+C_{\theta}S_{\phi}C_{\theta_{1}}$, $b_{3}=-S_{\theta}$, $c_{3}=b_{3}$, and $d_{3}=-a_{3}$.

Solving (4.19) and (4.20), then

[TABLE]

[TABLE]

[TABLE]

CASE 2: $\theta_{1}=0$

If $\theta_{1}=0$, then the sum $\theta_{2}+\psi$ can be determined. One can assume any value for $\psi$ and get $\theta_{2}$. Therefore, there are infinity of solutions. By putting $\psi$ = [math], the value of $\theta_{2}$ can be determined as follows

Inspecting $r_{11}$ and $r_{12}$, $\theta_{2}$ can be found as

[TABLE]

[TABLE]

where $a_{4}=C_{\theta}$, $b_{4}=-S_{\phi}S_{\theta}$, $c_{4}=-b_{4}$, and $d_{4}=a_{4}$.

Solving (4.24) and (4.25), then

[TABLE]

[TABLE]

[TABLE]

CASE 3: $\theta_{1}=\pi$

Since $S_{\theta_{1}}$ = [math], this is similar to case $2$. However, $\theta_{2}-\psi$ can be determined and by choosing $\psi$ = [math], an expression for $\theta_{2}$ can be determined as follows:

Inspecting $r_{11}$ and $r_{12}$, $\theta_{2}$ can be found as

[TABLE]

[TABLE]

where $a_{5}=a_{4}$, $b_{5}=-b_{4}$, $c_{5}=-b_{5}$, and $d_{5}=a_{5}$.

Solving (4.29) and (4.30), then

[TABLE]

[TABLE]

[TABLE]

As shown above, there are two possible solutions for the rotational inverse kinematics problem provided that one puts $\psi$ = [math] in cases $2$ and $3$. When one starts applying the above described algorithm in real time, one can select the solution of $\theta_{1}$ which coincide with the given configuration of the quadrotor manipulation system. After that, one can continue with the one of the two solutions which produces this $\theta_{1}$.

Finally, the inverse position is determined from (2.7) as

[TABLE]

There is one solution for this inverse position problem.

4.2 Verification

To verify the proposed solution of the inverse kinematics, the desired task space trajectories are chosen to make a circular helix in position and quintic polynomial [71] for the orientation.

After obtaining the joint space variables from the proposed algorithm, as shown in Fig. 4.1, the forward kinematics is applied to find the actual task space trajectory.

The comparison between the actual and the desired task space trajectories is shown in Figs. 4.2 and 4.3from which one can recognize that the actual and desired trajectories are coincided. In Figs. 4.4 and 4.5, another case study is investigated to show the capability of the proposed algorithm to deal with higher speed desired trajectories. In this study, the desired trajectories are chosen to be faster than that in Fig. 4.3 by ten times. Note that the maximum speed for the trajectories are 5 m/s for the translation motion and 6 rad/s for the rotational motion .These figures show that the actual and desired trajectories are coincided under both slower or faster trajectories, which ensures the validity of the proposed inverse kinematic algorithm, and proves that the proposed system has the ability to track arbitrary 6-DOF task space trajectory.

The obtained accurate inverse kinematics solution enables one to design the controller in the quadrotor/joint space to track the desired trajectories in the task space. This control design is the subject of the next chapter where certain control objectives are achieved for such complex dynamic system.

Chapter 5 CONTROL SYSTEM DEVELOPMENT

This chapter proposes motion control systems to address and solve the control issues of the quadrotor manipulation system. These issues include; robust position control, robust optimal position control, and robust force estimation and impedance control.

The control of the quadrotor manipulation system is quite challenging because it is naturally unstable, and has strong nonlinearities. In addition, it is very susceptible to parameters variations due to adding/releasing the payload and the actuators’ losses. Moreover, it is subjected to external disturbances like wind. Furthermore, its measurements are very noisy due to the thrusters that rotate at very high speed. Also, it has fast dynamics. Thus, there is a need to design and implement an efficient robust control technique to address these issues, which is presented in section 5.2.

Furthermore, the actuators (thrusters and manipulator motors) have constraints, which may degrade the performance if it is not considered during controller design. Moreover, we have to apply an optimal control effort to reduce the required power of the thrusters such that we can save power and thus increase the mission time and/or increase the payload. Therefore, in section 5.3, the Model Predictive Control (MPC) technique will be utilized based on the robust linearization capabilities of the DOb to address this issue.

Finally, in addition to the accurate positioning of the end-effector, the manipulation tasks require estimating (applying) certain force at the end-effector as well.. Thus, in section 5.4, a robust force estimation and impedance control scheme is proposed. The robustness will be achieved based on the DOb technique. Then, a linear impedance controller will be designed in the joint space. The contact force is be identified based on robust linearization capabilities of DOb and a Fast Tracking Recursive Least Squares (FTRLS) algorithm, which takes into consideration all the system dynamics with no need for the environment contact indication.

5.1 Control Objectives

We target the design of the control system in order to satisfy the following objectives:

Control Objective 1**.**

(Robust Stability) The considered robotic system is stable and robust in the presence of the external disturbances, uncertainties, and noises.

Control Objective 2**.**

(Trajectory Tracking) $6$-DOF task space trajectory tracking is achieved.

Control Objective 3**.**

(Optimal Performance) Minimization of the controller efforts, and constraints them to be within the allowable limits of the system actuators (i.e., thrust forces and motor’s torques).

Control Objective 4**.**

(Force Estimation) The end-effector contact force has to be estimated fast.

Control Objective 5**.**

($6$-DOF Impedance Control) In the presence of the contact force, desired impedance at the end-effector is achieved.

5.2 Robust Position Control

In this section, a modified version of the well-known Disturbance Observer-based robust control is proposed in order to achieve the robust 6-DOF task space trajectory tracking against the system nonlinearities, couplings, uncertainties, disturbances, and measurement noise.

To achieve control objectives 1 and 2, a control technique as shown in Fig. 5.1 is proposed. In this strategy, the system nonlinearities, uncertainties, external disturbances, $\tau^{dis}$, are treated as extended disturbances which will be estimated, $\hat{\tau}^{dis}$, and canceled by the DOb in the inner loop. The system can be now considered as linear SISO plants, and thus, simple PD controller is used in the external loop to achieved the target performance for the system by producing $\tau^{des}$. Another loop is presented to find the desired quadrotor/joint space trajectories from the target $6$-DOF task space based on the inverse kinematic analysis that is presented in chapter 4. Hence, the controller can be designed in the quadrotor/joint space.

5.2.1 DOb-based Controller

Disturbance Observer (DOb)-based controller is one of the most popular methods in the field of robust motion control due to its simplicity and computational efficiency. In DOb-based robust motion control systems, internal and external disturbances are observed by the DOb, and the robustness is simply achieved by feeding-back the estimated disturbances in an inner-loop. Another controller is designed in an outer loop so that the performance goals are achieved without considering internal and external disturbances.

A block diagram of the conventional DOb inner loop is shown in Fig. 5.2. In this figure, $M_{n}$ $\in\mathbb{R}^{8\times 8}$ is the system nominal inertia matrix, $\tau^{des}$ is the desired input vector, $P=diag([g_{1},...,g_{i},...,g_{8}])$ with $g_{i}$ is the bandwidth of the low pass filter of the $i^{th}$ variable of $q$, $Q(s)=diag([\frac{g_{1}}{s+g_{1}},...,\frac{g_{i}}{s+g_{i}},...,\frac{g_{8}}{s+g_{8}}])$ $\in\mathbb{R}^{8\times 8}$ is the matrix of the low pass filter of DOb. The DOb requires velocity measurements. $\tau^{dis}$ represents the system extended disturbances including the Coriolis, centrifugal and gravitational terms, and external disturbances, and $\hat{\tau}^{dis}$ represents the system estimated disturbances. $K$ is the external controller that is used to achieve the required control performance.

If one can apply the concept of disturbance observer to the proposed system, then the independent coordinate control is possible without considering the coupling effect of the other coordinates. The coupling terms such as centripetal and Coriolis and gravity terms are considered as disturbance and compensated by feeding back the estimated disturbance torque.

As we noted in this structure, the DOb utilizes the estimated velocity measurement to estimate the system disturbances.

However, it is well known that the linear accelerations and angular rates of the quadrotor can be measured directly from the IMU. In addition, the angular velocities of the joints can be measured via an encoder. Therefore, a block diagram of the modified DOb-based controller is proposed as in Fig. 5.3 for our robotic system. In this modified DOb-based control, two different DOb loops are used. One is based on the measured accelerations, while the other is based on the measured velocities.

In this figure, $M_{n}=\begin{bmatrix}M_{n_{a}}&O_{3\times 5}\\ O_{5\times 3}&\ M_{n_{v}}\end{bmatrix}$ $\in\mathbb{R}^{8\times 8}$ is the system nominal inertia matrix with $M_{n_{a}}$ $\in\mathbb{R}^{3\times 3}$ represents the nominal inertia for the translational coordinates, ${p}_{b}$, while $M_{n_{v}}$ $\in\mathbb{R}^{5\times 5}$ represents the nominal inertia for the rotational coordinates, ${\Phi}_{b}$ and ${\Theta}$. $Q_{a}(s)=diag([\frac{g_{1}}{s+g_{1}}\ ...\ \frac{g_{3}}{s+g_{3}}])$, and $Q_{v}(s)=diag([\frac{g_{4}}{s+g_{4}}\ ...\ \frac{g_{8}}{s+g_{8}}])$ are the matrices of the low pass filter of DOb for the translational and rotational coordinates, respectively. $P_{a}=diag([g_{1}\ g_{2}\ g_{3}])$ and $P_{v}=diag([g_{4}\ ...\ g_{i}\ ...\ g_{8}])$ vectors of bandwidths of the DOb filter for translational and rotational coordinate, respectively. $\tau=[\tau_{a}^{T}\ \tau_{v}^{T}]^{T}$ represents the robot’s generalized forces for the translational and rotational coordinates. $\hat{\tau}^{dis}=[\hat{\tau}_{a}^{{dis}^{T}}\ \hat{\tau}_{v}^{{dis}^{T}}]^{T}$ represents the system estimated disturbances.

The system disturbance, $\tau^{dis}$, can be calculated as

[TABLE]

The control input, $\tau$, in Fig. 5.3 can be calculated as

[TABLE]

where

[TABLE]

Thus, the control input, $\tau$, can be calculated as

[TABLE]

where $e_{v}=\dot{q}^{des}-\dot{q}$, and $\ddot{q}^{des}$ that is the output from the outer loop controller which will be designed later.

By the virtue that the DOb works ideal, then

[TABLE]

Thus, the dynamics from the DOb loop input $\tau^{des}$ to the output of the robot manipulator are given by

[TABLE]

Since $M_{n}$ is chosen to be a diagonal matrix, the system can be treated as multi-decoupled linear SISO systems as

[TABLE]

or simply in the acceleration space as

[TABLE]

All that remains in the design of the DOb based controller is the design of the tracking controller, $K(s)$, in the DOb outer loop. A PD-based tracking controller for the system of (5.8) is chosen as

[TABLE]

where $K_{P}$ and $K_{D}$ $\in R^{8\times 8}$ are the proportional and derivative gains of the PD controllers, receptively. $q_{r}$, $\dot{q}_{r}$, and $\ddot{q}_{r}$ are the references for linear/angular positions, velocities, and accelerations, respectively. By proper selection of $K_{P}$ and $K_{D}$, one can tune the response of the the error dynamics as necessary.

To ensure that the system stability under this control can be guaranteed, we do the following analysis.

The control law in (5.4) leads to the following error dynamics

[TABLE]

and

[TABLE]

where

[TABLE]

and $\Delta M(q)=M(q)-M_{n}$.

Equation (5.10) represents the error dynamics of the DOb loop.

Let us assume the following Lyapunov function

[TABLE]

The time derivative of this function is

[TABLE]

Substituting from (5.10), then (5.14) becomes

[TABLE]

The dynamic equation of motion (2.96) posses several well known properties [76, 71].

Property 1**.**

[TABLE]

Property 2**.**

[TABLE]

where $\dot{M}(q)-2C(q,\dot{q})$ is a skew-symmetric matrix, $\nu\in\mathbb{R}^{8}$ represents a $8$-dimensional vector, and $\lambda_{min}$ and $\lambda_{max}$ are positive real constants that are the minimum and maximum eigen values of the matrix $M(q)$, respectively.

Substituting from (5.17) into (5.15), then (5.15) will be

[TABLE]

From the robot dynamic property (5.16), one can find:

[TABLE]

where $\gamma=\frac{2K_{v}}{\lambda_{max}}$.

From the analysis presented in [77, 78], we complete the analysis as following.

Dividing (5.19) by $V^{0.5}\neq 0$ will give

[TABLE]

By multiplying (5.20) by $e^{-0.5\gamma}$ and integrating, then we get

[TABLE]

with

[TABLE]

One can rewrite $V_{c}$ as

[TABLE]

Taking the $\mathinner{\!\left\lVert.\right\rVert}_{p}$, gives

[TABLE]

Lemma 1**.**

([79]) Consider $\varUpsilon_{1}\in L_{1}$, $\varUpsilon_{2}\in L_{p}$, then

[TABLE]

From $Lemma$ 1, then (5.24) is

[TABLE]

but $\mathinner{\!\left\lVert e^{-0.5\gamma t}\right\rVert}_{1}$ = $0.5\gamma$, so

[TABLE]

Thus, (5.21) is

[TABLE]

This expression (5.28) can be simplified to be

[TABLE]

Thus, the error dynamics is $L_{p}$ input/output stable with respect to the pair ($\delta$, $e_{v}$) for all $p\in[1,\infty)$.

Lemma 2**.**

([80]) Let

[TABLE]

where $H_{PD}(s)$ is an strictly proper and exponentially stable transfer function. Then $e_{v}\in L_{p}$ implies that $e\in L_{p}$ and $\dot{e}\in L_{p}$.

Form (5.11), the transfer function between $e$ and $e_{v}$ is proper exponentially stable as long as the parameters $K_{P}$ and $K_{D}$ are positive definite matrices. Therefore, Lemma 2 implies that both $e$ and $\dot{e}$ $\in L_{p}$.

Remark 1*.*

Equation (5.19) indicates that the convergence rate increases proportionally with $K_{v}$. However, we have to take into considerations the constraints on $g_{i}$ due to the sampling time.

Fig. 5.4 presents the detailed block diagram of the proposed motion control system based on the inverse kinematic analysis and on the DOb quadrotor/joint space-based control. The desired trajectories for the end-effector’s position and orientation $\chi_{e,r}(t)$, $p_{e,r}(t)$ and $\Phi_{e,r}(t)$, are fed to the inverse kinematic algorithm together with $\sigma_{b,r}(t)$ generated from the nonholonomic constraints such that the desired trajectories in vehicle/joint space, $\zeta_{r}(t)$, are obtained. After that, the controller block receives the desired trajectories and the feedback signals from the system and provides the control signal, $\tau$ $=$ $Bu$.

Since the linear position, joints angles, and the yaw angle are usually the controlled variables while pitch and roll angles are used as intermediate control inputs for horizontal position control, the proposed control system consists from two DOb-based controllers; one for $\zeta$ (with $K_{\zeta}(s)$, $M_{n_{a}}$, $M_{nv_{1}}$ ,$P_{v_{1}}$, $Q_{v_{1}}$, and $Q_{a}$) and the other for $\sigma_{b}$ (with $K_{\sigma}(s)$, $M_{nv_{2}}$, $P_{v_{2}}$, $Q_{v_{2}}$).

The desired values $\sigma_{b,r}$ for the intermediate controller are obtained from the output of position controller, $\tau_{\zeta}$, through the relation

[TABLE]

which can be derived from (2.1) based on small angle approximation.

The desired value of the pitching and rolling, $\sigma_{b,r}$, is then fed back to the inverse kinematics algorithm.

The output of the two controllers, $\tau_{\zeta}$ and $\tau_{\sigma}$, are mixed to generate the final control vector $\tau$ which is converted to the forces/torques applied to quadrotor/manipulator by

[TABLE]

where $B_{6}$ $\in\mathbb{R}^{6\times 6}$ is part of $B$ matrix and it is given by $B_{6}=B(3\mathrel{\mathop{\mathchar 58\relax}}8,1\mathrel{\mathop{\mathchar 58\relax}}6)$.

Remark 2*.*

It is noted from the quadrotor dynamics [81, 82, 34] that the response of the intermediate, $\sigma$, controller must be faster than that of the position control loop. However, as stated in the above analysis, the convergence rate of errors dynamics ($e_{v}$ and $e$) is directly proportional to the $K_{v}$, $K_{P}$, $K_{D}$. Therefore, we have to tune $K_{v}$, $K_{P}$, and $K_{D}$, such that the response speed of $\sigma_{b}$ is much faster than that of $\zeta$.

5.2.2 Simulation Study

In this section, the previously proposed control strategy is simulated in MATLAB/SIMULINK program for the considered aerial manipulation system.

Simulation Environment

In order to make the simulation quite realistic, the following setup and assumptions have been made:

• •

The model has been identified on the basis of real data through experimental tests. The identified parameters are given in Table 3.1.

• •

Linear and angular position and velocity of the quadrotor are available at rate of $1$ kHz.

• •

The position and velocity of the manipulator joints are available at rate of $1$ kHz.

• •

A normally distributed measurement noise, with mean of $10^{-3}$ and standard deviation of $5\times 10^{-3}$, has been added to the measured signals.

• •

The controller outputs are computed at a rate of $1$ kHz.

• •

In order to test the robustness to the model uncertainties, a step change is introduced, at instant $15$ s, in the control matrix, $N$, which can simulate sudden actuators’ losses, whose elements are assumed to be equal to 0.9 times their true values (i.e., $10\%$ error).

• •

The end-effector has to pick a payload of value $200$ g at instant $20$ s and release it at $40$ s.

Results and Discussion

Tables 5.1 presents the controller parameter for the proposed control technique. The reference trajectories for the end-effector are chosen such that the end-effector moves on a circular helix, while its orientation follows quintic polynomial trajectories.

The simulation results are presented in Figs. 5.6, 5.5 and 5.7. From these figures, it is possible to recognize that there are small oscillations after starting the system operation due to the time needed to estimate the nonlinearities and disturbances. This time is about $4$ s. After this period, the controller can quickly recover this error and provides good tracking of the desired trajectories. Moreover, it is clear that the capability of the controller to recover, with fast response (about $0.05$ s), the tracking error due to the presence of the payload and the uncertainty in system parameters. Moreover, Fig. 5.5 indicates that the quadrotor/joint space trajectories are within the allowable limits of the joints (270*∘*). Furthermore, Fig. 5.8 indicates that the required actuators efforts, which are the required thrust force from each rotor and the torque from each manipulator’s motor, $u$, are in the allowable limit. The maximum thrust force for each rotor is $8$ N as obtained from the identification process, while the allowable input torque for the motor of joint $1$ is $0.7$ N.m and $0.4$ N.m for that of joint $2$ as stated in the motors’ data sheet. Therefore, one can contend that the control objectives 1 and 2 are achieved by using this motion control scheme.

5.3 Robust Optimal Position Control

In this section, based on the robust linearization capabilities of the DOb that presented in the previous section, the continuous MPC [83] technique will be used to achieve an optimal performance for the aerial manipulator, in addition to keep the actuators’s inputs within their limits.

To achieve the control objectives 1 - 3, a control technique as shown in Fig. 5.9 is proposed. In this strategy, the system nonlinearities, uncertainties, external disturbances, $\tau^{dis}$, are treated as extended disturbances which will be estimated, as $\hat{\tau}^{dis}$, and canceled by the DOb in the inner loop. The system can be now considered as linear SISO subsystems, and thus, the MPC is used in the external loop to achieve the target performance for the system by producing $\tau^{des}$. Another loop, inverse kinematics, is presented to find the desired quadrotor/joint space trajectories from the target $6$-DOF task space trajectory.

5.3.1 Linearized Model

By virtue of the rejection of the nonlinearities, disturbances, uncertainties, and noise by the usage of DOb internal loop as presented in section 5.2, the MPC controller has not to consider uncertainties and can be designed based on the nominal $8$ SISO decoupled models of the system with satisfying the actuators’ constraints, $u^{min}$ $\leq$ $u$ $\leq$ $u^{max}$. However, in determining $u^{max}$ and $u^{min}$, the contribution of $\hat{\tau}^{dis}$ should be considered.

The state space SISO model of the system of (5.8) is

[TABLE]

where $x_{p}=\begin{bmatrix}x_{1}\\ x_{2}\\ \end{bmatrix}=\begin{bmatrix}q_{i}\\ \dot{q}_{i}\\ \end{bmatrix}$, $A_{p}=\begin{bmatrix}0&1\\ 0&0\\ \end{bmatrix}$, $B_{p}=\begin{bmatrix}0\\ 1\\ \end{bmatrix}$, $C_{p}=\begin{bmatrix}1&0\\ \end{bmatrix}$, and $u_{mpc}(t)$ is the output of the external controller, MPC, which will be $\ddot{q}^{des}_{i}$.

5.3.2 Continuous time MPC

The MPC can be designed in continuous time or discrete time models. One of the key advantages in using continuous time predictive control instead of discrete-time predictive control is that the design model and the algorithm are robust in a fast sampling environment. The discrete-time models and predictive control algorithms could encounter problems when the system is under fast sampling condition. Another advantage is that because the design is performed in the continuous-time domain, the discretization is carried out in the implementation stage and the framework permits the control of an irregular sampled-data system. Thus, the continuous time MPC will be used in this research.

The general design philosophy of model predictive control is to compute a trajectory of a future manipulated variable, $u_{mpc}(t)$, to optimize the future behavior of the plant output $q_{i}(t)$. In other words, the MPC uses a dynamical model of the process to predict its future evolution and optimize the control signal. The optimization is performed within a limited time window starts by $t_{i}$ with length $T_{p}$. With a given initial time $t_{i}$, the window is taken from $t_{i}$ to $t_{i}$ + $T_{p}$. The length of the window $T_{p}$ remains constant. The prediction horizon equals the length of the moving horizon window $T_{p}$ and indicates how far the future will be predicted. Thus, it provides the advantage of repeated online optimization. Based on the receding horizon control principle, although the optimal trajectory of the future control signal is completely captured within the moving horizon window, the actual control input to the plant only takes the first instant of the control signal, while neglecting the rest of the trajectory. At a given time, the first value of this optimal sequence is applied to the process. When a new set of measurements is available to the controller, a re-optimization is performed, and the first value of this new optimal sequence of manipulated variables is implemented.

The continuous-time MPC design [83] uses the derivative of the control signal, $\dot{u}_{mpc}(t)$, and thus it is always designed based on an augmented model of the plant which is given by

[TABLE]

where $x_{a}(t)=\begin{bmatrix}\dot{x}_{p}(t)\\ q_{i}(t)\end{bmatrix}$, $A_{a}=\begin{bmatrix}A_{p}&O_{2\times 1}\\ C_{p}&1\end{bmatrix}$, $B_{a}=\begin{bmatrix}B_{p}\\ 0\end{bmatrix}$, $C_{a}=\begin{bmatrix}O_{1\times 2}&1\end{bmatrix}.$ This implies that the signal $\dot{u}_{mpc}(t)$ is an exponentially decay function that goes to 0.

Therefore, the control trajectory, $\dot{u}_{mpc}(t)$, can be modeled by a set of exponentially decaying functions within the interval $0\leq\iota\leq T_{p}$.

One candidate function, to describe the derivative of the control signal, is the set of Laguerre polynomials [84, 85], which is a set of orthonormal functions.

The following set of Laguerre functions is used

[TABLE]

where $A_{u}=\begin{bmatrix}-s_{u}&0&...&0\\ -2s_{u}&-s_{u}&...&0\\ ...&...&...&...\\ -2s_{u}&..&-2s_{u}&-s_{u}\end{bmatrix}$ $\in\mathbb{R}^{n_{u}\times n_{u}}$,

$s_{u}$ is the time scaling factor that determines the exponential decay rate, $n_{u}$ is the number of terms of Laguerre polynomials, and the initial condition is $X(0)=\sqrt{2s_{u}}[1,1,...,1]^{T}$ $\in\mathbb{R}^{n_{u}}$. The accuracy of the approximation increases as the number of terms increases. This approach provides a simple and efficient method to mathematically model the control trajectory with minimum prior information. Also it simplifies the solutions and enable us to tune the predictive control system via only its two parameters (i.e., $s_{u}$ and $n_{u}$). This is unlike the classic design of the MPC which is based on Taylor series expansion that provides non-parametric description for the MPC control trajectory.

Considering the moving time window from $t_{i}$ to $t_{i}$ + $T_{p}$, and the time variable within this window is $\iota$, the derivative of the control signal, $\dot{u}_{mpc}(\iota)$, can be described by this set of functions as

[TABLE]

where $\eta=[c_{u_{1}},c_{u_{1}},...,c_{u_{n_{u}}}]^{T}$ is a coefficient vector, which will be designed later.

5.3.3 Predicted plant response

The predicted future state vector at time $\iota$ is given as

[TABLE]

where $\Phi_{mpc}(\iota)^{T}=\int_{0}^{\iota}e^{A_{a}(\iota-\gamma)}BX(\gamma)^{T}d\gamma$. From the prediction of the state variable, the predicted output at time $\iota$ is

[TABLE]

Computation of $\Phi_{mpc}(\iota)$ can be obtained in a recursive solution in terms of $\iota$ = 0, $h$, 2$h$, …, $T_{p}$, by

[TABLE]

5.3.4 Optimal control strategy

The MPC design is based on the solution of the so-called Finite Horizon Optimal Control Problem which consists of minimizing a suitably defined cost function with respect to the control sequence. The cost function at time $t_{i}$ is

[TABLE]

where $Q_{mpc}$ and $R_{mpc}$ are weighting matrices that must be positive semi-definite and positive definite, respectively, to ensure the stability of the outer loop [86]. The optimal control $\dot{u}_{mpc}(\iota)$ can be calculated by minimizing this cost function, $J_{mpc}$, which is the optimal control within the moving window.

By substituting the prediction of the state variable $x_{a}(t_{i}+\iota|t_{i})$ from (5.37) into (5.40), the cost function becomes function of $\eta$ as

[TABLE]

Then, by minimizing this cost function in terms of $\eta$, the optimal $\eta$ is

[TABLE]

where $\Omega_{mpc}=\int_{0}^{T_{p}}\Phi_{mpc}(\iota)Q_{mpc}\Phi_{mpc}(\iota)^{T}d\iota+R_{mpc}$, and $\Psi_{mpc}=\int_{0}^{T_{p}}\Phi_{mpc}(\iota)Q_{mpc}e^{A_{a}\iota}d\iota$. Thus, the optimal control signal can be calculated, from (5.36), in the window from $0\leq\iota\leq T_{p}$.

It is difficult to obtain the analytical solutions for these integral expressions, $\Omega_{mpc}$ and $\Psi_{mpc}$. However, these matrices are computed over a given prediction horizon $T_{p}$. Thus, the integral expressions can be evaluated off-line using a numerical approximation as

[TABLE]

[TABLE]

By applying the principle of receding horizon control (i.e., the control action will use only the derivative of the future control signal at $\iota$ = [math]), one can get the derivative of the optimal control as following:

[TABLE]

5.3.5 Constrained control

The next step is to formulate the constraints as part of the design requirements, and relate them to the original model predictive control problem. Since the predictive control problem is formulated and solved in the framework of receding horizon control, the constraints are taken into consideration frame-by-frame for each moving horizon window. This gives us the means to solve the constrained control problem numerically.

The considered constraints, in this work, are that of the robot actuators ($u^{min}\leq u\leq u^{max}$). Thus, firstly, we have to relate them to the optimized control trajectory of the MPC (i.e., $u_{mpc}^{min}\leq u_{mpc}\leq u_{mpc}^{max}$) as following:

The input torque from the controller and applied to the robot, $\tau$, can be calculated from

[TABLE]

where $\tau_{mpc}$ is the torque generated from the MPC controller and it can be calculated from

[TABLE]

Thus, one can build the following constraints on $\tau_{mpc}$ as

[TABLE]

The generalized input forces, $\tau$, can be related to the actuators’ inputs via (2.90 - 2.93 and 5.31) as

[TABLE]

By reformulating the 3rd to the 6th element in (5.50), one can get the thrusters’ inputs as following

[TABLE]

To constrain the thrusters’ input to be $F^{min}\leq F_{i}\leq F^{max}$, relation (5.51) can be utilized to get the following constraints on the robot’s inputs $\tau_{z}$, $\tau_{\psi}$, $\tau_{\theta}$, and $\tau_{\phi}$.

Constraints for $\tau_{z}$:

To calculate the required constraints on $\tau_{z}$, each row in (5.51) is solved to satisfy the constrain $F_{i}=[F^{min},F^{max}]$. Thus, there are four constraints on $\tau_{z}$. The minimum values of the constraints are given by

[TABLE]

where $\tau_{z}^{min^{i}}$ is the lower limit of the $\tau_{z}$ constraint obtained from the constraint of $F_{i}$.

The maximum values of the constraints are given by

[TABLE]

While $\tau_{z}^{min}$ can be calculated from

[TABLE]

and $\tau_{z}^{max}$ can be calculated from

[TABLE]

The torques, $\tau_{\psi}$, $\tau_{\theta}$ and $\tau_{\phi}$, in the right side of (5.52 and 5.53) are substituted by their values from last time step (i.e., $\tau_{\psi}(t-\delta t)$, $\tau_{\theta}(t-\delta t)$ and $\tau_{\phi}(t-\delta t)$) or by their predicted values (i.e., $\tau_{\psi}(t-\delta t)+\Delta\tau_{\psi}(t-\delta t)$, $\tau_{\theta}(t-\delta t)+\Delta\tau_{\theta}(t-\delta t)$, and $\tau_{\phi}(t-\delta t)+\Delta\tau_{\phi}(t-\delta t)$). Where $\Delta\tau_{.}(t-\delta t)=\tau_{.}(t-\delta t)-\tau_{.}(t-2\delta t)$. Both of these two cases are tested.

Similarly, the constraints for $\tau_{\psi}$, $\tau_{\theta}$, and $\tau_{\phi}$ are calculated as:

Constraints for $\tau_{\psi}$:

[TABLE]

The maximum values of the constraints are given by

[TABLE]

While $\tau_{\psi}^{min}$ can be calculated from

[TABLE]

and $\tau_{\psi}^{max}$ can be calculated from

[TABLE]

Constraints for $\tau_{\theta}$:

[TABLE]

The maximum values of the constraints are given by

[TABLE]

While $\tau_{\theta}^{min}$ can be calculated from

[TABLE]

and $\tau_{\theta}^{max}$ can be calculated from

[TABLE]

Constraints for $\tau_{\phi}$:

[TABLE]

The maximum values of the constraints are given by

[TABLE]

While $\tau_{\phi}^{min}$ can be calculated from

[TABLE]

and $\tau_{\phi}^{max}$ can be calculated from

[TABLE]

Constraints for $\tau_{x}$ and $\tau_{y}$:

From the 1st and 2nd elements in (5.50), one can get the constraints for $\tau_{x}$ and $\tau_{y}$ as:

If $(\theta C_{\psi}+\phi S_{\psi})>0$, then

[TABLE]

If $(\theta C_{\psi}+\phi S_{\psi})<0$, then

[TABLE]

If $(\theta S_{\psi}-\phi C_{\psi})>0$, then

[TABLE]

If $(\theta S_{\psi}-\phi C_{\psi})<0$, then

[TABLE]

In the case that the coefficients, $(\theta C_{\psi}+\phi S_{\psi})$ and $(\theta S_{\psi}-\phi C_{\psi})$, are very small, the constraints on the $\tau_{x}$ and $\tau_{y}$ are released to keep a minimum distance between the upper and lower limits of $\tau_{x}$ and $\tau_{y}$ as following:

If $\tau_{x}^{max}\leq\tau_{x}^{s}$, then $\tau_{x}^{max}=\tau_{x}^{s}$. If $\tau_{x}^{min}\leq-\tau_{x}^{s}$, then $\tau_{x}^{min}=-\tau_{x}^{s}$. If $\tau_{y}^{max}\leq\tau_{y}^{s}$, then $\tau_{y}^{max}=\tau_{y}^{s}$. If $\tau_{y}^{min}\leq-\tau_{y}^{s}$, then $\tau_{y}^{min}=-\tau_{y}^{s}$. Where $\tau_{x}^{s}$ and $\tau_{y}^{s}$ are constants.

Constraints for $\tau_{m_{1}}$ and $\tau_{m_{2}}$ : These constraints can be obtained directly from the joints motors’ data sheet which they are the limits on the joints motors’ torques.

After calculating the limits for the constraints of the input torques, the next step is to calculate the limits for the $u_{mpc}$ as

[TABLE]

This means that

[TABLE]

[TABLE]

Since these constraints are time varying, they will be calculated at the start of each time window ($T_{p}$) then they are hold (i.e., zero order hold) for the reset period of this window. Now we will illustrate how the MPC will constrain the $u_{mpc}$.

Let us assume that $\delta t$ is the sampling interval for implementation, the first sample of the control signal at the optimization window, is calculated as

[TABLE]

where $u_{mpc}(t_{i}-\delta t)$ is the previous information of the control signal which is shifted backward by $\delta t$.

Thus, at the first sample time of the window, the control signal constraints are

[TABLE]

However, at the arbitrary time $\iota_{i}$, the control signal is

[TABLE]

With the information of $u(t_{i}-\delta t)$, the future control signal at time $\iota_{i}$ is presented as

[TABLE]

Thus, the inequality constraints of the control signal are expressed as

[TABLE]

where $C_{u}=X(0)^{T}\delta t+X(\iota_{i})^{T}A_{u}^{-T}-X(0)^{T}A_{u}^{-T}$.

These constraints can be reformulated as linear inequalities as following

[TABLE]

where

[TABLE]

and

[TABLE]

Since the objective function (5.42) is quadratic, and the constraints are linear inequalities, the problem of finding an optimal predictive control becomes one of finding an optimal solution to a standard quadratic programming problem. These inequality constraints consist of active constraints and inactive constraints. An inequality $M_{mpc}\eta\leq\gamma_{mpc}$ is said to be active if $M_{mpc}\eta=\gamma_{mpc}$ and inactive if $M_{mpc}\eta<\gamma_{mpc}$.

A simple and efficient algorithm, called Hildreth’s quadratic programming procedure [87], is proposed for solving this problem. In this method, to reduce the computational load, the constraints that are not active is identified systematically, then they are eliminated in the solution. The active constraints are equality constraints, so one can rely on the Lagrange multipliers, $\Lambda$, to solve this optimization problem (Quadratic objective function with equality constraints). This method will lead to very simple programming procedures for finding optimal solutions of constrained minimization problems. This algorithm is based on an element-by-element search, therefore, it does not require any matrix inversion.

In this method, the $i^{th}$ Lagrange multiplier at iteration $k+1$ is calculated from

[TABLE]

where

[TABLE]

$h_{ij}$ represents the $ij^{th}$ element of the matrix $H_{mpc}=M_{mpc}\Omega_{mpc}^{-1}M_{mpc}^{T}$, and $e_{i}$ is the $i^{th}$ element of the vector $E_{mpc}=\gamma_{mpc}+M_{mpc}\Omega_{mpc}^{-1}\Psi_{mpc}x_{a}$.

The iteration will be terminated when the iterative counter reaches its maximum value, $m_{max}$, at which the $\Lambda$ converges to its optimal value, $\Lambda^{*}$, which contains zeros for inactive constraints and the positive components corresponding to the active constraints. Then, the optimal $\eta$ can be calculated from

[TABLE]

and thus the optimal control signal can be obtained from

[TABLE]

with

[TABLE]

Summery of the MPC technique is presented as algorithm 3.

5.3.6 MPC tuning guidelines

The design variables in the continuous time model predictive control algorithm presented above are [84]:

• •

Weighting matrices, $Q_{mpc}$ and $R_{mpc}$: The smaller elements in $R_{mpc}$ and larger elements of $Q_{mpc}$ corresponds to faster closed-loop response speed.

• •

Pole location of the Laguerre model for the future control signal, $s_{u}$: is a tuning parameter for the closed-loop response speed. Larger p results in larger changes in the future control signal in the initial period, hence faster closed-loop response speed.

• •

Number of terms used in the Laguerre model to capture the control signal, $n_{u}$: As $n_{u}$ increases, the degree of freedom in describing the control trajectory increases. The control signal tends to be more aggressive when $n_{u}$ increases thus faster closed-loop response speed.

• •

Prediction horizon $T_{p}$: For stable system, the prediction horizon is recommended to be chosen approximately equal to the open loop process settling time. For unstable and integrating processes, which as in our case, $T_{p}$ is recommended to be chosen as a parameter of $s_{u}$ approximately equal to ($\frac{10}{s_{u}}$).

Fig. 5.10 shows the complete block diagram of the proposed control system. As it is stated before, the position, the manipulator joints’ angles, and the yaw angle are usually the controlled variables, while pitch and roll angles are used as intermediate control inputs for horizontal positions control. Therefore, the proposed control system consists from two DOb-based controllers by dividing $q$ into two parts; one for $\zeta=[x\ y\ z\ \psi\ \theta_{1}\ \theta_{2}]^{T}$ (with $MPC_{\zeta}$, $M_{n_{a}}$, $M_{nv_{1}}$, $P_{v_{1}}$, $Q_{v_{1}}$, and $Q_{a}$) and the other for $\sigma_{b}=[\theta\ \phi]^{T}$ (with $MPC_{\sigma}$, $M_{nv_{2}}$, $P_{v_{2}}$, and $Q_{v_{2}}$). The desired trajectories for the end-effector’s position and orientation ($p_{e,r}$ and $\Phi_{e,r}$) are fed to the inverse kinematics algorithm together with $\sigma_{b,r}(t)$ such that the desired quadrotor/joint space trajectories $\zeta_{r}(t)$ are obtained. After that, the controller block receives the desired trajectories and the feedback signals from the system and then provides the control signal, $\tau$. The desired values $\sigma_{b,r}$ for the intermediate controller are obtained from the output of position controller, $\tau_{\zeta}$, through (5.31)

The output of the two controllers, $\tau_{\zeta}$ and $\tau_{\sigma}$, are mixed to generate the final control vector $\tau$ which is then converted to the forces/torques applied to quadrotor/manipulator via the relation (5.32)

Remark 3*.*

It is better to make the response of the $\sigma$ controller faster than that of the quadrotor position controller such that it can track the changes in the position controller [81, 82]. This can be achieved by the tuning parameters of both DOb and MPC.

5.3.7 Simulation Results

In this section the previously proposed control strategy is applied in simulation MATLAB/SIMULINK program to the model of the considered aerial manipulation robot. The simulation environments similar to that in section 5.2.2 is used here. Parameters of the MPC controller are given in Table 5.2.

The system response in the quadrotor/joint space is presented in Fig. 5.11. Figs. 5.12 and 5.13 show responses of the proposed scheme in the task space (the actual end-effector position and orientation can be found from the forward kinematics). Note that the marker represents the end-effector orientation; Green, Blue, and Red for x-,y-, and z-axis. These results show that the proposed motion control scheme able to track the desired trajectories, and so the control objectives 1 - 3 are achieved. Moreover, the MPC reduces the oscillations that appears at the start of motion by using the PD-DOb method.

Fig. 5.14 shows the control effort, the required thrust forces and manipulator torques which are in the allowable limits. It is noted also from this figure that the control effort becomes lower and has no sharpness than that in the case of using the PD-DOb technique.

Table 5.3 presents a comparison study between PD-DOb and MPC-DOb controllers. The first part in this table compares the Root Mean Square Error (RMSE) in the tracking of the end-effector position and orientation. The RMSE of the MPC is slightly lower than that of the PD-DOb. The second part presents the summation of quadrotor thrusters efforts as well as the manipulator torques. This table enlightens the superior performance of the MPC-DOb over the PD-DOb. The using of the MPC leads to a 15 $\%$ reduction in the required thrust effort.

5.4 Robust Force Estimation and Impedance Control

In this section, a robust sensorless force estimation and impedance control system is presented to achieve the objectives 1, 2, 4, and 5 .

The proposed methodology to satisfy these control objectives is illustrated in Fig. 5.15. In this control strategy, the system nonlinearities, external disturbance (wind), $\tau_{w}$, and contact force, $\tau_{l}$, are treated as extended disturbances, $\tau^{dis}$, which will be estimated, as $\hat{\tau}^{dis}$, and canceled by the DOb in the inner loop. The system can now be considered as a group of linear SISO subsystems. The output of DOb with system measurements of quadrotor/joint space variables and the derived task space variables are used as the inputs to the Fast Tracking Recursive Least Squares (FTRLS) algorithm to obtain the end-effector contact force $\hat{F}_{e}$. The task space impedance control is used in the external loop of DOb and its output is transformed to the quadrotor/joint space through a coordination transformation algorithm.

5.4.1 Modeling of the Environment Dynamics and the Wind Effect

The dynamical model of the quadrotor-manipulator system (2.96) can be rewritten as

[TABLE]

where $\tau_{w}$ $\in\mathbb{R}^{8}$ is the vector of the external wind effect, and $\tau_{l}$ $\in\mathbb{R}^{8}$ is the vector of the contact force effect.

The environment dynamics, contact force, $\tau_{l}$, can be modeled as following.

[TABLE]

where the task space contact force, $F_{e}$, can be modeled from

[TABLE]

where the quantity $\chi_{e,o}$ is taken equal to the previous state until the contact occurs. At the contact, $\chi_{e,o}$ is equal to the state at the start of the interaction with the environment. $S_{c}=diag\{S_{c_{1}},S_{c_{2}},S_{c_{3}},S_{c_{4}},S_{c_{5}},S_{c_{6}}\}$ and $D_{c}=diag\{D_{c_{1}},D_{c_{2}},D_{c_{3}},D_{c_{4}},D_{c_{5}},D_{c_{6}}\}$ represent the environment stiffness and the environment damping, respectively. It is to be noted that the algorithm for contact force estimation is always run. If any component of the estimated force exceeds the threshold (0.1 N / 0.01 N.m), contact with the environment is indicated and its consequences are followed. Similarly, if the all components of the estimated force are below the threshold, non-contact condition with environment is indicated and its consequences are followed.

The wind dynamics, $\tau_{w}$, can be modeled as following [88, 89, 90]:

The wind velocity is determined by

[TABLE]

where $V_{wz}$ is the wind velocity at altitude $z$, $V_{w_{z_{0}}}$ is the specified wind velocity at altitude $z_{0}$.

To simulate wind disturbances, it is worth calculating the wind generalized force, $\tau_{w}$, which influences the platform. This force can be determined by

[TABLE]

where $0.61$ is the rate of wind velocity (m/s2) conversion to pressure (N/m2). $A_{e}$ is the effective area that depends on the quadrotor structure and its orientation which will be presented latter.

This force can be projected on the axes of frame $\Sigma$ as

[TABLE]

where

[TABLE]

[TABLE]

[TABLE]

[TABLE]

$\psi_{w}$ represents the angle of wind direction. Both $A_{e_{1}}$ and $A_{e_{2}}$ depend on the quadrotor design parameters and they are given as following:

Let us represent the quadrotor as a cylinder with height, $h_{q}$, and radius, $r_{q}$. Thus,

[TABLE]

[TABLE]

where $\beta_{q}$ and $\alpha_{q}$ are the fill factors of the cylinder areas.

5.4.2 Fast Tracking Recursive Least Squares

In this part, a technique is developed which utilizes a Fast Tracking Recursive Least Squares (FTRLS) to estimate the contact force with the aid of the DOb linearization capabilities. The FTRLS algorithm is one of the fast online least squares-based identification methods used for the identification of environments with varying dynamic parameters [91, 92]. To apply FTRLS, the dynamic equations (LABEL:eq:dyn_gen2- 5.97) have to be parametrized (i.e., to be product of measurement data regressor and dynamic parameters) as following.

The system dynamics part, $\tau_{int}=M(q)\ddot{q}+C(q,\dot{q})\dot{q}+G(q)$, can be rewritten as the product of data regressor, $Y_{i}(q,\dot{q},\ddot{q})$, and platform parameters, $h_{i}$.

[TABLE]

The environment dynamics, $\tau_{l}$, can be reformulated as $Y_{l}(q,\dot{q},\ddot{q},\chi_{e},\dot{\chi}_{e})\ h_{l}$, where, $Y_{l}=J^{T}Y_{e}$, $Y_{e}$ is a function of the end effector states, ($\chi_{e}$,$\dot{\chi}_{e}$), which can be calculated as

[TABLE]

$h_{l}$ is the vector of the environment parameters $S_{c}$ and $D_{c}$ and it is given as

[TABLE]

Finally, the wind effect is formulated as $Y_{w}(z,\theta,\phi)\ h_{w}$. $Y_{w}$ can be calculated as

[TABLE]

$h_{w}$ is the vector of the wind parameters and it is given as

[TABLE]

Therefore, the total dynamics can be reformulated as

[TABLE]

where $Y_{s}$ $\in\mathbb{R}^{8\times 40}$ and $h_{s}$ $\in\mathbb{R}^{40}$ are the data regressor and parameters vector of (LABEL:eq:dyn_gen2), respectively. The details of $Y_{i}$ and $Y_{l}$ are given in Appendix D.

The parameter estimation error is

[TABLE]

while the estimation error is

[TABLE]

By minimizing a cost function with respect to the parameter estimation error, one can find the time derivative of the estimated parameters vector, $\hat{h}_{s}$, as following

[TABLE]

where $R(t)$ is the parameters’ covariance matrix, and it can be calculated from

[TABLE]

where $\eta_{h}$ is the forgetting factor, and it is given as

[TABLE]

where $\eta_{h}^{min}$ is a constant representing the minimum forgetting factor, $NINT(.)$ is the round-off operator, and $\gamma_{g}$ is a design constant. This adaptive formulation of the forgetting factor enables the RLS to track the non-stationary parameters to be estimated.

Finally, the estimated contact generalized force in the quadrotor/joint space and task space can be calculated from

[TABLE]

Therefore, unlike the current developed schemes, this technique enables the isolation and the estimation of the end-effector contact force apart from the whole estimated forces in the systems. This estimated force will be utilized in the next section to design an impedance control for the proposed system. Furthermore, for the tele-manipulation, the use of force feedback is essential to assist the human operator in the remote handling of objects. Moreover, it is very critical to get the force feedback to achieve the cooperative grasping among aerial manipulation robots. If the load for the UAVs is heavier than the carrying capacity of a single UAV, multiple UAVs can distribute the load among them and move the object.

5.4.3 Impedance Control

The objective of the impedance control is to impose, along each direction of the task space, a desired dynamic relation between the end-effector pose and the force of interaction with the environment (i.e., desired impedance). The linear impedance control is designed in the task space. This is based on the linearization effect of the designed DOb in the quadrotor/joint space. The desired acceleration in the task space, $\ddot{\chi}^{des}_{e}$, can be calculated from

[TABLE]

where $M_{c,d}$, $S_{c,d}$, and $D_{c,d}$ are constant diagonal positive definite matrices representing the desired inertia, stiffness, and damping, respectively, which determine the desired impedance that the end-effector will apply to the environment.

The control law (5.116) is active during the contact between the end-effector and the environment. Another control is used during the non-contact which is a tracking PD controller and it is given as

[TABLE]

where $K_{P,e}$ and $K_{D,e}$ are constant diagonal positive definite matrices representing the proportional and derivative gains of the task space PD controller. Thereby, if the contact occurs the task space desired accelerations are computed from (5.116) while they are calculated from 5.117 if there is no contact.

Fig. 5.16 shows the complete block diagram of the proposed control system during contact. The quadrotor position, the manipulator joints’ angles and the yaw angle are usually the controlled variables, while pitch and roll angles are used as intermediate control inputs for horizontal positions control. Therefore, the proposed control system consists from two DOb-based controllers by dividing $q$ into two parts; one for $\zeta=[x\ y\ z\ \psi,\theta_{1}\ \theta_{2}]^{T}$ (with $K_{imp}$, $Q_{a}$, $M_{n_{a}}$, $M_{nv_{1}}$, $P_{v_{1}}$, $Q_{v_{1}}$) and the other for $\sigma_{b}=[\theta\ \phi]^{T}$ (with $K_{\sigma}$, $M_{nv_{2}}$, $P_{v_{2}}$, and $Q_{v_{2}}$). The desired trajectories for the end-effector’s position and orientation, $\chi_{e,r}$, their actual values (by using the forward kinematics), and the estimated end-effector force, are fed to the impedance control law, $K_{imp}$ that is described in (5.116). Then, a coordination transformation from task space to quadrotor/joint space is done by using (5.118) to obtain $\ddot{\zeta}^{des}$. The desired acceleration in the quadrotor/joint space, $\ddot{\zeta}^{des}$, can be calculated by differentiating (2.34) with respect to time as

[TABLE]

The desired acceleration in quadrotor/joint space, $\ddot{\zeta}^{des}$, is then applied to the DOb of the independent coordinates, $\zeta$, to produce $\tau_{\zeta}$. The desired values for the intermediate DOb controller, $\sigma_{b,r}$, are obtained from the output of position controller, $\tau_{\zeta}$, through (5.31).

The external controller of the DOb for $\sigma$, $\tau_{\sigma}$, is used as a proportional controller with velocity feedback, $K_{\sigma}$, as following:

[TABLE]

After that, $\ddot{\sigma}^{des}$ is applied to the DOb for $\sigma$ to generate $\tau_{\sigma}$.

However, the response of the $\sigma$ controller must be much faster than that of the quadrotor position controller such that it can track the changes in the position controller. This can be achieved by the tuning parameters of both the DOb and the proportional controller with velocity feedback of $\sigma_{b}$.

The output of the two controllers, $\tau_{\zeta}$ and $\tau_{\sigma}$, are combined together to generate the final control vector $\tau$ which is then converted to the forces/torques applied to quadrotor/manipulator through (5.32).

5.4.4 Simulation Results

In this section, the previously proposed control strategy is applied in MATLAB/SIMULINK to the model of the considered aerial manipulation robot with the same setup of section 5.2.2.

Parameters of the proposed algorithm are given in Table 5.4. The profile of the wind angle, $\psi_{w}$, is simulated as in Fig. 5.17. To emulate a real scenario, the value $V_{w_{z_{0}}}$ is chosen to has two parts; the first one is constant and the second part is random variable to simulate the gust effect (sudden and random changes in the wind speed), see Fig. 5.18.

The aim of considered case study is to perform tracking of the desired 6-DOF end-effector trajectories (i.e., circular helix for the position and Quintic trajectories for the orientation) while an external force of 1.6 N acts along the z-axis of the end-effector, $z_{e}$, for $z_{e}\leq 10$ cm (i.e., the contact occurs during period from 50 s to 60 s). This scenario simulates, for instance, the case of the manipulator in contact with a soft environment. Thereby, the manipulator end-effector is chosen to be rigid. Hence the stiffness matrix, $S_{c,d}$, has been tuned to $S_{c,d}$ = $diag\{20,20,20,50,50,50\}$, while the damping matrix value is $D_{c,d}$ = $diag\{15,15,15,100,100,100\}$.

Estimation of the End-effector Contact Force

Figs. 5.19 shows the actual end-effector contact force and its estimation. 5.20 illustrates the norm of the contact force estimation error. From these figures, it is possible to recognize that before instant 50 s there is no contact with the environment and the contact starts at instant 50 s. In addition, it is observed that the norm error of the end-effector generalized force in all directions have vales around zero during the non-contact period. At instant 50 s, at which the contact occurs, there is an increase in the estimation error norm. The norm of estimation error in both $x$ and $y$ directions have maximum value of $0.016$ N and $0.013$ N, respectively. While it has value of $0.03$ N in the $z$ direction, which is equivalent to 1.8 % with respect to the actual value. In $\phi$, $\psi$ and $\theta$ directions, the norms reach value of $0.0001$ N.m. Thus, it is possible to appreciate the estimation performance of the end-effector generalized forces. Consequently, one can contend that the control objective 4 is achieved.

Impedance Control

Fig. 5.21 shows the response of the system in the task space (the actual end-effector position and orientation can be found from the forward kinematics). From this figure, it is possible to recognize that the controller has good tracking of the desired trajectories of the end-effector due to the using of the tracking controller during the period of non-contact. It also possible to recognize that at the time of the contact the required impedance is applied and this produces small error in the position tracking to satisfy the impedance requirements.

Fig. 5.22 shows the motion of the end-effector in the 3D dimension (The markers represent the orientation). These results show that the proposed impedance motion control scheme provides a robust performance to track the desired end-effector trajectories as well as to achieve the desired impedance effect on the environment taken into consideration the external disturbances and noises. As a result, one can claim that the control objectives 1, 2, and 5 are achieved.

Finally, Fig. 5.23 shows the actual trajectories of the system in the quadrotor/joint space from which it is noted that the generated trajectories in the quadrotor/joint space are in the allowable limits of the manipulator joints.

Chapter 6 CONCLUSIONS

6.1 Conclusions

This thesis presents the development of control design and practical implementation of a new non-redundant quadrotor manipulation system that has superior features over the currently developed systems and has vital applications. The following conclusions are obtained:

Successful experimental verification is done for the design of a new quadrotor manipulation system that its 2-DOF manipulator has unique topology to enable the end-effector to track arbitrary 6-DOF trajectory. This system provide solutions to limitations found in the currently developed aerial manipulation systems by having maximum mobility with minimum weight. 2. 2.

The system parameters are identified to be utilized in both controller implementation and simulation of the developed dynamic model of the system. 3. 3.

A measurement and estimation scheme is proposed and implemented to get the six states representing the position and orientation of the system by using the onboard sensors. In contrast to previous works, this scheme enables utilizing the system within arbitrary place indoors and provides potential to work outdoors as well. This makes the system capable to do real practical applications. 4. 4.

The proposed system feasibility and efficiency are proved in both simulation and experiment with a scenario consisting of taking off with an object, moving, and releasing it at a desired position. Both simulation and experimental results demonstrate a satisfactory performance. 5. 5.

A novel inverse kinematics analysis is carried out to transform the task space trajectory to quadrotor/joint space trajectory and hence enables controlling the system in the quadrotor/joint space. Such analysis has not been done before either for redundant or non-redundant aerial manipulator. 6. 6.

An algorithm is developed to get approximate solution to the complex differential algebraic equations resulted from the inverse kinematics analysis that incorporate the high order nonholonomic constraints of the system. 7. 7.

Numerical results validate the high accuracy of the inverse kinematics solution. 8. 8.

The success of the inverse kinematics proves the ability of the system to track arbitrary 6-DOF task space trajectories that lay within the capability of the system’s actuators. 9. 9.

A quite realistic simulation setup is built to test the developed control systems. 10. 10.

Numerical results show robust performance and satisfactory tracking of a desired 6-DOF task space trajectory using the developed DOb-based controller. A modification of the DOb-based controller is carried out to use the direct measurements from the onboard sensors in order to reduce the disturbance estimation error. 11. 11.

Optimum performance while keeping the control signal within the allowable limits is achieved by developing a robust optimal control based on the DOb and MPC approaches. Numerical results show that the control efforts are reduced and thus the mission time is increased. 12. 12.

A robust sensorless force estimation and impedance control scheme is developed based on the FTRLS and DOb techniques, which takes into consideration all the system dynamics with no need for the environment contact indication. The efficiency of the proposed control systems is validated via numerical results.

6.2 Future Plans

Following are some of the problems for future research work:

• •

Increasing the accuracy of the measurement of the position in $z$-axis. This can be done by replacing the sonar with a downward laser beams.

• •

Repeat the experiment with more sophisticated tasks indoors and in the field outdoors.

• •

In addition to the laser ranger, vision based localization and navigation scheme can be built to improve the motion accuracy both indoors and specially outdoors.

• •

Robust state estimation to increase the accuracy of the state estimation.

• •

Experimental implementation of the inverse kinematics, PD-DOb, MPC-DOb, and FTRLS-DOb algorithms to verify their efficiency in real time.

• •

Autonomous motion for picking and placing an object or implementing specific task.

LIST OF PUBLICATIONS

Conference Papers

Ahmed Khalifa, Mohamed Fanni, Ahmed Ramadan, and Ahmed Abo-Ismail, “Controller Design of a New Quadrotor Manipulation System Based on Robust Internal-loop Compensator”, in the IEEE International Conference on Autonomous Robot Systems and Competitions (ICARSC), Vila Real, Portugal, April 8-10, 2015, pp. 97-102. 2. 2.

Ahmed Khalifa, Mohamed Fanni, and Toru Namerikawa, “MPC and DOb-based Robust Optimal Control of a New Quadrotor Manipulation System”, in the European Control Conference (ECC), Aalborg, Denmark, June 29 - July 1, pp. 483-488. 3. 3.

Ahmed Khalifa, Mohamed Fanni, and Toru Namerikawa, “On the Fault Tolerant Control of a Quadrotor Manipulation System via MPC and DOb Approaches”, in the $55^{th}$ Annual Conference of the Society of Instrument and Control Engineers of Japan (SICE), Tsukuba, Japan, September 20-23, 2016. 4. 4.

Ahmed Khalifa, Mohamed Fanni, and Toru Namerikawa, “Hybrid 5.

Acceleration/Velocity-based Disturbance Observer for a Quadrotor Manipulation System” , in * the 2016 IEEE Multi-Conference on Systems and Control (MSC), International Conference on Control Applications (CCA), Buenos Aires, Argentina*, September 19-22, 2016.

Journal Papers

Ahmed Khalifa and Mohamed Fanni, “Position Inverse Kinematics and Robust Internal-loop Compensator-based Control of a New Quadrotor Manipulation System”, in the International Journal of Imaging and Robotics, 16.1 (2016): 94-113. 2. 2.

Ahmed Khalifa and Mohamed Fanni, “POSITION ANALYSIS AND CONTROL OF A NEW QUADROTOR MANIPULATION SYSTEM”, in the International Journal of Robotics and Automation, (Accepted for publication). 3. 3.

Ahmed Khalifa and Mohamed Fanni, “A New Quadrotor Manipulation System: Modeling and Point-to-Point Task Space Control”, in the International Journal of Control, Automation and Systems, (Accepted for publication).

Appendix A Quadrotor Platform

We use a quadrotor from Ascending Technologies (AscTec) of type Pelican that is shown in Fig. A.1

The propellers are mounted as shown in Fig. A.2

Table A.1 presents the technical specification for Pelican quadrotor from AscTec.

The quadrotor dimensions are given in Fig. A.3

Fig. A.4 illustrates functional block diagram of the FCU. This flight control unit contains all necessary sensors to function as an IMU, it also has two onboard ARM7 microprocessors and various communication interfaces.

It can directly interpret the R/C data sent by the pilot (R/C) and the serial data sent from the ground PC. For automatic missions, you can program and upload our control algorithms directly onto the High Level processor of the UAV.

The LLP handles sensor data processing, data fusion as well as our fast and stable attitude control algorithm with an update rate of 1000 Hz. It also processes the position control algorithm, using the onboard magnetometer and GPS module as additional sensor inputs. Sensor data can be retrieved from the LLP via a serial interface using a predefined serial protocol. Furthermore, the serial interface can be used to send attitude commands (pitch angle, roll angle, yaw rate and thrust) or even waypoint commands to the vehicle depending on the flight mode. The data structures from and to the LLP are

• •

LLP STATUS: Status information of the low level processor.

• •

IMU RAWDATA: Sensor raw-values.

• •

IMU CALCDATA: Calibrated sensor outputs and data fusion results.

• •

CTRL OUT: Controller outputs.

• •

RC Data: Data received by the R/C receiver.

• •

GPS Data: GPS data.

• •

GPS Data Advanced: GPS data + position and speed estimates after data fusion.

The HLP can be flashed with a custom algorithm and let it control the flight system. To recover from critical flight situations during the test flights, the pilot can always switch back to the well proven control algorithms on the LLP as a safety backup.

The two microprocessors communicate with an extremely fast rate of 1000 Hz, hence all sensor data is available to the HLP as well. Control commands can be sent back to the LLP at the same frequency, for example the: rotational speed commands for each individual motor, pitch/roll/yaw/thrust commands, attitude commands or waypoint commands. The HLP offers UART (Universal Asynchronous Receiver Transmitter), SPI (Serial Peripheral Interface) and I2C (Inter-Integrated Circuit) interfaces as well as simple port I/Os to connect other devices like additional sensors, servo motors or extension boards. Also custom payloads, can be powered by a 5 V or 12 V supply of the AscTec AutoPilot.

Fig. A.5 shows the pinout and connections of the AscTec AutoPilot board.

Fig. A.6 shows the pinout and connections of the AscTec Power Board.

The AscTec Atomboard is the perfect choice for an x86-technology computer packed into a minimum form factor. By communicating with the AscTec Autopilot, IMU data can be included into your algorithms and high level commands can be sent to the flight system. Table A.2 presents the technical data of this onboard computer.

Fig. A.7 shows the top view of pinout and connections of the AscTec Atomboard (onboard computer).

Fig. A.8 shows the bottom view of pinout and connections of the AscTec Atomboard (onboard computer).

Appendix B Robotic Arm

Starting with design of the gripper mechanism, a little gripper (Fig. B.1), from “LYNXMOTION”, is used. It is made from injection molded ABS. This gripper can open to 3.3 cm. It has a driving motor ($mtr_{2}$), see Fig. B.2, of type servo motor HS-422. This gripper can carry a payload of 200 g [93].

The total length of the arm is chosen such that it can provide enough work space. If the length is increased , then a motor with larger output torque is required, and thus larger weight and more consumed power are resulted.

Based on this design, the required specifications of the selected joints motors are set from Fig. B.2.

The torques, $T_{mtr_{0}}$ and $T_{mtr_{1}}$, of motors, $mtr_{0}$, and $mtr_{1}$), respectively, see Fig. B.2, can be calculated from(B.1 and B.2):

[TABLE]

[TABLE]

where $W_{e}$ is the weight from the end effector and the payload that it has mass of $m_{p}$.

The average weight of the available motors in the light categories is 50 g (including the arm accessories that will be described next).

Therefore, from the values of lengths and masses shown in Fig. B.2, the value of $T_{mtr_{0}}$ is 0.45 N.m and $T_{mtr_{1}}$ is 0.26 N.m (@ payload $m_{p}$ = 200 g). Multiplying this value by factor of 1.2 for safety, the selected motors should have output torque at least of 0.55 N.m for $mtr_{0}$ and 0.31 N.m for $mtr_{1}$.

A digital standard servo (HS-5485HB), see Fig. B.3, is used to provide the rotational motion (Revolute Joint) for joint 1. It has a rotational range of $180^{o}$, weight of 45 g, speed of $350^{o}/s$, and operating voltage of 6 V with maximum running current of 1 A. It is capable of producing a torque of 0.63 N.m. Another digital standard servo (HS-422) is used for both joint 2 and the gripper. It has a rotational range of 180*∘, weight of 45 g, speed of 350∘*/s, operating voltage of 6 V with maximum running current of 0.51 A, and torque of 0.32 N.m [14]. Figs. B.4 and B.5 present the dimensions of motors 1 and 2 respectively.

Fig. B.6 presents the required accessories to build the the two arms of the manipulator. Also, the dimensions of these accessories are given in Fig. B.7.

Serial servo controller (SSC-32) from LYNXMOTION, see Fig. B.8, is a small preassembled servo controller with some big features. It has high resolution (1 $\mu$s) for accurate positioning, and extremely smooth moves. The range is 0.50 ms to 2.50 ms for a range of about $180^{circ}$. This board contains a MCU of Atmel ATMEGA168-20PU as well as driver interface between the controller unit and the motors. This driver unit operates at voltage of 12 V [14]. This unit will take its commands from the Arduino MEGA 2560 board.

The pin configuration of this board is as following.

The Low Dropout regulator will provide 5vdc out with as little as 5.5vdc coming in. This is important when operating your robot from a battery. It can accept a maximum of 9vdc in. The regulator is rated for 500mA, but we are de-rating it to 250mA to prevent the regulator from getting too hot. 2. 2.

This terminal connects power to servo channels 16 through 31. Apply 4.8vdc to 6.0vdc for most analog or digital servos. This can be directly from a 5-cell NiMH battery pack. 7.2vdc - 7.4vdc can be applied to HSR-5980 or HSR- 5990 servos. This can be directly from a 6-cell NiMH battery pack or a 2-cell LiPo battery pack. 3. 3.

These jumpers are used to connect VS1 to VS2. Use this option when you are powering all servos from the same battery. Use both jumpers. Alternately, if you want to use two separate battery packs, one on each side, then remove both of these jumpers. 4. 4.

This is the Logic Voltage, or VL. This input is normally used with a 9vdc battery connector to provide power to the ICs and anything connected to the 5vdc lines on the board. The valid range for this terminal is 6vdc - 9vdc. This input is used to isolate the logic from the Servo Power Input. It is necessary to remove the VS1=VL jumper when powering the servos separately from the logic VL. The SSC-32 should draw 35mA with nothing connected to the 5vdc output 5. 5.

This jumper allows powering the microcontroller and support circuitry from the servo power supply. This requires at least 6vdc to operate correctly. If the microcontroller resets when too many servos are moving at the same time, it may be necessary to power the microcontroller separately using the VL input. A 9vdc works nicely for this. This jumper must be removed when powering the microcontroller separately 6. 6.

This terminal connects power to servo channels 16 through 31. Apply 4.8vdc to 6.0vdc for most analog or digital servos. This can be directly from a 5-cell NiMH battery pack. 7.2vdc - 7.4vdc can be applied to HSR-5980 or HSR- 5990 servos. This can be directly from a 6-cell NiMH battery pack or a 2-cell LiPo battery pack. 7. 7.

This is where you connect the servos or other output devices. Use caution and remove power when connecting anything to the I/O bus. 8. 8.

This is where the Atmel IC chip goes. Be careful to insert it with Pin 1 in the upper right corner as pictured. Take care to not bend the pins. 9. 9.

The two BAUD inputs allow configuring the baud rate. 10. 10.

The ABCD inputs have both static and latching support. The inputs have internal weak (50k) pullups that are used when a Read Digital Input command is used. A normally open switch connected from the input to ground will work fine. 11. 11.

This is the Processor Good LED. It will light steady when power is applied and will remain lit until the processor has received a valid serial command. It will then go out and will blink whenever it is receiving serial data. 12. 12.

Simply plug a straight-through M/F DB9 cable from this plug to a free 9-pin serial port con your PC for receiving servo positioning data. Alternately a USB-to-serial adapter will work well. Note, many USB-to-serial adapters require a separate power supply to work well. 13. 13.

This is an 8-pin EEPROM socket. 14. 14.

This is the TTL serial port or DB9 serial port enable. Install two jumpers as illustrated below to enable the DB9 port. Install wire connectors to utilize TTL serial communication from a host microcontroller.

Fig. B.9 presents the way to mount the jumpers of the SSC32 board to enable the communication between this board and the Arduino board such that one can send the motor commands from the Arduino to the SSC32 board.

One microcontroller that has gotten special attention from the robotics community world-wide is the Arduino. This microcontroller platform is quite inexpensive and has a C-based language development environment that is very intuitive to use. From the different versions of the Arduino, the selected one for this project was the Arduino Mega 2560 (see Fig. B.10). It has 54 digital input/output pins of which 15 can be used as PWM outputs, 16 analog inputs, 4 UARTs (hardware serial ports), 16 MHz crystal oscillator, USB connection, power jack, and 256 KB flash memory for storing code [15]. This board has a open source ROS library.

Fig. B.11) shows the circuit diagram for the 12 V to 5V 1.5 A DC-DC converter that is used to power the manipulator’s motors. This circuit uses the 7805 IC to converts the 12 VDC from the quadrotor battery to the required 5 VDC at 1.5 A.

Appendix C Forward Kinematics

⬇

1function Xe = For_Kin(zt,sb)

2L0 = 30e-03;

3L1 = 70e-03;

4L2 = 85e-03;

5X = zt(1);

6Y = zt(2);

7Z = zt(3);

8ep = zt(4);

9th1 = zt(5);

10th2 = zt(6);

11ph = sb(2);

12th = sb(1);

13%% =================================

14% A0_B = Transformation matrix from frame 0 to frame B

15A0_B = [0 0 1 0; -1 0 0 0; 0 -1 0 -L0; 0 0 0 1];

16A1_0 = [cos(th1) 0 sin(th1) L1cos(th1); sin(th1) 0 -cos(th1) L1sin(th1);

170 1 0 0; 0 0 0 1];

18A2_1 = [cos(th2) -sin(th2) 0 L2cos(th2); sin(th2) cos(th2) 0 L2sin(th2);

190 0 1 0; 0 0 0 1];

20AB_I = [cos(ep)*cos(th), cos(ep)*sin(ph)*sin(th)-cos(ph)*sin(ep),

21 sin(ep)*sin(ph)+cos(ep)*cos(ph)*sin(th),X;

22cos(th)*sin(ep), cos(ep)*cos(ph)+sin(ep)*sin(ph)*sin(th),

23 cos(ph)*sin(ep)*sin(th)-cos(ep)*sin(ph),Y;

24-sin(th), cos(th)*sin(ph), cos(ph)*cos(th),Z;

250,0, 0, 1];

26T2_I = AB_I* A0_B * A1_0 * A2_1;

27% T2_I = eval(T2_I);

28xe = T2_I(1,4);

29ye = T2_I(2,4);

30ze = T2_I(3,4);

31%% Computing Euler angles from a rotation matrix

32R31 = T2_I(3,1);

33R32 = T2_I(3,2);

34R33 = T2_I(3,3);

35R21 = T2_I(2,1);

36R11 = T2_I(1,1);

37R12 = T2_I(1,2);

38R13 = T2_I(1,3);

39if (R31~=+1 && R31~=-1)

40the = -asin(R31);

41%or

42% the = pi - (-asin(R31))

43phe = atan2(R32/cos(the),R33/cos(the));

44epe = atan2(R21/cos(the),R11/cos(the));

45else

46epe =0; %anything; can set to 0

47if (R31 == -1)

48the = pi/2;

49phe = epe + atan2(R12,R13);

50else

51the = -pi/2;

52phe = -epe + atan2(-R12, -R13);

53end

54end

55Xe = [xe;ye;ze;epe;the;phe];

56end

Appendix D System Jacobian and Parametrization

The coding of the system Jacobian is given as following.

⬇

1function [Jz,Js,TAe] = jacob(Xdde_ref,zt,ztd,sb,sbd,Xe,sbdd,Jzd,Jsd,TAed,Xde)

2L0 = 30e-03 ;

3L1 = 70e-03;

4L2 = 85e-03;

5

6X=zt(1);

7Y=zt(2);

8Z=zt(3);

9ep=zt(4);

10th1=zt(5);

11th2=zt(6);

12

13ph=sb(2);

14th=sb(1);

15

16phd=sbd(2);

17thd=sbd(1);

18

19xe=Xe(1);

20ye=Xe(2);

21ze=Xe(3);

22epe=Xe(4);

23the=Xe(5);

24phe=Xe(6);

25%% ===================

26I3=eye(3); O3=zeros(3);

27A0_B = [0 0 1 0; -1 0 0 0; 0 -1 0 -L0; 0 0 0 1];

28A1_0 = [cos(th1) 0 sin(th1) L1cos(th1); sin(th1) 0 -cos(th1) L1sin(th1);

290 1 0 0; 0 0 0 1];

30A2_1 = [cos(th2) -sin(th2) 0 L2cos(th2); sin(th2) cos(th2) 0 L2sin(th2);

310 0 1 0; 0 0 0 1];

32

33Rb_I = [cos(ep)*cos(th), cos(ep)*sin(ph)*sin(th)-cos(ph)*sin(ep),

34 sin(ep)*sin(ph)+cos(ep)*cos(ph)*sin(th);

35cos(th)*sin(ep), cos(ep)*cos(ph)+sin(ep)*sin(ph)*sin(th),

36 cos(ph)*sin(ep)*sin(th)-cos(ep)*sin(ph);

37-sin(th), cos(th)*sin(ph), cos(ph)*cos(th)];

38

39R0_B=A0_B(1:3,1:3);

40R1_0=A1_0(1:3,1:3);

41RB_I=Rb_I;

42%

43a1=L1; d1=0;

44a2=L2; d2=0;

45z0=eye(3)[0;0;1];z1=R1_0[0;0;1];

46r1_0=[a1cos(th1);a1sin(th1);d1];

47r2_1=[a2cos(th2);a2sin(th2);d2];

48p2_2=[0;0;0];

49p2_1=R1_0*r2_1+p2_2;

50p2_0=eye(3)*r1_0+p2_1;

51alpha1=cross(z0,p2_0);

52beta1=z0;

53alpha2=cross(z1,p2_1);

54beta2=z1;

55A2_B=A0_BA1_0A2_1;

56peb_b=A2_B(1:3,4);

57Jt=[R0_Balpha1,R0_Balpha2];

58Jr=[R0_Bbeta1,R0_Bbeta2];

59Jeb_b=[Jt;Jr];

60Rb=RB_I;

61Tb=[0,-sin(ep),cos(ep)*cos(th);

620,cos(ep),sin(ep)*cos(th);

631,0,-sin(th)];

64TAb=[I3,O3;O3,Tb];

65Jb=[I3,-skew(Rb*peb_b);O3,I3];

66Jeb=[Rb,O3;O3,Rb]*Jeb_b;

67JbA=Jb*TAb;

68Jet=JbA(:,1:4);

69Jz=[Jet,Jeb];

70Js=JbA(:,5:6);

71Te=[0,-sin(epe),cos(epe)*cos(the);

720,cos(epe),sin(epe)*cos(the);

731,0,-sin(the)];

74TAe=[I3,O3;O3,Te];

75end

The details and coding of the regressor matrices, $Y_{w}$, $Y_{e}$, $Y_{l}$, $Y_{i}$, and $Y_{s}$, are given as following.

⬇

1syms ph th ep th1 th2 xd yd zd phd thd epd thd1 thd2 xdd ydd zdd phdd thdd epdd thdd1 thdd2

2syms mb m1 m2 Ix Iy Iz I1 I2 L0 L1 L2 g f1 f2 f3 f4 Tm1 Tm2 d c

3syms L0m1 L0m2 L1m1 L1m2 L2m1 L2m2 L02m1 L02m2 L12m1 L12m2 L22m1 L22m2

4syms L0L1m1 L0L1m2 L1L2m1 L1L2m2 L0L2m1 L0L2m2 zzz

5% x=sym('x','real'); y=sym('y','real'); z=sym('z','real');

6ph=sym('ph','real'); th=sym('th','real'); ep=sym('ep','real');

7th1=sym('th1','real'); th2=sym('th2','real');

8L0=sym('L0','real');L1=sym('L1','real'); L2=sym('L2','real');

9mb=sym('mb','real');m1=sym('m1','real'); m2=sym('m2','real');

10I1=sym('I1','real'); I2=sym('I2','real'); Ix=sym('Ix','real');

11g=sym('g','real');

12

13xd=sym('xd','real'); yd=sym('yd','real'); zd=sym('zd','real');

14phd=sym('phd','real'); thd=sym('thd','real'); epd=sym('epd','real');

15thd1=sym('thd1','real'); thd2=sym('thd2','real');

16

17xdd=sym('xdd','real'); ydd=sym('ydd','real'); zdd=sym('zdd','real');

18phdd=sym('phdd','real'); thdd=sym('thdd','real'); epdd=sym('epdd','real');

19thdd1=sym('thdd1','real'); thdd2=sym('thdd2','real');

20%% ====================================================

21load('Mqdd_Cqd_G.mat');

22

23TH = [m1;m2;mb;I1;I2;Ix;L0m1;L0m2;L1m1;L1m2;L2m1;L2m2;L0^2m1;L0^2m2;L1^2m1;L1^2m2;L2^2m1;L2^2m2;L0L1m1;L0L1m2;L1L2m1;L1L2m2;L0L2m1;L0L2m2];

24

25Y1_TH = zzz*ones(size(TH'))*TH + Mqdd_Cqd_G(1);

26Y2_TH = zzz*ones(size(TH'))*TH + Mqdd_Cqd_G(2);

27Y3_TH = zzz*ones(size(TH'))*TH + Mqdd_Cqd_G(3);

28Y4_TH = zzz*ones(size(TH'))*TH + Mqdd_Cqd_G(4);

29Y5_TH = zzz*ones(size(TH'))*TH + Mqdd_Cqd_G(5);

30Y6_TH = zzz*ones(size(TH'))*TH + Mqdd_Cqd_G(6);

31Y7_TH = zzz*ones(size(TH'))*TH + Mqdd_Cqd_G(7);

32Y8_TH = zzz*ones(size(TH'))*TH + Mqdd_Cqd_G(8);

33

34Y1_TH=subs(Y1_TH,[L0L1m1,L0L1m2,L1L2m1,L1L2m2,L0L2m1,L0L2m2,L0^2m1,L0^2m2,L1^2m1,L1^2m2,L2^2m1,L2^2m2,L0m1,L0m2,L1m1,L1m2,L2m1,L2m2] ...

35,[L0L1m1 ,L0L1m2 ,L1L2m1 ,L1L2m2 ,L0L2m1 ,L0L2m2 ,L02m1 ,L02m2 ,L12m1 ,L12m2 ,L22m1 ,L22m2 ,L0m1 ,L0m2 ,L1m1 ,L1m2 ,L2m1 ,L2m2]);

36[cy1,ty1] = coeffs(Y1_TH, [m1,m2,mb,I1,I2,Ix,L0m1,L0m2,L1m1,L1m2,L2m1,L2m2,L02m1,L02m2,L12m1,L12m2,L22m1,L22m2,L0L1m1,L0L1m2,L1L2m1,L1L2m2,L0L2m1,L0L2m2]);

37%===================================

38Y2_TH=subs(Y2_TH,[L0L1m1,L0L1m2,L1L2m1,L1L2m2,L0L2m1,L0L2m2,L0^2m1,L0^2m2,L1^2m1,L1^2m2,L2^2m1,L2^2m2,L0m1,L0m2,L1m1,L1m2,L2m1,L2m2] ...

39,[L0L1m1 ,L0L1m2 ,L1L2m1 ,L1L2m2 ,L0L2m1 ,L0L2m2 ,L02m1 ,L02m2 ,L12m1 ,L12m2 ,L22m1 ,L22m2 ,L0m1 ,L0m2 ,L1m1 ,L1m2 ,L2m1 ,L2m2]);

40[cy2,ty2] = coeffs(Y2_TH, [m1,m2,mb,I1,I2,Ix,L0m1,L0m2,L1m1,L1m2,L2m1,L2m2,L02m1,L02m2,L12m1,L12m2,L22m1,L22m2,L0L1m1,L0L1m2,L1L2m1,L1L2m2,L0L2m1,L0L2m2]);

41%===================================

42Y3_TH=subs(Y3_TH,[L0L1m1,L0L1m2,L1L2m1,L1L2m2,L0L2m1,L0L2m2,L0^2m1,L0^2m2,L1^2m1,L1^2m2,L2^2m1,L2^2m2,L0m1,L0m2,L1m1,L1m2,L2m1,L2m2] ...

43,[L0L1m1 ,L0L1m2 ,L1L2m1 ,L1L2m2 ,L0L2m1 ,L0L2m2 ,L02m1 ,L02m2 ,L12m1 ,L12m2 ,L22m1 ,L22m2 ,L0m1 ,L0m2 ,L1m1 ,L1m2 ,L2m1 ,L2m2]);

44[cy3,ty3] = coeffs(Y3_TH, [m1,m2,mb,I1,I2,Ix,L0m1,L0m2,L1m1,L1m2,L2m1,L2m2,L02m1,L02m2,L12m1,L12m2,L22m1,L22m2,L0L1m1,L0L1m2,L1L2m1,L1L2m2,L0L2m1,L0L2m2]);

45%===================================

46Y4_TH=subs(Y4_TH,[L0L1m1,L0L1m2,L1L2m1,L1L2m2,L0L2m1,L0L2m2,L0^2m1,L0^2m2,L1^2m1,L1^2m2,L2^2m1,L2^2m2,L0m1,L0m2,L1m1,L1m2,L2m1,L2m2] ...

47,[L0L1m1 ,L0L1m2 ,L1L2m1 ,L1L2m2 ,L0L2m1 ,L0L2m2 ,L02m1 ,L02m2 ,L12m1 ,L12m2 ,L22m1 ,L22m2 ,L0m1 ,L0m2 ,L1m1 ,L1m2 ,L2m1 ,L2m2]);

48[cy4,ty4] = coeffs(Y4_TH, [m1,m2,mb,I1,I2,Ix,L0m1,L0m2,L1m1,L1m2,L2m1,L2m2,L02m1,L02m2,L12m1,L12m2,L22m1,L22m2,L0L1m1,L0L1m2,L1L2m1,L1L2m2,L0L2m1,L0L2m2]);

49%===================================

50Y5_TH=subs(Y5_TH,[L0L1m1,L0L1m2,L1L2m1,L1L2m2,L0L2m1,L0L2m2,L0^2m1,L0^2m2,L1^2m1,L1^2m2,L2^2m1,L2^2m2,L0m1,L0m2,L1m1,L1m2,L2m1,L2m2] ...

51,[L0L1m1 ,L0L1m2 ,L1L2m1 ,L1L2m2 ,L0L2m1 ,L0L2m2 ,L02m1 ,L02m2 ,L12m1 ,L12m2 ,L22m1 ,L22m2 ,L0m1 ,L0m2 ,L1m1 ,L1m2 ,L2m1 ,L2m2]);

52[cy5,ty5] = coeffs(Y5_TH, [m1,m2,mb,I1,I2,Ix,L0m1,L0m2,L1m1,L1m2,L2m1,L2m2,L02m1,L02m2,L12m1,L12m2,L22m1,L22m2,L0L1m1,L0L1m2,L1L2m1,L1L2m2,L0L2m1,L0L2m2]);

53%===================================

54Y6_TH=subs(Y6_TH,[L0L1m1,L0L1m2,L1L2m1,L1L2m2,L0L2m1,L0L2m2,L0^2m1,L0^2m2,L1^2m1,L1^2m2,L2^2m1,L2^2m2,L0m1,L0m2,L1m1,L1m2,L2m1,L2m2] ...

55,[L0L1m1 ,L0L1m2 ,L1L2m1 ,L1L2m2 ,L0L2m1 ,L0L2m2 ,L02m1 ,L02m2 ,L12m1 ,L12m2 ,L22m1 ,L22m2 ,L0m1 ,L0m2 ,L1m1 ,L1m2 ,L2m1 ,L2m2]);

56[cy6,ty6] = coeffs(Y6_TH, [m1,m2,mb,I1,I2,Ix,L0m1,L0m2,L1m1,L1m2,L2m1,L2m2,L02m1,L02m2,L12m1,L12m2,L22m1,L22m2,L0L1m1,L0L1m2,L1L2m1,L1L2m2,L0L2m1,L0L2m2]);

57%===================================

58Y7_TH=subs(Y7_TH,[L0L1m1,L0L1m2,L1L2m1,L1L2m2,L0L2m1,L0L2m2,L0^2m1,L0^2m2,L1^2m1,L1^2m2,L2^2m1,L2^2m2,L0m1,L0m2,L1m1,L1m2,L2m1,L2m2] ...

59,[L0L1m1 ,L0L1m2 ,L1L2m1 ,L1L2m2 ,L0L2m1 ,L0L2m2 ,L02m1 ,L02m2 ,L12m1 ,L12m2 ,L22m1 ,L22m2 ,L0m1 ,L0m2 ,L1m1 ,L1m2 ,L2m1 ,L2m2]);

60[cy7,ty7] = coeffs(Y7_TH, [m1,m2,mb,I1,I2,Ix,L0m1,L0m2,L1m1,L1m2,L2m1,L2m2,L02m1,L02m2,L12m1,L12m2,L22m1,L22m2,L0L1m1,L0L1m2,L1L2m1,L1L2m2,L0L2m1,L0L2m2]);

61%===================================

62Y8_TH=subs(Y8_TH,[L0L1m1,L0L1m2,L1L2m1,L1L2m2,L0L2m1,L0L2m2,L0^2m1,L0^2m2,L1^2m1,L1^2m2,L2^2m1,L2^2m2,L0m1,L0m2,L1m1,L1m2,L2m1,L2m2] ...

63,[L0L1m1 ,L0L1m2 ,L1L2m1 ,L1L2m2 ,L0L2m1 ,L0L2m2 ,L02m1 ,L02m2 ,L12m1 ,L12m2 ,L22m1 ,L22m2 ,L0m1 ,L0m2 ,L1m1 ,L1m2 ,L2m1 ,L2m2]);

64[cy8,ty8] = coeffs(Y8_TH, [m1,m2,mb,I1,I2,Ix,L0m1,L0m2,L1m1,L1m2,L2m1,L2m2,L02m1,L02m2,L12m1,L12m2,L22m1,L22m2,L0L1m1,L0L1m2,L1L2m1,L1L2m2,L0L2m1,L0L2m2]);

65

66Yi = [cy1;cy2;cy3;cy4;cy5;cy6;cy7;cy8];

67

68%% =================================================================

69

70syms xe ye ze phe the epe

71syms xde yde zde phde thde epde

72syms Kc1 Mc1 Kc2 Mc2 Kc3 Mc3 Kc4 Mc4 Kc5 Mc5 Kc6 Mc6

73Xe=[xe; ye; ze; epe; the; phe];

74Xde=[xde; yde; zde; epde; thde; phde];

75

76%% ======= Yl ==============

77

78Fe_reg = [Xe(1),Xde(1),zeros(1,10);

79zeros(1,2),Xe(2),Xde(2),zeros(1,8);

80zeros(1,4),Xe(3),Xde(3),zeros(1,6);

81zeros(1,6),Xe(4),Xde(4),zeros(1,4);

82zeros(1,8),Xe(5),Xde(5),zeros(1,2);

83zeros(1,10),Xe(6),Xde(6)];

84

85Y_l = JT*Fe_reg;

86

87%% ======= Yw ==============

88D_ex_reg1 = [z^2sin(th), z^2cos(th), 0, 0];

89D_ex_reg2 = [0, 0,z^2sin(ph), z^2cos(ph)];

90D_ex_reg3 = zeros(1,4);

91D_ex_reg4 = zeros(1,4);

92D_ex_reg5 = zeros(1,4);

93D_ex_reg6 = zeros(1,4);

94D_ex_reg7 = zeros(1,4);

95D_ex_reg8 = zeros(1,4);

96%% ======= Yl ==============

97JT_Fe_reg1=[ xe, xde, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0];

98JT_Fe_reg2=[ 0, 0, ye, yde, 0, 0, 0, 0, 0, 0, 0, 0];

99JT_Fe_reg3=[ 0, 0, 0, 0, ze, zde, 0, 0, 0, 0, 0, 0];

100JT_Fe_reg4=[ -xe*((cos(ep)sin(ph) - cos(ph)sin(ep)sin(th))(L0 + L1sin(th1) + L2cos(th2)sin(th1)) - cos(th1)(L1 + L2cos(th2))(cos(ep)*cos(ph) + sin(ep)sin(ph)sin(th)) + L2cos(th)sin(ep)sin(th2)), -xde((cos(ep)sin(ph) - cos(ph)sin(ep)sin(th))(L0 + L1sin(th1) + L2cos(th2)sin(th1)) - cos(th1)(L1 + L2cos(th2))(cos(ep)cos(ph) + sin(ep)sin(ph)sin(th)) + L2cos(th)sin(ep)sin(th2)), ye(cos(th1)(L1 + L2cos(th2))(cos(ph)*sin(ep) - cos(ep)sin(ph)sin(th)) - (sin(ep)sin(ph) + cos(ep)cos(ph)sin(th))(L0 + L1sin(th1) + L2cos(th2)sin(th1)) + L2cos(ep)cos(th)sin(th2)), yde(cos(th1)(L1 + L2cos(th2))(cos(ph)*sin(ep) - cos(ep)*sin(ph)*sin(th)) - (sin(ep)sin(ph) + cos(ep)cos(ph)sin(th))(L0 + L1sin(th1) + L2cos(th2)sin(th1)) + L2cos(ep)*cos(th)*sin(th2)), 0, 0, 0, 0, 0, 0, phe, phde];

101JT_Fe_reg5=[ -xecos(ep)(L2sin(th)sin(th2) + cos(ph)cos(th)(L0 + L1sin(th1) + L2cos(th2)sin(th1)) + cos(th)cos(th1)sin(ph)(L1 + L2cos(th2))), -xdecos(ep)(L2sin(th)sin(th2) + cos(ph)cos(th)(L0 + L1sin(th1) + L2cos(th2)sin(th1)) + cos(th)cos(th1)sin(ph)(L1 + L2cos(th2))), -yesin(ep)(L2sin(th)sin(th2) + cos(ph)cos(th)(L0 + L1sin(th1) + L2cos(th2)sin(th1)) + cos(th)cos(th1)sin(ph)(L1 + L2cos(th2))), -ydesin(ep)(L2sin(th)sin(th2) + cos(ph)cos(th)(L0 + L1sin(th1) + L2cos(th2)sin(th1)) + cos(th)cos(th1)sin(ph)(L1 + L2cos(th2))), ze(L0cos(ph)sin(th) - L2cos(th)sin(th2) + L1cos(ph)*sin(th)sin(th1) + L1cos(th1)*sin(ph)sin(th) + L2cos(ph)*cos(th2)*sin(th)sin(th1) + L2cos(th1)*cos(th2)sin(ph)sin(th)), zde(L0cos(ph)sin(th) - L2cos(th)sin(th2) + L1cos(ph)*sin(th)sin(th1) + L1cos(th1)*sin(ph)sin(th) + L2cos(ph)*cos(th2)sin(th)sin(th1) + L2cos(th1)cos(th2)sin(ph)sin(th)), -epesin(ep), -epdesin(ep), thecos(ep), thdecos(ep), 0, 0];

102JT_Fe_reg6=[ xe*(sin(th)((cos(ep)sin(ph) - cos(ph)sin(ep)sin(th))(L0 + L1sin(th1) + L2cos(th2)sin(th1)) - cos(th1)(L1 + L2cos(th2))(cos(ep)cos(ph) + sin(ep)sin(ph)sin(th)) + L2cos(th)sin(ep)sin(th2)) - cos(th)sin(ep)(L2sin(th)sin(th2) + cos(ph)cos(th)(L0 + L1sin(th1) + L2cos(th2)sin(th1)) + cos(th)cos(th1)sin(ph)(L1 + L2cos(th2)))), xde(sin(th)((cos(ep)sin(ph) - cos(ph)sin(ep)sin(th))(L0 + L1sin(th1) + L2cos(th2)sin(th1)) - cos(th1)(L1 + L2cos(th2))(cos(ep)cos(ph) + sin(ep)sin(ph)sin(th)) + L2cos(th)sin(ep)sin(th2)) - cos(th)sin(ep)(L2sin(th)sin(th2) + cos(ph)cos(th)(L0 + L1sin(th1) + L2cos(th2)sin(th1)) + cos(th)cos(th1)sin(ph)(L1 + L2cos(th2)))), -ye(sin(th)(cos(th1)(L1 + L2cos(th2))(cos(ph)sin(ep) - cos(ep)sin(ph)sin(th)) - (sin(ep)sin(ph) + cos(ep)cos(ph)sin(th))(L0 + L1sin(th1) + L2cos(th2)sin(th1)) + L2cos(ep)cos(th)sin(th2)) - cos(ep)cos(th)(L2sin(th)sin(th2) + cos(ph)cos(th)(L0 + L1sin(th1) + L2cos(th2)sin(th1)) + cos(th)cos(th1)sin(ph)(L1 + L2cos(th2)))), -yde(sin(th)(cos(th1)(L1 + L2cos(th2))(cos(ph)sin(ep) - cos(ep)sin(ph)sin(th)) - (sin(ep)sin(ph) + cos(ep)cos(ph)sin(th))(L0 + L1sin(th1) + L2cos(th2)sin(th1)) + L2cos(ep)cos(th)sin(th2)) - cos(ep)cos(th)(L2sin(th)sin(th2) + cos(ph)cos(th)(L0 + L1sin(th1) + L2cos(th2)sin(th1)) + cos(th)cos(th1)sin(ph)(L1 + L2cos(th2)))), -zecos(th)((L2cos(ph + th1 - th2))/2 + L1cos(ph + th1) - L0sin(ph) + (L2cos(ph + th1 + th2))/2), -zdecos(th)((L2cos(ph + th1 - th2))/2 + L1cos(ph + th1) - L0sin(ph) + (L2cos(ph + th1 + th2))/2), epecos(ep)cos(th), epdecos(ep)cos(th), thecos(th)sin(ep), thdecos(th)sin(ep), -phesin(th), -phde*sin(th)];

103JT_Fe_reg7= [ -xe*((L1sin(th1) + L2cos(th2)sin(th1))(cos(ph)*sin(ep) - cos(ep)sin(ph)sin(th)) + (L1cos(th1) + L2cos(th1)cos(th2))(sin(ep)sin(ph) + cos(ep)cos(ph)sin(th))), -xde((L1sin(th1) + L2cos(th2)sin(th1))(cos(ph)sin(ep) - cos(ep)sin(ph)sin(th)) + (L1cos(th1) + L2cos(th1)cos(th2))(sin(ep)sin(ph) + cos(ep)cos(ph)sin(th))), ye((L1sin(th1) + L2cos(th2)sin(th1))(cos(ep)cos(ph) + sin(ep)sin(ph)sin(th)) + (L1cos(th1) + L2cos(th1)cos(th2))(cos(ep)sin(ph) - cos(ph)sin(ep)sin(th))), yde((L1sin(th1) + L2cos(th2)sin(th1))(cos(ep)cos(ph) + sin(ep)sin(ph)sin(th)) + (L1cos(th1) + L2cos(th1)cos(th2))(cos(ep)sin(ph) - cos(ph)sin(ep)sin(th))), -zecos(ph + th1)cos(th)(L1 + L2cos(th2)), -zdecos(ph + th1)cos(th)(L1 + L2cos(th2)), epecos(ep)cos(th), epdecos(ep)cos(th), thecos(th)sin(ep), thdecos(th)sin(ep), -phesin(th), -phdesin(th)];

104JT_Fe_reg8=[ xe*(L2*cos(ep)*cos(th)cos(th2) - L2cos(th1)sin(th2)(cos(ph)*sin(ep) - cos(ep)*sin(ph)sin(th)) + L2sin(th1)sin(th2)(sin(ep)*sin(ph) + cos(ep)cos(ph)sin(th))), xde(L2cos(ep)*cos(th)cos(th2) - L2cos(th1)sin(th2)(cos(ph)*sin(ep) - cos(ep)*sin(ph)sin(th)) + L2sin(th1)sin(th2)(sin(ep)*sin(ph) + cos(ep)cos(ph)sin(th))), ye(L2cos(th)*cos(th2)sin(ep) + L2cos(th1)sin(th2)(cos(ep)*cos(ph) + sin(ep)*sin(ph)sin(th)) - L2sin(th1)sin(th2)(cos(ep)*sin(ph) - cos(ph)sin(ep)sin(th))), yde(L2cos(th)*cos(th2)sin(ep) + L2cos(th1)sin(th2)(cos(ep)*cos(ph) + sin(ep)*sin(ph)sin(th)) - L2sin(th1)sin(th2)(cos(ep)*sin(ph) - cos(ph)sin(ep)sin(th))), ze(L2cos(ph)*cos(th)*sin(th1)sin(th2) - L2cos(th2)sin(th) + L2cos(th)*cos(th1)sin(ph)sin(th2)), zde(L2cos(ph)*cos(th)*sin(th1)sin(th2) - L2cos(th2)sin(th) + L2cos(th)*cos(th1)sin(ph)sin(th2)), epe(cos(th1)(sin(ep)*sin(ph) + cos(ep)*cos(ph)sin(th)) + sin(th1)(cos(ph)*sin(ep) - cos(ep)sin(ph)sin(th))), epde(cos(th1)(sin(ep)*sin(ph) + cos(ep)*cos(ph)sin(th)) + sin(th1)(cos(ph)*sin(ep) - cos(ep)sin(ph)sin(th))), -the(cos(th1)(cos(ep)*sin(ph) - cos(ph)*sin(ep)sin(th)) + sin(th1)(cos(ep)*cos(ph) + sin(ep)sin(ph)sin(th))), -thde(cos(th1)(cos(ep)*sin(ph) - cos(ph)*sin(ep)sin(th)) + sin(th1)(cos(ep)*cos(ph) + sin(ep)*sin(ph)sin(th))), phecos(ph + th1)cos(th), phdecos(ph + th1)*cos(th)];

105

106%% ==================== Ys =================================

107Ys=[Ya(1,:),JT_Fe_reg1,D_ex_reg1;

108Ya(2,:),JT_Fe_reg2,D_ex_reg2;

109Ya(3,:),JT_Fe_reg3,D_ex_reg3;

110Ya(4,:),JT_Fe_reg4,D_ex_reg4;

111Ya(5,:),JT_Fe_reg5,D_ex_reg5;

112Ya(6,:),JT_Fe_reg6,D_ex_reg6;

113Ya(7,:),JT_Fe_reg7,D_ex_reg7;

114Ya(8,:),JT_Fe_reg8,D_ex_reg8];

115

116%% ==================== hs ==================================

117 hs = [m1;m2;mb;I1;I2;Ix;L0m1;L0m2;L1m1;L1m2;L2m1;L2m2;L0^2m1;L0^2m2;L1^2m1;L1^2m2;L2^2*m

Appendix E Laser Range Finder

URG-04LX is a laser sensor for area scanning, see Fig. E.1. The light source of the sensor is infrared laser of wavelength 785nm with laser class 1 safety. Scan area is $240^{\circ}$ semicircle with maximum radius 4000mm. Principle of distance measurement is based on calculation of the phase difference, due to which it is possible to obtain stable measurement with minimum influence from object’s color and reflectance

It has the following features:

• •

High accuracy, high resolution and wide angle which provide the best solution for autonomous robots moving in the unknown environment.

• •

Compact size allows more designing freedom. Light weight and low power consumption contribute to the long time operations.

• •

No influence by how bright the environment will be. Excellent performance in darkness. Recognizing human body’s size and position without hurting their privacy.

Fig. E.2 illustrates the principle of operation of the URG-04LX. The laser emits an infrared beam, and a rotating mirror changes the beam’s direction. Then the laser hits the surface of an object and is reflected. The direction of reflected light is changed again by a rotating mirror, and captured by the photo diode. The phases of the emitted and received light are compared and the distance between the sensor and the object is calculated. A rotating mirror sweeps the laser beam horizontally over a range of $240^{\circ}$, with an angular resolution of $0.36^{\circ}$. As the mirror rotates at about 600 rpm, the scan rate is about 100 msec.

Technical specifications of this device is given in Table E.1.

Fig. E.3 illustrates schematic diagrams with the relevant dimensions.

There are two connectors RS-connector (Table E.2) and USB connector (Table E.3)

Appendix F Ultrasonic Ranger

The Devantech SRF04 ultrasonic range finder (see Fig. F.1) provides precise, non-contact distance measurements from about 3 cm to 3 m. The SRF04 works by transmitting an ultrasonic (well above human hearing range) pulse and measuring the time it takes to “hear” the pulse echo. Output from the SRF04 is in the form of a variable-width pulse that corresponds to the distance to the target.

Technical specifications of this device is given in Table F.1.

Fig. F.2 illustrates schematic diagrams of the sonar with the relevant dimensions.

Fig. F.3 shows the terminal connections of the sonar.

The SRF04 beam pattern is given in Fig. F.4. Its Timing diagram is shown in Fig. F.5. We only need to supply a short 10 $\mu s$ pulse to the trigger input to start the ranging. The SRF04 will send out an 8 cycle burst of ultrasound at 40khz and raise its echo line high. It then listens for an echo, and as soon as it detects one it lowers the echo line again. The echo line is therefore a pulse whose width is proportional to the distance to the object. By timing the pulse it is possible to calculate the range in inches/centimeters or anything else. If the width of the pulse is measured in $\mu s$, then dividing by 58 will give you the distance in cm.

Appendix G System Internal Connections

Internal connections (i.e., power and data connections) of the proposed robot are given in Fig. G.1

Appendix H Robot Operating System (ROS)

H.1 ROS

ROS is an open-source, meta-operating system for your robot. It provides the services you would expect from an operating system, including hardware abstraction, low-level device control, implementation of commonly-used functionality, message-passing between processes, and package management. It also provides tools and libraries for obtaining, building, writing, and running code across multiple computers.

Why do we use ROS?

There are specific issues in the development of software for robots that ROS can help to resolve, which are:

• •

Distributed computation: Many modern robot systems rely on software that spans many different processes and runs across several different computers. For example: Some robots carry multiple computers, each of which controls a subset of the robot’s sensors or actuators. Even within a single computer, it’s often a good idea to divide the robot’s software into small, stand-alone parts that cooperate to achieve the overall goal. This approach is sometimes called “complexity via composition ”.

• •

Software reuse: The rapid progress of robotics research has resulted in a growing collection of good algorithms for common tasks such as navigation, motion planning, mapping, and many others. Of course, the existence of these algorithms is only truly useful if there is a way to apply them in new contexts, without the need to reimplement each algorithm for each new system. As a result, developers that use ROS can expect—after, of course, climbing ROS’s initial learning curve—to focus more time on experimenting with new ideas, and less time reinventing wheels.

• •

Rapid testing: One of the reasons that software development for robots is often more challenging than other kinds of development is that testing can be time consuming and error-prone. Physical robots may not always be available to work with, and when they are, the process is sometimes slow and finicky. Working with ROS provides two effective workarounds to this problem.

• –

Well-designed ROS systems separate the low-level direct control of the hardware and high-level processing and decision making into separate programs. Because of this separation, we can temporarily replace those low-level programs (and their corresponding hardware) with a simulator, to test the behavior of the high-level part of the system.

• –

ROS also provides a simple way to record and play back sensor data and other kinds of messages.

Therefore, because the real robot, the simulator, and the bag playback mechanism can all provide identical (or at least very similar) interfaces, your software does not need to be modified to operate in these distinct scenarios, and indeed need not even “know” whether it is talking to a real robot or to something else.

All ROS software is organized into packages. A ROS package is a coherent collection of files, generally including both executables (nodes) and supporting files, that serves a specific purpose. A robot control system comprises many nodes. For example, one node controls a laser range-finder, one Node controls the robot’s wheel motors, one node performs localization.

The primary mechanism that ROS nodes use to communicate is to send messages. Messages in ROS are organized into named topics. The idea is that a node that wants to share information will publish messages on the appropriate topic; a node that wants to receive information will subscribe to the topic that it’s interested in. The ROS master takes care of ensuring that publishers and subscribers can find each other; the messages themselves are sent directly from publisher to subscriber.

H.2 C++ Node for Extracting the Euler Angles from the Quaternion

The ROS AscTec driver gives the quadrotor orientation in the form of quaternion. However, I need the roll and pitch angles to compensate the measured height from the sonar. Thus, I developed a ROS node to convert the quaternion to Euler angles.

⬇

1/****************************************************************************

2

3Conversion from a quaternion to roll, pitch and yaw.

4

5Nodes:

6subscribed /rotation_quaternion (message of type geometry_msgs::Quaternion)

7published /rpy_angles (message oftype geometry_msgs::Vector3.h)

8

9****************************************************************************/

10

11#include “ros/ros.h”

12#include “geometry_msgs/Vector3.h”

13#include “geometry_msgs/Quaternion.h”

14#include “tf/transform_datatypes.h”

15#include “LinearMath/btMatrix3x3.h”

16#include <sensor_msgs/Imu.h>

17

18// Here I use global publisher and subscriber, since I want to access the

19// publisher in the function MsgCallback:

20ros::Publisher rpy_publisher;

21ros::Subscriber quat_subscriber;

22

23// Function for conversion of quaternion to roll pitch and yaw. The angles

24// are published here too.

25void MsgCallback(const sensor_msgs::ImuPtr& msg)

26{

27 // the incoming geometry_msgs::Quaternion is transformed to a tf::Quaterion

28 tf::Quaternion quat;

29 tf::quaternionMsgToTF(msg->orientation, quat);

30

31 // the tf::Quaternion has a method to acess roll pitch and yaw

32 double roll, pitch, yaw;

33 tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);

34

35 // the found angles are written in a geometry_msgs::Vector3

36 geometry_msgs::Vector3 rpy;

37 rpy.x = roll;

38 rpy.y = pitch;

39 rpy.z = yaw;

40

41 // this Vector is then published:

42 rpy_publisher.publish(rpy);

43 ROS_INFO(“published rpy angles: roll=%f pitch=%f yaw=%f”, rpy.x, rpy.y, rpy.z);

44}

45

46int main(int argc, char **argv)

47{

48 ros::init(argc, argv, “talker”);

49 ros::NodeHandle n;

50 rpy_publisher = n.advertise<geometry_msgs::Vector3>(“rpy_angles”, 1000);

51 quat_subscriber = n.subscribe(“rotation_quaternion”, 1000, MsgCallback);

52

53 // check for incoming quaternions untill ctrl+c is pressed

54 ROS_INFO(“waiting for quaternion”);

55 ros::spin();

56 return 0;

57}

H.3 ROS-based Arduino Code

In the Arduino board, I write a program based on the rosserial library which enables me to communicate with ROS. Firstly, this C++ code reads the command from the WPS2 receiver, then process this command and then it to the SSC32 board via serial communications. Thirdly, it acquires and processes data from the sonar. Finally, it sends the height to the ROS.

⬇

1#include <DistanceSRF04.h>

2#include <Wire.h>

3#include <ros.h>

4#include <geometry_msgs/Vector3.h>

5#include <ros/time.h>

6#include <std_msgs/Float32.h>

7//=============== manp ===================

8#include <PS2X_lib.h>

9#define PS2_DAT 5 //14

10#define PS2_CMD 4 //15

11#define PS2_SEL 3 //16

12#define PS2_CLK 2 //17

13//#define pressures true

14#define pressures false

15//#define rumble true

16#define rumble false

17PS2X ps2x;

18int error = 0;

19byte type = 0;

20byte vibrate = 0;

21int angle0 = 90;

22int angle1 = 90;

23int grasp = 90;

24int angle0_pwm;

25int angle1_pwm;

26int grasp_pwm;

27int joint0_ch=0;

28int joint1_ch=4;

29int grasp_ch=8;

30int anglestep =1;

31int time = 1;

32//=======================================

33

34std_msgs::Float32 sonar_msg;

35ros::Publisher pub_sonar(“sonar”, &sonar_msg);

36ros::NodeHandle nh;

37

38DistanceSRF04 Dist;

39void setup()

40{

41Dist.begin(8,9);

42nh.initNode();

43nh.advertise(pub_sonar);

44

45// ================= Manp ================

46Serial1.begin(115200);

47delay(500);

48error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT,

49 pressures, rumble);

50type = ps2x.readType();

51// ======================================

52}

53

54long publisher_timer;

55

56void loop()

57{

58 if (millis() > publisher_timer) {

59sonar_msg.data = Dist.getDistancemeter();

60

61 pub_sonar.publish(&sonar_msg);

62 publisher_timer = millis() + 100; //publish once a second

63}

64manp();

65nh.spinOnce();

66}

67

68// ======================== Manp Functions =================

69void manp()

70{

71if(error == 1) //skip loop if no controller found

72return;

73

74if(type == 2){ //Guitar Hero Controller

75ps2x.read_gamepad(); //read controller

76}

77else { //DualShock Controller

78ps2x.read_gamepad(false, vibrate); //read controller and set large

79motor to spin at 'vibrate' speed

80

81if(ps2x.Button(PSB_PAD_UP)) //will be TRUE as long as button is

82 pressed

83angle0 +=anglestep;

84//=============================================================================================

85if(ps2x.Button(PSB_PAD_DOWN))

86angle0 -=anglestep;

87//=============================================================================================

88if(ps2x.Button(PSB_PAD_RIGHT))

89angle1 +=anglestep;

90

91//=============================================================================================

92if(ps2x.Button(PSB_PAD_LEFT)){

93angle1 -=anglestep;

94Serial1.print(“pressures = ”);

95}

96if(angle0<0) angle0=0;

97if(angle0>180) angle0=180;

98if(angle1<0) angle1=0;

99if(angle1>180) angle1=180;

100

101angle0_pwm= map(angle0, 0, 180, 500, 2500);

102move(joint0_ch,angle0_pwm, time);

103angle1_pwm= map(angle1, 0, 180, 500, 2500);

104move(joint1_ch,angle1_pwm, time);

105

106vibrate = ps2x.Analog(PSAB_CROSS); //this will set the large motor

107 vibrate speed based on how hard you press the blue (X) button

108

109if(ps2x.Button(PSB_L1) || ps2x.Button(PSB_R1)) { //print stick values if

110 either is TRUE

111if(ps2x.Button(PSB_L1)) { //print stick values if either is TRUE

112

113grasp-=anglestep;

114}

115if(ps2x.Button(PSB_R1)) { //print stick values if either is TRUE

116

117grasp+=anglestep;

118}

119}

120if(grasp<0) grasp=0;

121if(grasp>180) grasp=180;

122grasp_pwm= map(grasp, 0, 180, 500, 2500);

123move(grasp_ch,grasp_pwm, time);

124}

125delay(50);

126}

127

128void move(int servo, int position, int time) {

129Serial1.print(“#”);

130Serial1.print(servo);

131Serial1.print(“ P”);

132Serial1.print(position);

133Serial1.print(“ T”);

134Serial1.println(time);

135delay(time);

136}

H.4 Altitude Compensation Node

This node receives the height from the Arduino and then it compensates the altitude based on the measured roll and pitch angles.

⬇

1#include “ros/ros.h”

2#include <ros/time.h>

3#include “geometry_msgs/Vector3.h”

4#include “mav_msgs/Height.h”

5#include “std_msgs/Float32.h”

6//#include "var.h"

7

8ros::Publisher alt_pub;

9ros::Subscriber sonar_sub;

10ros::Subscriber rpy_sub;

11float sonar_alt, ph, th;

12char frameid[] = “”;

13

14void MsgCallback(const std_msgs::Float32& sonar_msg)

15{

16sonar_alt= sonar_msg.data;

17return;

18}

19

20void MsgCallbackrpy(const geometry_msgs::Vector3& rpy_msg)

21{

22ph=rpy_msg.x;

23th=rpy_msg.y;

24return;

25}

26int main(int argc, char **argv)

27{

28ros::init(argc, argv, “altitude”);

29ros::NodeHandle n;

30sonar_sub = n.subscribe(“sonar”, 5, MsgCallback);

31rpy_sub = n.subscribe(“rpy_angles”, 5, MsgCallbackrpy);

32alt_pub = n.advertise<mav_msgs::Height>(“alt”, 5);

33ros::Rate loop_rate(10);

34while (ros::ok())

35{

36mav_msgs::Height alt_msg;

37alt_msg.height = sonar_alt*cos(ph)*cos(th);

38alt_msg.header.stamp = ros::Time::now();

39alt_msg.header.frame_id = frameid;

40alt_msg.height_variance = 0;

41alt_msg.climb= 0.0;

42alt_msg.climb_variance= 0;

43alt_pub.publish(alt_msg);

44ros::spinOnce();

45loop_rate.sleep();

46}

47ros::spin();

48return 0;

49}

H.5 Joystick teleoperation Node

This node reads the joystick buttons and converts them to position and velocity commands.

⬇

1#include <ros/ros.h>

2#include <sensor_msgs/Joy.h>

3#include <std_msgs/Bool.h>

4#include <std_msgs/Float32.h>

5#include <stdlib.h>

6ros::Subscriber joy_sub ;

7ros::Publisher x_pub;

8ros::Publisher y_pub;

9ros::Publisher z_pub;

10ros::Publisher ep_pub;

11ros::Publisher reset_pub;

12ros::Publisher zvel_pub;

13//last_joy_event_ = ros::Time::now();

14

15float x_step_size=0.01;

16float x_old=0;

17float x_delta=0;

18float y_step_size=0.01;

19float y_old=0;

20float y_delta=0;

21float z_step_size=0.01;

22float z_old=0;

23float z_delta=0;

24float z_min=0;

25float ep_step_size=0.01;

26float ep_old=0;

27float ep_delta=0;

28float zvel_step_size=0.01;

29float zvel_old=0;

30float zvel_delta=0;

31float zvel_min=0;

32bool sw=0;

33bool b0,b1,b2,b3,b5,b7,b8,b10,b11,b4,b6,b9;

34// ****

35void joyCallback(const sensor_msgs::JoyPtr& joy_msg)

36{ // start of callback

37b0=joy_msg->buttons[0];

38b1=joy_msg->buttons[1];

39b2=joy_msg->buttons[2];

40b3=joy_msg->buttons[3];

41b5=joy_msg->buttons[5];

42b7=joy_msg->buttons[7];

43b8=joy_msg->buttons[8];

44b10=joy_msg->buttons[10];

45b11=joy_msg->buttons[11];

46b4=joy_msg->buttons[4];

47b6=joy_msg->buttons[6];

48b9=joy_msg->buttons[9];

49//=============================

50std_msgs::Float32 x_msg;

51std_msgs::Float32 y_msg;

52std_msgs::Float32 z_msg;

53std_msgs::Float32 ep_msg;

54std_msgs::Bool reset_msg;

55std_msgs::Float32 zvel_msg;

56// ================== x-axis ==============================

57if (b0 ==1 || b2 == 1)

58{

59if (b0 == 1)

60{

61x_delta= x_step_size ;

62}

63else if (b2 == 1)

64{

65x_delta= -x_step_size ;

66}

67

68x_msg.data = x_old + x_delta;

69x_old=x_msg.data;

70}

71else {x_msg.data = x_old;}

72// =======================================================

73

74// ================== y-axis ==============================

75if (b3 ==1 || b1 == 1)

76{

77if (b3 == 1)

78{

79y_delta= y_step_size ;

80}

81else if (b1 == 1)

82{

83y_delta= -y_step_size ;

84}

85

86y_msg.data = y_old + y_delta;

87y_old=y_msg.data;

88}

89else {y_msg.data = y_old;}

90// =======================================================

91

92// ================== z-axis ==============================

93if (b5 ==1 || b7 == 1)

94{

95if (b5 == 1)

96{

97z_delta= z_step_size ;

98}

99else if (b7 == 1)

100{

101z_delta= -z_step_size ;

102}

103

104z_msg.data = z_old + z_delta;

105z_msg.data = std::max(z_msg.data, z_min);

106z_old=z_msg.data;

107}

108else {z_msg.data = z_old;}

109

110// =======================================================

111

112// ================== ep-axis ==============================

113if (b11 ==1 || b10 == 1)

114{

115if (b11 == 1)

116{

117ep_delta= ep_step_size ;

118}

119else if (b10 == 1)

120{

121ep_delta= -ep_step_size ;

122}

123

124ep_msg.data = ep_old + ep_delta;

125

126ep_old=ep_msg.data;

127}

128else {ep_msg.data = ep_old;}

129

130// =======================================================

131

132// ================== reset ==============================

133

134if (b9 ==1 || sw==1 && b8==0) {reset_msg.data=1; sw=1;}

135else {reset_msg.data=0; sw=0;}

136

137// =======================================================

138// ================== zvel-axis ==============================

139if (b4 ==1 || b6 == 1)

140{

141if (b4 == 1)

142{

143zvel_delta= zvel_step_size ;

144}

145else if (b6 == 1)

146{

147zvel_delta= -zvel_step_size ;

148}

149

150zvel_msg.data = zvel_old + zvel_delta;

151zvel_old=zvel_msg.data;

152}

153else {zvel_msg.data = zvel_old;}

154// =======================================================

155

156x_pub.publish(x_msg);

157y_pub.publish(y_msg);

158z_pub.publish(z_msg);

159ep_pub.publish(ep_msg);

160reset_pub.publish(reset_msg);

161zvel_pub.publish(zvel_msg);

162//==================================

163return;

164} // end of call back

165

166int main(int argc, char ** argv)

167{

168ros::init(argc, argv, “quad_joy_teleop”);

169ros::NodeHandle nh;

170x_pub = nh.advertise<std_msgs::Float32>(“x_des”, 5);

171y_pub = nh.advertise<std_msgs::Float32>(“y_des”, 5);

172z_pub = nh.advertise<std_msgs::Float32>(“z_des”, 5);

173ep_pub = nh.advertise<std_msgs::Float32>(“ep_des”, 5);

174reset_pub = nh.advertise<std_msgs::Bool>(“reset_des”, 5);

175zvel_pub = nh.advertise<std_msgs::Float32>(“zvel_des”, 5);

176joy_sub= nh.subscribe(“joy”, 5, joyCallback);

177ros::spin();

178return 0;

179}

Appendix I 6-DOF Torque/Force Sensor

We use a torque force sensor of model, IFS-67M25A25-I40ANA, see Fig. I.1. In this sensor, foil strain gages sense the loads imposed on the sensor. The strain gage signals are amplified and combined to become analog representations of the force loads on the three axes and the moments or torques about the three axes.

Fig. I.2 presents the sensor axis orientation from the rotor side.

It utilizes a DE-9P connector to produce the analogue output. The pin assignment is given in Table I.1.

Data from analog sensors must be processed using the Calibration Matrix provided with the sensor.

The six by six calibration matrix is multiplied by the six element voltage (column) vector. The result is the calibrated force and moment data in the units of N and Nm. Multiply the calibration matrix and the sensor voltage vector to determine the loads (N and Nm).

[TABLE]

Appendix J Inverse Kinematics

⬇

1function zt_des = Inverse_Kin(Xe_des,sb)

2L0 = 30e-03;

3L1 = 70e-03;

4L2 = 85e-03;

5

6xe=Xe_des(1);

7ye=Xe_des(2);

8ze=Xe_des(3);

9epe=Xe_des(4);

10the=Xe_des(5);

11phe=Xe_des(6);

12ph = sb(2);

13th = sb(1);

14

15T_end = [ cos(epe)*cos(the), cos(epe)*sin(phe)*sin(the) - cos(phe)*sin(epe),

16 sin(epe)*sin(phe) + cos(epe)*cos(phe)*sin(the);

17cos(the)*sin(epe), cos(epe)*cos(phe) + sin(epe)*sin(phe)*sin(the),

18 cos(phe)*sin(epe)*sin(the) - cos(epe)*sin(phe);

19-sin(the), cos(the)*sin(phe),cos(phe)*cos(the)];

20

21r11 = T_end(1,1);

22r12 = T_end(1,2);

23r13 = T_end(1,3);

24r23 = T_end(2,3);

25r31 = T_end(3,1);

26r32 = T_end(3,2);

27r33 = T_end(3,3);

28

29

30a1=cos(ph)*cos(th);

31b1=cos(th)*sin(ph);

32if(((-2b1)^2 - 4(-a1-r33)*(a1-r33))<0)

33t2=0;

34else

35% t1=(-(-2b1) + sqrt((-2b1)^2 - 4*(-a1-r33)(a1-r33)))/(2(-a1-r33));

36t2=(-(-2b1) -sqrt((-2b1)^2 - 4*(-a1-r33)(a1-r33)))/(2(-a1-r33));

37% th1=2*atan(t1)

38% OR

39end

40th1=2*atan(t2);

41%% Case#2 ====>> Sin(th1) = 0

42if(th1<=0.0001&&th1>=-0.0001)

43th1=0;

44ep=0;

45a6=cos(th)*cos(ep);

46b6=cos(ph)*sin(ep)-sin(ph)*sin(th)*cos(ep);

47a7=sin(ph)*sin(th)*cos(ep)-cos(ph)*sin(ep);

48b7=cos(th)*cos(ep);

49sth2=(b7r11+b6r12)/(a6b7+a7b6);

50cth2=(a7r11+a6r12)/(a6b7+a7b6);

51th2=atan2(sth2,cth2);

52%% Case#3 ====>> Sin(th1) = pi

53elseif (th1==pi)

54th1=pi;

55ep=0;

56a6=cos(th)*cos(ep);

57b6=-cos(ph)*sin(ep)+sin(ph)*sin(th)*cos(ep);

58a7=-sin(ph)*sin(th)*cos(ep)+cos(ph)*sin(ep);

59b7=cos(th)*cos(ep);

60sth2=(b7r11+b6r12)/(a6b7+a7b6);

61cth2=(a7r11+a6r12)/(a6b7+a7b6);

62th2=atan2(sth2,cth2);

63%% Case#1 ====>> Sin(th1) != 0 ======>> r13~=0 && r23 ~=0

64else

65a2=cos(ph)*sin(th1)+sin(ph)*cos(th1);

66b2=cos(ph)*sin(th)*cos(th1)-sin(ph)*sin(th)*sin(th1);

67

68a3=cos(ph)*sin(th)*cos(th1)-sin(ph)*sin(th)*sin(th1);

69b3=-sin(ph)*cos(th1)-cos(ph)*sin(th1);

70

71a4=cos(ph)*cos(th)*sin(th1)+cos(th)*sin(ph)*cos(th1);

72b4=-sin(th);

73

74a5=-sin(th);

75b5=-cos(ph)*cos(th)*sin(th1)-cos(th)*sin(ph)*cos(th1);

76

77sep=(b3r13+b2r23)/(a2b3+a3b2);

78cep=(a3r13+a2r23)/(a2b3+a3b2);

79ep=atan2(sep,cep);

80sth2=(b5r32+b4r31)/(a4b5+a5b4);

81cth2=(a5r32+a4r31)/(a4b5+a5b4);

82th2=atan2(sth2,cth2);

83end

84%% Translational Part

85X=xe- (- L0*(sin(ph)*sin(ep) + cos(ph)*sin(th)*cos(ep)) +

86 L2cos(th2)(cos(th1)*(cos(ph)*sin(ep) - sin(ph)*sin(th)*cos(ep)) -

87 sin(th1)*(sin(ph)*sin(ep) + cos(ph)*sin(th)*cos(ep))) +

88 L1cos(th1)(cos(ph)*sin(ep) - sin(ph)*sin(th)*cos(ep)) -

89 L1sin(th1)(sin(ph)*sin(ep) + cos(ph)*sin(th)*cos(ep)) +

90 L2*cos(th)*cos(ep)*sin(th2));

91Y=ye- (+ L0*(sin(ph)*cos(ep) - cos(ph)*sin(th)*sin(ep)) -

92 L2cos(th2)(cos(th1)*(cos(ph)*cos(ep) + sin(ph)*sin(th)*sin(ep)) -

93 sin(th1)*(sin(ph)*cos(ep) - cos(ph)*sin(th)*sin(ep))) -

94 L1cos(th1)(cos(ph)*cos(ep) + sin(ph)*sin(th)*sin(ep)) +

95 L1sin(th1)(sin(ph)*cos(ep) - cos(ph)*sin(th)*sin(ep)) +

96 L2*cos(th)*sin(ep)*sin(th2));

97Z=ze- (- L2*sin(th)sin(th2) - L0cos(ph)*cos(th) -

98 L2cos(th2)(cos(ph)*cos(th)*sin(th1) + cos(th)*sin(ph)*cos(th1)) -

99 L1*cos(ph)*cos(th)sin(th1) - L1cos(th)*sin(ph)*cos(th1));

100%% ==========

101zt_des=[X;Y;Z;ep;th1;th2];

102end