Efficient Path Planning for Mobile Robotswith Adjustable Wheel Positions

Freya Fleckenstein Christian Dornhege Wolfram Burgard

Abstract— Efficient navigation planning for mobile robotsin complex environments is a challenging problem. In thispaper we consider the path planning problem for mobilerobots with adjustable relative wheel positions, which furtherincrease the navigation capabilities. In particular we accountfor changes of these relative wheel positions during planningtime, thus fully leveraging the capabilities of the robot. Whereasthese additional degrees of freedom increase flexibility, theyintroduce a more challenging planning problem. The approachproposed in this paper is built upon a search-based planner.We describe how to flexibly integrate joint angle changes inthe path planning process and furthermore propose a repre-sentation of the robot configuration that substantially reducesthe computational burden. In addition, we introduce searchguidance heuristics that are particularly useful in environmentsin which a robot is required to pass over obstacles, such ason agricultural fields. An extensive evaluation on simulatedand real-world data with our BoniRob agricultural robotdemonstrates the efficiency of our approach.

I. INTRODUCTION

Constructing feasible plans that enable robots to navigateautonomously in a safe and efficient manner has been a fieldof research in robotics for decades. We consider wheeledrobots with advanced capabilities that enable them to changetheir intrinsic configuration, such as the BoniRob systemdepicted in Fig. 1. The wheels of this robot can be rotatedindividually so that the robot can move in arbitrary directionsand orientations. In addition, the wheels are mounted onrotatable lever arms around a vertical axis (see Fig. 2). Asa result, the BoniRob is able to adjust its wheel positions topass over or along obstacles in confined areas. We focus oncases, where due to mechanical constraints, changes of theintrinsic configuration are not possible during navigation. Forexample, the BoniRob would incur damage due to high loadson the mechanism when the lever arms are rotated whiledriving. Robots with controllable intrinsic configurations ob-viously are more flexible and envisioned to have advantagesin in complex environments with many obstacles, in fieldswith plant rows or even in rescue scenarios. Incorporatingthese joints as part of the path planning process guaranteesthat all possible paths are found and therefore ensures thatthe full potential of the platform is used.

Adjustable joints introduce additional degrees of freedom(DoF) compared to robots that are only able to change theirposition and orientation. The higher dimensionality of theplanning space makes the problem harder as the numberof states is considerably increased. Common approaches are

All authors are with the University of Freiburg, Department of ComputerScience, Germany. This work has partially been supported by the EuropeanCommission under the grant number H2020-ICT-644227-FLOURISH.

Fig. 1: The BoniRob agricultural robot with high ground clearanceand wheels attached to adjustable arms, here collecting data on aleek field used in the evaluation. Each of the arms can be movedseparately around a vertical axis thereby changing the relative wheelpositions. Moving all arms to the sides of the robot leads to a largetrack width, enabling it to pass over extended obstacles. Moving allarms to the front and rear leads to a small track width, enabling itto get through narrow passages.

sampling-based or search-based planning. Many sampling-based approaches assume that different states can be con-nected directly. As we consider robots for which driving andchanging specific joint angles at the same time is undesirable,we cannot adopt this assumption, since only a subset ofthe state values is allowed to change in one motion. Also,customized cost functions are important. For example, unlessnecessary to reach a target, one wants to avoid the robotgoing backwards. A flexible search-based approach is wellsuited in this case.

In this paper we introduce a novel path planning approachthat is able to efficiently plan for robots with adjustablewheel positions. This is made possible by the followingcontributions. We achieve a significantly smaller search spacethrough a concise representation that effectively treats theadditional degrees of freedom introduced by the adjustablejoints as sets of configurations instead of individual config-urations. A key insight here is that these joints only enabledifferent motions, but do not move the robot itself. Thisallows us to summarize all configurations that enable thesame motion into a single state during search. Furthermore,we present heuristics particularly suitable for environmentsthat require the robot to pass over obstacles.

We show the effectiveness of our approach in exten-sive experiments in simulated and real-world environmentsevaluating the proposed improvements—namely an efficientstate space representation and informed search heuristics. Inparticular, we point out the necessity of considering the fullcapabilities of a robot in a demanding environment.

II. RELATED WORK

