Motion Planning for Actively Reconfigurable Mobile Robotsin Search and Rescue Scenarios

Michael Brunner, Bernd Bruggemann and Dirk SchulzFraunhofer Institute for Communication, Information

Processing and Ergonomics FKIE, Wachtberg, GermanyEmail: {michael.brunner, bernd.brueggemann, dirk.schulz}@fkie.fraunhofer.de

Abstract — In disaster scenarios, mobile robots can be employedin hazardous environments where it is too dangerous for humanrescuers. Robotic systems can assist rescue personnel as they can beused to explore those inaccessible areas and to assess the situation.Tracked platforms with actuators have been proven to be well suitedfor such deployments because they are agile enough to overcomequite challenging terrain. A very demanding task for operators is thenavigation of the robotic system in complex disaster environments.Hence, an important capability of future systems for search and rescuemissions is autonomous navigation in disaster scenarios.

In this paper we introduce a two-phase motion planning algorithmfor tracked robots with actively controlled actuators to find a fast andstable path to a user specified goal. In the first phase, we generatean initial path considering the platform’s operating limits and theterrain roughness. In the second phase, we limit the search space tothe area around the initial path and refine the preliminary solutionaccounting for the complete robot state including actuators and therobot’s stability and traction. A main distinction of our method isthat it does not rely on a previous classification of the terrain, thus,can be applied to a variety of environments. We present experimentsevaluating our algorithm in simulation and in two real-world scenariosto demonstrate the validity and feasibility of our approach.

Keywords: autonomy, motion planning, rough terrain, re-configurable chassis, mobile robot, actuators

I. INTRODUCTION

The goal in search and rescue robotics is two-fold; to extendthe capabilities of human rescue personnel and to improvetheir safety. Usually, a teleoperated system is deployed, con-trolled with or without line-of-sight. The mobile system is usedto initially explore the environment and assess the situation.Controlling a robotic system in complex environments likedisaster scenarios is a challenging task for operators, especiallybecause they are unable to use their natural human senses andare constrained to the limited and unusual views provided bythe robot’s sensors. In addition to judging the situation andsearching for victims, the operator must find valid paths forthe robot.

As achieving full autonomy of search and rescue robotsis very hard due to the complexity of those mission, manyassistance functions for teleoperated systems [1] or for semi-autonomous operation [2], [3] have been proposed in order toreduce the cognitive burden on the operator. These methods in-clude improved graphical user interfaces, processing of sensordata, information extraction algorithms and victim detectionstrategies.

Fig. 1: Method overview: First, a preprocessing step whichanalyses the environment, followed by a preliminary planningstep to find an initial path and a detailed motion planningstep which refines the initial solution in rough areas using theactuators. In flat areas, a default configuration is applied.

Navigating a mobile robot through complex environmentsunder the previously described conditions and searching fora path is very demanding for an operator. Especially becausethe path should not damage the robot, should avoid deadlocksand yet should still serve the mission. The operator mustconsider many different aspects when driving through roughterrain and over obstacles. The stability of the robot is criticalas the robot may fall over. Inertia and momentum becomeincreasingly important if a robot is operated close to its limits.Additionally, varying contact points of the mobile systemchange its behavior to actuator and driving commands.

In this paper, we present a two-phase motion planningalgorithm for tracked actively reconfigurable robots to finda fast and stable path to a user specified goal in roughterrain (Fig. 1). We first find an initial path quickly using therobot’s operating limits while neglecting its actuators. We thenrestrict the high dimensional search space, which includes theactuators, to the areas around those initial path segments whichlead through rough regions of the environment. Thus, we speedup planning while considering the actuators, the stability of thesystem and its traction. Our method does not rely on terrainclassifications or fixed motion sequences. Hence, it can beapplied to rough, unstructured outdoor environments as wellas obstacles in urban surroundings (e.g. stairs).

We consider our method to be the global planning compo-nent of a robotic system which provides a plan. Followed bya feedback controller which takes care of the plan execution

Fig. 2: Planning query involving a ramp and stairs in asimulated world.

using sensor data for localization and obstacle avoidance inpotentially dynamic environments. Such a controller is beyondthe scope of this paper.

