Combined Sampling and Optimization Based Planning for Legged-Wheeled Robots

Planning for legged-wheeled machines is typically done using trajectory optimization because of many degrees of freedom, thus rendering legged-wheeled planners prone to falling prey to bad local minima. We present a combined sampling and optimization-based planning approach that can cope with challenging terrain. The sampling-based stage computes whole-body configurations and contact schedule, which speeds up the optimization convergence. The optimization-based stage ensures that all the system constraints, such as non-holonomic rolling constraints, are satisfied. The evaluations show the importance of good initial guesses for optimization. Furthermore, they suggest that terrain/collision (avoidance) constraints are more challenging than the robot model's constraints. Lastly, we extend the optimization to handle general terrain representations in the form of elevation maps.


I. INTRODUCTION
Equipping robots with legs enables mobility over unstructured terrains, the same as the animals can traverse them. On the other hand, wheeled robots are not as mobile as their legged counterparts, but they are far more energyefficient. By combining legs and wheels into a hybrid system, roboticists have tried to keep the best characteristics of legged and wheeled systems [1], [2], [3]. Unfortunately, increased flexibility comes with an increase in complexity, especially true for motion planning since the combinatorial aspect of legged robots (contact schedule) is combined with wheeled robots' non-holonomic nature (rolling constraints). A powerful tool used to cope with the increased complexity is Trajectory Optimization (TO). TO has been shown to work well on hybrid systems with many Degrees of Freedoms (DoFs) ( [1], [4], [5]). However, optimization is prone to local minima. While some control over solution quality can be achieved through cost function shaping (e.g., [6], [7]), the optimization can ultimately fail due to the non-convexity of the planning problem. Failures happen especially often in challenging terrain where collision (avoidance) constraints become particularly difficult for optimizers. In this letter, we overcome this problem by providing good initializations (initialization step) for the optimization-based planner (refinement step); i.e we combine Sampling Based Planning (SBP) with TO. Our initialization step computes base poses, joint positions, and contact schedule, which are all then fed to the optimization. The gain is that our method can handle systems with many DoF while still being able to cope with challenging terrain.

A. Related Work
Early research on locomotion for hybrid systems treats the whole system as a driving robot and uses legs as an active suspension. Such a practice is widespread in the aerospace community [8], [9] and has more recently been applied to wheeled quadrupeds [10]. The main disadvantage of such an approach is that it does not fully leverage the hybrid system's legged nature, i.e., the robot cannot negotiate obstacles.
To simplify the combinatorial nature of contact schedule planning for legged robots, researchers often use cyclic gaits to restrict the solution space size. Planning with cyclic gaits has been widely used for quadrupedal robots with point feet ( [11], [12], [13], [14]) and more recently, it has been applied to hybrid robots as well ( [1], [15], [5]). Presented controllers run under the flat ground assumption in an Model Predictive Control (MPC) fashion with a prediction horizon of about 2 s. Such a strategy relies on reactive control behavior from MPC, and while it can traverse small irregularities in the terrain, large obstacles still pose an issue. Furthermore, these approaches are not suitable for temporally global plans because of the relatively short prediction horizon.
More recently, terrain-aware planning has been proposed for hybrid robots ( [7], [2]), which demonstrate the ability to traverse challenging terrain and plan motions in a wholebody fashion. However, the presented methods solely rely on trajectory optimization and often fall prey to the local minima. Furthermore, the environment is known excatly, and it remains unclear how the planner would handle maps generated from real sensory data since discretization and noise can lead to discontinuous gradients in the optimization.
Unlike the optimization, SBP cope well with the nonconvex environment. Attempts to use SBPs can be found in [16], [17], where the proposed approach samples base poses in SE(3) space and computes a guiding path for the base of the robot. The footholds are computed in the next stage using the guiding path. However, the proposed approach does not use any optimization, which would make the planning for robots with non-holonomic constraints difficult. Unlike [16], our approach does not accept a guiding path before having computed the footholds.
Recently, [18] has introduced a Contact Dynamic Roadmap (CDRM) data structure for rapid collision checking and foothold generation at runtime. The crux of the approach is computing a Probabilistic Roadmap (PRM) of collisionfree configurations offline and then using it for planning online. A similar idea is employed for self-collision avoidance during the initialization phase in our work. We extend the PRM with additional data, so that the approach is applicable to robots with heavy limbs such as walking excavators.
Finally, Klamt et al. [3], [19] use a graph-search based approach to plan hybrid motions for the Momaro robot. The robot decides when to drive and when to step based on a carefully crafted cost function. The transition sequences between driving and stepping are not computed in a wholebody fashion (as they are handcrafted). Lastly, the graphsearch algorithm choice is motivated by the fact that the robot can turn in place using its wheels only. Hence, the approach is unsuitable for robots with a minimal turning radius greater than zero in its current form.