Robots with an adjustable intrinsic configuration are com-mon in the field of search and rescue robotics as theadditional DoF allow the robot to overcome obstacles. Theseare mostly utilized by tele-operation or reactively. Trackedrobots with so-called flippers adjust these based on currentsensor data to determine whether changing the intrinsic robotconfiguration is necessary [13]. This does not allow the robotto plan ahead, if a change in configuration is necessary beforea critical situation arises.

There are several sampling-based approaches that dealwith a high number of DoF by sampling a subset of theconfiguration space, such as for example rapidly-exploringrandom trees [10] and variants thereof [9, 6] or probabilisticroad maps [7]. Many sampling-based approaches assume thatany state is reachable from any other state in a continuousmanner, if an obstacle-free transition exists. This is not thecase for us, as the robot is not able to change its jointangles and drive at the same time. In addition, producinghigh-quality plans is important for navigation. It has recentlyreceived interest for sampling-based planning [3], but is wellunderstood for search-based planning.

Therefore we aim for a search-based planning approachthat is often used for path planning. The simplest method isto perform a 2d Dijkstra search on a grid [2]. State-of-the-art approaches are able to plan for more DoF, for exampleusing state lattices [14]. We also base our approach on thisconcept. Search-based planning allows for flexible solutions,e.g., footstep planning for four-legged robots [16], navigationplanning with 3d collision checks [5] or path planning forparking cars [11]. Besides navigation, search-based planningwith state lattices has also been applied to manipulationtasks [1].

The main question we consider in this paper is how toefficiently plan a path for a robot with adjustable joint anglesthat introduce additional DoF. One way to tackle this issueis by using adaptive dimensionalities for planning. Gochevet al. propose to plan for a set of configuration parametersthat are crucial to a resulting path [4] They compute a pathin this low-dimensional configuration space and then deducethe values of the remaining configuration parameters fromthis path. In our case, the joint angles of the arms as well asthe robot pose are relevant for the feasibility of a motion, sothat a lower-dimensional configuration space does not exist.Another idea in the context of adaptive dimensionalities is toadaptively choose for which part of the configuration spaceto plan in the current planning phase, which is determinedby the distance to the start or goal [11] or also includingobstacles [15, 8]. In our case, we might encounter obstacleson the way and thus cannot only focus on the start and goalareas. In addition, while navigating on a field, obstacles inthe form of crops might always be close, so that such anapproach would always choose the highest dimensionality.

An important technique for speeding up search-based plan-ning are informative heuristics. A commonly used heuristicis the Euclidean distance to the goal. This does not take

into account any constraints of the robot and thus oftengreatly underestimates the true cost. A better estimate isthe freespace heuristic [11] that pre-computes the minimalcost based on the possible motions for a robot under theassumption that there are no obstacles. Another heuristic thatis often used is the Dijkstra heuristic, induced by the Dijkstraalgorithm [2]. For example, Hornung et al. propose to run aDijkstra search on a map with obstacles inflated by the innercircle of the robot [5]. However, for a robot with high groundclearance, the center being located above an obstacle doesnot imply a collision, as it is able to pass over an obstacle.Our proposed Wheel Dijkstra heuristic addresses this issue.

III. PROBLEM STATEMENT

We plan for a ground vehicle navigating in a 3-dimensionalenvironment. First, we describe the configuration space of therobot and then the action model that forms valid transitionsin the configuration space. We represent the pose of therobot in the world by a rigid body motion relative to a fixedcoordinate frame in 3-dimensional space SE (3). This definesthe extrinsic configuration space of the robot.

We consider robots that are also able to change theirintrinsic configuration. In our case the robot can adjust itsrelative wheel positions by rotating its arms (see Fig. 2) andthereby can fit through passages or pass over obstacles. Weinclude such joints that are relevant for the feasibility of arobot motion in the configuration space and call the spacethat describes the controllable configurations of these jointsthe intrinsic configuration space of the robot CR. The fullconfiguration space is then given by C = SE (3)× CR.

We represent the environment by a traversability map. Thisis 2.5d grid map that contains elevation and traversabilityinformation. The valid configuration space Cf ⊆ C containsall configurations c ∈ C that are safe for the robot accordingto this map, i. e., the robot is collision-free and stable. Aswe consider a ground vehicle, the height, roll and pitch inthe robot pose cannot be actively controlled. We thus definethe actively controllable configuration space Ca = SE (2)×CR. Each element in the actively controllable configurationspace Ca is induced by an element in the valid configurationspace Cf . Note that a change in the intrinsic configurationof the robot can cause a change in the robot pose in SE (3).For example, when the robot turns an arm onto a bump theattitude and elevation of the robot changes.