We focus our discussion of previous work on planningapproaches for rescue robotics and rough environments. Birket al. present an autonomous reactive navigation approachbased on three basic modules, obstacle avoidance, deadlockavoidance and backing-off [4]. Magid et al. use a robotmodel, similar to ours, to traverse comparable terrain [5]. Theydeveloped a planning algorithm for a tracked robot with fouractively controlled crawlers to propose plans for operatorsof search and rescue missions. The motion sequences theyprovide also include motions of controlled balance losing, likeinsignificant falls from smaller edges. However, we considersuch states too risky for an autonomous application.

To traverse rough terrain and challenging structures manyapproaches involve a terrain classification or structure detec-tion mechanism. Behavior maps which encode execution skillsincluding preconditions for traversal are used in [6] to explorerough disaster environments for a small tracked robot. Themap is generated using fuzzy rules and Markov random fieldsfor classification. Further, different algorithms are proposedfor climbing stairs with tracked robots [7], [8]. They detectthe stairs and estimate its dimensions and direction to ensuresafe motions by fixing the robot’s heading to the gradient ofthe stair case. Rusu et al. present a comprehensive frameworkto traverse rough outdoor terrain as well as stairs [9]. Theirsystem consists of different components; map building, terrainclassification and a two-phase planning algorithm. A high-level planner triggers specialized sub-planners for the differentterrain types which generates gait primitives for a RHex robot(e.g. stair climbing gait primitives).

Our work differs from works mentioned above in thefollowing aspects. On a very general level we distinguishbetween between flat and rough regions using a roughnessquantification. However, we do not rely on a detailed terrainclassification or on fixed motion sequences which would limitus to the set of terrain or structure classes (e.g. ramps, stairs,step, etc.) imposed by the categorization or the set of fixedmotion sequences. As a consequence, our algorithm can beapplied to a variety of environments (see Fig. 2 and 7). Further,instead of focusing the detailed motion planning on the entireinitial path - which is common practice - we concentrate thesearch on rough segments, avoiding expensive planning inflat areas. Also, this paper extends [10] by using the metrics

Fig. 3: Roughness quantification (right) of a map (left); Therisk value associated with a region is based on the heightdifferences in this area and is used to control the planningprocess. The colors are indicating the degree of roughness,ranging from green for flat regions over yellow to red, highrisk areas.

introduced in [11], by providing an analysis of the safetyweights and by presenting real-world experiments.

The remainder of this paper is structured as follows: insection II we introduce the roughness quantification used.Sections III and IV describe the preliminary planning andthe detailed motion planning phases, respectively. Simulationand real-world experiments are provided in section V, and weconclude in VI.

II. MAP AND TERRAIN ROUGHNESS

Like other approaches [5], [6], [9], we use a height mapto represent the environment because it is simple to use andsufficient for our application. Using a map of the environmentallows us to simplify the perceptual task of 3D navigation inrough terrain and to focus on the motion planning aspect ofrough terrain navigation [12]. However, building the map isbeyond the scope of this paper.

In order to assess the risk at a position within the map, weuse techniques from image processing to compute a roughnessquantification. We first apply a maximum filter with a windowof squared size to the height differences. A distortion ofthe range of values can be prevented by a threshold, whichconveniently can be set to the robot’s maximal traversableheight. The threshold is also used to scale the values to [0, 1].Subsequently, we apply a two dimensional Gaussian blur tosmooth the transitions. The maximum filter prevents isolatedpeaks to be smoothed by the Gaussian filter. Figure 3 showsan example of this roughness quantification.

Using an appropriate kernel size allows us to virtually inflatethe hazardous areas which is commonly done in 2D navigationto keep the robot away from obstacles. In contrast, our highrisk areas are avoided by the robot, but if required, do notprohibit traversal. Another benefit is that the computation issimple and highly parallelizable.

We use this roughness quantification in both the preliminaryplanning as well as the detailed motion planning to adjust thebehavior according to the difficulty of the environment.

III. PRELIMINARY PLANNING

Actively reconfigurable robots introduce a large searchspace during planning. Further, driving on rough terrain issubject to additional constraints, such as stability which are not

(a) (b) (c)

Fig. 4: (a) The motion graph of a map encodes the traversability of the terrain. Areas with small height differences are whiteand areas with higher differences are red. (b) The preliminary path split into segments through flat regions (green) and segmentsthrough rough regions (purple). (c) The tube around the rough segment (purple).