B. Contribution
We present a combined sampling and optimization based planner for legged-wheeled machines with many DoFs. We use terrain representation to generate a wide variety of locomotion behaviors for navigating complex terrain. The planner is divided into two stages: Initialization Step based on a sampling-based planner and Refinement Step through nonlinear optimization. These two stages produce kinematically feasible and statically stable plans, motivated by our use case on a walking excavator [20]. Given the base's initial and goal pose, our formulation computes base trajectory in SE(3), joint trajectories, and contact schedule. To the best of the author's knowledge, existing approaches provide only high-level waypoints (e.g., [21], [22], [19]). Finally, our approach is the first (to the best of our knowledge) to

II. PROBLEM STATEMENT
A legged-wheeled robot comprises N limbs with wheels and M limbs without wheels, e.g., a walking excavator has four limbs with wheels, and one non wheeled limb (see [20]). The base of the robot can move in SE(3) space. Robot's knowledge about the environment is contained in a map, which is a mapping h : R 2 → R that maps coordinates to various functions describing the environment (e.g., height, traversability). In this letter, a multilayered grid map [23] data structure is used. Robot's i th limb is in contact with the environment if it is close enough to the surface, i.e.
where p z denotes z component of the contact position p and h(p x , p y ) is height at the contact position. We assume that the contact point is below the center of the wheel (negative gravity direction). For a 30°slope (which is the limit beyond which we consider terrain untraversable), the approximation error is about 15 % of the wheel radius, which is well within the adaptation capabilities of the tracking controller, as demonstrated in [24]. The environment is divided into traversable part denoted with T and untraversable part denoted with ¬T . A contact is valid if the contact point lies in the traversable set T , i.e.
where sdf (·, ·) represents a 2D Signed Distance Function (SDF) with the positive distance meaning the point lies in T . We require all the contact points to stay at least δ away from the ¬T . The 2D SDF is stored as a grid map layer and calculated using marching parabolas [25]. Note that, unlike the limbs, robot's base is not required to stay in T since the base is not in contact with the terrain. Our goal is to find a trajectory τ with a length of T seconds such that p B (t = 0) = p B,start and p B (t = T ) = p B,goal where p B,start , p B,goal are given starting and goal position for the base of the robot. Apart from the base, we do not enforce any other constraints on robot's pose or joint angles although this is not a hard requirement of our approach.

III. INITIALIZATION STEP
The backbone of the initialization step is a sampling-based planner that samples base poses. We use Rapidly-Exploring Random Trees (RRTs) [26], [27] although the problem formulation also permits the use of multi-query planners such as PRMs [28]. Results presented are mostly generated using and RRT# [29], whose implementation is taken from [30]. In addition to RRT, a PRM of limb end-effector positions is precomputed for efficient online planning.

A. Offline Computation
We use the CDRM data structure introduced in [18]. CDRM can be used at runtime to generate collision-free movements of the robot's limbs. In our case, CDRM helps us avoid collision between arm and legs while using the arm as a supporting limb. For each limb, the mapping between joint angles q i and end-effector position B p i is stored. This mapping does not change over time since B p i is expressed in the base frame of the robot. Furthermore, we store the mapping between i th limb's Center of Mass (CoM) p i,com and joint configuration for the same limb. This allows us to evaluate the stability criterion during the online planning phase rapidly. The offline roadmap is created using the PRM algorithm; it is shown in Fig. 3. The roadmap does not change over time which motivates the use of PRM. In addition to the roadmap, we compute terrain normals and filtered height. This computation can be performed by fitting a tangent plane at each point (x, y) locally. Local fitting is done using least squares, and as a result, we get the normal of the plane and fitted height at point (x, y). We filter once with a local radius R of 0.3 m and once with 2.5 m, which is roughly the robot's footprint radius. Filtering result with large R is shown in Fig. 2. The idea behind this is to use the filtered height for computing base poses. Terrain discontinuities (e.g., steps) should not be reflected in the base movement since the base moves above the terrain. On the other hand, smoothed terrain (shown in yellow) is a good approximation for base movement, assuming that it stays at some (roughly) constant height above the smoothed terrain.