We study cases where simultaneous driving and changingof joint angles is undesirable or physically impossible. Forexample, changing the arm angles of the BoniRob whiledriving can lead to serious damage in the control mechanism.Therefore there are two kinds of valid motions M thatrepresent transitions in Ca. Changes in SE (2) correspondto driving, thus we call them drive motions. Changes in CRcorrespond to changes in the arm joint angles that we callarm motions. A motion is valid, if it does not require a wheelto cross an untraversable cell, and the ground clearance ofthe robot allows its body and arms to pass over any obstacles.A goal configuration cg ∈ Ca is reachable from a startconfiguration cs ∈ Ca, if there exists a cohesive sequence

ϕ2 ϕ1

ϕ3 ϕ4

ϕ2 ϕ1

ϕ3 ϕ4

ϕ2 ϕ1

ϕ3 ϕ4

Fig. 2: Schematic of the BoniRob showing its body and the fourarms in different joint configurations in a top-down view.

π = (mi)i∈1,...,k of valid motions mi that starts at cs andends at cg .

We define a cost function u :M→ R+,m 7→ u(m) thatmodels the time it takes to execute motion m. Additionally,the cost function models user preferences, e. g., we prefer therobot to drive forwards and not backwards. For a sequenceπ of motions, we define the cost as cost(π) =

∑ki=1 u (mi).

We then specify our planning problem as follows. Given astart configuration cs ∈ Cf and a goal configuration cg ∈ Cf ,determine the induced configurations cs2D , cg2D ∈ Ca. Finda sequence of valid transitions π = (m1,m2, . . . ,mk) withminimal cost (π) that starts at cs2D and ends at cg2D .

IV. PLANNING WITH ADJUSTABLE WHEEL POSITIONS

In this section, we present our approach for path planningwith adjustable relative wheel positions. This is a challengingproblem as the additional DoF need to be accounted forduring planning to be able to find plans that require thesejoints for completeness. We aim for a general and flexibleformulation that allows us to model robot-specific motionsand costs. Therefore, we follow a search-based planningapproach that easily optimizes for custom cost functions, andallows us to express drive and arm motions separately.

We first illustrate, how we represent a search space in astate lattice [14] that discretizes states and constructs validmotions that connect these states. They form a graph that issearched with any search algorithm such as A*. We build onformulations for state lattices in SE (2) and then present ourextensions to model planning with adjustable wheel positionsin state lattices in the controllable configuration space Ca.Finally, we introduce search guidance heuristics that tacklethe shortcomings of common path planning heuristics.

A. State Lattice Planning in SE (2)

Common approaches for state lattice planning in SE (2)discretize the configuration space, i. e., SE (2). The result-ing states are connected with so-called motion primitivesand hereby define the search space for planning. Motionprimitives represent drive motions that the robot is ableto execute and are constructed in a way that the repeatedapplication of motion primitives covers the complete space.The search space is incrementally built during planning.Motion primitives are only applied to a state if they representa valid motion in Cf . The motion primitives for our robotare shown in Fig. 3. A search in this space results in a planπ ∈ SE (2) consisting of motion primitives.

B. State Lattice Planning with Adjustable Wheel Positions

A state lattice in SE (2) accounts for the pose of a robotin the environment, but arm motions changing joint angles

Fig. 3: Motion primitives for our robot shown on a grid. The currentpose is highlighted in blue. Possible drive motions are indicatedby red lines. The outcomes are shown as numbered robot poses(illustrated by circles for the position and a line for the orientation).

are not considered. To represent the abilities of the robotand enable finding paths even in complex environments, weextend the SE (2) state lattice to include joint angles in CR.We model arm motions and drive motions separately. Fordrive motions we reuse the motion primitives as defined fora state lattice in SE (2). The state space representation alsodiscretizes SE (2) in the same way. In addition, we includethe intrinsic robot configuration CR in the state lattice andrepresent configuration changes of the robot by arm motions.

Here, the first step is to integrate the joint angles intothe state lattice to get the valid controllable configurationspace Ca = SE (2) × CR. This leads to n = dim (CR)additional dimensions in the configuration space. A straight-forward way to represent the intrinsic configuration of arobot is to store a single angle per joint. The intrinsic robotconfiguration is then given by a vector (ϕ1, . . . , ϕn)