satisfied naturally. Therefore, the planning process must pro-duce actuator commands and the path quality must be judgedby both its execution time and its safety. Our hierarchicalplanning algorithm is based on the idea that a detailed analysisof the robot state is only necessary in rough regions and thata good final path avoids areas of high risk. Therefore, weperform a preliminary path search to find a low-risk path andto identify the path segments through rough terrain which werefine in a second detailed motion planning step.

The preliminary plan determines a path preferring lower riskareas and avoiding high risk regions based on the roughnessquantification. Since in flat regions we do not need to considerthe complete robot state and can neglect the actuators, the taskbecomes a 2D planning problem. On the contrary, in roughregions the planning of the actuators is essential to increaserobot safety and ensure successful traversal. However, at thebeginning of the planning process we do not know throughwhich areas of the environment the path will lead and ifdetailed planning is really necessary. Therefore, we do notconsider the robot’s complete state in the first planning stageand use only the utmost operating limit of the robot, i.e. themaximal traversable height according to the robot model. Thisway we obtain the least restrictive limits. However, using agreater set of configurations generally improves the safety ofthe robot states as more configurations can compensate for agreater variety of situations.

Considering the operating limits of the robot, we build amotion graph (see Fig. 4a) which represents the ability of themobile robot to traverse the environment. Our cost functionmeasures the time required to traverse an edge in the graph. Todecrease the risk to the system and make the robot prefer lesshazardous parts of the environment, we reduce the velocityaccording to the roughness, i.e. increase the time needed topass those areas. Then we are able to perform a standardgraph search, e.g. A* search or Dijkstra search. The requiredamount of safety may change with different search and rescuemissions. The importance of the safety can be adjusted througha safety weight wp according to the mission parameters (seesection V-A for more information). Low safety weights leadto possibly shorter, but riskier paths; high values increasinglyforce the robot to avoid rough terrain. Please refer to [11] for amore detailed description of the metrics used by our algorithm.

While constructing the motion graph, we distinguish be-

tween areas of convenient risk levels (corresponding to mod-erate height differences) and regions with higher risks (cor-responding to challenging height differences). We will usethis basic distinction in the second planning phase to split upthe initial path into easy and hard segments and to determinewhether detailed planning of the robot motions is necessary.

Note, the preliminary path planning is efficient, since weneglect the robot state in favor of the operating limits and asimplified traversability assessment.

IV. DETAILED MOTION PLANNING

Rough terrain is more challenging and exposes the mobilerobot to a greater risk than flat environments. Therefore, werefine our initial path approximation by including the completerobot state to determine the system’s stability and traction. Thestate of a reconfigurable robot may consist of the 6D pose, thetranslational and rotational velocities v, ω and accelerationsv, ω plus the n actuator values ai:

(x, y, z, θ, ψ, φ, v, ω, v, ω, a1, . . ., an).

The controllable components are bold. They still lead to a largeintractable search space. Consequently, we use the preliminarypath to limit the state space for the second planning phase.First, we split the path into segments leading through flatareas with low risks (green) and segments through roughregions with high risk levels (purple) (Fig. 4b). Second, tofurther constrain the state space, we focus the refinement on atube-like area around the rough path segments (Fig. 4c). Thisconcentrates the search to the promising region and makes itfeasible.

Like in 2D navigation the robot’s stability can be assumedfor flat areas since any robot configuration may be appliedwith no or little risk. By avoiding unnecessary planning in ahigh dimensional space for the easily accessible parts of theenvironment we are able to speed up planning. However, roughregions require a detailed planning of the robot’s actuatorsand the validation of the stability to ensure safety and taskcompletion.

In the detailed motion planning step we build a graph of thediscrete state space involving the actuators. The cost functionis more accurate compared to the approximation of the firstplanning phase because it accounts for the environmental risk,the stability and traction, i.e. the robot’s safety, and for the