B. Online Planning
Having computed the roadmap shown in Fig. 3, an RRT planner finds a plan between base poses p B,start and p B,goal . We use RRT framework to enable re-planning in potentially Algorithm 1 Select n and h at position (x, y) if cost < c best then 10: h best = hc, n best = nc, c best = cost 11: return (h best , n best ) changing maps. Similar to previous work [16], our planner proposes a base pose before computing the limb contacts. Subsequently, we check whether contacts can be established and whether the robot is stable. In general, a sampling-based planner typically has three main components: sampling, connecting a new sample to the tree, and feasibility checking. In this section, we describe how each step works.
1) Sampling: Candidate base poses are sampled in SE(2) space instead of full-fledged SE (3). The idea behind this decision is straightforward: since limbs interact with the environment, the sampler uses terrain information to constrain some DoFs of the base pose, which reduces the dimension of the search space. Hence our planner samples x, y position of the base, and yaw angle γ from a uniform distribution. The remaining DoFs are computed based on local terrain features: roll angle α, pitch angle β, and z coordinate. Roll and pitch are computed from terrain normal n such that the base remains roughly parallel to the terrain underneath. Finally the z coordinate can be computed as z = h(x, y) + h desired where h(x, y) is the terrain elevation at sampled point (x, y) and h desired is user defined desired height above the terrain.
Selecting h(x, y) and n can be tricky. E.g., when crossing a deep gap, terrain height can be so low that the planner cannot generate any valid poses (despite the heavy filtering). Luckily, one can leverage a simple observation to chose good h and n. When moving over untraversable terrain ¬T , the robot only cares about the nearest T (traversable area). The rationale is that contacts should only be made with T , and the base pose should be selected such that limbs can reach the nearest T . Alg. 1 implements this proposition; it selects h and n such that contacts with T can be established. The Alg. 1 enumerates all candidate normals and heights (lines 1-6) and then does a small brute force search to select a pair (h best , n best ) that minimizes some criterion. The computePoseCost function in line 9, gives low cost to poses where all legs are grounded and penalizes big roll and pitch angles. Full base pose is then determined from (h best , n best ).
2) Connection to Tree: Upon drawing a random base pose (x, y, γ), the planner tries to connect it to the tree (using the weighted cost of euclidean distance and angular distance). The connection is done in SE(2) space using Reeds-Shepp (RS) curves [31]. RS curves give an optimal path between two poses while respecting the minimum turning radius con- Fig. 4. Addition of a new node into the RRT tree. Red circles and full black lines are nodes and paths that make the current RRT tree. The newly sampled node (green) has to be connected to the rest of the tree. For a successful connection, all subnodes on the connecting path (small green circles) have to be valid. selectContactArmConfiguration(T ) 14: selectSwingLegsCofiguration(T ) 15: return isStable() 16: case 1, 0 17: return FALSE straint. For a robot that can turn in place, one could use a very small turning radius. Attempting straight-line connections between base poses would make the subsequent refinement step (see Sec. IV) very hard since the satisfaction of the nonholonomic rolling constraint cannot be guaranteed. By using RS curves, this is implicitly ensured; however, the resulting trajectory might be longer. Computing the RS connection is done terrain agnostic completely.
3) Feasibility Checking: Fig. 4 depicts the feasibility checking. Upon drawing a new sample (large green node) as described in step 1, we compute a RS connection (dotted line) to the new node, as described in step 2. Subsequently, the dotted line is discretized into subnodes (small green circles) using an RS interpolation method [30]. Next, for each subnode we generate full 6 DoF pose using Alg. 1. Finally, each subnode undergoes feasibility checking, ensuring that the robot is statically stable and can establish enough contacts with the ground. In case a feasibility check passes for every subnode, the RRT adds the new state and the connecting path to the tree. It is crucial to discretize the RS path with high resolution since straight-line connections are assumed between two subsequent subnodes; in our implementation, we allow for a maximal distance of 20 cm. The length of the whole path is a tuning parameter (in our case, 15 m). Alg. 2 summarizes feasibility checking.
The feasibility check shown in Alg. 2 does not allow poses with a large roll or pitch angle (line 2). We then ground all the legs (line 5). A leg is grounded if a sufficient number of configurations in PRM are in contact with the surface and the contact location lies in T (see Sec. II). Among grounded legs configurations the algorithm picks the one closest to the default configuration. In case of four contact legs, the pose is deemed to be stable. In case three legs are in contact, the algorithm selects good joint configuration for the swing leg according to some criterion (e.g. ground clearance or proximity to the default configuration). The swing arm configuration is selected such that the CoM is as centralized as possible. In case only two legs are in contact, the algorithm checks whether the arm can be grounded (line 14) and then proceeds with selecting swing leg configurations. The isStable() function computes the CoM of the whole robot and verifies that it lies in the support polygon. Aside from performing feasibility checking, Alg. 2 computes the full joint state of the robot q for feasible poses. The CoM of the full joint configuration q can be computed using: where M is the mass of the whole robot, p i,com and m i are the position of CoM and mass of the i th limb, respectively. Thanks to the mapping between q i and p i,com computed offline, the sum in Eq. 3 can be evaluated rapidly. Note that unlike [16], we do not require all limbs to contact the environment while generating base poses, thus allowing for more flexibility. Once the RRT has reached p B,goal , the final path is post-processed. In the first step, the contact schedule is modified to ensure stability. We do not allow establishing/breaking more than one contact between two different successive nodes. If the robot wants to change more than one contact state at any point, we insert a node in between. Those situations happen only when the arm contact is established/broken. The robot tries to change the contact state of the arm and leg(s) simultaneously. We add a short full contact phase (legs + the arm) in between to ensure static stability. Secondly, we compute Inverse Kinematics (IK) for the non-wheeled limbs in contact. Any of those limbs has to satisfy the contact constraintṗ i = 0. For each non-wheeled limb in contact, we find base poses at the beginning and the end of its respective contact phase. The reference position p * for the IK is found by solving: and setting p * = (p * s +p * e )/2.0, where p s are all positions in the i th limb roadmap at the beginning of the contact phase and p e at the end of the contact phase. We then compute i th limb's joint angles for every contact node as q i = IK(p * ).
Alg. 2 ensures that the robot is stable and that limbs are not in a collision. Nevertheless, it assumes that straight line connections in joint space are collision-free. The assumption might be invalid, especially for the arm, which moves around the base and is used as a counterweight. To overcome this problem, we use the precomputed roadmap in which we, similar to [18], invalidate all vertices and edges that are in a collision with other limbs or the environment. Once the graph is updated, each limb's path is found using a graph search algorithm (A* in our case). In practice, legs always end up being collision-free, but the arm often collides with legs. Hence we run the graph search only between nodes where the arm moves.
IV. REFINEMENT STEP The refinement step uses TO to satisfy all system constraints. TO methods scale well with system dimension and can handle nonlinear constraints such as forward kinematics or non-holonomic rolling constraints. However, computing the correct contact schedule and dealing with obstacles remains challenging for the gradient-based methods, so we initialize optimization with trajectory computed in the initialization step. The optimization receives contact schedule, base position/velocity (6 DoF) and joint position/velocity (25 DoF in our case) and solves a feasibility problem. Adding an optimization objective allows for fine motion tuning, however it typically results in increased computation times. TO planner used in this paper is based upon our previous work [4], and below we present modifications that enable us to cope with more challenging scenarios.
Terrain maps To the best of the author's knowledge, there is no optimization-based planner for hybrid systems that can handle general terrain representations. Compared to their legged counterparts, hybrid robots keep their limbs in contact over long distances. Hence, map errors influence more variables and constraints, which makes the optimization more sensitive. So far, proposed terrain-aware optimization planners have used analytical descriptions of the environment [7], [32], [2]. Our planner integrates grid maps [23] into the optimization, thus allowing planning in any environment where a 2.5D map is a suitable representation. The elevation map comes into the optimization in the form of height constraint for all limbs in contact C.
The constraint (5) is problematic since height mapping h(·, ·) is discrete and discontinuous (e.g. gaps or steps) which can cause optimization to diverge. Far away from the cell center, Nearest Neighbor (NN) search or linear interpolation are poor approximations of the true elevation. Hence, approximation of partial derivatives [ ∂h ∂x ∂h ∂y ] using central finite differences renders them non smooth. For large grid cells (0.1 m), the key to improving the optimization convergence is using a higher-order approximation of the h(·, ·) function. We found that bicubic interpolation and bicubic convolution algorithms [33] work well. Implementations of both algorithms are integrated into the open-source package Grid Map and made available for the community 1 . Unfortunately, neither filtering nor higher-order approximation can help if the terrain is discontinuous (steps or gaps). To handle discontinuities, we use gradient clipping, a technique known from the machine learning community. Smaller clipping thresholds prevent getting stuck in bad minima. However, they usually require a few more iterations for convergence.
Traversability Constraint We require that all contacts stay in the traversable area; the constraint implements Eq. 2. The gradient is also computed using a central finite difference with bicubic interpolation.
Collision Avoidance Constraint imposes a minimum distance between the collision geometries of the robot. We use shape primitives such as spheres or cylinders. We impose that the minimal distance between collision geometries has to be greater than d min . The minimal distance between collision geometries is calculated using the Bullet physics engine.