T .The second step is to represent the robot motions that

change joint angles. One of these arm motions sets a singlejoint angle to a different configuration value. Like for motionprimitives in SE (2), we also test if an arm angle changeleads to a configuration in Cf . Otherwise the motion is notapplicable. These motions only affect entries in CR. This isin accordance with our condition that joint angles cannotbe changed while driving. This representation increases thedimensionality of the search space by dim (CR).

We therefore introduce a representation that instead ofsingle joint angles uses intervals of joint angles, thus rep-resenting sets of configurations in a single state in the statelattice together with the pose of the robot. An intrinsic robotconfiguration representation then contains an interval Ii =[ϕmini , ϕmax

i

]for each joint and is given by (I1, . . . , In)

T . Astate in this representation is valid, if and only if all jointconfigurations it represents are valid at the given pose, i. e.,all combinations of angles in the intervals are valid.

To understand why this works, consider that changes inCR do not cause major changes in the pose of the robot.Additionally, in many situations there are multiple joint angleconfigurations that allow a certain drive motion to be applied.Therefore, it is not necessary to know the exact values in CRas long as they lead to valid motions.

However, with a representation over sets of possible con-figurations, it is necessary to adapt the notion of applyinga motion to retain soundness. There are two reasons forthe angle interval of a specific joint to change. While armmotions increase intervals, drive motions potentially decreasethe represented valid intrinsic configurations, as illustrated inFig. 4. Arm motions expand the joint angle intervals to bothsides by a fixed increment, if the corresponding arm rotation

arm angles

move,

frontchange move

Fig. 4: Changing arm angle intervals due to arm motions andobstacles when moving. The intervals of valid arm angles are shownin blue. Obstacles are shown in gray. The robot starts with theintervals in the leftmost state. It first moves in freespace and changesthe angles of its front arms, thus their intervals increase. Whendriving further towards obstacles the intervals decrease as part ofthem becomes invalid.

move move

Fig. 5: An environment where splitting intervals happens. Part ofthe intervals become invalid as the wheels at the end of the armsof the robot would collide with the obstacle. The shown intervalparts are valid even when the arms extend over the obstacle as theirground clearance is high enough.

is valid. Applying such a motion thus generates only onesuccessor state representing all combinations of joint anglesthat are possible results of the applied motion.

Joint angle intervals are also updated when applyinga drive motion. This is the second reason that intervalscan change. If a drive motion is not valid for part of aninterval, we remove this part in the resulting state. Thisstate has to represent the resulting set of configurations.The example in Fig. 5 shows that in some scenarios thisleads to the interval being split. As our approach supportsonly single intervals, it cannot represent all possible resultingconfigurations in a single state. Therefore, in such a case, wecreate multiple successor states. A new successor is createdfor the combination of all resulting interval parts for eacharm, e.g., in the example in Fig. 5, we get four new states,for the angles of two arms being split in two intervals each.Note that the extreme case of splitting each interval in allindividual discretized parts is equivalent to the representationwith single arm angles. This is unlikely to happen unless theenvironment is highly complex. Therefore, in most cases,using the single arm angle representation produces a largenumber of successor states that the interval representationsummarizes into a single state.

C. Heuristics

While the previous section addressed a reduction of thesearch space to speed up planning, here we aim to guidethe search with informative heuristics. Therefore, we presentheuristics well suited for robots with high ground clearance,where contact with the ground only occurs with small partsof the robot. In particular we aim for admissible heuristicsthat underestimate the true cost as these allow the planner tofind optimal paths. The commonly used Euclidean distance isnot suited for our target applications as it neither considersthe cost of the motion primitives nor the structure of theenvironment. Instead, we use the freespace heuristic [11].

Fig. 6: Example of the Wheel Dijkstra heuristic for the rear wheelsof the BoniRob in an 8-neighborhood. Untraversable cells aremarked in red. Note that considering only a single point of therobot, e.g., one wheel can greatly underestimate the actual cost.

It precomputes minimal costs in a freespace environment,while considering the actual cost of motion primitives.