time consumed by translation, rotation and actuator move-ments. Moving the actuators needs time but does not resultin spatial gain. Therefore, any distance measure would be apoor indicator of the path length. A safety weight w allows toadjust the robot behavior and configurations; resulting in fasterpaths with less actuator movements or safer configurationsthrough better suited actuator positions. However, adjustingthe actuators to the current situation requires more time whichincreases the execution time of the path (see section V-B formore details). A detailed description of the cost function andthe different measures involved can be found in [11]. To finda path consisting of the complete robot states, we perform astandard graph search, e.g. A* search or Dijkstra search. Ifseveral rough path segments exist, the path planning can beparallelized.

Since the speed of the robot is very low when traversinghazardous areas, we set dynamics aside. We approximate thefootprint of the robot by the best fitting plane [13] to estimatethe stability and traction. This limits the current approach tomainly continuous environments (e.g. hills, stairs, ramps).

The restriction of the state space using the preliminary pathis necessary to make the second search in the large spacefeasible. Depending on the map size and planning query ourmethod would run out of memory before returning a valid path.Further, the focus on rough segments reduces the memoryconsumption as well and speeds up planning.

The detailed motion planning can fail to find a valid pathbecause the simplification of the preliminary planning discardsseveral aspects of the robot state, like the orientation and theactuator configuration. In this case, the preliminary planningmust be triggered to produce another path by adjusting thesafety weight (see section V-A for more information). Sinceits value depends on the objective of the search and rescuemission and the robotic system used, the adjustment shouldbe performed by the mission operator.

V. EXPERIMENTAL RESULTS

In this section, we present simulation experiments to illus-trate that the effect the two safety weights have on the planningresults. Next we implemented our method on a real robot tovalidate our approach in real-world scenarios.

In our experiments, we use the Telerob Telemax robotmodel. It is 60 cm long, 40 cm wide and weighs about 70 kg.It has 4 tracks which can be rotated 170◦ from entirely foldedall the way down lifting the robot about 45 cm up. Completelystretched the robot extends its length for about 60 cm toroughly 120 cm. The robot is skid-steered and its maximaltranslational speed is 1.2 m

s .

A. Preliminary Planning

The major direction of the preliminary path is affected bythe safety weight of the first planning phase as subsequentroute corrections are limited to the tubes around rough seg-ments. Higher safety weights increase the costs in riskier areas,thus, force the paths to avoid them. Keeping the start and goalpositions equal, we performed several planning queries varying

Fig. 5: Safety weight effect on the intial path. Five paths withdifferent safety weights wp ∈ {0.0, 0.25, 0.5, 0.75, 1.0}. Thehigher the value, the lighter the color of the path.

the safety weight wp from 0.0 to 1.0 in steps of 0.25 (see Fig.5). The lighter the color of the path, the higher the safetyweight. A value of wp = 0.0, i.e. considering no safety at all,leads to a straight line path within the motion graph. At 0.25the path does change slightly, crossing some of the high risk(red) areas directly with the gradient. Hence, decreasing thetime spend in these areas and increasing the safety by reducingthe system’s roll angle. At wp = 0.5 the path avoids the firsthigh risk region and crosses the second straight. With a valueof wp = 0.75 the path follows the orange dig in the hill’smiddle and circumvents the red areas. Finally, for wp = 1.0the path leads around the hill through the low risk (green)area.

The safety weight wp influences the direction of the pathsignificantly. Therefore, wp can adjust the route to suitedifferent missions. Similar to [5], this can also be used byoperators of teleoperated systems. Several routes with differentsafety weights provide valuable support for operators whohave to select an appropriate path for the mission.

B. Detailed Motion Planning

The safety weight of the second planning phase, however,mainly influences the choice of actuator configurations. Ahigher safety weight will force the robot to reach safer states ateach path position. To illustrate the effect of different safetyweights w of the second planning phase, we took an initialpath, kept it fix for all second phase queries and reducedthe tube to just include the initial path. Thus, we eliminateeffects of different routes (see Fig. 6). This also fixes thenumber of translations and rotations leaving only the actuatorconfigurations to be determined.

The curves of Fig. 6 show the accumulated safety (abstractvalue with no unit) of the entire paths plotted against the safetyweight and the execution time of the paths using differentsafety weights. As the number of actuator changes is themain reason for the increasing safety and execution time, wealso plotted them against the safety weight. As illustratedin Fig. 6, raising the safety weight causes the actuators toalter more often in order to reach better robot states in everysingle position. This in turn increases the total safety of thepath. Similar, the higher number of state corrections through

