IROS2019国际学术会议论文集 1459

Low cost Sonar Navigation System Tiziano GuadagninoBartolomeo Della CorteGiorgio Grisetti Abstract In this paper we present a sonar based navigation system designed to deploy a fl eet of autonomous mobile platforms at a reasonable cost In educational and hobbyist contexts a large number of robots is required By means of classical navigation approaches every robot should be provided with accurate vision or range sensors This limits the maximum number of robots in the fl eet due to the unaffordable cost of these sensors In contrast to that our system requires a single platform equipped with a higher quality sensor used to perform calibration and mapping tasks The rest of the fl eet able to localize and navigate is equipped solely with low cost sonars providing a notable reduction in the overall cost We achieve this task by presenting a novel calibration procedure to estimate the sonars extrinsic and adapting a classical monte carlo localization algorithm to the sonar model focusing on effi ciency We release an open source implementation of the system to the community I INTRODUCTION Navigating robots are nowadays a commodity This ba sic skill is effectively accomplished by appliances such as autonomous vacuum cleaners lawn mowers or even autonomous driving cars Before reaching our homes they have been effectively employed in contexts such as hospitals factories and logistics and before they appeared as museum guides 1 2 3 4 Most of these applications and in particular the indoor ones require the system to operate for a long time in the same environment To effectively instruct the robot on where to move and for the system to navigate a crucial aspect is the map The map is typically obtained by means of a SLAM system When using a laser scanner the map obtained by the SLAM system can be directly used to feed the localization and planning modules that are used during on line operations This is possible since the occupancy of the environment can be easily inferred by a laser scan In contrast when using cameras the occupancy has to be computed using other sensors such as infrared sonars or more commonly bumpers The shortcoming of an endurable laser sensor is its cost easily topping the price of a hobby or educational mobile platform Off the shelf algorithms for visual SLAM produce sparse maps that are subject to changes in the illumination Thus a map acquired in a bright morning might not be usable for localization in a dark evening Two decades ago sonars were popular devices to detect obstacles and navigate robots As the years passed the advent of lasers off the shelf cameras and more importantly with the increase in the All authors the are with Sapienza University of Rome Department of Computer Control and Management Engineering Antonio Ruberti Rome Italy Laser Scanner Ultrasonic Sensors Extrinsic Calibration Mapping Ultrasonic Sensors Localization Navigation Laser ScannerUltrasonic Sensors Initial StateLocalized Fig 1 Our system requires a single platform equipped with a high resolution sensor e g a laser scanner that is used to perform the calibration and to construct the map Given that a whole fl eet of mobile robots equipped solely with low cost ultrasonic sensors can perform navigation tasks computational power sonars have been dismissed in favor of richer and more accurate sensors Yet sonars remain the less expensive range sensing device a single sonar can be bought for less than 2 EUR by a hobbyist Compared to a camera the sonar has the advantage of not being subject to illumination changes nor to be infl uenced by the texture in the scene They can be directly used to construct an occupancy map adequate for navigation The cost is a key factor when deploying a large fl eet of robots especially in contexts where this devices tend to broke frequently for example schools In this paper we discuss all aspects required to deploy a fl eet of autonomous navigating robots relying on sonars as shown in Fig 1 More in detail we present an open hardware and software mobile platform that can be assembled with off the shelf components for less than 70 EUR excluding the host computer We took particular care in adapting the navigation algorithms to cope with the limited processing power of an SBC such as the Raspberry PI 3 with the belief that the on board computer should not cost more than the robot itself In addition to that we present a novel technique to calibrate the position of the sonars on the base Such a calibration is required to maximize the effectiveness of the system Our calibration method can successfully determine the pose of a sonar on the base with less than 1 cm and 0 5 of accuracy starting from nominal parameters that are off from the real value by more than 30 cm and 15 Whereas we still rely on a laser scanner to calibrate both the sensor 2019 IEEE RSJ International Conference on Intelligent Robots and Systems IROS Macau China November 4 8 2019 978 1 7281 4003 2 19 31 00 2019 IEEE692 positions and the odometry parameters this procedure needs to be carried on only once presumably at the factory To build an occupancy map suitable for navigation with the sonars we rely on the trajectory estimated by an external SLAM system We conducted comparative experiments by using laser SLAM method This allows us to directly com pare the map obtained by the sonars with the one obtained with the laser However during typical operation the user is free to choose any SLAM system capable of generating a trajectory Further the typical use of a fl eet of robots requires mapping the environment once during initial deployment Once the map is known the robots execute a usual navigation pipeline consisting of a localization and a path planning module In this paper we address this specifi c case and we provide a highly effective implementation of a particle fi lter for localization On an RPI 3 our localization system can execute a cycle in less than 15 ms by using 5000 samples in a global localization setting We addressed the path planning by using a variant of an A algorithm with a precomputed heuristic described in 5 We conducted comparative experiments on real data aim ing at measuring the accuracy of our calibration procedure measuring the difference between occupancy grids ob tained by using laser and sonars measuring the difference in localization accuracy for sonars and lasers we effectively run the navigation system as demon strated by the video material Finally we release an open source implementation of our system to the community1 II RELATEDWORK Since the late 90s mobile platforms have been introduced in social contexts with the goal of providing support to a human being Platforms such as RHINO 1 MINERVA 2 Robox 3 and Jinny 4 have been deployed in museums and fairs to perform tour guiding tasks Similarly robotic platforms have been deployed in the domestic context for vacuum cleaning 6 or lawn mowing Recently many re searchers worked on the possibility to provide inexpensive platforms for educational purposes 7 All these works share the need for a robust navigation system able to operate within non static environments To perform indoor navigation many companies and research institutes use the ROS middleware whose navigation system based on move base layered costmaps 8 and Gmapping with improved grid mapping techniques 9 provides a valid solution when using laser scanners In this context Conner and Willis presented Flexible Navigation 10 a fi nite state machine based architecture for navigation and control of mobile platforms Flexible Navigation is built on top of move base and it integrates a decision making module in the navigation stack P utz et al recently presented Move 1http srrg gitlab io sonar navigation html Base Flex MBF 11 a fl exible and modular navigation system MBF has been developed to provide more fl exibility in navigation and decision making context by a plugin based architecture built on top of move base In 2010 Marder Eppstein et al presented a system that is reported to operate continuously for more than 40 kilometers in an indoor environment 12 The system showed impres sive robustness by making use of a voxel grid to navigate and deal with 3D obstacles Despite the increasing robustness shown by these systems they mostly rely on cameras and laser scanner sensors whose use for massive robot deployment may be too expensive In particular cameras albeit inexpensive require signifi cant processing power and suffer from changing light conditions Furthermore algorithms that recover the depth from cameras stream require real time dense block matching and accurate ego motion estimation that are known to be computationally heavy Indeed depth is required to deal with textureless obstacles In contrast laser data is compact and laser based navigation systems can be run by low power computers Unfortunately the cost of a durable laser scanner tops by far the cost of the platform In this paper we present all aspects of a navigation system to build a fl eet of low cost navigating robots We still rely on laser scanners but only during the initial calibration of the robot to be presumably done at the factory Once calibrated our system computes once the map of an environment by constructing a sonar map based on the trajectory estimated by an external SLAM system Subsequently the entire fl eet can navigate solely based on the sonars The computation of our system is negligible being it able to run under 15 ms per update on a Raspberry PI 3 sbc III MOBILEPLATFORM As stated in the introduction this work summarizes our efforts in providing a cheap and effective mobile platform for use in educational contexts Our robot is a differential drive platform equipped with up to 8 sonars The electronics is realized off the shelf components namely an ATMega2560 board and an L298 motor driver The board runs an open source fi rmware 13 that acts as a server for the connected PC client The microcontroller communicates with the client using an extensible packet based serial protocol at up to 100 Hz The board offers services to control commu nication motors sonars and the platform These services are organized in 4 subsystems namely System controls the communication the watchdog the internal clock rate and other global settings Joints implements the open and closed loop control of the motors The current system supports up to 4 axes albeit only 2 are used in this platform Drive implements the dead reckoning and offers a high level interface to control the mobile base specifying the translational and rotational velocities Sonars reads the sonars and allows to specify the activation pattern 693 Fig 2 The platform used in this work is a custom differential drive robot PartQty Unit cost EUR Arduino Mega 2560113 40 ELEGOO Sonar52 L298 Motor Driver Board13 20 12 V motors with encoder215 50 Lead Battery113 40 Total69 160 TABLE I Components of the Mobile Platform used in the experiments Each subsystem offers the possibility of saving restoring parameters from the microcontroller eeprom and web based client assists the user in the assembling of the robot while allowing to set the parameters A thin low level part of the fi rmware is microcontroller specifi c while most of the code is architecture independent We provide a version also for the dsPIC currently without sonar support The components we used in our design are listed in Tab I where we do not report the cost of cables frame and mechanics which introduces a big variable Still we realized cheap wood frames with less than 5 EUR or folded alluminium frames costing more than 150 EUR per prototype An example of such kind of platform is reported in Fig 2 The communication with the host happens through an RS232 serial tunneled over USB To reduce the cable clutter Simone Sch utten designed a shield for the Mega board that organizes the connections and allows to control up to 4 motors in closed loop 14 IV PLATFORMSETUP In this section we describe a procedure to confi gure the platform parameters after assembly Typically this procedure has to be executed once In this phase we add a laser scanner to the platform which will be removed at the end of the calibration Two are the main aspects that need to be confi g ured the kinematic parameters wheel distance and radii and the sonar poses To carry on the kinematics calibration we use our open source package described in 15 This system uses a scan matcher to determine a ground truth of the robot motion and determines both kinematics parameters and laser pose through least squares minimization For what concern ultrasonic sensor calibration 16 collect measurements with maneuvers allowing to observe a set of uncluttered features detectable with the sonar Unfortunately the procedure and results of their experiments are not avail able for comparison In the remainder we describe our novel procedure to carry on this task We regard the sonars as independent since we fi re them in a non interfering pattern Thus without loss of generality we illustrate how to calibrate a single sonar To this extent we require laser measurements acquired in an environment with vertical walls and the sonar measurements gathered more or less at the same instant In addition to that we need a coarse initial guess of the relative pose between the laser and the sonar with a precision up to 15 degrees in orientation and 30 cm in position The laser data are preprocessed to extract the dominant line that fi ts the scan by using split and merge 17 routine More formally given a mobile platform equipped with an ultrasonic sensor S and another reference range sensor R whose pose on the platform PXR is known our goal is to estimate the extrinsic parameters of each sensor Si i e the relative pose of the sensor with respect to the platform Given the sensor pose XS we can then obtain the sensors poses in the platform reference frame as PXS i PXRXS i Assuming that the reference sensor R is placed at distance d from a fl at surface that is represented by a line l and given a sonar measure z referred to a point p on l we defi ne our error function as e X X p nl d 1 where p is the perceived point expressed in the frame of the sensor nlis the normal of the line expressed in the frame of the reference sensor R is the operator that apply the transformation X to the point p Fig 3 depicts the quantities involved in Eq 1 The distance beween the sonar S and the line l is equal to the projection of the measurement z along the line normal Fig 3 The components involved in the extrinsic calibration procedure We formulate the calibration problem as a least squares minimization problem on a local parameterization of the manifold space SE 2 as explained in 15 Our goal is to 694 fi nd the transformation X that minimizes the error function expressed by Eq 1 X argminX P ikei X k i argminX P iei X iei X 2 The matrix idenotes the information matrix related to the measurement i It is important to notice that each measure zican be related to a different line lithat should contain pi By linearizing each error ei X x around the pertu bation x of the current guess X we compute a quadratic approximation of the problem f X x xTH x 2b c 3 where H X i JT i iJi 4 b X i JT i iei 5 The symbol refers to the operator described in 18 The Jacobian matrix J of the measurement results from the 1st order taylor expansion of the error as ei X x ei X z ei ei X x x x 0 z Ji x 6 Subsequently and we minimize Eq 3 by solving the following linear system w r t the perturbation x H x b 7 Finally we update the current solution by applying the optimal perturbation resulting from Eq 7 as X X x For completeness we report the Jacobian matrix in Eq 6 Ji X nli I X pi 8 where I is the identity matrix and a is the cross product matrix of the generic vector a A Map Construction Using ultrasonic sensors to build an occupancy grid requires to take into account several issues related to the technology of the sensor the sound wave return depends on both material and surface hit e g a porous surface completely absorbs the wave while a parabolic surface perfectly returns it the bounce of the material varies the wave power the sound wave propagates as a spherical wave To mitigate the effect of these phenomena Moravec and Elfes 19 proposed a probability model for the ultrasonic sensor onto a rasterized map We used this method due to its effi ciency and simplicity to model our ultrasonic sensors However note that this approach performs only mapping with known poses In our experiments we restricted ourselves to determine the robot trajectory by using a laser based SLAM system to compare the maps obtained with laser and sonar Still the SLAM engine used to determine the trajectory can be replaced by any visual SLAM system This substantially cuts the costs of the mapping setup Clearly the quality of the map computed by the sonars heavily depends on the accuracy of their pose estimate on the platform Fig 4 depicts a qualitative comparison between the maps obtained with a laser scanner and with a set of sonars Fig 4 We mapped a 40 by 40 m indoor offi ce and corridor environment The left image represents the occupacy grid obtained with the laser scanner while the right one is the map obtained with the sonars Our goal is to perform localization and navigation tasks on these maps using solely ultrasonic measurements B Localization We develop a localization module for our system to estimate the pose of the platform in the occupancy grid This module is based on a particle fi lter which sequentially refi nes the estimated pose of the robot by integrating sonar readings The goal of the fi lter is to update a distribution over the possible robot poses expressed as a set of samples x i The denser the samples in a state space region the more likely it is that the robot is in that region We use a sample importance resampling schema where the prediction step is carried on by sampling noise from the controls utas x i t t 1 x i t 1 t 1 ut