The freespace heuristic is a better estimate than theEuclidean distance, but it also does not consider the en-vironment. In environments with many obstacles and littleopen space, it greatly underestimates the true cost. In suchenvironments, it is imperative to take the structure intoaccount. A heuristic that does this is the Dijkstra heuristicthat computes shortest paths from the robot center to allcells in a 2d grid map of the environment and thus gives anestimate of an obstacle-free path that the robot could take.However, it disregards the orientation of the robot and, in ourcase, intrinsic configuration changes. For robots with highground clearance, a Dijkstra heuristic on the robot center isnot informative, as the robot center is not on the ground.

Instead, we compute a Dijkstra heuristic for the wheels(see Fig. 6) that naturally have ground contact and thereforerequire traversable locations. For each wheel, given a plan-ning query, we compute the cell in the traversability map thatit occupies in the goal configuration. Then, once per planningquery, we perform a single source shortest path search on thetraversability map with Dijkstra’s algorithm from that celland store the costs. During planning, the heuristic value fora specific wheel is then determined by a fast look-up.

In comparison to a search in the full controllable configu-ration space of the robot, searching for paths in the 2d spaceof the traversability map does not require much time, evenwhen performing this search for all wheels. Fig. 6 illustratesthe importance of considering all wheels for more realisticestimates, as distances traveled by single wheels to theirrespective goal positions can differ greatly. The results areinformative, as the search avoids collisions of the wheelsand represents the minimal distance that each wheel has totravel. We acquire a cost estimate for the complete robot bymaximizing over all wheel distances and call this heuristicthe Wheel Dijkstra heuristic. We also combine the WheelDijkstra heuristic with the freespace heuristic by maximizing.

V. EXPERIMENTS

In order to show the performance of our planner for robotswith adjustable relative wheel positions, we ran extensiveexperiments on simulated and real-world data. We first in-vestigate the influence of our proposed interval representationfor the intrinsic robot configuration. Afterwards we present acomparison of the planner performance with various searchguidance heuristics. As an evaluation platform, we modelthe BoniRob with its four rotatable arms. Each arm is

constrained to an angle between 0◦ and 90◦. The intrinsicconfiguration space is thus given by CR = C4l where Cl =[0◦, 90◦] represents the configuration of a single arm.

A freespace environment without obstacles serves as thebaseline for our experiments. In addition, we created twosimulation environments shown in Fig. 7. The field environ-ment represents an agricultural field with plant rows. Therocks environment is cluttered with tight passages that therobot has to circumvent or rocks that it has to pass over tonavigate. It is made intentionally hard to drive the plannerand different representations to their limits. Here, adjustingthe wheel positions is likely to be required for finding a path.Furthermore, we evaluate on the 3d map of a leek field builtfrom real-world data that is shown in Fig. 8 (see Fig. 1 for animpression of the field). This allows us to compare planningon real-world data to the simulation environments.

In all environments, we uniformly sampled 25 poses andconstructed planning queries between each pair of poses.In the leek environment, we manually aligned pose orien-tations with the field to generate realistic robot poses. Inthe freespace and field environment, we ran the planner onqueries where poses were more than 0.5m and less than 20mapart and chose a timeout of 30 s. As planning is harder inthe rocks environment, we ran queries for poses with at least0.5m and at most 10m distance and increased the timeoutto 3min. In the leek environment, we allowed for a distancebetween 0.5m and 20m and used a timeout of 90 s. Ourimplementation uses the search-based planning library withthe Anytime Repairing A* as a search algorithm [11, 12].We set the initial weighting factor to ε = 5. The experimentswere run on an Intel i7 4 GHz machine.

We are particularly interested in the time it took theplanner to find a first plan, as this allows a robot to start nav-igating. We plot a cumulative histogram of solved planningqueries over time, where, for example, an entry of 50% at10 s means that after 10 s 50% of all queries returned a plan.Ideally a graph here rises up quickly as this indicates queriesbeing solved fast and reaches a high percentage of solvedqueries at the timeout. We also investigate the sub-optimality,to determine if the plans are reasonable. The sub-optimalityvalue γ for a plan guarantees that it has at most γ times thecost of the optimal plan [12]. To compare algorithms we plotthe percentage of queries that resulted in a plan with sub-optimality of γ or better after a third of the timeout. Ideallya large percentage of plans has a small γ.

A. Evaluation of Configuration Representations

In this section, we analyze the efficiency gain of usingjoint angle intervals instead of single joint angles for therepresentation of the intrinsic configuration of the robot.In addition, we compare with a representation that doesnot allow to change joint angles at all. This serves as abaseline algorithm as its search space is notably smaller,but—although incomplete—should still be able to find plansin most cases. Since preliminary experiments showed that acombination of the Euclidean, freespace and Wheel Dijkstraheuristic leads to the best performance, we used this. We