Fig. 6: Safety weight effect on the detailed motion planning. We fixed the initial path (left), leaving only the actuators mutable.The curves show the paths’ total safety (abstract value), the execution time of the paths and the number of actuator changesfor different safety weights.

Fig. 7: Real-world scenarios: Google Maps images and themaps built with a laser range finder of the first (top row) andsecond (bottom row) scenario. The sizes of the maps are 36.4×30.45m and 43.95× 32.95m, respectively.

actuator adjustments results in a higher execution time com-prising the time required for the adjustments. The executiontime almost triples from about 29 s at w = 0.0 to roughly83 s at w = 1.0. Although the actuator changes decreasefor higher safety weights, both the safety and the executiontime still rise. The rising safety weight is explained by thefact that a very stable actuator configuration is applied overseveral consecutive positions making changes unnecessary; therising execution time is due to the lower rotational velocities ofmore stable configurations with higher traction. For example,if the Telemax robot’s flippers are completely expanded, itwill be 120 cm long with maximal ground contact. In thisconfiguration rotation takes considerable longer than with allflippers folded.

Ultimately, the planning of the actuator positions is essentialto increase the safety of the robotic system during rough terraintraversal. However, this leads to a significant increase of thepath’s execution time.

C. Real-World Experiments

We also performed several tests with a real robot, theTelerob Telemax model, in different environments. We se-lected two runs to present here. The environments are shown inFig. 7. Both maps were recorded using a 3D laser range finder.Missing information was filled and the values were smoothedto facilitate planning. However, they do not contain any surfaceinformation, like friction parameters or granulation size. Thesizes of the maps are 36.4 × 30.45m and 43.95 × 32.95m,

Fig. 8: First real-world experiment: The top image shows theplan (green and purple) over the hill of rubble and the path(blue) taken by the robot. The other images show our robot indifferent situations during the traversal of the rubble.

respectively. These are quite large environments compared torelated works which usually focus on smaller patches of purelyrough terrain.

For the tests above we used the following setup: Theresolution of the maps was 0.05m and of the motion graphs0.3m (half the robot length) which renders validity tests atintermediate positions unnecessary. We considered 8 orienta-tions in each position (45◦ steps). The actuator values rangefrom −45◦ to 45◦ in steps of 15◦. Further, both front andboth rear actuators were required to be the same. The safetyweights were set to wp = 0.75 and w = 0.5.

In the first scenario the robot had to traverse a hill of rubble.Figure 8 shows the plan, the path taken by the robot andpictures of the execution by our robot. The robot had to crossthe hill of rubble through the low risk areas, avoiding the

Fig. 9: Second real-world experiment: The top image showsthe plan (green and purple) over our testing hill and the path(blue) taken by the robot. The other images show our robot indifferent situations during the traversal of the hill.

high elevations. We implemented a simple controller whichperformed three basic actions; adjust the actuators, rotate anddrive. The robot was able to follow the proposed path. Theplanned actuator configurations prevented tipping, whereasrotations were performed using configurations with lowertraction estimates. However, the small-grained material of therubble caused problems during rotation and caused the robotto slip occasionally.

The second scenario was a testing hill which had to becrossed (see Fig. 9). The path consisted of a steep ramp atthe beginning, a flat section on top of the hill and a ramp ofmedium inclination at the end. To climb the steep inclinationthe robot adopted the most stable actuator configuration withmaximal traction. The robot continued on top of the hillassuming the default configuration for flat ground. Finally, itadjusted the actuators anew to leave the hill down the finalramp. The localization of our system is solely based on GPScausing occasionally jumps or drifts. This and the inaccuracyof the map made it difficult for the controller to determineits position in the map and which part of the plan should beexecuted.

However, in general, the robot was able to execute all plansof our motion planner and successfully traversed the roughterrain while the proposed configurations increased the safetyof the robot. Problems during the execution were related toterrain (the small-grained rubble) or to inaccuracies in the

sensor data (the localization and the map).

VI. CONCLUSION AND FUTURE WORK

