├── .gitignore ├── Makefile ├── README.md ├── figures ├── AStar2.png ├── Cf_hi.png ├── Cf_lo.png ├── Cf_mid.png ├── Dijkstra.png ├── GlobalPlanner.png ├── GridPath.png ├── NC_hi.png ├── NC_lo.png ├── NC_mid.png ├── ObstacleTypes.png ├── OptimalCostmapDecayCurve.png ├── Raytracing.png ├── Recovery.png ├── Resolution Compare.png ├── costmapspec.png ├── goodinflation.png ├── notgoodinflation.png ├── param1.png ├── param2.png ├── resolutionholes.png ├── scanner1.png ├── scanner2.png ├── simtime15.png ├── simtime4.png ├── voxellayerparam1.png ├── voxellayerparam2.png └── voxellayerscene.png ├── main.bib ├── main.pdf └── main.tex /.gitignore: -------------------------------------------------------------------------------- 1 | __MACOSX 2 | auto 3 | *.aux 4 | *.out 5 | *.bbl 6 | *.blg 7 | *.fdb_latexmk 8 | *.log 9 | *.synctex.gz 10 | root.pdf -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | TARGET = main 2 | pdf: 3 | pdflatex $(TARGET) && bibtex $(TARGET) && pdflatex $(TARGET) && \ 4 | pdflatex $(TARGET) 5 | 6 | quick: 7 | pdflatex $(TARGET) 8 | 9 | clean: 10 | rm -f *.aux *.log *.blg *.bbl *~ *.dvi *.out *.nav *.snm *.toc $(TARGETS) 11 | 12 | final: 13 | ps2pdf13 -dPDFSETTINGS=/prepress -dEmbedAllFonts=true -dUseFlateCompression=true -dCompatibilityLevel=1.4 $(TARGET).pdf $(TARGET)_final.pdf 14 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ROSNavigationGuide 2 | This is a guide for ROS navigation parameters tuning. Hopefully it is helpful for you to understand concepts and reasonings behind 3 | different components in ROS navigation stack and how to tune them well. It is also a summary for part of my research work. 4 | 5 | [The PDF file](main.pdf) is here. 6 | 7 | Feel free to open issues about mistakes, or contribute directly by sending pull requests. 8 | 9 | **Update:** This document is now published as a chapter in the [Robot Operating System (ROS): The Complete Reference (Volume 6)](https://link.springer.com/book/10.1007/978-3-030-75472-3#aboutBook). It is still available on [arxiv](https://arxiv.org/pdf/1706.09068.pdf). Please use the following citation: 10 | ``` 11 | @Inbook{zheng-ros-navguide, 12 | author = "Zheng, Kaiyu", 13 | editor = "Koubaa, Anis", 14 | title = "ROS Navigation Tuning Guide", 15 | bookTitle = "Robot Operating System (ROS): The Complete Reference (Volume 6)", 16 | year = "2021", 17 | publisher = "Springer International Publishing", 18 | address = "Cham", 19 | pages = "197--226", 20 | isbn = "978-3-030-75472-3", 21 | doi = "10.1007/978-3-030-75472-3_6", 22 | url = "https://doi.org/10.1007/978-3-030-75472-3_6" 23 | } 24 | ``` 25 | 26 | Contributor: 27 | 28 | - [Kaiyu Zheng](http://kaiyuzh.me) 29 | -------------------------------------------------------------------------------- /figures/AStar2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zkytony/ROSNavigationGuide/217b50b4633fc5179081e736282be93c96a6f460/figures/AStar2.png -------------------------------------------------------------------------------- /figures/Cf_hi.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zkytony/ROSNavigationGuide/217b50b4633fc5179081e736282be93c96a6f460/figures/Cf_hi.png -------------------------------------------------------------------------------- /figures/Cf_lo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zkytony/ROSNavigationGuide/217b50b4633fc5179081e736282be93c96a6f460/figures/Cf_lo.png -------------------------------------------------------------------------------- /figures/Cf_mid.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zkytony/ROSNavigationGuide/217b50b4633fc5179081e736282be93c96a6f460/figures/Cf_mid.png -------------------------------------------------------------------------------- /figures/Dijkstra.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zkytony/ROSNavigationGuide/217b50b4633fc5179081e736282be93c96a6f460/figures/Dijkstra.png -------------------------------------------------------------------------------- /figures/GlobalPlanner.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zkytony/ROSNavigationGuide/217b50b4633fc5179081e736282be93c96a6f460/figures/GlobalPlanner.png -------------------------------------------------------------------------------- /figures/GridPath.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zkytony/ROSNavigationGuide/217b50b4633fc5179081e736282be93c96a6f460/figures/GridPath.png -------------------------------------------------------------------------------- /figures/NC_hi.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zkytony/ROSNavigationGuide/217b50b4633fc5179081e736282be93c96a6f460/figures/NC_hi.png -------------------------------------------------------------------------------- /figures/NC_lo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zkytony/ROSNavigationGuide/217b50b4633fc5179081e736282be93c96a6f460/figures/NC_lo.png -------------------------------------------------------------------------------- /figures/NC_mid.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zkytony/ROSNavigationGuide/217b50b4633fc5179081e736282be93c96a6f460/figures/NC_mid.png -------------------------------------------------------------------------------- /figures/ObstacleTypes.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zkytony/ROSNavigationGuide/217b50b4633fc5179081e736282be93c96a6f460/figures/ObstacleTypes.png -------------------------------------------------------------------------------- /figures/OptimalCostmapDecayCurve.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zkytony/ROSNavigationGuide/217b50b4633fc5179081e736282be93c96a6f460/figures/OptimalCostmapDecayCurve.png -------------------------------------------------------------------------------- /figures/Raytracing.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zkytony/ROSNavigationGuide/217b50b4633fc5179081e736282be93c96a6f460/figures/Raytracing.png -------------------------------------------------------------------------------- /figures/Recovery.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zkytony/ROSNavigationGuide/217b50b4633fc5179081e736282be93c96a6f460/figures/Recovery.png -------------------------------------------------------------------------------- /figures/Resolution Compare.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zkytony/ROSNavigationGuide/217b50b4633fc5179081e736282be93c96a6f460/figures/Resolution Compare.png -------------------------------------------------------------------------------- /figures/costmapspec.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zkytony/ROSNavigationGuide/217b50b4633fc5179081e736282be93c96a6f460/figures/costmapspec.png -------------------------------------------------------------------------------- /figures/goodinflation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zkytony/ROSNavigationGuide/217b50b4633fc5179081e736282be93c96a6f460/figures/goodinflation.png -------------------------------------------------------------------------------- /figures/notgoodinflation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zkytony/ROSNavigationGuide/217b50b4633fc5179081e736282be93c96a6f460/figures/notgoodinflation.png -------------------------------------------------------------------------------- /figures/param1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zkytony/ROSNavigationGuide/217b50b4633fc5179081e736282be93c96a6f460/figures/param1.png -------------------------------------------------------------------------------- /figures/param2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zkytony/ROSNavigationGuide/217b50b4633fc5179081e736282be93c96a6f460/figures/param2.png -------------------------------------------------------------------------------- /figures/resolutionholes.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zkytony/ROSNavigationGuide/217b50b4633fc5179081e736282be93c96a6f460/figures/resolutionholes.png -------------------------------------------------------------------------------- /figures/scanner1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zkytony/ROSNavigationGuide/217b50b4633fc5179081e736282be93c96a6f460/figures/scanner1.png -------------------------------------------------------------------------------- /figures/scanner2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zkytony/ROSNavigationGuide/217b50b4633fc5179081e736282be93c96a6f460/figures/scanner2.png -------------------------------------------------------------------------------- /figures/simtime15.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zkytony/ROSNavigationGuide/217b50b4633fc5179081e736282be93c96a6f460/figures/simtime15.png -------------------------------------------------------------------------------- /figures/simtime4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zkytony/ROSNavigationGuide/217b50b4633fc5179081e736282be93c96a6f460/figures/simtime4.png -------------------------------------------------------------------------------- /figures/voxellayerparam1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zkytony/ROSNavigationGuide/217b50b4633fc5179081e736282be93c96a6f460/figures/voxellayerparam1.png -------------------------------------------------------------------------------- /figures/voxellayerparam2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zkytony/ROSNavigationGuide/217b50b4633fc5179081e736282be93c96a6f460/figures/voxellayerparam2.png -------------------------------------------------------------------------------- /figures/voxellayerscene.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zkytony/ROSNavigationGuide/217b50b4633fc5179081e736282be93c96a6f460/figures/voxellayerscene.png -------------------------------------------------------------------------------- /main.bib: -------------------------------------------------------------------------------- 1 | @inproceedings{brock1999high, 2 | title={High-speed navigation using the global dynamic window approach}, 3 | author={Brock, Oliver and Khatib, Oussama}, 4 | booktitle={Proceedings 1999 IEEE International Conference on Robotics and Automation (Cat. No. 99CH36288C)}, 5 | volume={1}, 6 | pages={341--346}, 7 | year={1999}, 8 | organization={IEEE} 9 | } 10 | 11 | @article{fox1997dynamic, 12 | title={The dynamic window approach to collision avoidance}, 13 | author={Fox, Dieter and Burgard, Wolfram and Thrun, Sebastian}, 14 | journal={IEEE Robotics \& Automation Magazine}, 15 | volume={4}, 16 | number={1}, 17 | pages={23--33}, 18 | year={1997}, 19 | publisher={IEEE} 20 | } 21 | 22 | @book{furrer2016robot, 23 | title={Robot operating system (ros): The complete reference (volume 1)}, 24 | author={Furrer, F. and Burri, M. and Achtelik, M. and and Siegwart, R.}, 25 | year={2016}, 26 | pages={595--625}, 27 | publisher={by A. Koubaa. Cham: Springer International Publishing} 28 | } 29 | 30 | @book{thrun2005probabilistic, 31 | title={Probabilistic robotics}, 32 | author={Thrun, Sebastian and Burgard, Wolfram and Fox, Dieter}, 33 | year={2005}, 34 | publisher={MIT press} 35 | } -------------------------------------------------------------------------------- /main.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zkytony/ROSNavigationGuide/217b50b4633fc5179081e736282be93c96a6f460/main.pdf -------------------------------------------------------------------------------- /main.tex: -------------------------------------------------------------------------------- 1 | \documentclass[12pt]{article} 2 | 3 | \usepackage{amsmath,amsthm,amssymb, listings, tikz, pgf} 4 | \usepackage[top=1.25in, bottom=1.25in, left=1.25in, right=1.25in]{geometry} 5 | \usepackage{pgf} 6 | \usepackage{tikz} 7 | 8 | \usepackage{multicol} 9 | \usepackage{enumitem} 10 | \usepackage{sectsty} 11 | \subsectionfont{\normalsize} 12 | \subsubsectionfont{\small} 13 | \usepackage{listings} 14 | 15 | \usepackage{csquotes} 16 | \usepackage{gensymb} 17 | \usepackage[font=footnotesize]{caption} 18 | 19 | \usetikzlibrary{arrows,automata} 20 | \usepackage[latin1]{inputenc} 21 | \def\imp{\rightarrow} 22 | \def\Imp{\Rightarrow} 23 | \def\bicon{\leftrightarrow} 24 | \def\nilstr{\varepsilon} 25 | \newcommand\XOR{\mathbin{\char`\^}} 26 | 27 | \usepackage[hidelinks]{hyperref} 28 | \hypersetup{ 29 | colorlinks, 30 | linkcolor={red!50!black}, 31 | citecolor={blue!50!black}, 32 | urlcolor={blue!80!black} 33 | } 34 | 35 | \lstset{ 36 | mathescape, 37 | frame=tb, 38 | aboveskip=3mm, 39 | belowskip=3mm, 40 | showstringspaces=false, 41 | columns=flexible, 42 | basicstyle={\ttfamily\small}, 43 | numbers=none, 44 | numberstyle=\tiny\color{gray}, 45 | keywordstyle=\color{blue}, 46 | commentstyle=\color{dkgreen}, 47 | stringstyle=\color{mauve}, 48 | breaklines=true, 49 | breakatwhitespace=true, 50 | tabsize=3 51 | } 52 | 53 | \colorlet{punct}{red!60!black} 54 | \definecolor{background}{HTML}{EEEEEE} 55 | \definecolor{delim}{RGB}{20,105,176} 56 | \colorlet{numb}{magenta!60!black} 57 | \lstdefinelanguage{json}{ 58 | basicstyle=\normalfont\ttfamily, 59 | numbers=left, 60 | numberstyle=\scriptsize, 61 | stepnumber=1, 62 | numbersep=8pt, 63 | showstringspaces=false, 64 | breaklines=true, 65 | frame=lines, 66 | backgroundcolor=\color{background}, 67 | literate= 68 | *{:}{{{\color{punct}{:}}}}{1} 69 | {,}{{{\color{punct}{,}}}}{1} 70 | {\{}{{{\color{delim}{\{}}}}{1} 71 | {\}}{{{\color{delim}{\}}}}}{1} 72 | {[}{{{\color{delim}{[}}}}{1} 73 | {]}{{{\color{delim}{]}}}}{1}, 74 | } 75 | 76 | \graphicspath{{figures/}} 77 | 78 | \author{Kaiyu Zheng} 79 | \date{September 2, 2016\footnote{Update on April 8th, 2019 (added information on \texttt{amcl})}} 80 | \title{ROS Navigation Tuning Guide} 81 | 82 | \begin{document} 83 | \maketitle 84 | 85 | \section*{Abstract} 86 | The ROS navigation stack is powerful for mobile robots to move from place to place reliably. The job of navigation stack is to produce a safe path for the robot to execute, by processing data from odometry, sensors and environment map. Maximizing the performance of this navigation stack requires some fine tuning of parameters, and this is not as simple as it looks. One who is sophomoric about the concepts and reasoning may try things randomly, and wastes a lot of time. 87 | 88 | This article intends to guide the reader through the process of fine tuning navigation parameters. It is the reference when someone need to know the "how" and "why" when setting the value of key parameters. This guide assumes that the reader has already set up the navigation stack and ready to optimize it. This is also a summary of my work with the ROS navigation stack.\\ 89 | 90 | \begin{multicols}{2} 91 | [ 92 | \section*{Topics} 93 | ] 94 | \begin{flushleft} 95 | \begin{enumerate} 96 | \item Velocity and Acceleration\\ 97 | \item Global Planner\\ 98 | \begin{enumerate} 99 | \item Global Planner Selection 100 | \item Global Planner Parameters 101 | \end{enumerate} 102 | \item Local Planner\\ 103 | \begin{enumerate} 104 | \item Local Planner Selection 105 | \item DWA Local Planner 106 | \begin{enumerate} 107 | \item DWA algorithm 108 | \item DWA forward simulation 109 | \item DWA trajectory scoring 110 | \item Other DWA parameters 111 | \end{enumerate} 112 | \end{enumerate} 113 | \item Costmap Parameters 114 | \item AMCL 115 | \item Recovery Behavior 116 | \item Dynamic Reconfigure 117 | \item Problems 118 | \end{enumerate} 119 | \end{flushleft} 120 | \end{multicols} 121 | 122 | 123 | \newpage 124 | 125 | \section{Velocity and Acceleration} 126 | This section concerns with synchro-drive robots. The dynamics (e.g. velocity and acceleration of the robot) of the robot is essential for local planners including dynamic window approach (DWA) and timed elastic band (TEB). In ROS navigation stack, local planner 127 | takes in odometry messages ("odom" topic) and outputs velocity commands ("cmd\_vel" topic) that controls the robot's motion. \\ 128 | 129 | \noindent Max$/$min velocity and acceleration are two basic parameters for the mobile base. Setting them correctly is very helpful for optimal local planner behavior. In ROS navigation, we need to know translational and rotational velocity and acceleration. 130 | 131 | \subsection{To obtain maximum velocity} 132 | Usually you can refer to your mobile base's manual. For example, SCITOS G5 has maximum velocity 1.4 m$/$s\footnote{This information is obtained from \href{http://www.metralabs.com/en/research}{MetraLabs's website}.}. In ROS, you can also subscribe to the \texttt{odom} topic to obtain the current odometry information. If you can control your robot manually (e.g. with a joystick), you can try to run it forward until its 133 | speed reaches constant, and then echo the odometry data. 134 | 135 | \textit{Translational velocity} ($m/s$) is the velocity when robot is moving in a straight line. Its max value is the same as the maximum velocity we obtained above. \textit{Rotational velocity} ($rad/s$) is equivalent as angular velocity; its maximum value is the angular velocity of the robot when it is rotating in place. To obtain maximum rotational velocity, we can control the robot by a joystick and rotate the robot 360 degrees after the robot's speed reaches constant, and time this movement. %Another method is to measure the radius of the circumscribed circle of robot's footprint, then compute the maximum angular velocity by $\omega_{max}=v_{max}/r$. 136 | 137 | For safety, we prefer to set maximum translational and rotational velocities to be lower than their actual maximum values. 138 | 139 | \subsection{To obtain maximum acceleration} 140 | There are many ways to measure maximum acceleration of your mobile base, if your manual does not tell you directly. 141 | 142 | In ROS, again we can echo odometry data which include time stamps, and them see how long it took the robot to reach constant maximum translational velocity ($t_{i}$). Then we use the position and velocity information from odometry (nav\_msgs/Odometry message) to compute the acceleration in this process. Do several trails and take the average. Use $t_t, t_r$ to denote the time used to reach translationand and rotational maximum velocity from static, respectively. The maximum translational acceleration $a_{t,max}=\text{max }dv / dt\approx v_{max}/t_t$. Likewise, rotational acceleration can be computed by $a_{r,max}=\text{max }d\omega / dt\approx \omega_{max}/t_r$. 143 | 144 | % We can also use some physics method to measure accelerations. To measure translational acceleration, we can let the initially stationary robot 145 | % move along a straight line with known length, and time its movement. Then plug in distance as $s$ and time as $t$ into this formula 146 | % to obtain maximum translational acceleration: 147 | % $$a_{trans}=\frac{2s}{t_{trans}^2}$$ 148 | % For maximum rotational acceleration, the method is almost the same. Let the initially stationary mobile base rotate in place for a certain number 149 | % of full circles, and time its movement. Then, use this formula where $l$ is the total length of rotation and $t$ is the time: 150 | % $$a_{rot}=\frac{2l}{t_{rot}^2}$$ 151 | 152 | \subsection{Setting minimum values} 153 | Setting minimum velocity is not as formulaic as above. For minimum translational velocity, we want to set it to a large negative value 154 | because this enables the robot to back off when it needs to unstuck itself, but it should prefer moving forward in most cases. For minimum rotational velocity, we also want to set it to negative (if the parameter allows) so that the robot can rotate in either directions. Notice that DWA Local Planner takes the absolute value of robot's minimum rotational velocity. 155 | 156 | \subsection{Velocity in x, y direction} 157 | 158 | \textbf{$x$ velocity} means the velocity in the direction parallel to robot's straight movement. It is the same as translational velocity. \textbf{$y$ velocity} is the velocity in the direction perpendicular to that straight movement. It is called "strafing velocity" in \texttt{teb\_local\_planner}. $y$ velocity should be set to zero for non-holonomic robot (such as 159 | differential wheeled robots).\\ 160 | 161 | \section{Global Planner} 162 | 163 | \subsection{Global Planner Selection} 164 | 165 | To use the \texttt{move$\_$base} node in navigation stack, we need to have a global planner 166 | and a local planner. There are three global planners that adhere to \texttt{nav\_core::BaseGlobal\\Planner} interface: \texttt{carrot$\_$planner}, 167 | \texttt{navfn} and \texttt{global$\_$planner}. 168 | 169 | \subsubsection{carrot\_planner} 170 | 171 | This is the simplest one. It checks if the given goal is an obstacle, and if so it picks an alternative goal close to the original one, by moving back along the vector between the robot and the goal point. Eventually it passes this valid goal as a plan to the local planner or controller (internally). Therefore, this planner does not do any global path planning. It is helpful if you require your robot to move close to the given goal even if the goal is unreachable. In complicated indoor environments, this planner is not very practical. 172 | 173 | \subsubsection{navfn and global\_planner} 174 | 175 | \texttt{navfn} uses Dijkstra's algorithm to find a global path with minimum cost between start point and 176 | end point. \texttt{global$\_$planner} is built as a more flexible replacement of \texttt{navfn} with more options. These 177 | options include (1) support for A$*$, (2) toggling quadratic approximation, (3) toggling grid path. Both \texttt{navfn} and 178 | global planner are based on the work by \cite{brock1999high}: 179 | 180 | \subsection{Global Planner Parameters} 181 | \normalsize 182 | 183 | Since \texttt{global\_planner} is generally the one that we prefer, let us look at some of its key parameters. 184 | Note: not all of these parameters are listed on ROS's website, but you can see them if you run the rqt dynamic reconfigure program: with \begin{center} 185 | \texttt{rosrun rqt\_reconfigure rqt\_reconfigure} 186 | \end{center} 187 | 188 | We can leave \texttt{allow\_unknown}(true), \texttt{use\_dijkstra}(true), \texttt{use\_quadratic}(true), \texttt{use\_grid\_path}(false), \texttt{old\_navfn\_behavior}(false) to 189 | their default values. Setting \texttt{visualize\_potential}(false) to true is helpful when we would like to visualize the potential map in RVIZ. 190 | 191 | \begin{figure}[!htb] 192 | \minipage{0.49\textwidth} 193 | \includegraphics[width=\linewidth]{Dijkstra.png} 194 | \caption{Dijkstra path} 195 | \endminipage\hfill 196 | \minipage{0.49\textwidth} 197 | \includegraphics[width=\linewidth]{AStar2.png} 198 | \caption{A* path} 199 | \endminipage\hfill 200 | \end{figure} 201 | 202 | \begin{figure}[!htb] 203 | \minipage{0.49\textwidth} 204 | \includegraphics[width=\linewidth]{GlobalPlanner.png} 205 | \caption{Standard Behavior} 206 | \endminipage\hfill 207 | \minipage{0.49\textwidth} 208 | \includegraphics[width=\linewidth]{GridPath.png} 209 | \caption{Grid Path} 210 | \endminipage\hfill 211 | \end{figure} 212 | 213 | 214 | Besides these parameters, there are three other unlisted parameters that actually determine the quality of the planned 215 | global path. They are \texttt{cost\_factor}, \texttt{neutral\_cost}, \texttt{lethal\_cost}. Actually, these parameters also present 216 | in \texttt{navfn}. The source code\footnote{\scriptsize\url{https://github.com/ros-planning/navigation/blob/indigo-devel/navfn/include/navfn/navfn.h}} \normalsize has one paragraph explaining how \texttt{navfn} computes cost values. 217 | 218 | \newpage 219 | 220 | \begin{figure}[!htb] 221 | \minipage{0.32\textwidth} 222 | \includegraphics[width=\linewidth]{Cf_lo.png} 223 | \caption{\texttt{cost\_factor} = 0.01} 224 | \endminipage\hfill 225 | \minipage{0.32\textwidth} 226 | \includegraphics[width=\linewidth]{Cf_mid.png} 227 | \caption{\texttt{cost\_factor} = 0.55} 228 | \endminipage\hfill 229 | \minipage{0.32\textwidth} 230 | \includegraphics[width=\linewidth]{Cf_hi.png} 231 | \caption{\texttt{cost\_factor} = 3.55} 232 | \endminipage\hfill 233 | \end{figure} 234 | 235 | \begin{figure}[!htb] 236 | \minipage{0.32\textwidth} 237 | \includegraphics[width=\linewidth]{NC_lo.png} 238 | \caption{\texttt{neutral\_cost} = 1} 239 | \endminipage\hfill 240 | \minipage{0.32\textwidth} 241 | \includegraphics[width=\linewidth]{NC_mid.png} 242 | \caption{\texttt{neutral\_cost} = 66} 243 | \endminipage\hfill 244 | \minipage{0.32\textwidth} 245 | \includegraphics[width=\linewidth]{NC_hi.png} 246 | \caption{\texttt{neutral\_cost} = 233} 247 | \endminipage\hfill 248 | \end{figure} 249 | 250 | \newpage 251 | 252 | 253 | \texttt{navfn} cost values are set to 254 | 255 | $$\texttt{cost = COST\_NEUTRAL + COST\_FACTOR * costmap\_cost\_value.}$$ 256 | 257 | Incoming costmap cost values are in the range 0 to 252. The comment also says: 258 | \begin{displayquote} 259 | With \texttt{COST\_NEUTRAL} of 50, the \texttt{COST\_FACTOR} needs to be about 0.8 to 260 | ensure the input values are spread evenly over the output range, 50 261 | to 253. If \texttt{COST\_FACTOR} is higher, cost values will have a plateau 262 | around obstacles and the planner will then treat (for example) the 263 | whole width of a narrow hallway as equally undesirable and thus 264 | will not plan paths down the center. 265 | \end{displayquote} 266 | 267 | \paragraph{Experiment observations} Experiments have confirmed this explanation. Setting \texttt{cost\_factor} to too low or too high lowers 268 | the quality of the paths. These paths do not go through the middle of obstacles on each side and have relatively flat curvature. Extreme 269 | \texttt{neutral\_cost} values have the same effect. For \texttt{lethal\_cost}, setting it to a low value may result 270 | in failure to produce any path, even when a feasible path is obvious. Figures $5-10$ show the effect of \texttt{cost\_factor} and \texttt{neutral\_cost} on global path planning. The green line is the global path produced by \texttt{global\_planner}. 271 | 272 | After a few experiments we observed that when \texttt{cost\_factor} = 0.55, \texttt{neutral\_cost = 66}, and \texttt{lethal\_cost = 253}, the global path is quite desirable. 273 | 274 | \section{Local Planner Selection} 275 | 276 | Local planners that adhere to \texttt{nav\_core::BaseLocalPlanner} interface are \texttt{dwa\_local\\\_planner}, \texttt{eband\_local\_planner} and \texttt{teb\_local\_planner}. 277 | They use different algorithms to generate velocity commands. Usually \texttt{dwa\_local\_planner} is the go-to choice. We will discuss it in detail. More 278 | information on other planners will be provided later. 279 | 280 | \subsection{DWA Local Planner} 281 | \subsubsection{DWA algorithm} 282 | 283 | See next page. 284 | 285 | \newpage 286 | \texttt{dwa\_local\_planner} uses Dynamic Window Approach (DWA) algorithm. ROS Wiki provides a summary of its implementation of this algorithm: 287 | 288 | \begin{center} 289 | \setlength{\fboxsep}{1em} 290 | \fbox{\begin{minipage}{33 em} 291 | \begin{enumerate} 292 | \item Discretely sample in the robot's control space \textit{(dx,dy,dtheta)} 293 | \item For each sampled velocity, perform forward simulation from the robot's current state to predict what would happen if the sampled velocity were applied for some (short) period of time. 294 | \item Evaluate (score) each trajectory resulting from the forward simulation, using a metric that incorporates characteristics such as: proximity to obstacles, proximity to the goal, proximity to the global path, and speed. Discard illegal trajectories (those that collide with obstacles). 295 | \item Pick the highest-scoring trajectory and send the associated velocity to the mobile base. 296 | \item Rinse and repeat. 297 | \end{enumerate} 298 | \end{minipage}} 299 | \end{center} 300 | 301 | \noindent DWA is proposed by \cite{fox1997dynamic}. According to this paper, the goal of DWA is to produce a $(v,\omega)$ pair which 302 | represents a circular trajectory that is optimal for robot's local condition. DWA reaches this goal by searching the velocity space in the next time interval. 303 | The velocities in this space are restricted to be admissible, which means the robot must be able to stop before reaching the closest obstacle on the 304 | circular trajectory dictated by these admissible velocities. Also, DWA will only consider velocities within a dynamic window, which is defined to be the 305 | set of velocity pairs that is reachable within the next time interval given the current translational and rotational velocities and accelerations. DWA maximizes an objective 306 | function that depends on (1) the progress to the target, (2) clearance from obstacles, and (3) forward velocity to produce the optimal velocity pair. 307 | 308 | Now, let us look at the algorithm summary on ROS Wiki. The first step is to sample velocity pairs $(v_x, v_y, \omega)$ in 309 | the velocity space within the dynamic window. The second step is basically obliterating velocities (i.e. kill off bad trajectories) that are not admissible. The third step is 310 | to evaluate the velocity pairs using the objective function, which outputs \textit{trajectory score}. The fourth and fifth steps are easy to understand: take the current best 311 | velocity option and recompute.\\ 312 | 313 | This DWA planner depends on the local costmap which provides obstacle information. Therefore, tuning the parameters for the local costmap is crucial for optimal behavior of DWA local planner. Next, we will look at parameters in forward simulation, trajectory scoring, costmap, and so on. 314 | 315 | \subsubsection{DWA Local Planner : Forward Simulation} 316 | 317 | Forward simulation is the second step of the DWA algorithm. In this step, the local planner takes the velocity samples in robot's control space, and examine the circular trajectories represented by those velocity samples, and finally 318 | eliminate bad velocities (ones whose trajectory intersects with an obstacle). Each velocity sample is simulated as if it is applied to the robot for a set time interval, controlled by \texttt{sim\_time}($s$) 319 | parameter. We can think of \texttt{sim\_time} as the time allowed for the robot to move with the sampled velocities. 320 | 321 | Through experiments, we observed that the longer the value of \texttt{sim\_time}, the heavier the computation load becomes. Also, when \texttt{sim\_time} gets 322 | longer, the path produced by the local planner is longer as well, which is reasonable. Here are some suggestions on how to tune this \texttt{sim\_time} parameter. 323 | 324 | \paragraph{How to tune \texttt{sim\_time}} 325 | Setting \texttt{sim\_time} to a very low value ($<= 2.0$) will result in limited performance, especially when the robot needs to pass a narrow doorway, or gap between furnitures, because there is insufficient time to obtain the optimal trajectory that actually goes through the narrow passway. On the other hand, since with DWA Local Planner, all trajectories are simple arcs, 326 | setting the \texttt{sim\_time} to a very high value ($>= 5.0$) will result in long curves that are not very flexible. This problem is not that unavoidable, 327 | because the planner actively replans after each time interval (controlled by \texttt{controller\_frequency}($Hz$)), which leaves room for small adjustments. A value of 4.0 seconds should be enough even for high performance computers. 328 | 329 | \begin{figure}[!htb] 330 | \minipage{0.49\textwidth} 331 | \includegraphics[width=\linewidth]{simtime15.png} 332 | \caption{\texttt{sim\_time} = 1.5} 333 | \endminipage\hfill 334 | \minipage{0.49\textwidth} 335 | \includegraphics[width=\linewidth]{simtime4.png} 336 | \caption{\texttt{sim\_time} = 4.0} 337 | \endminipage\hfill 338 | \end{figure} 339 | 340 | Besides \texttt{sim\_time}, there are several other parameters that worth our attention. 341 | \paragraph{Velocity samples} 342 | Among other parameters, \texttt{vx\_sample}, \texttt{vy\_sample} determine how many translational velocity samples to take in x, y direction. \texttt{vth\_sample} controls the number of rotational velocities samples. The number of samples you would like to take depends on how much computation power you have. In most cases we prefer to set \texttt{vth\_samples} to 343 | be higher than translational velocity samples, because turning is generally a more complicated condition than moving straight ahead. If you set \texttt{max\_vel\_y} to be zero, 344 | there is no need to have velocity samples in y direction since there will be no usable samples. We picked \texttt{vx\_sample} = 20, and \texttt{vth\_samples} = 40. 345 | 346 | \paragraph{Simulation granularity} 347 | \texttt{sim\_granularity} is the step size to take between points on a trajectory. It basically means how frequent should the points on this trajectory be examined (test if they intersect with any obstacle or not). A lower value means higher frequency, which requires more computation power. The default value of 0.025 is generally enough for turtlebot-sized mobile base. 348 | 349 | \subsubsection{DWA Local Planner : Trajactory Scoring} 350 | 351 | As we mentioned above, DWA Local Planner maximizes an objective function to obtain optimal velocity pairs. In its paper, the value of this objective function relies on three 352 | components: progress to goal, clearance from obstacles and forward velocity. In ROS's implementation, the cost of the objective function is calculated like this: 353 | 354 | \footnotesize 355 | \begin{eqnarray*} 356 | cost &=& \texttt{path\_distance\_bias} * (\text{distance($m$) to path from the endpoint of the trajectory})\\ 357 | &+& \texttt{goal\_distance\_bias} * (\text{distance($m$) to local goal from the endpoint of the trajectory})\\ 358 | &+& \texttt{occdist\_scale} * (\text{maximum obstacle cost along the trajectory in obstacle cost (0-254)}) 359 | \end{eqnarray*} 360 | 361 | \normalsize 362 | \noindent The objective is to get the lowest cost. \texttt{path\_distance\_bias} is the weight for how much the local planner should stay close to the global path \cite{furrer2016robot}. A high value will make the local planner prefer trajectories 363 | on global path. 364 | 365 | \texttt{goal\_distance\_bias} is the weight for how much the robot should attempt to reach the local goal, with whatever path. Experiments show that increasing this 366 | parameter enables the robot to be less attached to the global path. 367 | 368 | \texttt{occdist\_scale} is the weight for how much the robot should attempt to avoid obstacles. A high value for this 369 | parameter results in indecisive robot that stucks in place. Currently for SCITOS G5, we set \texttt{path\_distance\_bias} to 32.0, \texttt{goal\_distance\_bias} to 20.0, \texttt{occdist\_scale} to 0.02. They work well in simulation. 370 | 371 | \newpage 372 | \subsubsection{DWA Local Planner : Other Parameters} 373 | 374 | \paragraph{Goal distance tolerance} These parameters are straightforward to understand. Here we will list their description shown on ROS Wiki: 375 | 376 | \begin{itemize} 377 | \item \texttt{yaw\_goal\_tolerance} (double, default: 0.05) 378 | The tolerance in radians for the controller in yaw/rotation when achieving its goal. 379 | \item \texttt{xy\_goal\_tolerance} (double, default: 0.10) 380 | The tolerance in meters for the controller in the x \& y distance when achieving a goal. 381 | \item \texttt{latch\_xy\_goal\_tolerance} (bool, default: false) If goal tolerance is latched, if the robot ever reaches the goal xy location it will simply rotate in place, even if it ends up outside the goal tolerance while it is doing so. 382 | \end{itemize} 383 | 384 | \paragraph{Oscilation reset} In situations such as passing a doorway, the robot may oscilate back and forth because its local planner is producing paths leading to two opposite directions. If the robot keeps oscilating, the navigation stack will let the robot try its recovery behaviors. 385 | \begin{itemize} 386 | \item \texttt{oscillation\_reset\_dist} (double, default: 0.05) How far the robot must travel in meters before oscillation flags are reset. 387 | \end{itemize} 388 | 389 | 390 | 391 | \section{Costmap Parameters} 392 | 393 | As mentioned above, \textit{costmap} parameters tuning is essential for the success of local planners (not only for DWA). In ROS, costmap is composed of static map layer, obstacle map layer and inflation layer. Static map layer directly interprets the given static SLAM map provided to the navigation stack. Obstacle map layer includes 2D obstacles and 3D obstacles (voxel layer). Inflation layer is where obstacles are inflated to calculate cost for each 2D costmap cell. 394 | 395 | Besides, there is a \textit{global costmap}, as well as a \textit{local costmap}. Global costmap is generated by inflating the obstacles on the map provided to the navigation stack. Local costmap is generated 396 | by inflating obstacles detected by the robot's sensors in real time. \\ 397 | 398 | \begin{figure}[!h] 399 | \includegraphics[width=\linewidth]{costmapspec.png} 400 | \caption{inflation decay} 401 | \end{figure} 402 | 403 | \noindent There are a number of important parameters that should be set as good as possible. 404 | 405 | \subsection{footprint} 406 | Footprint is the contour of the mobile base. In ROS, it is represented by a two dimensional array of the form $[x_0, y_0],[x_1,y_1],[x_2,y_2],...]$, no need to repeat the first coordinate. This footprint will be used to compute the radius of inscribed circle and circumscribed circle, which are used to inflate obstacles in a way that fits this robot. Usually for safety, we want to have the footprint to be slightly larger 407 | than the robot's real contour. 408 | 409 | To determine the footprint of a robot, the most straightforward way is to refer to the drawings of your robot. Besides, you can manually take a picture of the top view of its base. Then use CAD software (such as Solidworks) to scale the image appropriately and move your mouse around the contour of the base and read its coordinate. The origin of the coordinates should be the center of the robot. Or, you can move your robot on a piece of large paper, then draw the contour of the base. Then pick some vertices and use rulers to figure out their coordinates. 410 | 411 | \subsection{inflation} 412 | Inflation layer is consisted of cells with cost ranging from 0 to 255. Each cell is either occupied, free of obstacles, or unknown. Figure 13 shows a diagram \footnote{Diagram is from \url{http://wiki.ros.org/costmap_2d}} illustrating how inflation decay curve is computed. 413 | 414 | \texttt{inflation\_radius} and \texttt{cost\_scaling\_factor} are the parameters that determine the inflation. \texttt{inflation\_radius} controls how far away the zero cost point is from the obstacle. \texttt{cost\_scaling\_factor} is inversely proportional to the cost of a cell. Setting it higher will make the decay curve more steep. 415 | 416 | Dr. Pronobis sugggests the optimal costmap decay curve is one that has relatively low slope, so that the best path is as far as possible from the obstacles on each side. The advantage is that the robot would prefer to move in the middle of obstacles. As shown in Figure 8 and 9, with the same starting point and goal, when costmap curve is steep, the robot tends to be close to obstacles. In Figure 14, \texttt{inflation\_radius} = 0.55, \texttt{cost\_scaling\_factor} = 5.0; In Figure 15, \texttt{inflation\_radius} = 1.75, \texttt{cost\_scaling\_factor} = 2.58 417 | 418 | \begin{figure}[!htb] 419 | \minipage{0.49\textwidth} 420 | \includegraphics[width=\linewidth]{notgoodinflation.png} 421 | \caption{steep inflation curve} 422 | \endminipage\hfill 423 | \minipage{0.49\textwidth} 424 | \includegraphics[width=\linewidth]{goodinflation.png} 425 | \caption{gentle inflation curve} 426 | \endminipage\hfill 427 | \end{figure} 428 | 429 | Based on the decay curve diagram, we want to set these two parameters such that the inflation radius almost covers the corriders, and the decay of cost value is moderate, which means decrease the value of \texttt{cost\_scaling\_factor} . 430 | 431 | \subsection{costmap resolution} 432 | This parameter can be set separately for local costmap and global costmap. They affect computation load and path planning. With low resolution ($>=0.05$), in narrow passways, the obstacle region may overlap and thus the local planner will not be able to find a path through. 433 | 434 | For global costmap resolution, it is enough to keep it the same as the resolution of the map provided to navigation stack. If you have more than enough computation power, 435 | you should take a look at the resolution of your laser scanner, because when creating the map using gmapping, if the laser scanner has lower resolution than your desired 436 | map resolution, there will be a lot of small "unknown dots" because the laser scanner cannot cover that area, as in Figure 10. 437 | \begin{figure}[!h] 438 | \begin{center} 439 | \includegraphics[width=22em]{resolutionholes.png} 440 | \caption{gmapping resolution = 0.01. Notice the unknown\\ dots on the right side of the image} 441 | \end{center} 442 | \end{figure} 443 | 444 | For example, Hokuyo URG-04LX-UG01 laser scanner has metric resolution of 0.01mm\footnote{data from \url{https://www.hokuyo-aut.jp/02sensor/07scanner/download/pdf/URG-04LX_UG01_spec_en.pdf}}. Therefore, scanning a map with resolution $<=0.01$ will require the robot to rotate several times in order to clear unknown dots. We found 0.02 to be a sufficient resolution 445 | to use. 446 | 447 | 448 | \subsection{obstacle layer and voxel layer} These two layers are responsible for marking obstacles on the costmap. They can be called altogether as \textit{obstacle layer}. According to ROS wiki, the obstacle layer tracks in two dimensions, whereas the voxel layer tracks in three. Obstacles are marked (detected) or cleared (removed) based on data from robot's sensors, which has topics for costmap to subscribe to. 449 | 450 | In ROS implementation, the voxel layer inherits from obstacle layer, and they both obtain obstacles information by interpreting laser scans or data sent with \texttt{PointCloud} or \texttt{PointCloud2} type messages. Besides, the voxel layer requires depth sensors such as Microsoft Kinect or ASUS Xtion. 3D obstacles are eventually projected down to the 2D costmap for inflation. 451 | 452 | \paragraph{How voxel layer works} Voxels are 3D volumetric cubes (think 3D pixel) which has certain relative position in space. It can be used to be associated with data or properties of the volume near it, e.g. whether its location is an obstacle. There has been quite a few research around online 3D reconstruction with the depth cameras via voxels. Here are some of them. 453 | 454 | \begin{itemize} 455 | \item \href{http://delivery.acm.org/10.1145/2050000/2047270/p559-izadi.pdf?ip=128.208.7.188&id=2047270&acc=ACTIVE\%20SERVICE&key=B63ACEF81C6334F5\%2EF43F328D6C8418D0\%2E4D4702B0C3E38B35\%2E4D4702B0C3E38B35&CFID=830915711&CFTOKEN=23054788&__acm__=1472349664_9fd28ae246d72a507f6a93c5ac84a516}{KinectFusion: Real-time 3D Reconstruction and Interaction Using a Moving Depth Camera} 456 | \item \href{https://people.mpi-inf.mpg.de/~mzollhoef/Papers/SGASIA2013_VH/paper.pdf}{Real-time 3D Reconstruction at Scale using Voxel Hashing} 457 | % \item \href{https://www.ri.cmu.edu/pub_files/2014/6/VoxelCarving.pdf}{Object Modeling and Recognition from Sparse, Noisy Data via Voxel Depth Carving} 458 | \end{itemize} 459 | 460 | \texttt{voxel\_grid} is a ROS package which provides an implementation of efficient 3D voxel grid data structure that stores voxels with three states: marked, free, unknown. The \textit{voxel grid} occupies the volume within the costmap region. During each update of the voxel layer's boundary, the voxel layer will mark or remove some of the voxels in the voxel grid based on observations from sensors. It also performs ray tracing, which is discussed next. Note that the voxel grid is not recreated when updating, but only updated unless the size of local costmap is changed. 461 | 462 | 463 | \paragraph{Why ray tracing} in obstacle layer and voxel layer? Ray tracing is best known for rendering realistic 3D graphics, so it might be confusing why it is used in dealing with obstacles. One big reason is that obstacles of different type can be detected by robot's sensors. Take a look at figure 17. In theory, we are also able to know if an obstacle is rigid or soft (e.g. grass)\footnote{ mentioned in \textit{Using Robots in Hazardous Environments} by Boudoin, Habib, pp.370}. 464 | 465 | \begin{figure}[!h] 466 | \begin{center} 467 | \includegraphics[width=23em]{ObstacleTypes.png} 468 | \caption{With ray tracing, laser scanners is able to recognize different types of obstacles.} 469 | \end{center} 470 | \end{figure} 471 | 472 | A good blog on voxel ray tracing versus polygong ray tracing: \url{http://raytracey.blogspot.com/2008/08/voxel-ray-tracing-vs-polygon-ray.html} \\ 473 | 474 | With the above understanding, let us look into the parameters for the obstacle layer\footnote{Some explanations are directly copied from costmap2d ROS Wiki}. These parameters are global filtering parameters that apply to all sensors. 475 | 476 | \begin{itemize} 477 | \item \texttt{max\_obstacle\_height}: The maximum height of any obstacle to be inserted into the costmap in meters. This parameter should be set to be slightly higher than the height of your robot. For voxel layer, this is basically the height of the voxel grid. 478 | 479 | \item \texttt{obstacle\_range}: The default maximum distance from the robot at which an obstacle will be inserted into the cost map in meters. This can be over-ridden on a per-sensor basis. 480 | \item \texttt{raytrace\_range}: The default range in meters at which to raytrace out obstacles from the map using sensor data. This can be over-ridden on a per-sensor basis. 481 | \end{itemize} 482 | 483 | These parameters are only used for the voxel layer (VoxelCostmapPlugin). 484 | 485 | \begin{itemize} 486 | \item \texttt{origin\_z}: The z origin of the map in meters. 487 | \item \texttt{z\_resolution}: The z resolution of the map in meters/cell. 488 | \item \texttt{z\_voxels}: The number of voxels to in each vertical column, the height of the grid is z\_resolution * z\_voxels. 489 | \item \texttt{unknown\_threshold}: The number of unknown cells allowed in a column considered to be "known" 490 | \item \texttt{mark\_threshold}: The maximum number of marked cells allowed in a column considered to be "free". 491 | \end{itemize} 492 | 493 | \paragraph{Experiment observations} Experiments further clarify the effects of the voxel layer's parameters. We use ASUS Xtion Pro as our depth sensor. We found that position of Xtion matters in that it determines the range of "blind field", which is the region that the depth sensor cannot see anything. 494 | 495 | In addition, voxels representing obstacles only update (marked or cleared) when obstacles appear within Xtion range. Otherwise, some voxel information will remain, and their influence on costmap inflation remains. 496 | 497 | Besides, \texttt{z\_resolution} controls how dense the voxels is on the $z$-axis. If it is higher, the voxel layers are denser. If the value is too low (e.g. 0.01), all the voxels will be put together and thus you won't get useful costmap information. If you set z\_resolution to a higher value, your intention should be to obtain obstacles better, therefore you need to increase \texttt{z\_voxels} parameter which controls how many voxels in each vertical column. It is also useless if you have too many voxels in a column but not enough resolution, because each vertical column has a limit in height. Figure 18-20 shows comparison between different voxel layer parameters setting. 498 | 499 | \begin{figure}[!h] 500 | \end{figure} 501 | 502 | \begin{figure}[!h] 503 | \minipage{0.32\textwidth} 504 | \includegraphics[width=\linewidth]{voxellayerscene.png} 505 | \caption{Scene: Plant in front of the robot} 506 | \endminipage\hfill 507 | \minipage{0.32\textwidth} 508 | \includegraphics[width=\linewidth]{voxellayerparam1.png} 509 | \caption{high \texttt{z\_resolution}} 510 | \endminipage\hfill 511 | \minipage{0.32\textwidth} 512 | \includegraphics[width=\linewidth]{voxellayerparam2.png} 513 | \caption{low \texttt{z\_resolution}} 514 | \endminipage\hfill 515 | \end{figure} 516 | 517 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 518 | \section{AMCL} 519 | 520 | \texttt{amcl} is a ROS package that deals with robot localization. It is the abbreviation of Adaptive Monte Carlo Localization (AMCL), also known as partical filter localization. This localization technique works like this: Each sample stores a position and orientation data representing the robot's pose. Particles are all sampled randomly initially. When the robot moves, particles are resampled based on their current state as well as robot's action using recursive Bayesian estimation. 521 | 522 | More discussion on AMCL parameter tuning will be provided later. Please refer to \url{http://wiki.ros.org/amcl} for more information. For the details of the original algorithm Monte Carlo Localization (MCL), read Chapter 8 of \textit{Probabilistic Robotics} \cite{thrun2005probabilistic}.\\ 523 | 524 | \noindent We now summarize several issues that may affect the quality of AMCL localization\footnote{Added on April 8th, 2019. This investigation was done in May, 2017, yet not reported in this guide at the time.}. We hope this information makes this guide more complete, and you find it useful. 525 | 526 | 527 | \begin{figure}[!tb] 528 | \minipage{0.49\textwidth} 529 | \includegraphics[width=\linewidth]{scanner1.png} 530 | \caption{When \texttt{LaserScan} fields are not correct} 531 | \label{fig:scanner1} 532 | \endminipage\hfill 533 | \minipage{0.49\textwidth} 534 | \includegraphics[width=\linewidth]{scanner2.png} 535 | \caption{When \texttt{LaserScan} fields are correct} 536 | \label{fig:scanner2} 537 | \endminipage\hfill 538 | \end{figure} 539 | 540 | Through experiments, we observed three issues that affect the localization with AMCL. As described in \cite{thrun2005probabilistic}, MCL maintains two probabilistic models, a \emph{motion model} and a \emph{measurement model}. In ROS \texttt{amcl}, the motion model corresponds to a model of the odometry, while the measurement model correspond to a model of laser scans. With this general understanding, we describe three issues separately as follows. 541 | 542 | \subsection{Header in \texttt{LaserScan} messages} 543 | 544 | Messages that are published to \texttt{scan} topic are of type \texttt{sensor\_msgs/LaserScan}\footnote{See: \url{http://docs.ros.org/melodic/api/sensor_msgs/html/msg/LaserScan.html}}. This message contains a header with fields dependent on the specific laser scanner that you are using. These fields include (copied from the message documentation) 545 | 546 | \begin{itemize} 547 | \item \texttt{angle\_min} (float32) start angle of the scan [rad] 548 | \item \texttt{angle\_max} (float32) end angle of the scan [rad] 549 | \item \texttt{angle\_increment} (float32) start angle of the scan [rad] 550 | \item \texttt{time\_increment} (float32) time between measurements [seconds] - if your scanner is moving, this will be used in interpolating position of 3d points 551 | \item \texttt{scan\_time} (float32) time between scans [seconds] 552 | \item \texttt{range\_min} (float32) minimum range value [m] 553 | \item \texttt{range\_max} (float32) maximum range value [m] 554 | \end{itemize} 555 | 556 | We observed in our experiments that if these values are not set correctly for the laser scanner product on board, the quality of localization will be affected (see Figure~\ref{fig:scanner1} and \ref{fig:scanner2}. We have used two laser scanners products, the SICK LMS 200 and the SICK LMS 291. We provide their parameters below\footnote{For LMS 200, thanks to {this Github issue} (\url{https://github.com/smichaud/lidar-snowfall/issues/1})}. 557 | 558 | \noindent SICK LMS 200: 559 | \lstset{language=json} 560 | \begin{lstlisting} 561 | { 562 | "range_min": 0.0, 563 | "range_max": 81.0, 564 | "angle_min": -1.57079637051, 565 | "angle_max": 1.57079637051, 566 | "angle_increment": 0.0174532923847, 567 | "time_increment": 3.70370362361e-05, 568 | "scan_time": 0.0133333336562 569 | } 570 | \end{lstlisting} 571 | 572 | \noindent SICK LMS 291: 573 | \lstset{language=json} 574 | \begin{lstlisting} 575 | { 576 | "range_min": 0.0, 577 | "range_max": 81.0, 578 | "angle_min": -1.57079637051, 579 | "angle_max": 1.57079637051, 580 | "angle_increment": 0.00872664619235, 581 | "time_increment": 7.40740724722e-05, 582 | "scan_time": 0.0133333336562 583 | } 584 | \end{lstlisting} 585 | 586 | 587 | \subsection{Parameters for measurement and motion models} 588 | There are parameters listed in the \texttt{amcl} package about tuning the laser scanner model (measurement) and odometry model (motion). Refer to the package page for the complete list and their definitions. A detailed discussion requires great understanding of the MCL algorithm in \cite{thrun2005probabilistic}, which we omit here. We provide an \emph{example} of fine tuning these parameters and describe their results qualitatively. The actual parameters you use should depend on your laser scanner and robot. 589 | 590 | For \textbf{laser scanner model}, the default parameters are: 591 | 592 | \lstset{language=json} 593 | \begin{lstlisting} 594 | { 595 | "laser_z_hit": 0.5, 596 | "laser_sigma_hit": 0.2, 597 | "laser_z_rand" :0.5, 598 | "laser_likelihood_max_dist": 2.0 599 | } 600 | \end{lstlisting} 601 | 602 | To improve the localization of our robot, we increased \texttt{laser\_z\_hit} and \texttt{laser\_sigma\_hit} to incorporate higher measurement noise. The resulting parameters are: 603 | 604 | \lstset{language=json} 605 | \begin{lstlisting} 606 | { 607 | "laser_z_hit": 0.9, 608 | "laser_sigma_hit": 0.1, 609 | "laser_z_rand" :0.5, 610 | "laser_likelihood_max_dist": 4.0 611 | } 612 | \end{lstlisting} 613 | The behavior is illustrated in Figure~\ref{fig:param1} and \ref{fig:param2}. It is clear that in our case, adding noise into the measurement model helped with localization. 614 | 615 | 616 | \begin{figure}[!tb] 617 | \minipage{0.49\textwidth} 618 | \includegraphics[width=\linewidth]{param1.png} 619 | \caption{Default measurement model parameters} 620 | \label{fig:param1} 621 | \endminipage\hfill 622 | \minipage{0.49\textwidth} 623 | \includegraphics[width=\linewidth]{param2.png} 624 | \caption{After tuning measurement model parameters (increase noise)} 625 | \label{fig:param2} 626 | \endminipage\hfill 627 | \end{figure} 628 | 629 | 630 | 631 | For the \textbf{odometry model}, we found that our odometry was quite reliable in terms of stability. Therefore, we tuned the parameters so that the algorithm assumes there is low noise in odometry: 632 | \lstset{language=json} 633 | \begin{lstlisting} 634 | { 635 | "kld_err": 0.01, 636 | "kld_z": 0.99, 637 | "odom_alpha1": 0.005, 638 | "odom_alpha2": 0.005, 639 | "odom_alpha3": 0.005, 640 | "odom_alpha4": 0.005 641 | } 642 | \end{lstlisting} 643 | To verify that the above paremeters for motion model work, we also tried a set of parameters that suggest a noisy odometry model: 644 | \lstset{language=json} 645 | \begin{lstlisting} 646 | { 647 | "kld_err": 0.10, 648 | "kld_z": 0.5', 649 | "odom_alpha1": 0.008, 650 | "odom_alpha2": 0.040, 651 | "odom_alpha3": 0.004, 652 | "odom_alpha4": 0.025 653 | } 654 | \end{lstlisting} 655 | We observed that when the odometry model is less noisy, the particles are more condensed. Otherwise, the particles are more spread-out. 656 | 657 | \subsection{Translation of the laser scanner} 658 | 659 | There is a \texttt{tf} transform from \texttt{laser\_link} to \texttt{base\_footprint} or \texttt{base\_link} that indicates the pose of the laser scanner with respect to the robot base. If this transform is not correct, it is very likely that the localization behaves strangely. In this situation, we have observed constant shifting of laser readings from the walls of the environment, and sudden drastic change in the localization. It is straightforward enough to make sure the transform is correct; This is usually handled in \texttt{urdf} and \texttt{srdf} specification of your robot. However, if you are using a \texttt{rosbag} file, you may have to publish the transform youself. 660 | 661 | 662 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 663 | \section{Recovery Behaviors} 664 | An annoying thing about robot navigation is that the robot may get stuck. Fortunately, the navigation stack has recovery behaviors built-in. Even so, sometimes the robot will exhaust all available recovery behaviors and stay still. Therefore, we may need to figure out a more robust solution. 665 | 666 | \paragraph{Types of recovery behaviors} ROS navigation has two recovery behaviors. They are \texttt{clear\_costmap\_recovery} and \texttt{rotate\_recovery}. Clear costmap recovery is basically reverting the local costmap to have the same state as the global costmap. Rotate recovery is to recover by 667 | rotating 360 degrees in place. 668 | 669 | \paragraph{Unstuck the robot} Sometimes rotate recovery will fail to execute due to rotation failure. At this point, the robot may just give up because it has tried 670 | all of its recovery behaviors - clear costmap and rotation. In most experiments we observed that when the robot gives up, there are actually many ways to unstuck the robot. 671 | To avoid giving up, we used SMACH to continuously try different recovery behaviors, with additional ones such as setting a temporary goal that is very close to the robot, and returning to some previously visited pose (i.e. backing off). These methods turn out to improve the robot's durability substantially, and unstuck it from previously hopeless tight spaces from our experiment observations\footnote{Here is a video demo of my work on mobile robot navigation: \url{https://youtu.be/1-7GNtR6gVk}}. 672 | 673 | \begin{figure}[!h] 674 | \begin{center} 675 | \includegraphics[width=26em]{Recovery.png} \caption{Simple recovery state in SMACH} 676 | \end{center} 677 | \end{figure} 678 | 679 | 680 | \paragraph{Parameters} The parameters for ROS's recovery behavior can be left as default in general. For clear costmap recovery, if you have a relatively 681 | high \texttt{sim\_time}, which means the trajectory is long, you may want to consider increasing \texttt{reset\_distance} parameter, so that bigger area on local costmap is removed, and there is a 682 | better chance for the local planner to find a path. 683 | 684 | \section{Dynamic Reconfigure} 685 | 686 | One of the most flexible aspect about ROS navigation is dynamic reconfiguration, since different parameter setup might be more helpful for certain situations, e.g. when robot is close to the goal. Yet, it is not necessary to do a lot of dynamic reconfiguration. 687 | 688 | \paragraph{Example} One situation that we observed in our experiments is that the robot tends to fall off the global path, 689 | even when it is not necessary or bad to do so. Therefore we increased \texttt{path\_distance\_bias}. Since a high \texttt{path\_distance\_bias} will make the robot stick to the global path, which does not actually lead to the final goal due to tolerance, we need a way to let the robot reach the goal with no hesitation. We chose to dynamically 690 | decrease the \texttt{path\_distance\_bias} so that \texttt{goal\_distance\_bias} is emphasized when the robot is close to the goal. After all, doing more experiments is the ultimate way to find problems and figure out solutions. 691 | 692 | \section{Problems} 693 | 694 | \begin{enumerate} 695 | \item Getting stuck 696 | 697 | This is a problem that we face a lot when using ROS navigation. In both simulation and reality, the robot gets stuck and gives up the goal. 698 | 699 | \item Different speed in different directions 700 | 701 | We observed some weird behavior of the navigation stack. When the goal is set in the -x direction with respect to TF origin, dwa local planner plans less stably (the local planned trajectory jumps around) and the robot moves really slowly. But when the goal is set in the +x direction, dwa local planner is much more stable, and the robot can move faster. 702 | 703 | I reported this issue on Github here: \url{https://github.com/ros-planning/navigation/issues/503}. Nobody attempted to resolve it yet. 704 | 705 | \item Reality VS. simulation 706 | 707 | There is a difference between reality and simulation. In reality, there are more obstacles with various shapes. For exmaple, in the lab there is a vertical stick that is used to hold to door 708 | open. Because it is too thin, the robot sometimes fails to detect it and hit on it. There are also more complicated human activity in reality. 709 | 710 | \item Inconsistency 711 | 712 | Robots using ROS navigation stack can exhibit inconsistent behaviors, for example when entering a door, the local costmap is generated again and again with slight difference each time, and this may affect path planning, especially when resolution is low. Also, there is no memory for the robot. It does not remember how it entered the room from the door the last time. So it needs to start out fresh again every time it tries to enter a door. Thus, if it enters the door in a different angle than before, it may just get stuck and give up. 713 | 714 | \end{enumerate} 715 | 716 | 717 | \section*{Thanks} 718 | 719 | Hope this guide is helpful. Please feel free to add more information from your own experimental observations. 720 | 721 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 722 | % References 723 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 724 | \nocite{*} 725 | \bibliographystyle{apalike} 726 | \bibliography{main} 727 | 728 | 729 | \end{document} 730 | --------------------------------------------------------------------------------