Fig. 7: The environments for our experiments are both 30m×30m.Untraversable cells are marked in red, traversable cells in blue. Left:The field environment representing a field with plant rows, whichare low enough for the robot to pass over them. Right: The rocksenvironment, where changing arm angles multiple times in a singleplan can be necessary.

Fig. 8: The real-world data recorded on a leek field that isrepresented in the leek environment (top). The bottom shows thecorresponding traversability map that is automatically generatedfrom this data.

ran the planner on the queries of all environments in threedifferent configuration space representations: Once withoutchanging the arm angles during the search (fixed), thus rep-resenting a configuration space of SE (2), once representingthe intrinsic configuration with angle intervals (intervals) andonce with individual arm angles (single angles).

Freespace: We evaluate in the freespace environmentas a baseline to determine the overhead resulting fromplanning with joint angles. Fig. 9(a) shows that the intervalsrepresentation leads to a similar performance as the fixedrepresentation, while single angles is slightly worse. Inthis simple environment, we observe no overhead with ourproposed intervals representation despite the four additionalDoF resulting from the adjustable relative wheel positions.Almost all queries also reach a sub-optimality of 2 or better.

Field: This represents a crop field with plant rows asa typical environment for an agricultural robot. Fig. 9(b)illustrates that planning with intervals is notably more ef-ficient than using single angles. At the timeout, the planneremploying the interval representation found paths for about95% of the planning queries, while planning with single an-gles resulted in only 65% of solved planning queries. Againwe observe that planning in the more expressive intervalsrepresentation is just as efficient as planning without jointangles (fixed). The sub-optimality for intervals is also onpar with the fixed representation at about 3 for more than70% of the queries, where at the evaluation point of a thirdof the timeout 80% of the queries were solved. Again thereis a notable difference to single angles.

Leek: We validate our results from simulated environ-ments in the leek environment based on real-world data.Fig. 9(c) shows a similar performance as in the field envi-ronment, i. e., planning with intervals is more efficient thanplanning with single angles and in this case even slightly

020

4060

8010

0

0 2 4 6 8 10perc

enta

ge o

f sol

ved

quer

ies

time in s

fixedintervalssingle angles

020

4060

8010

0

0 5 10 15 20 25 30perc

enta

ge o

f sol

ved

quer

ies

time in s

fixedintervalssingle angles

020

4060

8010

0

0 20 40 60 80perc

enta

ge o

f sol

ved

quer

ies

time in s

fixedintervalssingle angles

020

4060

8010

0

0 50 100 150perc

enta

ge o

f sol

ved

quer

ies

time in s

fixedintervalssingle angles

020

4060

8010

0

5 4 3 2 1

perc

enta

ge o

f que

ries

suboptimality

fixedintervalssingle angles

020

4060

8010

0

5 4 3 2 1

perc

enta

ge o

f que

ries

suboptimality

fixedintervalssingle angles

020

4060

8010

0

5 4 3 2 1

perc

enta

ge o

f que

ries

suboptimality

fixedintervalssingle angles

020

4060

8010

0

5 4 3 2 1

perc

enta

ge o

f que

ries

suboptimality

fixedintervalssingle angles

(a) freespace (b) field (c) leek (d) rocks

Fig. 9: Performance of the planner using different configuration space representations in the test environments. Plotted are cumulativehistograms of the number of solved planning queries over the time until the first plan was found in the top row and the percentage ofqueries resulting in a certain sub-optimality in the bottom row.

better than planning with fixed angles. Sub-optimality hereis similar for all representations as the environment is morenarrow and thus any found plan is already close to optimal.As the simulated field environment has similar properties asthe leek environment, this confirms the real-world applica-bility of our configuration representation.

Rocks: The rocks environment provides a hard testcase enforcing difficult situations. Changing arm angles is acommon requirement for finding plans in this environment.Fig. 9(d) demonstrates that planning with fixed arm anglesis not sufficient any more due to its incompleteness. Inparticular we see that after about 50 s no new plans arefound. Both the single angles and the intervals representationcontinue to find plans and at the timeout solved about twiceas many queries. The sub-optimality is 1 for fixed armangles for the queries that had solutions. These are mainlythe simpler queries that did not require arm reconfigurationactions. The cost for the arm motions is large for ourrobot in comparison to drive motions, so that the plannerprevents these from being executed. Nevertheless there were1.2 reconfiguration actions on average for single angles andintervals. This clearly shows that planning with arm anglesis necessary in constrained environments.