In this paper we introduced a rough terrain motion planningalgorithm for tracked robots with actively controlled actuators.Our method is a hierarchical roadmap planner which quicklydetermines an initial path using the robot’s operating limitsdisregarding the actuators. The initial path is used to constrainthe large state space of the detailed motion planning phasewhich then considers the actuators and the system’s stabilityand traction. Our algorithm does not rely on predefinedmotion sequences or on any terrain classification. Thus, itcan be applied to range of different unstructured environmentscommonly encountered in search and rescue missions.

Future work will focus on overcoming more challengingobstacles, like coarse debris, boxes or high steps. This willrequire a more detailed representation of the robot’s footprintand with a more sophisticated identification of the contactpoints. Also, we will investigate planning in continuous spaceusing optimal sampling-based methods.

REFERENCES

[1] Y. Choi, K. Jeong, J. Kang, Y. Seo, S. uk Lee, S. Jung, and S. Kim,“A remotely operated mobile robot with modular track mechanisms,” inIEEE Int. Conf. on Control, Automation and Systems, 2007.

[2] K. Ohno, S. Morimura, S. Tadokoro, E. Koyanagi, and T. Yoshida,“Semi-autonomous control system of rescue crawler robot having flip-pers for getting Over unknown-Steps,” in IEEE/RSJ Int. Conf. onIntelligent Robots and Systems, 2007.

[3] A. Birk and H. Kenn, “A control architecture for a rescue robot ensuringsafe semi-autonomous operation,” in RoboCup02: Soccer World Cup VI,ser. LNAI, G. Kaminka, P. Lima, and R. Rojas, Eds. Springer, 2002.

[4] A. Birk, S. Markov, I. Delchev, and K. Pathak, “Autonomous RescueOperations on the IUB Rugbot,” in IEEE International Workshop onSafety, Security, and Rescue Robotics (SSRR), 2006.

[5] E. Magid, T. Tusubouchi, E. Koyanagi, T. Yoshida, and S. Tadokoro,“Controlled Balance Losing in Random Step Environment for PathPlanning of a Teleoperated Crawler-Type Vehicle,” Journal of FieldRobotics, vol. 28, pp. 932–949, 2011.

[6] C. Dornhege and A. Kleiner, “Behavior maps for online planning ofobstacle negotiation and climbing on rough terrain,” in IEEE/RSJ Int.Conf. on Intelligent Robots and Systems, 2007.

[7] A. I. Mourikis, N. Trawny, S. I. Roumeliotis, D. M. Helmick, andL. Matthies, “Autonomous Stair Climbing for Tracked Vehicles,” Int.Journal of Robotics Research, vol. 26, no. 7, pp. 737–758, 2007.

[8] E. Mihankhah, A. Kalantari, E. Aboosaeedan, H. Taghirad, and S. A. A.Moosavian, “Autonomous Staircase Detection and Stair Climbing for aTracked Mobile Robot Using Fuzzy Controller,” in IEEE Int. Conf. onRobotics and Biomimetics, 2009.

[9] R. B. Rusu, A. Sundaresan, B. Morisset, K. Hauser, M. Agrawal, J.-C. Latombe, and M. Beetz, “Leaving Flatland: Efficient Real-TimeThree-Dimensional Perception and Motion Planning,” Journal of FieldRobotics, vol. 26, pp. 841–862, 2009.

[10] M. Brunner, B. Bruggemann, and D. Schulz, “Towards autonomouslytraversing complex obstacles with mobile robots with adjustable chas-sis,” in International Carpathian Control Conference, 2012.

[11] M. Brunner, B. Brueggemann, and D. Schulz, “Autonomously TraversingObstacles: Metrics for Path Planning of Reconfigurable Robots onRough Terrain,” in Int. Conf. on Informatics in Control, Automationand Robotics (ICINCO), 2012, Best Paper Award.

[12] M. Kalakrishnan, J. Buchli, P. Pastor, M. Mistry, and S. Schaal, “Learn-ing, Planning, and Control for Quadruped Locomotion over ChallengingTerrain,” Int. Journal of Robotics Research, vol. 30, pp. 236–258, 2010.

[13] E. Magid, K. Ozawa, T. Tsubouchi, E. Koyanagi, and T. Yoshida,“Rescue Robot Navigation: Static Stability Estimation in RandomStep Environment,” in Simulation, Modeling, and Programming forAutonomous Robots, ser. Lecture Notes in Computer Science. Springer,2008, vol. 5325, pp. 305–316.