V. RESULTS
We tested our planner on HEAP [20], which is a customized Menzi Muck M545 walking excavator; five limbs, 25 joints, and a floating base make it a challenging test bench. The whole planning pipeline is implemented in C++ programming language, and tests are performed on the Intel Xeon E3-1535M processor with 32 GB of RAM.
1) Roadmap Generation: We use a roadmap size of 300 vertices per leg and about 3000 vertices for the arm since it has a much larger workspace (see Fig. 3). More vertices yield better workspace approximation; consequently, finding stable configurations is more likely. However, with more vertices, more computation is required to find stable configurations. We found the proposed number of vertices to be enough for finding solutions for the scenarios tested. For each vertex in the roadmap, we attempt the connection to its ten nearest (a) Prefer Shortest Path Looking for connections that are shorter than d max is the most computationally intensive operation.
2) Terrains: The proposed planning pipeline can compute plans in various terrains (see Fig. 1, also see the video attached 2 ). The planner uses the same set of parameters for all scenarios. Traversing such challenging terrains would not be possible with a purely optimization-based planner and handcrafting a contact schedule for those scenarios would be difficult. We show success rates on complex terrain features in Fig. 5b, by averaging five trials for each difficulty. All terrains except flat ground are shown in Fig. 1: rough (middle left image), gap (bottom right), step (middle right), hole (top right). For terrains flat, rough, gap and step, the planner was asked to find a path of about 15 m in length. For terrain hole, the length was about 30 m such that the planner has to navigate around the hole to get to the other side (see Fig. 1, top right). The maximal distance parameter in the RRT was set to 10 m. We consider planning successful if both initialization and refinement step find a solution. Times and costs in Fig. 5a have been obtained by averaging ten successful trials across all terrain difficulties. All the plans are found using the same RRT optimizing planner with planning time of 4 s. Fig. 5a shows that the first stage finds initial solutions quickly and that they are close to the optimal ones. Short planning times suggest that the initialization step could be used in a receding horizon fashion.
3) Importance of Initialization: To quantify effect of good initialization on the convergence, we use linear interpolation (between start and goal pose) as a baseline strategy. The other strategy is using a whole-body plan from the first stage to initialize all the variables. Linear interpolation uses the contact schedule from the first stage since it cannot compute one alone. The computation times until convergence when initialized with linear interpolation are shown in Fig. 5c (red color). The times were obtained by averaging ten successful trials with different initializations over all terrains. The computation times variance is caused by different durations of initialization trajectories from the first stage (longer durations 2 https://youtu.be/B-NHY4xwgwY require more computation). A good initialization makes a small difference in continuous terrain (flat terrain and rough terrain with roughness ±0.5). However, it becomes essential for harder terrains (hole, gap, step) since linear interpolation fails to produce a solution. This result corroborates our hypothesis from the Sec. I that TO can easily handle nonholonomic and nonlinear constraints from the robot model, whereas terrain constraints and contact schedule discovery present challenges for the optimization. Unlike agile quadrupeds in [32], HEAP cannot execute full flight phases, which makes the terrain constraints especially challenging.

4) Contact Schedule Discovery:
The way of terrain traversing can be influenced by tuning the cost function inside SBP. This is illustrated in Fig. 6. HEAP is commanded to reach the other side of the gap. Trajectory of the base is shown in green color. In the first scenario (Fig. 6a), HEAP incurs no cost for lifting the legs off. Upon introducing the stepping penalty, HEAP realizes that it can use a small bridge to avoid breaking contact on all legs, Fig. 6c. This behavior emerges merely by introducing the stepping penalty and without any other modifications. Such flexibility is made possible by optimizing over contact schedule and full-body poses simultaneously. The resulting contact schedules are shown in Fig. 6b and Fig. 6d.

VI. CONCLUSION AND OUTLOOK
We present a combined sampling and TO based planner for legged-wheeled robots. The sampling-based stage computes whole body configurations and contact schedule; it is based on RRT planner and a roadmap that is pre-computed offline. Compared to existing work, the roadmap is extended to store the mapping between joint angles and CoM, which allows for quick stability checks in the presence of heavy limbs. Our SBP planner achieves fast planning times and could be used interactively. In the second planning stage, TO satisfies all system constraints, such as non-holonomic rolling constraint. We integrate elevation maps into TO and demonstrate planning on general map representations. Evaluations of the proposed approach suggest that the main difficulty for TO stems from terrain/collision (avoidance) constraints and contact schedule planning, problems that are mitigated using the proposed two-stage approach.
Future developments will verify the approach on real machines. Besides, we plan to investigate using TO for tracking plans from the first stage in an MPC fashion.