In addition here we also investigated, how many dis-cretized configurations are represented in a single search stateof the intervals representation. On average there were 25.2configurations captured in the intervals of that representationthat would be individual search states in the single an-gles representation. This explains, why in the environments,where arm motions are for most queries not necessary,the intervals representation shows a performance similar toplanning without arm angle changes.

B. Evaluation of Heuristics

To compare the performance of the heuristics, we ran ourplanner in all environments and evaluated different heuristics.We chose the intervals configuration representation. Wetested the planner with the Euclidean distance heuristicserving as the baseline and compared with the combination

020

4060

8010

0

0 5 10 15 20 25 30perc

enta

ge o

f sol

ved

quer

ies

time in s

EuclideanFreespace

Wheel DijkstraCombined WD + F 0

2040

6080

100

0 5 10 15 20 25 30perc

enta

ge o

f sol

ved

quer

ies

time in s

EuclideanFreespace

Wheel DijkstraCombined WD + F

(a) freespace (b) field0

2040

6080

100

0 20 40 60 80perc

enta

ge o

f sol

ved

quer

ies

time in s

EuclideanFreespace

Wheel DijkstraCombined WD + F 020

4060

8010

0

0 50 100 150perc

enta

ge o

f sol

ved

quer

ies

time in s

EuclideanFreespace

Wheel DijkstraCombined WD + F

(c) leek (d) rocks

Fig. 10: Performance of the planner using different heuristics.Plotted are cumulative histograms of the number of solved planningqueries over the time until the first plan was found.

of the Euclidean distance and the freespace heuristic, theWheel Dijkstra heuristic, or the combined Wheel Dijkstraand freespace heuristic (denoted as Combined WD + F). Wefocus the discussion on the percentage of solved queries asthe performance differences with regard to the sub-optimalitybetween algorithms were similar to the solved queries.

Freespace: The freespace environment provides a mea-sure for how severely heuristics underestimate the true costin an empty environment. The freespace heuristic is theoptimal heuristic for planning with a state lattice planner inthis environment. It is thus not surprising that in Fig. 10(a)the freespace heuristic and combinations with it performnotably better than others. Planning with the Wheel Dijkstraheuristic or only the Euclidean heuristic is slower as theyboth underestimate the true cost of drive motions resultingfrom orientation changes and user preferences.

Field: As we see in Fig. 10(b), initially (up to 5 s)the performance of the freespace heuristic is on par withthe Combined WD + F heuristic. These are easier planningqueries that require driving either along the side of the

field, i. e., mostly in freespace, or along a row of thefield, i. e., where obstacles do not influence the preferredpath. Larger times indicate more complex queries that, forexample, require changing rows. Here, the Wheel Dijkstraheuristic performs best, as it is designed for scenarios likethis. The Combined WD + F heuristic here covers bothscenarios performing similar to the freespace heuristic forshorter queries and similar to the Wheel Dijkstra heuristicfor more complex ones. Without the Wheel Dijkstra heuristiconly about 80% of planning queries were solved. The WheelDijkstra heuristic (also in combination with the freespaceheuristic) was able to solve about 95% of planning queries.

Leek: In the leek environment based on real-worlddata, a row of crops is not one continuous obstacle, butcontains gaps between crops that allow a wheel to passthrough, although the complete robot cannot. This explainswhy in Fig. 10(c) we observe that the performance of theWheel Dijkstra heuristic alone is not as good as in the fieldenvironment. The freespace heuristic leads to a notably betterperformance. A reason for this is that rows were quite narrowand when the robot was driving it was constrained by therow to straight-line movements that are modeled well bythe freespace heuristic. Nevertheless, the combination thereofwith the Wheel Dijkstra heuristic still performed similarly.

Rocks: In this environment evaluated in Fig. 10(d) thereare no major differences between heuristics. The CombinedWD + F heuristic leads to slightly more solved planningqueries at the timeout, followed by the Wheel Dijkstra,freespace and the Euclidean heuristic. Here the path plannerhad to plan complex maneuvers that are not modeled wellby any heuristic.

