ScienceDirect - Jochen Sprickerhof

9th IFAC Symposium on Intelligent Autonomous Vehicles 9th IFAC Intelligent Autonomous June 29 -Symposium July 1, 2016.on Messe Leipzig, Germany Vehicles 9th IFAC Symposium on Intelligent Autonomous Vehicles 9th IFAC Intelligent Autonomous Vehicles June 29 -Symposium July 1, 2016.on Messe Leipzig, Germanyonline at www.sciencedirect.com June 29 - July 1, 2016. Messe Leipzig,Available Germany June 29 - July 1, 2016. Messe Leipzig, Germany

ScienceDirect 3D 3D 3D 3D

IFAC-PapersOnLine 49-15 (2016) 212–217

Navigation Mesh Generation for Navigation Mesh Generation for Navigation Mesh Generation for Navigation Mesh Generation for Planning in Uneven Terrain Planning in Uneven Terrain Planning in Uneven Terrain Planning in Uneven Terrain ∗ ∗∗

Sebastian Sebastian Sebastian Sebastian

Path Path Path Path

Pütz ∗ Pütz ∗ Pütz Pütz ∗

Thomas Wiemann ∗∗ Jochen Sprickerhof ∗∗∗ ∗∗∗ Thomas Wiemann Sprickerhof ∗∗ Jochen ∗∗∗ ∗∗∗∗ Thomas Wiemann Sprickerhof ∗∗ Jochen ∗∗∗ Joachim Hertzberg ∗∗∗∗ Thomas Wiemann Jochen Sprickerhof Joachim Hertzberg ∗∗∗∗ Joachim Hertzberg ∗∗∗∗ Joachim Hertzberg ∗ ∗ Osnabrück University, Germany ([email protected]) Osnabrück University, Germany ([email protected]) ∗∗ ∗ Osnabrück University, Germany ([email protected]) ∗ ∗∗ Osnabrück OsnabrückUniversity, University,Germany Germany([email protected]) ([email protected]) Osnabrück University, Germany ([email protected]) ∗∗ ∗∗∗ Osnabrück University, Germany ([email protected]) ∗∗ Osnabrück University, Germany ([email protected]) ∗∗∗ Osnabrück University, Germany ([email protected]) Osnabrück University, Germany ∗∗∗ ∗∗∗∗Osnabrück University, Germany ([email protected]) ∗∗∗ OsnabrückUniversity, University Germany and DFKI([email protected]) Robotics Innovation Center, ∗∗∗∗Osnabrück Robotics Innovation Center, ∗∗∗∗ Osnabrück University and DFKI([email protected]) University DFKI Robotics ∗∗∗∗ Osnabrück Osnabrück Branch, Germanyand ([email protected]) Osnabrück University and DFKI Robotics Innovation Innovation Center, Center, Osnabrück Branch, Germany ([email protected]) Osnabrück Branch, Germany ([email protected]) Osnabrück Branch, Germany ([email protected]) Abstract: We present a 3D mesh surface navigation system for mobile robots. This system uses Abstract: We present a 3D mesh surface navigation system for mobile robots. This system uses We a surface for robots. uses aAbstract: 3D point cloud to reconstruct a triangle mesh of thesystem environment in real timeThis thatsystem is enriched Abstract: We present present a 3D 3D mesh mesh surface navigation navigation system for mobile mobile robots. This system uses a 3D point cloud to reconstruct a triangle mesh of the environment in real time that is enriched a 3D point cloud to reconstruct a triangle mesh of the environment in real time that is enriched with a graph structure to represent local connectivity. This Navigation Mesh is then analyzed a 3D apoint cloud to reconstruct a triangle mesh of the environment in real timeisthat is analyzed enriched with graph structure to represent local connectivity. This Navigation Mesh then with a to This Navigation is analyzed for and trafficability and local used connectivity. for online path planning. The Mesh presented approach is withroughness a graph graph structure structure to represent represent local connectivity. This Navigation Mesh is then then analyzed for roughness and trafficability and used for online path planning. The presented approach is for roughness and trafficability and used for online path planning. The presented approach evaluated with a VolksBot XT platform in a real life outdoor environment. for roughness and trafficability and usedinfor online path planning. The presented approach is is evaluated with a VolksBot XT platform a real life outdoor environment. evaluated with a VolksBot XT platform in aa real life outdoor environment. evaluated with a VolksBot XT platform in real life outdoor environment. © 2016, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Keywords: Robot Navigation, Path Planning, Surface Reconstruction, Mesh Generation Keywords: Robot Navigation, Navigation, Path Planning, Planning, Surface Reconstruction, Reconstruction, Mesh Generation Generation Keywords: Keywords: Robot Robot Navigation, Path Path Planning, Surface Surface Reconstruction, Mesh Mesh Generation 1. INTRODUCTION In this paper we present an open source, robot-independent 1. INTRODUCTION INTRODUCTION In this this paper paper we we present present an an open open source, source, robot-independent robot-independent 1. In 3D mesh navigation solution robotrobot-independent navigation with 1. INTRODUCTION In this paper we present an openfor source, 3D mesh navigation solution for robot navigation with mesh navigation solution for robot navigation In flat terrain most obstacles can be safely perceived by 3D high extensibility. mesh navigation solution for robot navigation with with In flat flat terrain terrain most most obstacles obstacles can can be be safely safely perceived perceived by by 3D high extensibility. In using 2D obstacles laser scanners. to that, In flat horizontal terrain most can be Compared safely perceived by high high extensibility. extensibility. using horizontal 2D laser scanners. Compared to that, using horizontal laser that, 2. RELATED WORK it is much more 2D difficult to operateCompared in roughto terrain using horizontal laser scanners. scanners. that, 2. RELATED RELATED WORK it is is much much more 2D difficult to operate operateCompared in rough roughtoterrain terrain 2. it more difficult to in and to check the trafficability of uneven ground. Robots 2. RELATED WORK WORK it is tomuch more difficult to of operate inground. rough Robots terrain and check the trafficability uneven and the uneven Robots Octree Mapping and Digital Elevation Maps operating outdoors like in rescueof or exploration and to to check check the trafficability trafficability ofoperations uneven ground. ground. Robots 2.1 2.1 Octree Mapping and Digital Elevation Maps Maps operating outdoors like in rescue operations or exploration operating outdoors like operations exploration of unknown environments need to cope withor uneven 2.1 2.1 Octree Octree Mapping Mapping and and Digital Digital Elevation Elevation Maps operating outdoors like in in rescue rescue operations orsuch exploration of unknown environments need to cope with such uneven of unknown environments need to cope with such uneven terrain and must still be able find a path their goal. Many navigation and path planning approaches use Ocof unknown environments needto cope withto such uneven terrain and must must still be be able able toto find a path path to their goal. Many Many navigation navigation and and path path planning approaches approaches use use OcOcterrain and still to find a to their goal. Therefore, they have to classify if something is a potential ornavigation similar representations that are created from 3D terrain andthey musthave stillto beclassify able toiffind a path to their goal. trees Many and path planning planning approaches use OcTherefore, something is a potential trees or similar representations that are created from 3D Therefore, they have to classify if something is a potential trees or similar representations that are created from 3D obstacle or if they can pass it, e.g., by traversing over point cloud data. A popular implementation for it is the Therefore, havecan to classify something is a potential trees similar representations that are created 3D obstacle or orthey if they they pass it, it,if e.g., e.g., by traversing traversing over point point or cloud data. A popular popular implementation implementation for from it is is the the obstacle if can pass by over cloud data. A for it it. To reach target, the trafficability of the ground package in popular ROS (Hornung et al., 2013). obstacle or ifthe they can pass it, e.g., by traversing over OctoMap point cloud data. A implementation for itA isplanthe it. To reach the target, the trafficability of the ground OctoMap package in ROS (Hornung et al., 2013). A planit. To the the trafficability of ground package in (Hornung et al., 2013). planbetween the current position the goal pose is essential. ning system for Octrees described al. it. To reach reach the target, target, theand trafficability of the the ground OctoMap OctoMap package in ROS ROS is et in al.,Morisset 2013). A A et planbetween the current current position and the goal goal pose pose is essential. essential. ning system system for Octrees Octrees is(Hornung described in Morisset et al. between the position and the is ning for is described in Morisset et al. Rough terrain involves certain risks, such as holes in the (2009). It decomposes the 3D Octree map into several between the current position andrisks, the goal pose is essential. ning system for Octreestheis 3D described inmap Morisset et al. Rough terrain involves certain such as holes in the (2009). It decomposes Octree into several Rough terrain involves certain risks, such as the It the 3D into several ground, bold peaks or sharp objects. be in heavy regions that are locally 2D. regionsmap can than used Rough involves certain risks,There such might as holes holes the (2009). (2009). It decomposes decomposes theThese 3D Octree Octree into be several ground,terrain bold peaks peaks or sharp sharp objects. There might be in heavy regions that that are locally locally 2D. 2D. These regionsmap can than than be used ground, bold objects. There might heavy are regions can be used obstacles thatpeaks pose or danger to the robot and that be must be regions like a ordinary grid map2D. forThese path planning. Also, semantic ground, bold or sharp objects. There might be heavy regions that are locally These regions can than be used obstacles that pose danger to the robot and that must be like a ordinary grid map for path planning. Also, semantic obstacles danger to the robot and must be like aa ordinary for path planning. semantic avoided inthat anypose case. But there might also that be obstacles classification ofgrid the map represented surfaces canAlso, be integrated obstacles that pose danger to the robot and that must be like ordinary grid map for path planning. Also, semantic avoided in in any any case. case. But But there there might might also also be be obstacles obstacles classification classification of of the the represented represented surfaces surfaces can can be be integrated integrated avoided the robotincan with at higher costs (energy, speed up the computation. Fig. 1b shows of avoided anydeal case. But there might also be execution obstacles to classification of the represented surfaces can an be example integrated the robot can deal with at higher costs (energy, execution to speed up the computation. Fig. 1b shows an example of the robot can deal with at higher costs (energy, execution to speed up the computation. Fig. 1b shows an example of time) in order to reach the target. the Octree representation of a natural scene. the robot can deal with the at higher costs (energy, execution to speed uprepresentation the computation. Fig. 1b shows an example of time) in order to reach target. the Octree of a natural scene. time) in to the the of scene. time) in order order to reach reach the target. target. the Octree Octree representation representation of aa natural natural scene. We have developed a general mesh planner extending the Occupancy maps, like Octrees, represent obstacles in a We have developed a general mesh planner extending the Occupancy maps, like Octrees, represent obstacles in in aa We have developed a general mesh planner extending the Occupancy maps, like Octrees, represent obstacles ideas of Breitenmoser and Siegwart (2012) and Gingras binary way in form of a grid map. These maps only define We have developed a general mesh planner extending the binary Occupancy maps, like Octrees, represent obstacles in a ideas of Breitenmoser and Siegwart (2012) and Gingras way in form of a grid map. These maps only define ideas Breitenmoser and Siegwart and Gingras way in form of aa grid maps only define et al. of (2010). This paper our(2012) approach local binary obstacles, of obstacles a specific ideas of Breitenmoser andpresents Siegwart (2012) and for Gingras binary waybut in not formthe of trafficability grid map. map. These These maps in only define et al. (2010). This paper presents our approach for local obstacles, but not the trafficability of obstacles in a specific et al. This our for but not obstacles in specific planning and robot controlpresents in the current field of view and obstacles, context. For an obstacleof can provide et al. (2010). (2010). This paper paper our approach approach for local local obstacles, but example, not the the trafficability trafficability obstacles in aadrivable specific planning and robot robot controlpresents in the the current current field of of view view and context. context. For For example, an obstacle obstacleof can can provide drivable planning and control in field and example, an provide drivable demonstrates the usability on a mobile robot operating in ground, if the robot is on top of it, but also an inaccessible planning and robot control on in the current fieldoperating of view and context. For example, antop obstacle can provide drivable demonstrates the usability a mobile robot in ground, if the robot is on of it, but also an inaccessible demonstrates the on robot operating in the robot is located on it,a but also an ademonstrates real world outdoor environment. Unlike grid map ledge, if if the lower Generally, the usability usability on aa mobile mobile robot2D operating in ground, ground, therobot robotis on top top of ofat alsolevel. an inaccessible inaccessible a real real world world outdoor outdoor environment. Unlike 2D grid map map ledge, if if ifthe the robot isis located located atit,aa but lower level. Generally, aapproaches, environment. Unlike 2D grid ledge, robot is at lower level. Generally, this 3D planner works on triangle meshes obstacle detection depends on the obstacle’s size and the a real world outdoor environment. Unlike 2D grid map ledge, if the robot is located at a lower level. Generally, approaches, this 3D planner works on triangle meshes obstacle detection depends on the obstacle’s size and the the approaches, this 3D planner works on triangle meshes obstacle detection depends on the obstacle’s size and instead of 2D grid map planes. Working with such 3D distance to the obstacle, as evaluated in Schwarz approaches, this 3Dmap planner works on triangle meshes obstacle detection depends as on evaluated the obstacle’s size and and the instead of 2D grid planes. Working with such 3D distance to the obstacle, in Schwarz and instead of grid Working with such to obstacle, evaluated in and surfaces in 2D navigation hasplanes. the advantage conformity of distance Behnke (2014). deal withas shortcomings, elevation instead of 2D grid map map planes. Workingof with such 3D 3D distance to the theTo obstacle, asthese evaluated in Schwarz Schwarz and surfaces in navigation has the advantage of conformity of Behnke (2014). To deal with these shortcomings, elevation surfaces in has the advantage of of (2014). with shortcomings, the navigation costs based differences, maps build on To topdeal of simple occupancy maps. elevation Starting surfaces in navigation navigation has on thedistances, advantageheight of conformity conformity of Behnke Behnke (2014). with these these shortcomings, the navigation navigation costs based based on distances, height differences, maps build build on To topdeal of simple simple occupancy maps. elevation Starting the costs on distances, height differences, maps on top of occupancy maps. Starting roughness, forbidden areas and other criteria. Additionally, with height maps, where a height z is stored in the cells the navigation costs based on distances, height differences, maps build on top of simple occupancy maps. Starting roughness, forbidden areas and other criteria. Additionally, with height maps, where a height z is stored in the cells roughness, forbidden areas and other criteria. Additionally, with height maps, where a height z is stored in the cells meshes represent 3D space directly and can easily be anof the grid map with z = f (x, y). These solutions are roughness, forbidden areas and other and criteria. Additionally, with height maps, where a= height z isThese storedsolutions in the cells meshes represent 3D space directly can easily be anof the grid map with z f (x, y). are meshes represent 3D space directly and can easily be anof the grid map with z = f (x, y). These solutions are notated with semantic labels as described in Deeken et al. usually called 2.5D maps (cf. Fig. 1c). A special type meshes 3D space and can easily beetanof the grid map with z =(cf. f (x,Fig. y). 1c). TheseA solutions are notated represent with semantic semantic labelsdirectly as described described in Deeken Deeken al. usually usually called 2.5D maps special type type notated with labels as in et al. called 2.5D maps (cf. Fig. 1c). A special (2014). Compared to 2.5D solutions, full 3D mesh surface 2.5D called maps are Digital notated with semantic labels as described in Deeken et al. of usually 2.5D mapsElevation (cf. Fig. Maps 1c). A(DEM). specialThese type (2014). Compared to 2.5D solutions, full 3D mesh surface of 2.5D maps are Digital Elevation Maps (DEM). These (2014). Compared to solutions, full mesh 2.5D Digital Maps navigation algorithms enable navigating through multi of maps aremaps usedare to compute local trafficability maps,These e.g., (2014). Compared to 2.5D 2.5D solutions, full 3D 3D mesh aasurface surface 2.5D Digital Elevation Elevation Maps (DEM). (DEM). navigation algorithms enable navigating through multi of maps aremaps usedare to compute compute local trafficability trafficability maps,These e.g., navigation algorithms enable navigating through a multi maps are used to local maps, e.g., level environment, such as multistory buildings or tunnel by calculating the navigation costs based on the height navigation algorithms enable navigating throughoratunnel multi maps are used the to compute local trafficability maps, e.g., level environment, such as multistory buildings by calculating navigation costs based on the height level environment, such as or calculating the costs on systems. In particular navigationbuildings is appropriate for by differences on multiple scales (Schwarz and Behnke, 2014). level environment, suchmesh as multistory multistory buildings or tunnel tunnel by calculating the navigation navigation costs based based on the the height height systems. In particular mesh navigation is appropriate for differences on multiple scales (Schwarz and Behnke, 2014). systems. In particular mesh navigation appropriate for on scales Behnke, 2014). climbing robots or other robots with theis ability to access However, these techniques are(Schwarz not ableand to handle multiple systems. particular mesh navigation appropriate for differences differences on multiple multiple scales Behnke, 2014). climbing In robots or other other robots with the theisability ability to access access However, these these techniques are(Schwarz not able ableand to handle handle multiple climbing robots or robots with to However, techniques are not to multiple walls or ceilings. overlapping levels, such as tunnel systems or multistory climbing robots or other robots with the ability to access However, these techniques are not able to handle multiple walls or or ceilings. overlapping levels, levels, such as as tunnel systems systems or multistory multistory walls overlapping walls or ceilings. ceilings. overlapping levels, such such as tunnel tunnel systems or or multistory

Copyright © 2016, 2016 IFAC 212Hosting by Elsevier Ltd. All rights reserved. 2405-8963 © IFAC (International Federation of Automatic Control) Copyright © 2016 IFAC 212 Copyright 2016 responsibility IFAC 212Control. Peer review©under of International Federation of Automatic Copyright © 2016 IFAC 212 10.1016/j.ifacol.2016.07.734

IFAC IAV 2016 June 29 - July 1, 2016. Messe Leipzig, Germany Sebastian Pütz et al. / IFAC-PapersOnLine 49-15 (2016) 212–217

(a) Lab example scene with a step field.

(b) Octree representation of the scene.

(c) 2.5D Map (DEM) representation of the scene.

213

(d) Mesh representation of the scene.

Fig. 1. State of the art map representations to estimate trafficability in 3D in uneven terrain. buildings and are not practicable for climbing robots. For evaluation we implemented the DEM trafficability estimation schemes presented in Schwarz and Behnke (2014) in a costmap_2d layer plugin for move base in ROS. An example DEM costmap is shown in Fig. 1d together with the reconstructed mesh for comparison.

Organized Fast Mesh LVR PCL

2.2 Terrain Reconstruction

...

An alternative approach is described in Gingras et al. (2010) and Breitenmoser and Siegwart (2012). This method is able to handle multiple levels by reconstructing the rough terrain into a triangle mesh. Such triangle mesh representations are much more flexible and can be used for multiple purposes like environment representations in robotic simulators (Wiemann et al., 2013), visualization for human robot interaction and ground classification or path planning (Wiemann et al., 2010). In such a representation, the alignment of surface parts or change between adjacent triangles can be evaluated for drivability estimation. This allows to determine navigation costs based on distances, height difference, roughness, forbidden areas and semantic information.

Graph Half Edge Mesh Generation

Obstacle Marking

Path Planning

Local Roughness Cost Estimation

Obstacle Inflation

Local Robot Control

Local Height Difference Cost Estimation

Graph Edge Region Growing

Fig. 2. Structure of our terrain classification approach. Triangle mesh representations are converted into a graph that is used for planning using navigation costs estimated for the represented terrain. criteria into account. Besides global navigation, the Navigation Mesh can be used for a local control of the robot. Details on the single computation steps are presented in the following sections. 4. SURFACE RECONSTRUCTION 4.1 Organized Point Clouds

3. SYSTEM ARCHITECTURE The components of our approach are shown in Fig. 2. The main input is a triangle mesh constructed with some kind of surface reconstruction procedure. In principle, arbitrary meshes from different sources like LVR (Wiemann et al., 2012) or PCL (Rusu and Cousins, 2011) could also be used. In this paper, we use the Organized Fast Mesh method to generate a triangle mesh surface representation from 3D laser scanner data at the current position of the robot in real time (Holz and Behnke, 2012). In this paper we do not present an approach for global mapping. However, our approach also works for meshes representing larger environment sections, coequal with some kind of a global map. First, a so called Navigation Mesh is generated and analyzed to estimate distances, height differences, and roughness. If specific safety thresholds are violated, these areas will be marked as lethal obstacles, since the robot must avoid them. Areas around such lethal obstacles are inflated (marked as dangerous) and graph optimization algorithms are applied, e.g., Graph Edge Region Growing to implicitly smooth the discretized mesh path. Now it is possible to apply shortest path algorithms on the resulting optimized Navigation Mesh to compute paths taking the evaluated 213

The organized fast meshes we use in this paper rely on organized point clouds. The data flow to generate them is displayed in Fig. 3. After removing outliers from a single 2D laser scan and applying a robot self filter, we assemble the single 2D laser scans to one organized point cloud. In such a representation, neighboring points are stored in a matrix structure resulting in a depth image. To generate this matrix, we exploit the fact that we know the polar angles φ and θ for every single point. Our rotation unit delivers for every single point the φ value. The θ value can be reconstructed from the order of points in each 2D scan taking the point count and the resolution into account. In summary: The Cartesian coordinates (x, y, z) of a scan point corresponding to the spherical coordinates φ and θ neighboring scan line distance values are stored in a matrix structure. This structural knowledge about the data allows us to connect neighboring points in a single 360 degree scan to construct the mesh. Unsensed or removed points are marked with NaN to maintain the matrix structure. Another advantage of the organized structure is that it enables a fast and robust computation of point normals by using integral image techniques such as Average 3D Gradient, Average Depth Change or Covariance Matrix as described in Holzer et al. (2012).

IFAC IAV 2016 214 Sebastian Pütz et al. / IFAC-PapersOnLine 49-15 (2016) 212–217 June 29 - July 1, 2016. Messe Leipzig, Germany

2D Laser Scans

Laser Scan Filtering

Organized Point Normals Estimation

Hole Filling

Organized Laser Assembling

Organized Fast Mesh Generation

Surface Smoothing

Organized Point Cloud

Organized Mesh

Triangle Mesh

Fig. 3. Data flow for organized point cloud and Organized Fast Mesh generation. 4.2 Organized Fast Mesh The used fast organized mesh generation is similar to the irregular triangular mesh (ITM) used in Gingras et al. (2010) and Breitenmoser and Siegwart (2012). The basic assumption is that φ and θ of neighboring scan lines represent points that are often neighbors in the real world. To address the problem of shadow edges - edges that violate continuity, e.g. connecting two objects, one in the front and one in the back - we implemented rules that filter these edges before insertion. After the raw mesh is created, we fill up the robot’s shadow in the input data. We assume that the robot stands on solid ground, since it successfully navigated to the current position. First, the shape of the robot shadow is extracted by finding the first transition from a NaN value to a valid one and following the contour line defined by this transition (NaN to valid) until the contour is closed. Once we found the robot shadow contour, we triangulate the 3D contour polygon and to fill it up, as shown in Fig. 6. The robot produces a shadow in the input scan, filled up with the red triangles in the reconstruction. The filled up mesh is then used as input for the computation of a Navigation Mesh. 5. NAVIGATION MESH 5.1 Graph Half Edge Mesh Generation The basic idea behind the Navigation Mesh is to reduce the challenge of planning in uneven and rough terrain to a graph theoretical problem. We combined the well known Half Edge Mesh structure (Wiemann et al., 2012) with two different graphs, a vertex graph Gv and a triangle graph Gt , resulting in a Graph Half Edge Mesh structure. The underlying triangle mesh is represented by a set of triangles F and a set of vertices or points P in 3D space. The vertex graph Gv = (Vv , Ev ) connects all vertices, which are connected by triangle edges in the mesh with Ev = {e(i,j) | i and j ∈ f ∈ F}, forming a network of all vertices in the triangle mesh. The set of graph nodes Vv corresponds to the set of vertices in the mesh. Similarly, the triangle graph Gt = (Vt , Et ) represents a network of mesh triangles with deg(v) ≤ 3 ∀ v ∈ Vt , since each triangle can have a maximum of three triangle neighbors. Within the graph, we encode various characteristics of the surfaces into several layers of navigation costs for all edges that are fused into a single cost function c(i,j) for each 214

edge e(i,j) ∈ E connecting the graph nodes i and j. The different cost functions are depending on values like the distance between two vertices or faces, the riskiness of accessing certain vertices or faces, and the local roughness assigned to vertices or faces. Graph edge costs can be estimated by taking distances or height differences into account. Graph node costs correspond to local roughness, local height difference and riskiness. Riskiness expresses forbidden areas in a gradual way. Additionally, each triangle and vertex can be associated with a normal vector that can be included in the computation of trafficability. In particular, these normal vectors (cf. Sec.4) are used to calculate the local roughness for each vertex considering their respective local network. After fusing these layers into a single cost function, we can apply standard graph search algorithms for planning. The different cost layers are described in detail in the following sub sections. 5.2 Lethal Obstacle Marking and Inflation After the initial mesh is created, we mark inaccessible areas or positions on the surface. Since different robots are able to traverse different kinds of surfaces, the physical abilities of the used robot have to be considered when determining the costs within the mesh planning graph. We define all borders of the Navigation Mesh as lethal as the environment beyond the sensed area is unknown and hence potentially dangerous. Borders could result from shadows in the laser scans or by the limited range of the scanner. On the basis of the robot’s abilities, high graph node costs are attributed to areas where obstacles are detected. First we calculate roughness and height differences on the surface. Then areas at heavy roughness and height difference costs are marked as lethal obstacles. Thus we are able to avoid dangerous areas like walls, crags, cliffs or ceilings implicitly. After defining the lethal obstacles we inflate these forbidden areas so that we can treat the robot in navigation planning as a point. We developed a gradual inflation algorithm with an inner lethal inflation radius ri and an outer maximum inflation radius ro . In between the inner and outer radius a cosine function fades down the costs to zero continuously. The corresponding riskiness r(v) for a graph node v ∈ V is defined as shown in Eq. 1, where cmax is the total maximum cost.

r(v) = cmax · cos

dlethal − ri π +1 ro − ri

(1)

We use a wave front algorithm to calculate the smallest distance dlethal to a lethal obstacle. For a simple optimization, the squared euclidean distance is used to differentiate between inside and outside of the inflation area. An example of a mesh with inflated regions around obstacles is shown in Fig. 4. The inflation radius to 0.3 m and the total inflation was set to a radius of 0.4 m, according to the specs of our robot. 5.3 Local Roughness Estimation While we navigate in rough or uneven terrain, examination of roughness can be interesting for several reasons (cf.


Fig. 4. 3D Navigation Mesh with a potential field to the goal, visualized in RViz. It shows a small section of the Botanical Garden of Osnabrück University with a cliff on the left and a planar region on the right. Lethal obstacles are red. Fig. 5). It can be more convenient to prefer surfaces and areas with a higher continuity. This results in preferring paved roads or lanes which consequently results in faster locomotion. Roughness describes the continuity of the surface, independent of the alignment of the surface in world coordinates. That means that a higher local continuity of a local area is described by smaller roughness values for each vertex or triangle in the network. Therefore a wall can also exhibit continuity like the ground. In Eq. 3 we define the roughness ρ(v) as the average angle of the fluctuation of the corresponding normals in a local neighborhood Nl (v) of the graph node v ∈ V . Nl (v) contains all graph nodes in a predefined local Euclidean range l, analogous to the height difference in the next section. Therefore the angle between the normals are calculated (u,v) = acos(n(u) · n(v)) for every edge e(u,v) ∈ E with cr and a vertex average angle αav (v) is defined as in Eq. 2, where Nd (v) are all direct neighbors of v. αav (v) =

ρ(v) =

αav (v) +

(u,v) u∈Nd (v) cr

|Nd (v)|

u∈Nl (v)

αav (u)

|Nl (v)| + 1

(2) (3)

215

Fig. 5. Roughness of the environment. - Scene: A small section of the Botanical Garden of Osnabrück University. specific threshold, we mark the node v as a lethal obstacle, because the robot is not able to handle the magnitude of height difference. (p(v) − p(u)) · z b(v) = max (4) |z| u∈Nl (v) 6. ROBOT NAVIGATION

6.1 Global Path Planning For global path planning, we have to define final edge costs based in distances, riskiness, local roughness and height difference. Depending on the weighting of the different cost functions, path planning prefers different paths with different characteristics. We determine the final edge costs c(u,v) for each edge e(u,v) ∈ E as described in Eq. 5. The coefficients kρ and kb are used for weighting the local roughness ρ and the local height difference b. d(u,v) is the distance between the position vectors corresponding to u and v. Executing Dijkstra’s shortest path algorithm or A* search on the graph Gt or Gv with edge costs c(u,v) results in a path with the smallest cost for the given coefficients, if a path can be found. c(u,v) = ((kρ max (ρ(u), ρ(v))) + 1) · ((kb max (b(u), b(v))) + 1)

· d(u,v) + |r(u) − r(v)|

5.4 Local Height Difference Cost Estimation To enable navigation over staircases or other obstacles with a predefined maximum height, we use evaluate height differences, as proposed in Schwarz and Behnke (2014) for 2.5D digital elevation maps. They define bumpiness in a local environment for one cell. Analogously, we define bumpiness for one vertex v for a local vicinity Nl (v). Defining a direction z pointing upwards, we are able to calculate the maximum difference in a local field in the direction of z as shown in Eq. 4, where p(v) is the position vector corresponding to the graph node v. If b(v) exceeds a 215

(5)

6.2 Local Robot Control Following the idea of Breitenmoser and Siegwart (2012) it is easy to extract a vector field defined on the surface from our Navigation Mesh. After execution of the Dijkstra shortest path algorithm from the goal pose to the target pose, all smallest cost paths from every point to the target pose are available. For each node v, a predecessor Π(v) is registered. At every point on the surface we can interpolate the next pose by evaluate the predecessors of

IFAC IAV 2016 216 Sebastian Pütz et al. / IFAC-PapersOnLine 49-15 (2016) 212–217 June 29 - July 1, 2016. Messe Leipzig, Germany

values, red colored regions are areas with the highest path costs.

R Fig. 6. (left) The robot Pluto based on the VolksBot XT, equipped with a rotating 2D Sick LMS 200, a Velodyne 360 degree laser scanner and a PhidgetSpatial 1044 inertial measurement unit. (right) Organized Fast Mesh displayed green; robot model of Pluto; filled up robot shadow displayed red; visualized in RViz. the surrounding graph nodes. The resulting current path can be used to calculate velocity commands influenced by the costs of the path segments, the roughness and height differences. This vector field directly corresponds to the potential field as shown in Fig. 4.

7. EVALUATION 7.1 Robot Setup We evaluated the trafficability estimation with a VolksBot XT that is equipped with a rotating 2D Sick laser scanner. The setup is shown in Fig. 6 (left). The aim is to navigate through rough terrain and to map its environment in a robust way. We implemented a mapping system which works in a stop-and-go scheme analogous to Schadler et al. (2014). In the stop-phase the robot assembles 360 degree 3D organized point clouds. These assembled clouds will be registered successively to one global point cloud. The organized point clouds enable a fast estimation of point normals (Holzer et al., 2012) and therefore a more robust registration result using point to plane ICP, taking the surface normals into account as shown in Segal et al. (2009). 7.2 Exemplary Path Planning Results To demonstrate the ability of our robot to drive and plan in rough terrain, we tested the setup in the Botanical Garden of Osnabrück University. The garden is located in a former stone quarry and shows a variety of different surfaces at different levels of roughness ranging from road-like paved ways to small paths and cobblestone trails with steep stairs. To evaluate our approach, we chose three exemplary areas within the garden to show the benefits of our methods in different kinds of environments. Pictures of the evaluated scenes together with the computed roughness estimation, height difference map and fused values in the potential field approach are shown in Fig. 7. At each position, we took a 360 degree laser scan, computed the terrain classification and gave the robot a target goal within the scanned area to drive to. The goal poses are marked with a white arrow, the planned paths are rendered in black. The color coding shows the determined path costs for the different areas in a blue to red rainbow color gradient, i.e., blue and violet colors correspond to low 216

The first scene is a relatively smooth path with a steep cliff on the left side and a slope to the right. The second is small path with rough stones fallen down from a cliff covering the right side of the scene. The third example was taken on a crossroads of a larger path and a small path with a staircase. The chosen scenes show different characteristics. On the path, the scene is dominated by a large, sharply distinguished flat surface that is surrounded by undriveable areas. Fallen rocks form hazardous areas in the cliff scene that are not that well distinguished from the drivable areas. The slope towards the cliff raises relatively slow from the path towards the cliff. Similarly, the slope of the grass area raises smoothly from the path. In principle this region is drivable for our robot, but it should avoid the stairs in any case, as the height difference on the steps is too high. 7.3 Discussion The results of the terrain classification shown in Fig. 7 nicely reflect the properties of the scanned environments. In the path scene, the driveable areas on the path are classified with low costs both in the roughness and height difference classification. The planned path is smooth and follows the middle of path. Since in this example both the height difference and roughness classification deliver similar drivability estimations, the resulting potential field also prefers the pathway. In the cliff scene the height difference and roughness metrics compute different costs. The height difference assigns relatively low costs to the slope towards the cliff, since the local height difference is low. However, the roughness estimation classifies this area as hazardous, due to the unevenness resulting from the fallen rocks. This example clearly demonstrates that a classification based on local height differences alone does not always deliver safe classification results. Consequently, using the fused information in the potential field, the planner prefers a route on the pathway. In the third example, both metrics punish the area with the steps and mark them dangerous. The presented examples show that the presented metrics for the Navigation Meshes deliver coherent classifications results with respect the underlying classification heuristic. The example of the cliff data set shows that using a single metric may come to false drivability assumptions. However, the proposed fusion of both metrics in the potential field allows save navigation, if one the heuristics fails. That clearly demonstrates the strengths of the Navigation Mesh approach, since the underlying graph structure can encode different modalities (roughness, height difference, path length) in a single representation. 8. CONCLUSION AND FUTURE WORK We introduced the Organized Fast Mesh as mesh generation method to compute a Navigation Mesh which can directly be used to perform path planning in the current field of view of the robot. In future research we will use the Navigation Mesh in a global mapping process to perform global path planning in a multilevel environment. The


Scene Photo

Roughness Estimation

Height Difference

217

Potential Field

Fig. 7. Evaluation examples of the Navigation Mesh and its metrics. The left column shows a picture of the scene, the right three columns present the classification results. Navigation Mesh is used to evaluate different drivability measures in a single data structure for path planning and local navigation. We evaluated the proposed method on real life application examples and demonstrated that our approach delivers reliable terrain classification and plans safe paths. In future work, we will integrate path optimization methods to create smoother trajectories. Additionally, the system has to be evaluated in varying environments and different use cases with different robots. However, the presented results show that the proposed mixture of triangle mesh and graph representation looks promising for future work in the area of drivability estimation and path planning. We plan to make the software available as open source for the robotic’s community whilst adapting and extending it. Further tests will be done on other robot platforms to prove the benefits of the presented methods. REFERENCES Breitenmoser, A. and Siegwart, R. (2012). Surface reconstruction and path planning for industrial inspection with a climbing robot. In Proc. CARPI 2012. Deeken, H., Pütz, S., Wiemann, T., Lingemann, K., and Hertzberg, J. (2014). Integrating semantic information in navigational planning. In Proc. ISR/Robotik 2014. München. Gingras, D., Lamarchemarche, T., Bedwani, J., and Dupuis, E. (2010). Rough terrain reconstruction for rover motion planning. In Proc. CRV 2010, 191–198. Holz, D. and Behnke, S. (2012). Fast range image segmentation and smoothing using approximate surface reconstruction and region growing. In Proc. IAS-12. Holzer, S., Rusu, R.B., Dixon, M., Gedikli, S., and Navab, N. (2012). Adaptive neighborhood selection for real-time 217

surface normal estimation from organized point cloud data using integral images. In Proc. IROS 2012, 2684– 2689. IEEE. Hornung, A., Wurm, K.M., Bennewitz, M., Stachniss, C., and Burgard, W. (2013). OctoMap: An efficient probabilistic 3D mapping framework based on octrees. Autonomous Robots. Morisset, B., Rusu, R.B., Sundaresan, A., Hauser, K.K., Agrawal, M., Latombe, J.C., and Beetz, M. (2009). Leaving flatland: Toward real-time 3d navigation. In Proc. ICRA 2009. IEEE. Rusu, R.B. and Cousins, S. (2011). 3D is here: Point Cloud Library (PCL). In Proc. ICRA 2011. IEEE. Schadler, M., Stückler, J., and Behnke, S. (2014). Rough terrain 3d mapping and navigation using a continuously rotating 2d laser scanner. KI, 28(2), 93–99. Schwarz, M. and Behnke, S. (2014). Local navigation in rough terrain using omnidirectional height. In Proc. ISR/Robotik 2014. Segal, A., Haehnel, D., and Thrun, S. (2009). Generalizedicp. In Proc. RSS 2009. Seattle, USA. Wiemann, T., Lingemann, K., and Hertzberg, J. (2013). Automatic map creation for environment modelling in robotic simulators. In Proc. ECMS 2013. Älesund. Wiemann, T., Lingemann, K., Nüchter, A., and Hertzberg, J. (2012). A toolkit for automatic generation of polygonal maps - las vegas reconstruction. In Proc. Robotik 2012, 446–451. VDE Verlag, München. Wiemann, T., Nüchter, A., Lingemann, K., Stiene, S., and Hertzberg, J. (2010). Automatic construction of polygonal maps from point cloud data. In Proc. SSRR 2010. Bremen.