├── .gitignore ├── Makefile ├── report ├── quadreport.pdf ├── img │ ├── attractive.png │ ├── feedback.png │ ├── gazebo-ros.png │ ├── potential.png │ ├── quad_model.png │ ├── repulsive.png │ ├── attractive_grad.png │ ├── boundarysphere.png │ └── plot │ │ ├── matlab │ │ ├── sim1.png │ │ ├── sim2.png │ │ ├── sim1_pdf.png │ │ ├── sim1_xy.png │ │ ├── sim2_pdf.png │ │ ├── sim2_xz.png │ │ ├── sim2_yz.png │ │ └── simulation1.png │ │ └── gazebo │ │ ├── rumore.png │ │ ├── rumore_zoom.png │ │ ├── simulazione1.png │ │ ├── simulazione2.png │ │ ├── simulazione3.png │ │ ├── simulazione1_xy.png │ │ ├── simulazione2_xz.png │ │ ├── simulazione2_yz.png │ │ ├── simulazione3_xy.png │ │ ├── simulazione1_pdf.png │ │ ├── simulazione2_pdf.png │ │ └── simulazione3_pdf.png ├── references.bib ├── .gitignore └── quadreport.tex ├── mesh └── quadrotorblender.stl ├── README.md ├── manifest.xml ├── mainpage.dox ├── include └── quadrotor_obstacle_avoidance │ ├── functionlib.h │ └── feedbacklinearization.h ├── urdf ├── obstacle.urdf.xacro ├── quad_hokuyo_laser.xacro └── quadrotor.urdf.xacro ├── launch └── quadrotor.launch ├── CMakeLists.txt ├── world └── empty.world └── src ├── functionlib.cpp └── feedbacklinearization.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | bin 3 | lib 4 | 5 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /report/quadreport.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/report/quadreport.pdf -------------------------------------------------------------------------------- /mesh/quadrotorblender.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/mesh/quadrotorblender.stl -------------------------------------------------------------------------------- /report/img/attractive.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/report/img/attractive.png -------------------------------------------------------------------------------- /report/img/feedback.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/report/img/feedback.png -------------------------------------------------------------------------------- /report/img/gazebo-ros.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/report/img/gazebo-ros.png -------------------------------------------------------------------------------- /report/img/potential.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/report/img/potential.png -------------------------------------------------------------------------------- /report/img/quad_model.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/report/img/quad_model.png -------------------------------------------------------------------------------- /report/img/repulsive.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/report/img/repulsive.png -------------------------------------------------------------------------------- /report/img/attractive_grad.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/report/img/attractive_grad.png -------------------------------------------------------------------------------- /report/img/boundarysphere.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/report/img/boundarysphere.png -------------------------------------------------------------------------------- /report/img/plot/matlab/sim1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/report/img/plot/matlab/sim1.png -------------------------------------------------------------------------------- /report/img/plot/matlab/sim2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/report/img/plot/matlab/sim2.png -------------------------------------------------------------------------------- /report/img/plot/gazebo/rumore.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/report/img/plot/gazebo/rumore.png -------------------------------------------------------------------------------- /report/img/plot/matlab/sim1_pdf.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/report/img/plot/matlab/sim1_pdf.png -------------------------------------------------------------------------------- /report/img/plot/matlab/sim1_xy.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/report/img/plot/matlab/sim1_xy.png -------------------------------------------------------------------------------- /report/img/plot/matlab/sim2_pdf.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/report/img/plot/matlab/sim2_pdf.png -------------------------------------------------------------------------------- /report/img/plot/matlab/sim2_xz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/report/img/plot/matlab/sim2_xz.png -------------------------------------------------------------------------------- /report/img/plot/matlab/sim2_yz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/report/img/plot/matlab/sim2_yz.png -------------------------------------------------------------------------------- /report/img/plot/gazebo/rumore_zoom.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/report/img/plot/gazebo/rumore_zoom.png -------------------------------------------------------------------------------- /report/img/plot/gazebo/simulazione1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/report/img/plot/gazebo/simulazione1.png -------------------------------------------------------------------------------- /report/img/plot/gazebo/simulazione2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/report/img/plot/gazebo/simulazione2.png -------------------------------------------------------------------------------- /report/img/plot/gazebo/simulazione3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/report/img/plot/gazebo/simulazione3.png -------------------------------------------------------------------------------- /report/img/plot/matlab/simulation1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/report/img/plot/matlab/simulation1.png -------------------------------------------------------------------------------- /report/img/plot/gazebo/simulazione1_xy.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/report/img/plot/gazebo/simulazione1_xy.png -------------------------------------------------------------------------------- /report/img/plot/gazebo/simulazione2_xz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/report/img/plot/gazebo/simulazione2_xz.png -------------------------------------------------------------------------------- /report/img/plot/gazebo/simulazione2_yz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/report/img/plot/gazebo/simulazione2_yz.png -------------------------------------------------------------------------------- /report/img/plot/gazebo/simulazione3_xy.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/report/img/plot/gazebo/simulazione3_xy.png -------------------------------------------------------------------------------- /report/img/plot/gazebo/simulazione1_pdf.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/report/img/plot/gazebo/simulazione1_pdf.png -------------------------------------------------------------------------------- /report/img/plot/gazebo/simulazione2_pdf.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/report/img/plot/gazebo/simulazione2_pdf.png -------------------------------------------------------------------------------- /report/img/plot/gazebo/simulazione3_pdf.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THeK3nger/ros-quadrotor-obstacle-avoidance/HEAD/report/img/plot/gazebo/simulazione3_pdf.png -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ROS Quadrotor Obstacle Avoidance 2 | 3 | **This repo is an archive for a very old project I did for a University course. I canno guarantee that this project still works with the current version of ROS (probably not!) or any additional work on it.** 4 | 5 | **For more info, I have included [the original PDF report (in Italian)](./report/quadreport.pdf). Unfortunately, this is in Italian. If somebody wants to do a PR to translate that, I'll include it.** 6 | 7 | **But as I said, the project is very old. The best things you can extract from this is the `cpp` feedback linearization code and include it in a new project based on a recent ROS version.** -------------------------------------------------------------------------------- /manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Implementation of obstacle avoidance through the use of artificial 5 | potential field for a quadrotor. 6 | 7 | 8 | Davide Aversa, Vittorio Perera 9 | BSD 10 | 11 | http://ros.org/ 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b quadrotor_obstacle_avoidance is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /include/quadrotor_obstacle_avoidance/functionlib.h: -------------------------------------------------------------------------------- 1 | /** 2 | This file is part of ROS Quadrotor Feedback Controller. 3 | 4 | Foobar is free software; you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation; either version 2 of the License, or 7 | any later version. 8 | 9 | ROS Quadrotor Feedback Controller is distributed in the hope that it will 10 | be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General 12 | Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License along 15 | with ROS Quadrotor Feedback Controller; if not, write to the Free Software 16 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 17 | **/ 18 | #ifndef FUNCTIONLIB_HH 19 | #define FUNCTIONLIB_HH 20 | 21 | double * controller (double* X, double *V ); 22 | 23 | double* backward_derivative(double* X,double* X_new, double dt); 24 | 25 | double* force_vector(double* position, double yaw, double* goal, double* obstacle); 26 | 27 | double* damping(double* velocity, double* acceleration, double* jerk, double yaw_d); 28 | 29 | double denormalize_angle(double angle, double previous_angle); 30 | 31 | void toMatlab(FILE* id, double* X, double* U, double* FV, double* V); 32 | #endif 33 | -------------------------------------------------------------------------------- /urdf/obstacle.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 12 | 13 | 14 | 15 | Gazebo/${color} 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /launch/quadrotor.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 17 | 18 | 19 | 20 | 28 | 29 | 30 | 31 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Set the build type. Options are: 5 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 6 | # Debug : w/ debug symbols, w/o optimization 7 | # Release : w/o debug symbols, w/ optimization 8 | # RelWithDebInfo : w/ debug symbols, w/ optimization 9 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 10 | #set(ROS_BUILD_TYPE RelWithDebInfo) 11 | 12 | rosbuild_init() 13 | 14 | #set the default path for built executables to the "bin" directory 15 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 16 | #set the default path for built libraries to the "lib" directory 17 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 18 | 19 | #uncomment if you have defined messages 20 | #rosbuild_genmsg() 21 | #uncomment if you have defined services 22 | #rosbuild_gensrv() 23 | 24 | #common commands for building c++ executables and libraries 25 | #rosbuild_add_library(${PROJECT_NAME} src/example.cpp) 26 | #target_link_libraries(${PROJECT_NAME} another_library) 27 | #rosbuild_add_boost_directories() 28 | #rosbuild_link_boost(${PROJECT_NAME} thread) 29 | #rosbuild_add_executable(example examples/example.cpp) 30 | #target_link_libraries(example ${PROJECT_NAME}) 31 | 32 | 33 | rosbuild_add_boost_directories() 34 | find_package(Armadillo) 35 | 36 | rosbuild_add_library(feedbacklinearization src/feedbacklinearization.cpp src/functionlib.cpp) 37 | rosbuild_link_boost(feedbacklinearization thread) 38 | target_link_libraries(feedbacklinearization ${ARMADILLO_LIBRARY}) 39 | -------------------------------------------------------------------------------- /urdf/quad_hokuyo_laser.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | true 45 | 45 46 | 45 47 | 1 48 | 49 | 0.0 0.0 0.035 50 | false 51 | 52 | -180 53 | 180 54 | 55 | -90 56 | 90 57 | 58 | 90 59 | 90 60 | 61 | 62 | 1 63 | 4.0 64 | 0.001 65 | 10.0 66 | 67 | 68 | 0.005 69 | true 70 | 10.0 71 | full_cloud 72 | base_scan_link 73 | 74 | 75 | 76 | 77 | 78 | 79 | Erratic/Red 80 | 81 | 82 | 83 | Erratic/Red 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | -------------------------------------------------------------------------------- /report/references.bib: -------------------------------------------------------------------------------- 1 | @inproceedings{Al-Hiddabi2009, 2 | author = {Al-Hiddabi, S.A.}, 3 | booktitle = {Mechatronics and its Applications, 2009. ISMA'09. 6th International Symposium on}, 4 | file = {:home/davide/Documents/Library/Al-Hiddabi - 2009 - Quadrotor control using feedback linearization with dynamic extension.pdf:pdf}, 5 | isbn = {9781424434817}, 6 | keywords = {controll,feedback linearizzation,quadrotor,robotics}, 7 | mendeley-tags = {controll,robotics}, 8 | number = {1}, 9 | pages = {1--3}, 10 | publisher = {IEEE}, 11 | title = {{Quadrotor control using feedback linearization with dynamic extension}}, 12 | url = {http://ieeexplore.ieee.org/xpls/abs\_all.jsp?arnumber=5164788}, 13 | year = {2009} 14 | } 15 | @inproceedings{Isidori1986, 16 | author = {Isidori, A. and Moog, CH and {De Luca}, A.}, 17 | booktitle = {Decision and Control, 1986 25th IEEE Conference on}, 18 | file = {:home/davide/Documents/Library/Isidori, Moog, De Luca - 1986 - A sufficient condition for full linearization via dynamic state feedback.pdf:pdf}, 19 | keywords = {controll,feedback linearizzation,non-linear controll,robotics}, 20 | mendeley-tags = {controll,robotics}, 21 | number = {December}, 22 | pages = {203--208}, 23 | publisher = {IEEE}, 24 | title = {{A sufficient condition for full linearization via dynamic state feedback}}, 25 | url = {http://ieeexplore.ieee.org/xpls/abs\_all.jsp?arnumber=4048739}, 26 | volume = {25}, 27 | year = {1986} 28 | } 29 | @inproceedings{Mistler2001, 30 | abstract = {This paper presents a nonlinear dynamic model for a four rotors helicopter in a form suited for control de- sign. We show that the input-output decoupling prob- lem is not solvable for this model by means of a static state feedback control law. Then, a dynamic feed- back controller is developed which render the closed- loop system linear, controllable and nonintemctive af- ter a change of coordinates in the state-space. Finally, the stability and the robustness of the proposed control law in the presence of uind, turbulences and pammet- ric uncertainties is analyzed through a simulated case study.}, 31 | author = {Mistler, V. and Benallegue, A. and M'Sirdi, N.K.}, 32 | booktitle = {Robot and Human Interactive Communication, 2001. Proceedings. 10th IEEE International Workshop on}, 33 | doi = {10.1109/ROMAN.2001.981968}, 34 | file = {:home/davide/Documents/Library/Mistler, Benallegue, M'Sirdi - 2001 - Exact linearization and noninteracting control of a 4 rotors helicopter via dynamic feedback.pdf:pdf;:home/davide/Documents/Library/Mistler, Benallegue, M'Sirdi - 2001 - Exact linearization and noninteracting control of a 4 rotors helicopter via dynamic feedback.pdf:pdf}, 35 | isbn = {0-7803-7222-0}, 36 | keywords = {quadrotor,robotics}, 37 | mendeley-tags = {quadrotor,robotics}, 38 | pages = {586--593}, 39 | publisher = {IEEE}, 40 | title = {{Exact linearization and noninteracting control of a 4 rotors helicopter via dynamic feedback}}, 41 | url = {http://ieeexplore.ieee.org/lpdocs/epic03/wrapper.htm?arnumber=981968 http://ieeexplore.ieee.org/xpls/abs\_all.jsp?arnumber=981968}, 42 | year = {2001} 43 | } 44 | @inproceedings{Park2001, 45 | author = {Park, M.G. and Jeon, J.H. and Lee, M.C.}, 46 | booktitle = {Industrial Electronics, 2001. Proceedings. ISIE 2001. IEEE International Symposium on}, 47 | file = {:home/davide/Documents/Library/Park, Jeon, Lee - 2001 - Obstacle avoidance for mobile robots using artificial potential field approach with simulated annealing.pdf:pdf}, 48 | pages = {1530--1535}, 49 | publisher = {IEEE}, 50 | title = {{Obstacle avoidance for mobile robots using artificial potential field approach with simulated annealing}}, 51 | url = {http://ieeexplore.ieee.org/xpls/abs\_all.jsp?arnumber=931933}, 52 | volume = {3}, 53 | year = {2001} 54 | } 55 | 56 | @Book{RoboticsMPC, 57 | author = {B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo}, 58 | ALTeditor = {Springer}, 59 | title = {Robotics Modelling, Planning and Control}, 60 | publisher = {Springer}, 61 | year = {2008} 62 | } 63 | 64 | @Book{NonLinearKhalil, 65 | author = {H.K. Khalil}, 66 | ALTeditor = {Prentice Hall}, 67 | title = {Non-Linear System}, 68 | publisher = {Prentice Hall}, 69 | year = {1996} 70 | } 71 | 72 | @Book{NonLinearIsidoril, 73 | author = {A. Isidori}, 74 | ALTeditor = {Springer}, 75 | title = {Non-Linear Control System}, 76 | publisher = {Springer}, 77 | year = {1995} 78 | } 79 | 80 | -------------------------------------------------------------------------------- /report/.gitignore: -------------------------------------------------------------------------------- 1 | ## Core latex/pdflatex auxiliary files: 2 | *.aux 3 | *.lof 4 | *.log 5 | *.lot 6 | *.fls 7 | *.out 8 | *.toc 9 | *.fmt 10 | *.fot 11 | *.cb 12 | *.cb2 13 | .*.lb 14 | 15 | ## Intermediate documents: 16 | *.dvi 17 | *.xdv 18 | *-converted-to.* 19 | # these rules might exclude image files for figures etc. 20 | # *.ps 21 | # *.eps 22 | # *.pdf 23 | 24 | ## Generated if empty string is given at "Please type another file name for output:" 25 | .pdf 26 | 27 | ## Bibliography auxiliary files (bibtex/biblatex/biber): 28 | *.bbl 29 | *.bcf 30 | *.blg 31 | *-blx.aux 32 | *-blx.bib 33 | *.run.xml 34 | 35 | ## Build tool auxiliary files: 36 | *.fdb_latexmk 37 | *.synctex 38 | *.synctex(busy) 39 | *.synctex.gz 40 | *.synctex.gz(busy) 41 | *.pdfsync 42 | 43 | ## Build tool directories for auxiliary files 44 | # latexrun 45 | latex.out/ 46 | 47 | ## Auxiliary and intermediate files from other packages: 48 | # algorithms 49 | *.alg 50 | *.loa 51 | 52 | # achemso 53 | acs-*.bib 54 | 55 | # amsthm 56 | *.thm 57 | 58 | # beamer 59 | *.nav 60 | *.pre 61 | *.snm 62 | *.vrb 63 | 64 | # changes 65 | *.soc 66 | 67 | # comment 68 | *.cut 69 | 70 | # cprotect 71 | *.cpt 72 | 73 | # elsarticle (documentclass of Elsevier journals) 74 | *.spl 75 | 76 | # endnotes 77 | *.ent 78 | 79 | # fixme 80 | *.lox 81 | 82 | # feynmf/feynmp 83 | *.mf 84 | *.mp 85 | *.t[1-9] 86 | *.t[1-9][0-9] 87 | *.tfm 88 | 89 | #(r)(e)ledmac/(r)(e)ledpar 90 | *.end 91 | *.?end 92 | *.[1-9] 93 | *.[1-9][0-9] 94 | *.[1-9][0-9][0-9] 95 | *.[1-9]R 96 | *.[1-9][0-9]R 97 | *.[1-9][0-9][0-9]R 98 | *.eledsec[1-9] 99 | *.eledsec[1-9]R 100 | *.eledsec[1-9][0-9] 101 | *.eledsec[1-9][0-9]R 102 | *.eledsec[1-9][0-9][0-9] 103 | *.eledsec[1-9][0-9][0-9]R 104 | 105 | # glossaries 106 | *.acn 107 | *.acr 108 | *.glg 109 | *.glo 110 | *.gls 111 | *.glsdefs 112 | *.lzo 113 | *.lzs 114 | 115 | # uncomment this for glossaries-extra (will ignore makeindex's style files!) 116 | # *.ist 117 | 118 | # gnuplottex 119 | *-gnuplottex-* 120 | 121 | # gregoriotex 122 | *.gaux 123 | *.gtex 124 | 125 | # htlatex 126 | *.4ct 127 | *.4tc 128 | *.idv 129 | *.lg 130 | *.trc 131 | *.xref 132 | 133 | # hyperref 134 | *.brf 135 | 136 | # knitr 137 | *-concordance.tex 138 | # TODO Uncomment the next line if you use knitr and want to ignore its generated tikz files 139 | # *.tikz 140 | *-tikzDictionary 141 | 142 | # listings 143 | *.lol 144 | 145 | # luatexja-ruby 146 | *.ltjruby 147 | 148 | # makeidx 149 | *.idx 150 | *.ilg 151 | *.ind 152 | 153 | # minitoc 154 | *.maf 155 | *.mlf 156 | *.mlt 157 | *.mtc[0-9]* 158 | *.slf[0-9]* 159 | *.slt[0-9]* 160 | *.stc[0-9]* 161 | 162 | # minted 163 | _minted* 164 | *.pyg 165 | 166 | # morewrites 167 | *.mw 168 | 169 | # nomencl 170 | *.nlg 171 | *.nlo 172 | *.nls 173 | 174 | # pax 175 | *.pax 176 | 177 | # pdfpcnotes 178 | *.pdfpc 179 | 180 | # sagetex 181 | *.sagetex.sage 182 | *.sagetex.py 183 | *.sagetex.scmd 184 | 185 | # scrwfile 186 | *.wrt 187 | 188 | # sympy 189 | *.sout 190 | *.sympy 191 | sympy-plots-for-*.tex/ 192 | 193 | # pdfcomment 194 | *.upa 195 | *.upb 196 | 197 | # pythontex 198 | *.pytxcode 199 | pythontex-files-*/ 200 | 201 | # tcolorbox 202 | *.listing 203 | 204 | # thmtools 205 | *.loe 206 | 207 | # TikZ & PGF 208 | *.dpth 209 | *.md5 210 | *.auxlock 211 | 212 | # todonotes 213 | *.tdo 214 | 215 | # vhistory 216 | *.hst 217 | *.ver 218 | 219 | # easy-todo 220 | *.lod 221 | 222 | # xcolor 223 | *.xcp 224 | 225 | # xmpincl 226 | *.xmpi 227 | 228 | # xindy 229 | *.xdy 230 | 231 | # xypic precompiled matrices and outlines 232 | *.xyc 233 | *.xyd 234 | 235 | # endfloat 236 | *.ttt 237 | *.fff 238 | 239 | # Latexian 240 | TSWLatexianTemp* 241 | 242 | ## Editors: 243 | # WinEdt 244 | *.bak 245 | *.sav 246 | 247 | # Texpad 248 | .texpadtmp 249 | 250 | # LyX 251 | *.lyx~ 252 | 253 | # Kile 254 | *.backup 255 | 256 | # gummi 257 | .*.swp 258 | 259 | # KBibTeX 260 | *~[0-9]* 261 | 262 | # TeXnicCenter 263 | *.tps 264 | 265 | # auto folder when using emacs and auctex 266 | ./auto/* 267 | *.el 268 | 269 | # expex forward references with \gathertags 270 | *-tags.tex 271 | 272 | # standalone packages 273 | *.sta 274 | 275 | # Makeindex log files 276 | *.lpz 277 | 278 | # xwatermark package 279 | *.xwm 280 | 281 | # REVTeX puts footnotes in the bibliography by default, unless the nofootinbib 282 | # option is specified. Footnotes are the stored in a file with suffix Notes.bib. 283 | # Uncomment the next line to have this generated file ignored. 284 | #*Notes.bib -------------------------------------------------------------------------------- /world/empty.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 20 | 21 | 4 22 | 5 23 | 24 | 25 | 26 | 27 | 28 | 0.001 29 | 0 0 -9.8 30 | 0.0000000001 31 | 0.2 32 | true 33 | 10 34 | 1.3 35 | 100.0 36 | 0.001 37 | 38 | 39 | 40 | 37.4270909558-122.077919338 41 | 42 | 43 | 44 | fltk 45 | 480 320 46 | 0 0 47 | 48 | 49 | 50 | 0.3 0 3 51 | 0 90 90 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 0.5 0.5 0.5 0.5 60 | 61 | Gazebo/CloudySky 62 | 63 | false 64 | 10. 65 | none 66 | false 67 | 68 | 69 | 70 | 71 | 0 0 0 72 | 0 0 0 73 | true 74 | 75 | 76 | 77 | 2000.0 78 | 50.0 79 | 50.0 80 | 1000000000.0 81 | 1.0 82 | 0 0 1 83 | 51.3 51.3 84 | 10 10 85 | 100 100 86 | Gazebo/GrayGrid 87 | 88 | 89 | 90 | 91 | 98 | 116 | 117 | 118 | 119 | 0.0 0.0 8 120 | false 121 | 122 | point 123 | 0.5 0.5 0.5 124 | .1 .1 .1 125 | 0.2 0.1 0 126 | 10 127 | 128 | 129 | 130 | 131 | -------------------------------------------------------------------------------- /include/quadrotor_obstacle_avoidance/feedbacklinearization.h: -------------------------------------------------------------------------------- 1 | /** 2 | This file is part of ROS Quadrotor Feedback Controller. 3 | 4 | Foobar is free software; you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation; either version 2 of the License, or 7 | any later version. 8 | 9 | ROS Quadrotor Feedback Controller is distributed in the hope that it will 10 | be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General 12 | Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License along 15 | with ROS Quadrotor Feedback Controller; if not, write to the Free Software 16 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 17 | **/ 18 | #ifndef FEEDBACKLINEARIZATION_HH 19 | #define FEEDBACKLINEARIZATION_HH 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | // ROS 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | // Custom Callback Queue 37 | #include 38 | #include 39 | 40 | // Boost 41 | #include 42 | #include 43 | 44 | using namespace std; 45 | 46 | namespace gazebo 47 | { 48 | class Joint; 49 | class Entity; 50 | 51 | /** 52 | This is an implementation of a Gazeebo Controller for a Quadrotor using 53 | Feedback Linearization for controll and Artificial Potential 54 | Field for autonomous navigation task. 55 | 56 | Feedback Linarization is a control tecnique that transform a non-linear 57 | system x'(t) = f(t) + g(t)U(t) in a linear system z'(t) = Az(t) + Bv(t) 58 | through a particular input U(t). 59 | 60 | Instead Artificial Potential Field is a navigation mathod based on an 61 | artificial force field computed by sensors inputs and applied to the 62 | robot. 63 | **/ 64 | class FeedbackLinearization : public Controller 65 | { 66 | public: 67 | /** 68 | Constructor 69 | */ 70 | FeedbackLinearization(Entity *parent); 71 | virtual ~FeedbackLinearization(); 72 | 73 | protected: 74 | /* GAZEBO STANDARD CONTROLLER METHODS */ 75 | /** 76 | This code is executed when the controlled node is loaded. 77 | **/ 78 | virtual void LoadChild(XMLConfigNode *node); 79 | 80 | /** 81 | This code is ecexuted when the controlled node has to be saved. 82 | **/ 83 | void SaveChild(std::string &prefix, std::ostream &stream); 84 | 85 | /** 86 | This code is executed after that the controlled node is loaded and 87 | has to be initialized. It's very similar to LoadChild method. 88 | **/ 89 | virtual void InitChild(); 90 | 91 | /** 92 | This code is executed on Gazebo restart signal. After the execution 93 | of this piece of code the node should be restored to the initial 94 | configuration. 95 | **/ 96 | void ResetChild(); 97 | 98 | /** 99 | This code is executed every time that the controlled node has to be 100 | updated. The update rate is specified in the Gazebo world file. 101 | **/ 102 | virtual void UpdateChild(); 103 | 104 | /** 105 | This code is executed when the controlled node has to be destroyed. 106 | **/ 107 | virtual void FiniChild(); 108 | 109 | private: 110 | Model *parent_; 111 | ros::NodeHandle* rosnode_; 112 | 113 | Body* body_main; 114 | 115 | ros::Subscriber sub_goal; 116 | ros::Subscriber sub_obstacle; 117 | boost::mutex lock; 118 | ros::CallbackQueue queue_goal; 119 | ros::CallbackQueue queue_obstacle; 120 | boost::thread* goal_callback_queue_thread_; 121 | boost::thread* obstacle_callback_queue_thread_; 122 | 123 | /* 124 | QUADROTOR STATE 125 | */ 126 | double* X; 127 | double* X_prev; 128 | double* X_d; 129 | double* X_d_prev; 130 | double* X_dd_prev; 131 | double* U_tilde; 132 | double Eta; 133 | double U1_reale; 134 | double* goal; 135 | double* obstacle; 136 | 137 | #if ENABLE_MATLAB_OUTPUT 138 | FILE* id; 139 | #endif 140 | 141 | void goalCallback(const geometry_msgs::Point::ConstPtr& goal_msg); 142 | void obstacleCallback(const sensor_msgs::PointCloud::ConstPtr& obstacle_msg); 143 | void lookatCallback(const geometry_msgs::Point::ConstPtr& lookat_msg); 144 | void GoalQueueThread(); 145 | void ObstacleQueueThread(); 146 | void LookatQueueThread(); 147 | void saturationForce(); 148 | }; 149 | 150 | } 151 | 152 | #endif 153 | -------------------------------------------------------------------------------- /urdf/quadrotor.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 12 | 13 | 14 | 15 | Gazebo/${color} 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 162 | 163 | 164 | 165 | 166 | 167 | true 168 | 1000 169 | 1000.0 170 | 171 | 172 | chassis_body 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | -------------------------------------------------------------------------------- /src/functionlib.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | This file is part of ROS Quadrotor Feedback Controller. 3 | 4 | Foobar is free software; you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation; either version 2 of the License, or 7 | any later version. 8 | 9 | ROS Quadrotor Feedback Controller is distributed in the hope that it will 10 | be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General 12 | Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License along 15 | with ROS Quadrotor Feedback Controller; if not, write to the Free Software 16 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 17 | **/ 18 | #include 19 | #include "quadrotor_obstacle_avoidance/functionlib.h" 20 | #include 21 | #include 22 | 23 | #define PI 3.14159265358 24 | 25 | double mass=0.64; 26 | double Ix=0.02; 27 | double Iy=0.02; 28 | double Iz=0.04; 29 | double Ir=10^-3; 30 | double g=9.81; 31 | double omega=0; // Remove gyroscopic effect. 32 | double l=0.165; 33 | double d=4.5* pow(10,-7); 34 | 35 | 36 | 37 | using namespace arma; 38 | using namespace std; 39 | 40 | double * controller (double* X, double * V) 41 | { 42 | double * U = new double[4]; 43 | 44 | double roll=X[3]; 45 | double pitch=X[4]; 46 | double yaw=X[5]; 47 | 48 | double p=X[9]; 49 | double q=X[10]; 50 | double r=X[11]; 51 | double u1=X[12]; 52 | double eta=X[13]; 53 | 54 | // Computing the inverse of the decoupling matrix J. Its expression 55 | // has been computed with the symoblic toolbox provided by Matlab 56 | // and then copy-pasted into the code. 57 | mat J_inv=mat(4,4); 58 | 59 | // First row. 60 | J_inv(0,0)= mass*sin(roll)*sin(yaw) + mass*cos(roll)*cos(yaw)*sin(pitch); 61 | J_inv(0,1)= mass*cos(roll)*sin(pitch)*sin(yaw) - mass*cos(yaw)*sin(roll); 62 | J_inv(0,2)= mass*cos(pitch)*cos(roll); 63 | J_inv(0,3)= 0; 64 | 65 | // Second row. 66 | J_inv(1,0)= (Ix*mass*(cos(roll)*sin(yaw) - cos(yaw)*sin(pitch)*sin(roll)))/(l*u1); 67 | J_inv(1,1)= -(Ix*mass*(cos(roll)*cos(yaw) + sin(pitch)*sin(roll)*sin(yaw)))/(l*u1); 68 | J_inv(1,2)= -(Ix*mass*cos(pitch)*sin(roll))/(l*u1); 69 | J_inv(1,3)= 0; 70 | 71 | // Third row. 72 | J_inv(2,0)= (Iy*mass*cos(pitch)*cos(yaw))/(l*u1); 73 | J_inv(2,1)= (Iy*mass*cos(pitch)*sin(yaw))/(l*u1); 74 | J_inv(2,2)= -(Iy*mass*sin(pitch))/(l*u1); 75 | J_inv(2,3)= 0; 76 | 77 | // Fourth row. 78 | J_inv(3,0)= -(Iz*mass*cos(pitch)*cos(yaw)*sin(roll))/(d*u1*cos(roll)); 79 | J_inv(3,1)= -(Iz*mass*cos(pitch)*sin(roll)*sin(yaw))/(d*u1*cos(roll)); 80 | J_inv(3,2)= (Iz*mass*sin(pitch)*sin(roll))/(d*u1*cos(roll)); 81 | J_inv(3,3)= (Iz*cos(pitch))/(d*cos(roll)); 82 | 83 | 84 | mat L=mat(4,1); 85 | 86 | // Also for the vector L the expression has been computed with the 87 | // symoblic toolbox provided by Matlab and then copy-pasted into the code. 88 | 89 | L(0,0)= (q*cos(roll) - r*sin(roll))*((u1*(r*cos(roll)*(pow(tan(pitch),2) + 1) \ 90 | + q*sin(roll)*(pow(tan(pitch),2) + 1))*(cos(roll)*sin(yaw) - cos(yaw)*sin(pitch)*sin(roll)))/mass \ 91 | + (eta*cos(pitch)*cos(roll)*cos(yaw))/mass - (u1*cos(roll)*sin(yaw)*(r*cos(roll) \ 92 | + q*sin(roll)))/mass - (u1*cos(pitch)*cos(yaw)*sin(roll)*(p + r*cos(roll)*tan(pitch) \ 93 | + q*tan(pitch)*sin(roll)))/mass - (u1*cos(roll)*cos(yaw)*sin(pitch)*(q*cos(roll) - r*sin(roll)))/mass \ 94 | + (u1*sin(pitch)*(cos(yaw)*sin(roll) - cos(roll)*sin(pitch)*sin(yaw))*(r*cos(roll) + q*sin(roll)))/(mass*pow(cos(pitch),2))) \ 95 | - ((Ir*omega*p)/Iy + (p*r*(Ix - Iz))/Iy)*((u1*sin(roll)*(cos(yaw)*sin(roll) \ 96 | - cos(roll)*sin(pitch)*sin(yaw)))/(mass*cos(pitch)) + (u1*cos(pitch)*pow(cos(roll),2)*cos(yaw))/mass \ 97 | + (u1*tan(pitch)*sin(roll)*(cos(roll)*sin(yaw) - cos(yaw)*sin(pitch)*sin(roll)))/mass) + (p + r*cos(roll)*tan(pitch) \ 98 | + q*tan(pitch)*sin(roll))*((eta*(cos(roll)*sin(yaw) - cos(yaw)*sin(pitch)*sin(roll)))/mass - (u1*(sin(roll)*sin(yaw) \ 99 | + cos(roll)*cos(yaw)*sin(pitch))*(p + r*cos(roll)*tan(pitch) + q*tan(pitch)*sin(roll)))/mass \ 100 | + (u1*(q*cos(roll)*tan(pitch) - r*tan(pitch)*sin(roll))*(cos(roll)*sin(yaw) - cos(yaw)*sin(pitch)*sin(roll)))/mass \ 101 | + (u1*(cos(roll)*cos(yaw) + sin(pitch)*sin(roll)*sin(yaw))*(r*cos(roll) + q*sin(roll)))/(mass*cos(pitch)) \ 102 | + (u1*(cos(yaw)*sin(roll) - cos(roll)*sin(pitch)*sin(yaw))*(q*cos(roll) - r*sin(roll)))/(mass*cos(pitch)) \ 103 | - (u1*cos(pitch)*cos(roll)*cos(yaw)*(r*cos(roll) + q*sin(roll)))/mass - (u1*cos(pitch)*cos(yaw)*sin(roll)*(q*cos(roll) \ 104 | - r*sin(roll)))/mass) + eta*(((cos(roll)*sin(yaw) - cos(yaw)*sin(pitch)*sin(roll))*(p + r*cos(roll)*tan(pitch) \ 105 | + q*tan(pitch)*sin(roll)))/mass + ((cos(yaw)*sin(roll) - cos(roll)*sin(pitch)*sin(yaw))*(r*cos(roll) \ 106 | + q*sin(roll)))/(mass*cos(pitch)) + (cos(pitch)*cos(roll)*cos(yaw)*(q*cos(roll) - r*sin(roll)))/mass) \ 107 | + ((r*cos(roll) + q*sin(roll))*((eta*(cos(yaw)*sin(roll) - cos(roll)*sin(pitch)*sin(yaw)))/mass + (u1*(cos(roll)*cos(yaw) \ 108 | + sin(pitch)*sin(roll)*sin(yaw))*(p + r*cos(roll)*tan(pitch) + q*tan(pitch)*sin(roll)))/mass - (u1*(sin(roll)*sin(yaw) \ 109 | + cos(roll)*cos(yaw)*sin(pitch))*(r*cos(roll) + q*sin(roll)))/(mass*cos(pitch)) \ 110 | - (u1*cos(pitch)*cos(roll)*sin(yaw)*(q*cos(roll) - r*sin(roll)))/mass))/cos(pitch) + (u1*(cos(roll)*sin(yaw) \ 111 | - cos(yaw)*sin(pitch)*sin(roll))*((Ir*omega*q)/Ix + (q*r*(Iy - Iz))/Ix))/mass \ 112 | + (p*q*(Ix - Iy)*((u1*cos(roll)*(cos(yaw)*sin(roll) - cos(roll)*sin(pitch)*sin(yaw)))/(mass*cos(pitch)) \ 113 | + (u1*cos(roll)*tan(pitch)*(cos(roll)*sin(yaw) - cos(yaw)*sin(pitch)*sin(roll)))/mass \ 114 | - (u1*cos(pitch)*cos(roll)*cos(yaw)*sin(roll))/mass))/Iz; 115 | 116 | L(1,0)=eta*(((sin(roll)*sin(yaw) + cos(roll)*cos(yaw)*sin(pitch))*(r*cos(roll) + q*sin(roll)))/(mass*cos(pitch)) \ 117 | - ((cos(roll)*cos(yaw) + sin(pitch)*sin(roll)*sin(yaw))*(p + r*cos(roll)*tan(pitch) + q*tan(pitch)*sin(roll)))/mass \ 118 | + (cos(pitch)*cos(roll)*sin(yaw)*(q*cos(roll) - r*sin(roll)))/mass) - ((Ir*omega*p)/Iy \ 119 | + (p*r*(Ix - Iz))/Iy)*((u1*sin(roll)*(sin(roll)*sin(yaw) + cos(roll)*cos(yaw)*sin(pitch)))/(mass*cos(pitch)) \ 120 | + (u1*cos(pitch)*pow(cos(roll),2)*sin(yaw))/mass - (u1*tan(pitch)*sin(roll)*(cos(roll)*cos(yaw) \ 121 | + sin(pitch)*sin(roll)*sin(yaw)))/mass) - (p + r*cos(roll)*tan(pitch) + q*tan(pitch)*sin(roll))*((eta*(cos(roll)*cos(yaw) \ 122 | + sin(pitch)*sin(roll)*sin(yaw)))/mass - (u1*(cos(yaw)*sin(roll) - cos(roll)*sin(pitch)*sin(yaw))*(p \ 123 | + r*cos(roll)*tan(pitch) + q*tan(pitch)*sin(roll)))/mass + (u1*(q*cos(roll)*tan(pitch) \ 124 | - r*tan(pitch)*sin(roll))*(cos(roll)*cos(yaw) + sin(pitch)*sin(roll)*sin(yaw)))/mass - (u1*(sin(roll)*sin(yaw) \ 125 | + cos(roll)*cos(yaw)*sin(pitch))*(q*cos(roll) - r*sin(roll)))/(mass*cos(pitch)) - (u1*(cos(roll)*sin(yaw) \ 126 | - cos(yaw)*sin(pitch)*sin(roll))*(r*cos(roll) + q*sin(roll)))/(mass*cos(pitch)) \ 127 | + (u1*cos(pitch)*sin(roll)*sin(yaw)*(q*cos(roll) - r*sin(roll)))/mass + (u1*cos(pitch)*cos(roll)*sin(yaw)*(r*cos(roll) \ 128 | + q*sin(roll)))/mass) - (q*cos(roll) - r*sin(roll))*((u1*(r*cos(roll)*(pow(tan(pitch),2) + 1) \ 129 | + q*sin(roll)*(pow(tan(pitch),2) + 1))*(cos(roll)*cos(yaw) + sin(pitch)*sin(roll)*sin(yaw)))/mass \ 130 | - (u1*cos(roll)*cos(yaw)*(r*cos(roll) + q*sin(roll)))/mass - (eta*cos(pitch)*cos(roll)*sin(yaw))/mass \ 131 | + (u1*cos(roll)*sin(pitch)*sin(yaw)*(q*cos(roll) - r*sin(roll)))/mass \ 132 | + (u1*cos(pitch)*sin(roll)*sin(yaw)*(p + r*cos(roll)*tan(pitch) + q*tan(pitch)*sin(roll)))/mass \ 133 | - (u1*sin(pitch)*(sin(roll)*sin(yaw) + cos(roll)*cos(yaw)*sin(pitch))*(r*cos(roll) + q*sin(roll)))/(mass*pow(cos(pitch),2))) \ 134 | + ((r*cos(roll) + q*sin(roll))*((eta*(sin(roll)*sin(yaw) + cos(roll)*cos(yaw)*sin(pitch)))/mass \ 135 | + (u1*(cos(roll)*sin(yaw) - cos(yaw)*sin(pitch)*sin(roll))*(p + r*cos(roll)*tan(pitch) \ 136 | + q*tan(pitch)*sin(roll)))/mass + (u1*(cos(yaw)*sin(roll) - cos(roll)*sin(pitch)*sin(yaw))*(r*cos(roll) \ 137 | + q*sin(roll)))/(mass*cos(pitch)) + (u1*cos(pitch)*cos(roll)*cos(yaw)*(q*cos(roll) - r*sin(roll)))/mass))/cos(pitch) \ 138 | - (u1*(cos(roll)*cos(yaw) + sin(pitch)*sin(roll)*sin(yaw))*((Ir*omega*q)/Ix + (q*r*(Iy - Iz))/Ix))/mass \ 139 | - (p*q*(Ix - Iy)*((u1*cos(roll)*tan(pitch)*(cos(roll)*cos(yaw) + sin(pitch)*sin(roll)*sin(yaw)))/mass \ 140 | - (u1*cos(roll)*(sin(roll)*sin(yaw) + cos(roll)*cos(yaw)*sin(pitch)))/(mass*cos(pitch)) \ 141 | + (u1*cos(pitch)*cos(roll)*sin(roll)*sin(yaw))/mass))/Iz; 142 | 143 | L(2,0)= ((Ir*omega*p)/Iy + (p*r*(Ix - Iz))/Iy)*((u1*pow(cos(roll),2)*sin(pitch))/mass + (u1*sin(pitch)*pow(sin(roll),2))/mass) \ 144 | - ((eta*cos(pitch)*sin(roll))/mass + (p*u1*cos(pitch)*cos(roll))/mass)*(p + r*cos(roll)*tan(pitch) \ 145 | + q*tan(pitch)*sin(roll)) - (q*cos(roll) - r*sin(roll))*((eta*cos(roll)*sin(pitch))/mass \ 146 | + (q*u1*cos(pitch)*pow(cos(roll),2))/mass + (q*u1*cos(pitch)*pow(sin(roll),2))/mass - (p*u1*sin(pitch)*sin(roll))/mass) \ 147 | - eta*((p*cos(pitch)*sin(roll))/mass + (q*pow(cos(roll),2)*sin(pitch))/mass + (q*sin(pitch)*pow(sin(roll),2))/mass) \ 148 | - (u1*cos(pitch)*sin(roll)*((Ir*omega*q)/Ix + (q*r*(Iy - Iz))/Ix))/mass; 149 | 150 | L(3,0)= ((q*cos(roll) - r*sin(roll))*(p + r*cos(roll)*tan(pitch) + q*tan(pitch)*sin(roll)))/cos(pitch) \ 151 | - (sin(roll)*((Ir*omega*p)/Iy + (p*r*(Ix - Iz))/Iy))/cos(pitch) + (tan(pitch)*(r*cos(roll) + q*sin(roll))*(q*cos(roll) \ 152 | - r*sin(roll)))/cos(pitch) + (p*q*cos(roll)*(Ix - Iy))/(Iz*cos(pitch)); 153 | 154 | mat V_ar = mat(4,1); 155 | 156 | V_ar(0,0) = V[0]; 157 | V_ar(1,0) = V[1]; 158 | V_ar(2,0) = V[2]; 159 | V_ar(3,0) = V[3]; 160 | 161 | // The product between J_inv and (V-L) is computed using the ARMADILLO 162 | // library. 163 | mat U_tmp = J_inv * (V_ar-L); 164 | 165 | U[0]= U_tmp(0,0); 166 | U[1]= U_tmp(1,0); 167 | U[2]= U_tmp(2,0); 168 | U[3]= U_tmp(3,0); 169 | 170 | return U; 171 | } 172 | 173 | // Given a vector at time t-1 (X) and at time t (X_new) we compute the derivative. 174 | double* backward_derivative(double* X,double* X_new, double dt){ 175 | 176 | 177 | double* X_p=new double[14]; 178 | 179 | for(int i=0; i<14; i++){ 180 | X_p[i]=(X_new[i]-X[i])/dt; 181 | } 182 | 183 | return X_p; 184 | 185 | } 186 | 187 | double* force_vector(double* position, double yaw, double* goal, double* obstacle){ 188 | 189 | double* grad_att=new double[4]; 190 | double* grad_rep=new double[4]; 191 | double* grad=new double[4]; 192 | double ka=5; 193 | double kr=8; 194 | double kyaw=5; 195 | double dmax=6; 196 | double r = 0.5; 197 | double gain; 198 | double ob_distance; 199 | 200 | // Computing the actractive gradient for the position as the derivative of the 201 | // potential field. The potential U is defined as 1/2*Ka*(distance^2). 202 | grad_att[0]=-ka*(position[0]-goal[0]); 203 | grad_att[1]=-ka*(position[1]-goal[1]); 204 | grad_att[2]=-ka*(position[2]-goal[2]); 205 | grad_att[3]=-kyaw*(yaw - goal[3]); 206 | 207 | // Computing the repulsive gradient. 208 | ob_distance=sqrt(pow(obstacle[0],2) + pow(obstacle[1],2) + pow(obstacle[2],2))-r; 209 | 210 | // NOTE: This formula is an approximation. 211 | // TODO: Change gain with the true equation. 212 | if(ob_distance0){ 263 | do{ 264 | delta=previous_angle-(angle+2*PI); 265 | if(abs(delta) 20 | #include 21 | 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | /* 49 | Utility Macros 50 | */ 51 | 52 | #define STAMPA(X,Y) printf("%s : %f\n",(X),(Y)) 53 | #define LINE printf("----------\n") 54 | 55 | #define MAX 20 // MAX THRUST 56 | #define MIN 1 // MIN THRUST 57 | #define MAX_ROT 15 // MAX WRENCH 58 | #define MIN_ROT -15 // MIN WRENCH 59 | 60 | #define ENABLE_MATLAB_OUTPUT 0 61 | 62 | using namespace gazebo; 63 | 64 | /** 65 | This macro link this controller to Gazebo core. 66 | **/ 67 | GZ_REGISTER_DYNAMIC_CONTROLLER("feedback_linearization", FeedbackLinearization); 68 | 69 | //////////////////////////////////////////////////////////////////////////////// 70 | // Constructor 71 | FeedbackLinearization::FeedbackLinearization(Entity *parent) : Controller(parent) 72 | { 73 | 74 | this->parent_ = dynamic_cast (parent); 75 | 76 | if (!parent_) { 77 | gzthrow("Controller_Test controller requires a Model as its parent"); 78 | } 79 | 80 | this->X=new double[14]; // state returned by gazebo at time t. 81 | this->X_prev=new double[14]; // state returned by gazebo at time t-1. 82 | this->X_d=new double[14]; // first order derivative of the state at time t. 83 | this->X_d_prev=new double[14]; // first order derivative of the expanded state at time t-1. 84 | this->X_dd_prev=new double[14]; // second order derivative of the expanded state at time t-1. 85 | this->U_tilde= new double[4]; // input of the expanded model. 86 | this->Eta = 0; // ETA. 87 | this->U1_reale = 6.5; // U1 of the real quadrotor. 88 | 89 | this->goal=new double[4]; // Goal Coordinate (goal in configuration space) 90 | this->obstacle=new double[3]; // Near Obstacle Coordinate 91 | 92 | #if ENABLE_MATLAB_OUTPUT 93 | this->id=fopen("/home/vittorio/Scrivania/presentazione/ROS/quadrotor_obstacle_avoidance/toMatlab.txt", "w"); 94 | #endif 95 | } 96 | 97 | //////////////////////////////////////////////////////////////////////////////// 98 | // Destructor 99 | FeedbackLinearization::~FeedbackLinearization() 100 | { 101 | delete goal_callback_queue_thread_; 102 | delete obstacle_callback_queue_thread_; 103 | delete rosnode_; 104 | } 105 | 106 | //////////////////////////////////////////////////////////////////////////////// 107 | // Load the controller 108 | void FeedbackLinearization::LoadChild(XMLConfigNode *node) 109 | { 110 | int argc = 0; 111 | char** argv = NULL; 112 | ros::init(argc, argv, "quadrotor_feedback_linearization", ros::init_options::NoSigintHandler | ros::init_options::AnonymousName); 113 | rosnode_ = new ros::NodeHandle("/"); 114 | 115 | 116 | // Setting options of the goal's subscriber. 117 | ros::SubscribeOptions sg_options = ros::SubscribeOptions::create("goal", 1, 118 | boost::bind(&FeedbackLinearization::goalCallback, this, _1), 119 | ros::VoidPtr(), &queue_goal); 120 | 121 | // Subcribing the topic in which the goal will be published. 122 | sub_goal = rosnode_->subscribe(sg_options); 123 | 124 | // Setting options for the laser subscriber. 125 | ros::SubscribeOptions so_options = ros::SubscribeOptions::create("/full_cloud", 1, 126 | boost::bind(&FeedbackLinearization::obstacleCallback, this, _1), 127 | ros::VoidPtr(), &queue_obstacle); 128 | 129 | // Subcribing the topic in which the laser messages will be published. 130 | sub_obstacle = rosnode_->subscribe(so_options); 131 | 132 | body_main = this->parent_->GetBody("chassis_body"); 133 | 134 | 135 | // WARNING: both X and X_tilde shuld be initialized in such a way to be coherent with the launch file. 136 | for(int i=0; i<14; i++){ 137 | X_prev[i]=0; 138 | } 139 | X_prev[0]=0; 140 | X_prev[2]=7; 141 | 142 | for(int i=0; i<14; i++){ 143 | X[i]=0; 144 | X_d[i]=0; 145 | X_d_prev[i]=0; 146 | X_dd_prev[i]=0; 147 | } 148 | X[0]=0; 149 | X[2]=7; 150 | X[12]=6.0; 151 | X_prev[0]=0; 152 | X_prev[2]=7; 153 | X_prev[12]=6.0; 154 | 155 | for(int i=0; i<4; i++){ 156 | U_tilde[i]=0; 157 | } 158 | 159 | // The goal point is initialized to simulate a take off 160 | // phase. 161 | this->goal[0]=20; 162 | this->goal[1]=20; 163 | this->goal[2]=7; 164 | this->goal[3]=0; 165 | 166 | // The obstacle is initialized at [1000, 1000, 1000] in order to 167 | // avoid any problem during the "take off" phase. 168 | this->obstacle[0]=1000; 169 | this->obstacle[1]=1000; 170 | this->obstacle[2]=1000; 171 | 172 | } 173 | 174 | void FeedbackLinearization::InitChild() 175 | { 176 | goal_callback_queue_thread_ = new boost::thread(boost::bind(&FeedbackLinearization::GoalQueueThread, this)); 177 | obstacle_callback_queue_thread_ = new boost::thread(boost::bind(&FeedbackLinearization::ObstacleQueueThread, this)); 178 | } 179 | 180 | void FeedbackLinearization::SaveChild(std::string &prefix, std::ostream &stream) 181 | { 182 | 183 | } 184 | 185 | void FeedbackLinearization::ResetChild() 186 | { 187 | 188 | } 189 | 190 | void FeedbackLinearization::UpdateChild() 191 | { 192 | //Reading actual state from Gazebo. 193 | Pose3d pose=this->body_main->GetWorldPose(); // World Pose. 194 | Vector3 linear_velocity=this->body_main->GetWorldLinearVel(); // Linear Velocity. 195 | Vector3 angular_velocity=this->body_main->GetWorldAngularVel(); // Angular Velocity. 196 | Quatern frame_rot=pose.rot; 197 | Vector3 rpy=frame_rot.GetAsEuler(); // Converting pose from quaternion to rpy angles. 198 | double dt=0.001; // Time step used to compute numeric derivative or integral. 199 | 200 | // Saving in X_prev the previous state of the quadrotor. 201 | for(int i=0; i<14; i++){ 202 | X_prev[i]=X[i]; 203 | } 204 | // Loading the actual state from GAZEBO. 205 | X[0]= pose.pos.x; 206 | X[1]= pose.pos.y; 207 | X[2]= pose.pos.z; 208 | X[3]= rpy.x; 209 | X[4]= rpy.y; 210 | X[5]= rpy.z; 211 | X[6]= linear_velocity.x; 212 | X[7]= linear_velocity.y; 213 | X[8]= linear_velocity.z; 214 | X[9]= angular_velocity.x; 215 | X[10]= angular_velocity.y; 216 | X[11]= angular_velocity.z; 217 | X[12]= this->U1_reale; 218 | X[13]= this->Eta; 219 | 220 | // Angles in GAZEBO are normalized between -PI and PI in order to 221 | // get the real values we denormalize them. 222 | X[5]=denormalize_angle(X[5],X_prev[5]); 223 | 224 | // Saving the previous derivative of the state. 225 | for(int i=0; i<14; i++){ 226 | X_d_prev[i]=X_d[i]; 227 | } 228 | 229 | // Deleting the previous derivative of the state in order to compute 230 | // the new one. 231 | delete X_d; 232 | X_d=backward_derivative(X_prev,X,dt); 233 | 234 | // Compute the second dertivative of the state. 235 | double* X_dd=backward_derivative(X_d_prev, X_d, dt); 236 | for(int i=0; i<14; i++){ 237 | X_dd_prev[i]=X_dd[i]; 238 | } 239 | 240 | // Computing the feedforward term using the first and second order 241 | // derivative. 242 | double position[3]={X[0], X[1], X[2]}; 243 | double velocity[3]={X[6], X[7], X[8]}; 244 | 245 | double acc[3]={X_dd[0], X_dd[1], X_dd[2]}; 246 | double jerk[3]={X_dd[6], X_dd[7], X_dd[8]}; 247 | 248 | double* FV=force_vector(position, X[5], this->goal, this->obstacle); 249 | double* DP=damping(velocity, acc, jerk, X[11]); 250 | 251 | // Computing the V vector from the PDDDD. 252 | double* V=new double[4]; 253 | for (int i=0; i<4; i++){ 254 | V[i]=FV[i]-DP[i]; 255 | } 256 | 257 | // Deleting the previous U_tilde vector in order to compute the new one. 258 | delete U_tilde; 259 | U_tilde=controller(X, V); 260 | 261 | // Numerical integration of ETA and U1. 262 | this->Eta = this->Eta+(U_tilde[0])*dt; 263 | this->U1_reale = this->U1_reale+(this->Eta)*dt; 264 | 265 | #if ENABLE_MATLAB_OUTPUT 266 | // Saving some usefull variables. 267 | toMatlab(id, X, U_tilde, FV, V); 268 | #endif 269 | 270 | delete FV; 271 | delete DP; 272 | 273 | // Saturating input. 274 | FeedbackLinearization::saturationForce(); 275 | 276 | // Rotating the forces so the results are expressed in the quadrotor 277 | // reference frame and then applying them. 278 | Vector3* v = new Vector3(0.0,0.0,(this->U1_reale)); 279 | Vector3 torque(U_tilde[1], U_tilde[2], U_tilde[3]); 280 | 281 | Vector3 actual_force=frame_rot.RotateVector(*v); 282 | 283 | 284 | Vector3 actual_torque=frame_rot.RotateVector(torque); 285 | 286 | this->body_main->SetForce(actual_force); 287 | this->body_main->SetTorque(torque); 288 | 289 | } 290 | 291 | 292 | void FeedbackLinearization::FiniChild() 293 | { 294 | 295 | } 296 | 297 | //////////////////////////////////////////////////////////////////////////////// 298 | // Callback function for the goal topic 299 | void FeedbackLinearization::goalCallback(const geometry_msgs::Point::ConstPtr& goal_msg) 300 | { 301 | 302 | lock.lock(); 303 | 304 | this->goal[0] = goal_msg->x; 305 | this->goal[1] = goal_msg->y; 306 | this->goal[2] = goal_msg->z; 307 | this->goal[3] = 0; // TODO: Change message from Point to Pose and set this with `yaw`. 308 | 309 | lock.unlock(); 310 | 311 | } 312 | 313 | void FeedbackLinearization::GoalQueueThread() 314 | { 315 | static const double timeout = 0.01; 316 | 317 | while (rosnode_->ok()) 318 | { 319 | queue_goal.callAvailable(ros::WallDuration(timeout)); 320 | } 321 | } 322 | 323 | //////////////////////////////////////////////////////////////////////////////// 324 | // Callback function for the obstacle topic 325 | void FeedbackLinearization::obstacleCallback(const sensor_msgs::PointCloud::ConstPtr& obstacle_msg) 326 | { 327 | 328 | lock.lock(); 329 | 330 | int max; 331 | float min=200; 332 | double distance; 333 | double x1, y1, z1, x2, y2, z2; 334 | 335 | max=obstacle_msg->get_points_size(); 336 | 337 | vector > vect; 338 | 339 | x1=obstacle_msg->points[0].x; 340 | y1=obstacle_msg->points[0].y; 341 | z1=obstacle_msg->points[0].z; 342 | 343 | min=sqrt( pow(x1,2) + pow(y1,2) + pow(z1,2)); 344 | 345 | // First we read from the PointCloud message the scans of the laser 346 | // and then update the variable obstacle with the closest point. 347 | 348 | for(int i=1; ipoints[i].x; 350 | y2=obstacle_msg->points[i].y; 351 | z2=obstacle_msg->points[i].z; 352 | 353 | distance=sqrt( pow(x2,2) + pow(y2,2) + pow(z2,2)); 354 | if(distanceobstacle[0]=x1; 363 | this->obstacle[1]=y1; 364 | this->obstacle[2]=z1; 365 | 366 | lock.unlock(); 367 | 368 | } 369 | 370 | void FeedbackLinearization::ObstacleQueueThread() 371 | { 372 | static const double timeout = 0.01; 373 | 374 | while (rosnode_->ok()) 375 | { 376 | queue_obstacle.callAvailable(ros::WallDuration(timeout)); 377 | } 378 | } 379 | 380 | /** 381 | Saturation Function. 382 | 383 | We have to saturate thrust and wrench of the 384 | quadrotor to avoid "explosive" variations in the simulation. 385 | **/ 386 | void FeedbackLinearization::saturationForce() 387 | { 388 | // THRUST SATURATION 389 | if(this->U1_reale >= MAX && this->Eta >0){ 390 | this->U1_reale=MAX; 391 | this->Eta=0; 392 | } 393 | 394 | if(this->U1_reale <= MIN && this->Eta <0){ 395 | this->U1_reale=MIN; 396 | this->Eta=0; 397 | } 398 | 399 | // WRENCH SATURATION 400 | for(int i=1; i<4; i++ ){ 401 | if(U_tilde[i]>MAX_ROT){ 402 | U_tilde[i]=MAX_ROT; 403 | } 404 | if(U_tilde[i] \rho_{max} 314 | \end{cases} 315 | \label{eq:repulsive_field} 316 | \end{equation} 317 | 318 | dove $\rho$ è espresso come 319 | 320 | \begin{equation} 321 | \rho = \underset{x \in \mathcal{CO}}{\operatorname{argmin}}(d(\mathbf{p},x)) 322 | \end{equation} 323 | 324 | \noindent ovvero la minima distanza fra la configurazione corrente ed ogni altro punto della frontiera dello spazio $\mathcal{C}$-object. Dall'equazione \ref{eq:repulsive_field} ricaviamo 325 | 326 | \begin{equation} 327 | F_{rep}(\mathbf{p}) = 328 | \begin{cases} 329 | k_r \left( \frac{1}{\rho} - \frac{1}{\rho_{max}} \right) \frac{1}{\rho^2} \nabla \rho ,& \mbox{if } \rho \le \rho_{max} \\ 330 | 0 ,& \mbox{if } \rho > \rho_{max} 331 | \end{cases} 332 | \label{eq:repulsive_force} 333 | \end{equation} 334 | 335 | \noindent che è la nostra espressione per la forza repulsiva. 336 | 337 | \begin{figure} 338 | \begin{center} 339 | \includegraphics[scale=0.5]{img/attractive.png} 340 | \caption{Un esempio di potenziale attrattivo per uno spazio di configurazione bidimensionale e $\mathbf{p}_{goal} = [0 \, 0]^T$. } 341 | \end{center} 342 | \end{figure} 343 | 344 | \begin{figure} 345 | \begin{center} 346 | \includegraphics[scale=0.5]{img/repulsive.png} 347 | \caption{Un esempio di potenziale repulsivo per uno spazio di configurazione bidimensionale con un ostacolo circolare di raggio 2 centrato in $\mathbf{p}_{obs} = [0 \, 0]^T$. } 348 | \end{center} 349 | \end{figure} 350 | 351 | \subsection{Potenziale artificiale applicato al quadrirotore} 352 | 353 | Lo spazio di di configurazione del quadrirotore può essere limitato alle sole variabili controllabili ovvero $x$, $y$, $z$ e lo \emph{yaw} $\psi$. 354 | 355 | \begin{equation} 356 | \mathbf{p} = \left[ 357 | \begin{matrix} 358 | x \\ y \\ z \\ \psi 359 | \end{matrix} 360 | \right] 361 | \end{equation} 362 | 363 | Costruire la versione in $\mathcal{CO}$ degli ostacoli avvolgiamo il quadrirotore in una sfera di raggio $r$ (Fig. \ref{fig:boundary}) e usiamo la stessa per estendere gli ostacoli reali. 364 | 365 | \begin{figure} 366 | \begin{center} 367 | \includegraphics[scale=0.5]{img/boundarysphere.png} 368 | \caption{La sfera (\emph{boundary}) teorica che avvolge il quadrirotore. } 369 | \end{center} 370 | \label{fig:boundary} 371 | \end{figure} 372 | 373 | Come conseguenza di questa scelta abbiamo che la distanza minima del quadrirotore da un ostacolo nello spazio delle configurazioni dipenderà solamente dalla sua distanza dalla posizione $x y z$ del quadrirotore e sarà valida la seguente relazione 374 | 375 | \begin{equation} 376 | \rho = \begin{cases} 377 | \rho_{reale} - r ,& \mbox{if } \rho_{reale} \ge r \\ 378 | 0 ,& \mbox{if } \rho_{reale} < r 379 | \end{cases} 380 | \end{equation} 381 | 382 | \noindent dove $\rho_{reale}$ è la distanza minima del centro di massa del quadrirotore dall'ostacolo più vicino \emph{nello spazio di lavoro}. Tale distanza è immediatamente ricavabile, ad esempio tramite un sensore \emph{laser}. 383 | 384 | Un altra conseguenza di questa scelta è che gli ostacoli non influiscono sullo \emph{yaw}. In ogni momento quindi la forza repulsiva sullo \emph{yaw} è nulla 385 | 386 | \begin{equation} 387 | F_{rep}(\mathbf{p}) = \left[\begin{matrix} 388 | F_{rep}^x \\ 389 | F_{rep}^y \\ 390 | F_{rep}^z \\ 391 | 0 392 | \end{matrix}\right] 393 | \end{equation} 394 | 395 | Ora bisogna scegliere come applicare la forza derivante dal campo potenziale artificiale alla feedback linearization del quadrirotore. Partendo dalle 4 leggi di controllo descritte in (\ref{pddd}) possiamo annullare tutti gli errori e imporre 396 | 397 | \begin{equation}\begin{split}\label{pot-pddd} 398 | v_1 &= y_{1d}^{(4)} = F_x(\mathbf{p}) + \delta_x \\ 399 | v_2 &= y_{2d}^{(4)} = F_y(\mathbf{p}) + \delta_y \\ 400 | v_3 &= y_{3d}^{(4)} = F_z(\mathbf{p}) + \delta_z \\ 401 | v_4 &= y_{1d}^{(2)} = F_{\psi}(\mathbf{p}) + \delta_{\psi} \\ 402 | \end{split} 403 | \end{equation} 404 | 405 | Tale assunzione è valida poiché, nel trajectory tracking basato sui potenziali artificiali, assumiamo che il robot si trovi \emph{sempre} sulla traiettoria e, di conseguenza, l'errore è nullo. Il vettore di forze entra nel controllo quindi come \emph{snap} desiderato per quanto riguarda la posizione e come \emph{accelerazione} angolare desiderata dello \emph{yaw}. 406 | 407 | Come al solito, quando si imposta un riferimento di ordine superiore al primo, è necessario smorzare il comportamento oscillante o divergente del sistema aggiungendo alla forza un adeguato vettore di \emph{damping} $\mathbf{\delta}$. 408 | 409 | \begin{equation} 410 | \mathbf{\delta} = \left[ \begin{matrix} 411 | - k_1 \dot{x} - k_2 \ddot{x} - k_3 \dddot{x} \\ 412 | - k_1 \dot{y} - k_2 \ddot{y} - k_3 \dddot{y} \\ 413 | - k_1 \dot{z} - k_2 \ddot{z} - k_3 \dddot{z} \\ 414 | - k_1 \dot{\psi} 415 | \end{matrix} \right] 416 | \label{eq:damping} 417 | \end{equation} 418 | 419 | \subsubsection*{Riferimenti Bibliografici} 420 | 421 | Teoria \cite{RoboticsMPC} ed esempi di applicazione \cite{Park2001}. 422 | \newpage 423 | 424 | \section{Implementazione} 425 | 426 | \subsection{Matlab} 427 | 428 | In prima istanza il modello del quadrirotore, il controllore tramite feedback linearization e l'obstacle avoidance tramite artificial potential field sono stati implementati in un modello Simulink. 429 | 430 | \subsubsection{Modello del quadrirotore} 431 | 432 | \begin{center} 433 | \includegraphics[scale=0.5]{img/quad_model.png} 434 | \end{center} 435 | 436 | Questo primo blocco, ad ogni istante di tempo $t$, prende come input il vettore $U_t$, che rappresenta le variabili di controllo, e lo stato all'istante $t-1$, $x_{t-1}$. Esso calcola, tramite il modello teorico \ref{modello_totale} del quadrirotore come un corpo rigido a 6 DOF, lo stato $\dot{x}_t$ 437 | 438 | \subsubsection{Feedback linearization} 439 | \begin{center} 440 | \includegraphics[scale=0.5]{img/feedback.png} 441 | \end{center} 442 | 443 | Questo secondo blocco prende come input lo stato $x$ ottenuto integrando l'uscita del blocco precedente ed il vettore $v$ calcolato in accordo con l'equazione \ref{pddd}. All'interno del blocco vengono calcolate l'inversa della matrice di disaccoppiamento $J$ e il vettore $l$. L'output di questo blocco, ovvero gli input di controllo del sistema, vengono ottenuti applicando la \ref{eq:control_input}. 444 | 445 | \subsubsection{Campo Potenziale Artificiale} 446 | 447 | \begin{center} 448 | \includegraphics[scale=0.4]{img/potential.png} 449 | \end{center} 450 | 451 | Per calcolare il campo potenziale artificiale abbiamo utilizzato due differenti blocchi. Il primo, sulla destra nell'immagine, prende in input tre vettori $x$, $x_g$ e $x_o$ che rappresentano, rispettivamente, lo stato del quadrotor, la posizione del goal e quella dell'ostacolo. Qesto blocco calcola il valore dei potenziali in accordo con le definizioni delle equazioni \ref{eq:attractive_force} e \ref{eq:repulsive_force}. 452 | 453 | Il secondo blocco, sulla sinistra nell'immagine, prende in input lo stato $x$ e le derivate successive fino al terzo ordine. Esso è necessario per introdurre dei fattori di damping $\mathbf{\delta}$ che stabilizzino asintoticamente la convergenza ai valori desiderati in accordo con l'equazione \ref{eq:damping}. 454 | 455 | \subsection{GAZEBO e ROS} 456 | 457 | Terminato lo sviluppo del modello Simulink, il quadrotor è stato implementato in ROS e GAZEBO. 458 | 459 | ROS, acronimo di \emph{Robot Operating system}, ``è un sistema che offre librerie e strumenti atti ad aiutare gli sviluppatori di software a creare applicazioni per robot. In particolare offre un meccanismo di astrazione dell'hardware, drivers, librerie, visualizatori, strumenti per la trasmissione di messaggi, per la gestione dei pacchetti e molto altro''. Nel nostro lavoro ROS è stato utilizzato principalmente come piattaforma per lo scambio di messaggi tra le diverse conponenti del sistema. GAZEBO è invece un simultore multi-robot per ambienti aperti che ci ha permesso di visualizzare i risulatati ma soprattutto di simulare le dinamiche dell'ambiente, del quadrotor e dei sensori. 460 | 461 | In fig. \ref{GAZ_ROS} viene rappresentato lo schema a blocchi della simulazione implementata. Il blocco etichettato come GAZEBO simula l'ambiente, il quadrotor ed il sensore laser. Mentre l'ambiente e il quadrotor vengono visualizzati tramite un'interfaccia grafica i dati raccolti dal sensore vengono pubblicati, via ROS, su un apposito topic. Il blocco Feedback Controller, implementato in C++, calcola le forze e le coppie necessarie e le applica al quadrotor nell'ambiente simulato tramite le API di GAZEBO. Le forze e le coppie sono derivate dallo stato attuale del quadrotor, ottenuto direttamente da GAZEBO tramite API, dai rilevamenti del sensore, letti dall'apposito topic di ROS, e dalla posizione del goal. Il goal viene letto da un secondo topic, sempre implementato in ROS, sul quale pubblica il blocco Control Shell; quest'ultimo è una semplice shell implementate in C++ che permette di cambiare dinamicamente il goal del quadrotor. 462 | 463 | Nelle simulazioni condotte tramite GAZEBO abbiamo però rilevato che il motore di integrazione su cui si basa crea un grosso rumore numerico. Questo comportamento è ben visibile analizzando l'andamento della velocità angolare dello $\psi$ graficato in fig.\ref{rumore} 464 | 465 | \begin{figure} 466 | \begin{center} 467 | \includegraphics[scale=0.4]{img/gazebo-ros.png} 468 | \end{center} 469 | \caption{Schema a blocchi del modello simulato tramite GAZEBO e ROS} 470 | \label{GAZ_ROS} 471 | \end{figure} 472 | 473 | 474 | \begin{figure} 475 | \centering 476 | \subfigure[Simulazione complessiva]{\includegraphics[scale=0.5]{img/plot/gazebo/rumore.png}} 477 | \subfigure[$t=(0,250)$ ms]{\includegraphics[scale=0.5]{img/plot/gazebo/rumore_zoom.png}} 478 | \caption{Rumore Numerico} 479 | \label{rumore} 480 | \end{figure} 481 | 482 | 483 | \newpage 484 | \FloatBarrier 485 | \section{Simulazioni} 486 | Nelle seguenti simulazione i parametri adottati sono i seguenti: 487 | \begin{center} 488 | \subsubsection*{Guadagni} 489 | \begin{tabular}{|c|c|} 490 | \hline $k_a$ & 3 \\ 491 | \hline $k_r$ & 5 \\ 492 | \hline $k_y$ & 3 \\ 493 | \hline $k_{damping\, vel}$ & 40 \\ 494 | \hline $k_{damping\, acc}$ & 20 \\ 495 | \hline $k_{damping\, jerk}$ & 10 \\ 496 | \hline $k_{damping\, \dot{\psi}}$ & 8\\ 497 | \hline 498 | \end{tabular} 499 | 500 | \subsubsection*{Parametri Dinamici} 501 | \begin{tabular}{|c|c|} 502 | \hline $mass$ & 0.64 \\ 503 | \hline $I_x$ & 0.2 \\ 504 | \hline $I_y$ & 0.2 \\ 505 | \hline $I_z$ & 0.4 \\ 506 | \hline $I_r$ & $10^{-3}$ \\ 507 | \hline $g$ & 9.81 \\ 508 | \hline $\Omega_m$ & 0\\ 509 | \hline $\l$ & 0.165\\ 510 | \hline $d$ & $4.5*10^-7$\\ 511 | \hline 512 | \end{tabular} 513 | 514 | 515 | \subsubsection*{Altri Parametri} 516 | \begin{tabular}{|c|c|} 517 | \hline $\rho_{max}$ & 3 \\ 518 | \hline $r$ & 0.5 \\ 519 | \hline 520 | \end{tabular} 521 | \end{center} 522 | 523 | \subsection{Matlab} 524 | \subsubsection*{Simulazione n°1} 525 | In questa prima simulazione lo stato iniziale del quadrotor è $[0,0,7,0]$ mentre il goal è posto in $[7,7,7,-\pi/2]$, è inoltre presente un ostacolo di forma cilindrica e raggio unitario centrato in $[3,5]$. 526 | \begin{figure} 527 | \begin{center} 528 | \includegraphics[scale=0.6]{img/plot/matlab/sim1_xy.png} 529 | \end{center} 530 | \caption{Simulazione n°1, Matlab, piano $xy$} 531 | \label{sim:1xy_M} 532 | \end{figure} 533 | 534 | In fig. \ref{sim:1xy_M} è rappresentata la traiettoria seguita dal quadrirotrore sul piano $xy$ nell'aggirare l'ostacolo sia nel caso in cui lo stesso sia controllato in \emph{FL} (in blu), sia nel caso di un sistema perfettamente linearizzato simulato con 4 integratori in catena (in rosso). In fig. \ref{sim:1M} vengono invece presentati gli andamenti delle variabili controllate $[ x , y , z , \psi ]$ al variare del tempo. 535 | 536 | A prima vista l'andamento è più brusco di quanto si potrebbe immaginare, notiamo infatti una rapida sterzata del quadrirotore in prossimità delle coordinate $<3,3>$. Tuttavia è bene ricordare che le forze repulsive agiscono sul quadrirotore al livello della derivata quarta e che di conseguenza il loro effetto sulla posizione è piuttosto lento. 537 | 538 | Tale comportamento può essere variato e reso più o meno \emph{smooth} giocando con il rapporto fra $k_a$ e $k_r$. Ad esempio, lasciando $k_r$ fisso e diminuendo il guadagno attrattivo si ottiene una traiettoria molto più morbida al prezzo di una minore velocità del mezzo (con $k_a < 3$ il quadrirotore non converge entro i 60 secondi). 539 | 540 | \subsubsection*{Simulazione n°2} 541 | In questa seconda simulazione lo stato iniziale ed il goal sono i medesimi della precedente, l'ostacolo però è una sfera centrata in $[4, 4, 8]$. In fig. \ref{sim:2xyz_M} vengono rappresente la traiettorie del quadrotor sui piani $xz$ ed $yz$. Come è facile notare, i due grafici presentano il medesimo andamento come era logico aspettarsi tenendo in considerazione la simmetria del quadrotor e la posizione dell'ostacolo. 542 | 543 | Ancora una volta in fig. \ref{sim:2M} vengono presentati gli andamenti delle variabili controllate al variare del tempo. 544 | 545 | \begin{figure} 546 | \centering 547 | \subfigure[piano $xz$]{\includegraphics[scale=0.5]{img/plot/matlab/sim2_xz.png}} 548 | \subfigure[piano $yz$]{\includegraphics[scale=0.5]{img/plot/matlab/sim2_yz.png}} 549 | \caption{Simulazione n°2, Matlab} 550 | \label{sim:2xyz_M} 551 | \end{figure} 552 | 553 | 554 | \subsection{GAZEBO} 555 | 556 | \subsubsection*{Simulazione n°1} 557 | Le prima simulazione effettuata in GAZEBO riproduce nella maniere più fedele possibile la prima simulazione effettuata in Matlab: stato iniziale e goal sono i medesimi così come i guadagni dei campi attrattivi e repulsivi e il range of influence dell'ostacolo. 558 | Osservado fig. \ref{sim:1xy_G} possiamo notare che la traiettoria seguita manifesta un andamento oscillante, questo comportamento è da attribuirsi principalmente alla modellazione fisica dell quadrotor che ne "aumenta" le dimensioni e di conseguenza l'ostacolo viene avvertito come più vicino. L'intensità di queste oscillazioni, infatti, dipende sia dal raggio di influenza dell'ostacolo che dal guadagno del campo potenziale repulsivo. 559 | 560 | In fig. \ref{sim:1M} vengono riportati gli andamenti delle variabili controllate in funzione del tempo. 561 | 562 | \begin{figure} 563 | \centering 564 | \subfigure[piano $xz$]{\includegraphics[scale=0.5]{img/plot/gazebo/simulazione1_xy.png}} 565 | \caption{Simulazione n°1, GAZEBO} 566 | \label{sim:1xy_G} 567 | \end{figure} 568 | 569 | \subsubsection*{Simulazione n°2} 570 | Anche nella seconda simulazione abbiamo ricalcato la situazione inizialmente presentata su Matlab in GAZEBO, ed anche in questo caso dalla fig. \ref{sim:2xyz_G} osserviamo che la diversa modellazione fisica del quadrotor e gli errori di misurazione introdotti del laser risultano in una traiettoria differente. 571 | 572 | In fig. \ref{sim:2G} vengono presentati gli andamenti di tutte e quattro le variabili di controllo al variare del tempo. 573 | 574 | \begin{figure} 575 | \centering 576 | \subfigure[piano $xz$]{\includegraphics[scale=0.5]{img/plot/gazebo/simulazione2_xz.png}} 577 | \subfigure[piano $yz$]{\includegraphics[scale=0.5]{img/plot/gazebo/simulazione2_yz.png}} 578 | \caption{Simulazione n°2, GAZEBO} 579 | \label{sim:2xyz_G} 580 | \end{figure} 581 | 582 | 583 | \subsubsection*{Simulazione n°3} 584 | In questa terza simulazione lo stato iniziale è $[0,0,7,0]$ ed il goal $[18.5, 17.5, 7, -\pi/2]$. In questo caso sono stati posizionati due ostacoli, di forma cilindrica e raggio unitario, centrati rispetivamente in $[5,3,0]$ e $[13,13,0]$. In fig. \ref{sim:3xy_G} viene rappresentata la traiettoria seguita dal quadrotor sul piano $xy$ ciò che colpisce è il comportamento osservato nell'evitare il secondo ostacolo: una traiettoria che lascia l'ostacolo sulla destra potrebbe sembrare più corretta ma, così facendo, il quadrotor si troverebbe in una posizione più lontana dal goal rispetto a quella raggiunta seguendo la traiettoria tracciata. Scegliere una traiettoria che lascia l'ostacolo sulla destra signifierebbe, di fatto, andare in direzione opposta al gradiente. 585 | 586 | Per completezza, in fig. \ref{sim:3G} vengono riportati anche gli andamenti di $[x,y,z,\psi]$ al variare del tempo. 587 | 588 | \begin{figure} 589 | \begin{center} 590 | \includegraphics[scale=0.5]{img/plot/gazebo/simulazione3_xy.png} 591 | \end{center} 592 | \caption{Simulazione n°3, GAZEBO, piano $xy$} 593 | \label{sim:3xy_G} 594 | \end{figure} 595 | 596 | \newpage 597 | \begin{figure}[p] 598 | \hspace*{-1.5cm} 599 | \begin{center} 600 | \includegraphics[scale=0.5]{img/plot/matlab/simulation1.png} 601 | \caption{Simulazione n°1, Matlab} 602 | \label{sim:1M} 603 | \end{center} 604 | \end{figure} 605 | 606 | \begin{figure}[p] 607 | \hspace*{-3.5cm} 608 | \includegraphics[scale=0.5]{img/plot/matlab/sim2_pdf.png} 609 | \caption{Simulazione n°2, Matlab} 610 | \label{sim:2M} 611 | \end{figure} 612 | 613 | \begin{figure}[p] 614 | \hspace*{-3.5cm} 615 | \includegraphics[scale=0.5]{img/plot/gazebo/simulazione1_pdf.png} 616 | \caption{Simulazione n°1, GAZEBO} 617 | \label{sim:1G} 618 | \end{figure} 619 | 620 | \begin{figure}[p] 621 | \hspace*{-3.5cm} 622 | \includegraphics[scale=0.5]{img/plot/gazebo/simulazione2_pdf.png} 623 | \caption{Simulazione n°2, GAZEBO} 624 | \label{sim:2G} 625 | \end{figure} 626 | 627 | \begin{figure}[p] 628 | \hspace*{-3.5cm} 629 | \includegraphics[scale=0.5]{img/plot/gazebo/simulazione3_pdf.png} 630 | \caption{Simulazione n°3, GAZEBO} 631 | \label{sim:3G} 632 | \end{figure} 633 | 634 | \FloatBarrier 635 | \newpage 636 | 637 | \bibliography{references} 638 | \bibliographystyle{plain} 639 | 640 | 641 | \end{document} 642 | --------------------------------------------------------------------------------