VI. CONCLUSION

We presented an approach for path planning with ad-justable relative wheel positions. Our planner efficientlyhandles the increased number of degrees of freedom re-sulting from planning for the additional joint angles. Inorder to achieve this, we introduced a novel configurationrepresentation for changeable joint angles. Furthermore, weintroduced search guidance heuristics that are particularlyuseful in complex environments with many obstacles. Inextensive experiments, we demonstrated that the proposedconfiguration representation by intervals allows us to plan in-cluding joint angles from the intrinsic configuration withouta notable decrease in performance compared to planning inSE (2). Note that planning only in SE (2) is incomplete. Thisis also shown experimentally in constrained environments,where taking joint angles into account during planning isnecessary to find paths. The evaluation of the heuristicsindicated different behavior of heuristics depending on theenvironment. However, the combined Wheel Dijkstra andfreespace heuristic performs at least as well as other testedheuristics in all environments and situations.

REFERENCES

[1] B. J. Cohen, S. Chitta, and M. Likhachev. Search-basedplanning for manipulation with motion primitives. InInt. Conf. on Robotics & Automation (ICRA), 2010.

[2] E. W. Dijkstra. A note on two problems in connexionwith graphs. Numerische Mathematik, 1(1):269–271,1959.

[3] J. D. Gammell, S. S. Srinivasa, and T. D. Barfoot.Batch informed trees (bit*): Sampling-based optimalplanning via the heuristically guided search of implicitrandom geometric graphs. In Int. Conf. on Robotics &Automation (ICRA), 2015.

[4] K. Gochev, B. J. Cohen, J. Butzke, A. Safonova, andM. Likhachev. Path Planning with Adaptive Dimen-sionality. In Int. Symposium on Combinatorial Search(SoCS), 2011.

[5] A. Hornung, M. Phillips, E. G. Jones, M. Bennewitz,M. Likhachev, and S. Chitta. Navigation in three-dimensional cluttered environments for mobile manipu-lation. In Int. Conf. on Robotics & Automation (ICRA),2012.

[6] S. Karaman and E. Frazzoli. Incremental sampling-based algorithms for optimal motion planning. InRobotics: Science and Systems (RSS), 2010.

[7] L. E. Kavraki, P. Švestka, J.-C. Latombe, and M. H.Overmars. Probabilistic roadmaps for path planning inhigh-dimensional configuration spaces. Transactions onRobotics and Automation, 12(4):566–580, 1996.

[8] D. Kim, Y. Choi, T. Park, J. Y. Lee, and C. Han.Efficient path planning for high-dof articulated robotswith adaptive dimensionality. In Int. Conf. on Robotics& Automation (ICRA), 2015.

[9] J. J. Kuffner and S. M. Lavalle. RRT-Connect: Anefficient approach to single-query path planning. InInt. Conf. on Robotics & Automation (ICRA), 2000.

[10] S. M. Lavalle. Rapidly-Exploring Random Trees: ANew Tool for Path Planning. Technical report, 1998.

[11] M. Likhachev and D. Ferguson. Planning long dy-namically feasible maneuvers for autonomous vehicles.Int. J. of Robotics Research, 28(8):933–945, 2009.

[12] M. Likhachev, G. J. Gordon, and S. Thrun. ARA*:Anytime A* with provable bounds on sub-optimality.In Proc. of the Conf. on Neural Information ProcessingSystems (NIPS), 2003.

[13] K. Ohno, S. Morimura, S. Tadokoro, E. Koyanagi, andT. Yoshida. Semi-autonomous control system of rescuecrawler robot having flippers for getting over unknown-steps. In Int. Conf. on Intelligent Robots and Systems(IROS), 2007.

[14] M. Pivtoraiko and A. Kelly. Efficient constrained pathplanning via search in state lattices. In Int. Symposiumon Artificial Intelligence, Robotics, and Automation inSpace (i-SAIRAS), 2005.

[15] N. Vahrenkamp, C. Scheurer, T. Asfour, J. J. Kuffner,and R. Dillmann. Adaptive motion planning for hu-manoid robots. In Int. Conf. on Intelligent Robots andSystems (IROS), 2008.

[16] M. Zucker, J. Andrew, B. Christopher, G. Atkeson, andJ. Kuffner. An optimization approach to rough terrainlocomotion. In Int. Conf. on Robotics & Automation(ICRA), 2010.