Deep Reinforcement Learning for Multi-contact Motion Planning of Hexapod Robots

1 Introduction

Legged robots have redundant degrees of freedom and multiple footholds for passing through challenging environments, and have wide application prospects in disaster rescue, material transportation, planet exploration and other fields [Lee et al., 2020]. However, it is still a challenging task to improve the motion efficiency of the legged robots in unstructured environments. Traditional methods focus on the single-step optimization using kinematic criteria and ignore the global footstep planning, which always leads to a poor passibility in complex environments [Belter et al., 2016; Hwangbo et al., 2019]. Discrete environments are a special case of the unstructured environment which need more efficient and reliable planning methods. These kind of environments can characterize any unstructured environment for the motion planning of legged robots.

∗Contact Author

Trajectory optimization (TO) [Betts, 1998] has been proved to be effective for legged locomotion. This TO method generates a motion trajectory that minimizes some measure of performance with a set of constraints. For the multi-contact motion planning problem in discrete environments, such as plum-blossom piles shown in Fig.1, TO is difficult to solve directly and usually needs to be combined with some traditional planning methods, such as A*, Probabilistic Roadmap (PRM) planner, Rapidly-exploring Random Tree (RRT). These methods can plan a feasible trajectory quickly when the dimension of the state and action space are low, but often face the curse of dimensionality problem in the high dimensional or continuous state and action space.

Reinforcement Learning (RL) [Sutton and Barto, 2018] is a machine learning methodology that has witnessed great progress in the artificial intelligence community. Recent breakthroughs of Deep Reinforcement Learning (DRL) algorithms have proved RL to be one of the state-of-the-art technologies for complex learning and control tasks [Mnih et al., 2015; Silver et al., 2016]. Based on the framework of Markov Decision Processes (MDP), RL addresses the problem that how an autonomous active agent learns the optimal policies while interacting with an initially unknown environment. The self-learning property from unknown environments makes RL a promising candidate for the optimization and control of real systems [Arulkumaran et al., 2017]. Nevertheless, to the best of our knowledge, the DRL-based multi-contact motion planning for hexapod robots on plumblossom piles has not been well addressed yet.

In this paper, we develop an integrated DRL-based method for multi-contact motion planning of hexapod robots moving on uneven and randomly generated plum-blossom piles. Our main contributions are threefold.

• We formulate the motion of hexapod robots as a discretetime finite MDP problem with a specified reward function. The motion policies are optimized using the designed DRL algorithm.
• We build a multi-contact centroid dynamics model for hexapod robots, and formulate the transition feasibility of the state transition using the TO method, which in turn determines the rewards.
• We test the trained policies on different settings of plumblossom piles, and both of the simulation and experimental results demonstrate the feasibility and efficiency of the proposed method.

2 RelatedWork

Multi-contact motion planning. Multi-contact motion planning is an important subject in robotics and there have been a variety of planning strategies for legged systems. Some focus on simply choosing the next best reachable footholds ignoring the global footstep planning [Rebula et al., 2007], while others consider the optimal footstep sequence from the start to the goal [Zucker et al., 2011; Mastalli et al., 2015]. Recently, [Ding et al., 2020] used the Monte Carlo tree search algorithm to generate reliable gait and foothold sequences for hexapod robots in a sparse foothold environment. But most of the above methods only consider the kinematic criteria to select footholds. In recent years, the multi-contact TO method has attracted extensive attention in the field of legged locomotion. [Winkler, 2018] used the simplified centroid dynamics and TO to generate a motion trajectory with rich possible behaviors. [Mastalli et al., 2017] searched in the elevation map for legged locomotion on rough terrains using TO.

Transition feasibility. The transition feasibility describes whether the robot can transfer from the current state to the target state. [Tonneau et al., 2018] solved the problem from the perspective of kinematics and dynamics. They first considered a conservative but exact formulation of the dynamics, and then relaxed while preserving the kinematic constraints of the motion. [Fernbach et al., 2018] proposed an efficient dynamic feasibility check based on a conservative and convex reformulation of the problem, and then solved the problem with a Linear Program (LP). Different from the former, [Klamt and Behnke, 2019] used a CNN to output the feasibility and costs values and generated an abstract representation of a detailed planning problem.

RL for legged motion planning. RL has been proved to be effective for legged motion planning on rough terrains [Rivlin et al., 2020]. Unlike traditional planning methods, DRL can deal with complex tasks in high-dimensional continuous state and action spaces. [Shahriari and Khayyat, 2013] proposed a gait generation strategy based on RL and fuzzy reward, and planned the motion strategy on discontinuous terrain through iterative updating. [Peng et al., 2017] separated the responsibilities for planning footholds and executing swing-leg motions and uses the hierarchical DRL to learn locomotion skills. [Tsounis et al., 2020] combined DRL with the model-based motion planning method, and formulated the MDP using the evaluation of dynamic feasibility criteria in place of physical simulation, so as to realize the motion planning on challenging terrains.

3 DRL for Multi-contact Motion Planning of Hexapod Robots

In this section, the integrated DRL-based multi-contact motion planning method is presented for hexapod robots moving on uneven plum-blossom piles. First, we introduce the overall control structure of our method. Second, the motion of hexapod robots is mathematically formulated as an MDP with a specified reward function. Third, a transition feasibility model is proposed, which describes the feasibility of the state transition under the condition of satisfying kinematics and dynamics, and in turn determines the rewards. Finally, using the defined MDP, we propose a DRL-based algorithm for multi-contact motion planning of hexapod robots.

3.1 Overall Control Structure

The task of hexapod robots is to move from the initial point to the target area on uneven plum-blossom piles. We formulate this process as a discrete-time finite MDP. The overall control structure are shown as in Fig. 2. The current state st, containing the proprioceptive information Φt, the exteroceptive information Mp and the target ptarget, is first input into a policy network πθ. We parameterize πθ as a diagonal Gaussian distribution, the mean of which is output by a Neural Network (NN). First, the coordinates of all plum-blossom piles, which are randomly distributed in the environment, are input into a Graph Attention Network (GAT) [Veliˇckovi´c et al., 2018] with sparse matrix operations. The resulting latent output from the coordinates is concatenated with the remaining part of the state, then fed into a Multilayer Perceptron (MLP). The action at is sampled from the diagonal Gaussian distribution and next input into the environment.

According to the input of the action at, the environment outputs the undetermined next state s′t+1 , which contains both target Center-of-Mass (CoM) and footholds positions, where the target footholds are found by the K-nearest Neighbors (KNN) algorithm according to at. We propose a transition feasibility model for hexapod robots, which describes the feasibility of the state transition under the condition of satisfying kinematics and dynamics, and in turn determines the rewards. The input of the transition feasibility model is st and s′t+1 , and the output is the next state st+1 and the reward rt. If the the hexapod robot can transform form st to s′t+1 according the transition feasibility model, then st+1 = s′t+1 and rt is positive, otherwise st+1 = st and rt is negative. The optimal footholds and CoM sequences are obtained using the optimized policy π∗ θ , which is trained by the designed DRL algorithm. Finally, the hexapod robot follows the optimal sequences by inverse kinematics.

3.2 MDP Formulation

The motion of hexapod robots on plum-blossom piles can be described as a discrete-time infinite MDP of a 4-tuple 〈S,A, P,R〉, where S is the state space, A is the action space, P is the state transition probability and R is the reward function. In a RL process, an agent interacts with the environment to maximize the cumulative discounted future rewards: Rt = ∑T-t k=0 γ k rt+k, where γ ∈ [0, 1] is a discount factor and T is the terminate time.

State Space

The state space contains both exteroceptive and proprioceptive measurements. The hexapod robot moves forward with the tripod gait on plum-blossom piles, and there are three legs in contact and others in swing at each time-step t. The state st can be expressed as

where Mp ∈ R3×Np is the coordinates of all the plumblossom piles in the environment, Np is the number of plumblossom piles, Φt is the proprioceptive information which contains the CoM position of the hexapod robot rBt, the Euler angles of the body θt, the foothold position of the ith contact leg pct and the state of each leg ct ∈ {0, 1}. If ct = 0, legs 1, 3 and 5, as shown in Fig. 3, are in contact. If ct = 1, legs 2, 4 and 6 are in contact. ptarget is the center coordinate of the target area. At each moment when the swing legs and the support legs switched, we constrain the linear velocities ˙rBt and the angular velocities ωt of the hexapod robot to zero.

Action Space

The action at time-step t can be expressed as

where ∆rBt ∈ R3 is the increment of coordinates of the CoM in the world frame, ∆θt ∈ R2 is the roll angle and pitch angle increment of the body, and the yaw angle increment of the body is zero. kt ∈ (-1, 1) is the selection parameters for the landing points of the swing legs. Specifically, when the state of the CoM gets the next desired state at time-step t+1, according to the simplified kinematic model of the hexapod robot as shown in Fig. 3, we can determine the center of the kinematic cube ¯pi for the ith swing leg. Then we can find the np nearest plum-blossom piles to ¯pi in the environment using KNN. The piles are then arranged in the ascending order from near to far. Finally, the kp-th plum-blossom pile is selected as the target landing point psi of the ith swing leg at the next time-step t + 1.

where ⌊∗ ⌋ denotes the rounding down operation.

Reward Function

The goal of the hexapod robot moving on plum-blossom piles is to reach the target area from the initial point with the shortest path while satisfying all the kinematic and dynamic constraints. We design the reward function as follows:

where rbt is the boundary reward penalizing the CoM moving beyond the boundary of the environment, rkt penalizes the state transition violating the kinematic constraint, rft penalizes the unfeasible state transition calculated by the transition feasibility model. The distance reward forces the robot to move to the target area with the shortest path and is defined as

where pinitial is the initial point. The hexapod robot gets the reward rgt when arriving the target area.

3.3 Transition Feasibility

We employ the TO technique to determine the transition feasibility of the state transition, and in turn determine the rewards. In this subsection, we first introduce the formulation of the multi-contact transition feasibility. Then, we give the hexapod robot model and the multi-contact model used in the transition feasibility.

Multi-contact Transition Feasibility Formulation

The multi-contact transition feasibility model determines whether the state transition of the hexapod robot is feasible given the current state x0 and the target state xT . The state x includes the CoM position rBt, the linear velocities ˙rBt, the Euler angles θt and the angular velocities ωt.

We formulate the transition feasibility as a nonlinear constrained optimization problem using the direct multiple shooting method and solve it by Nonlinear Programming (NLP). The initial state x0, the desired final state xT , the foothold position of the ith contact leg pci , the target foothold position of the ith swing leg psi , and the total duration T are provided, where i from 1 to ni is the ith contact leg. On plum-blossom piles, the hexapod robot takes the tripod gait as the default gait to complete the state transition, therefore the number of contact legs ni = 3.

The hexapod robot model used in the transition feasibility is shown in Fig. 3. The decision variables of the problem include the contact force ft and the state xt. The constrains include the initial state xt0 = x0, the target state xtF = xT , the dynamic model, the kinematic model, the pushing force and the friction cone. The optimizer uses the provided information to find a trajectory for the state x and the contact force f , and makes the objective function

meet the local minimum. We use Ftf ∈ {0, 1} to describe the state transition. If the trajectory satisfies all the given constraints, then the transition is feasible and Ftf = 1, otherwise the transition is unfeasible and Ftf = 0.

Kinematic Model

The kinematic model describes the workspace of the body and foots, which avoids the behavior of the hexapod robot violating its own mechanical structure constraints. Since the original kinematic model of the hexapod robot is highly nonlinear, we conservatively approximate it by keeping the ith foot pi inside the blue cube Ri in Fig. 3.

In order to get the cubes, based on the dimension parameters of the hexapod robot and the rotation range of each joint shown in Table 1, we randomly sample within the rotation range to generate the point cloud of the foots. According to the point cloud, we can conservatively find the side length of the cube. The workspace of each foot i can be expressed as

where BW R is the rotation matrix from the world frame to the body frame, RZ(αi) is the rotation matrix for rotations around the z-axis, αi is the deflection angle of the ith coxa relative to the x-axis in the body frame, ¯pi is the center of the ith cube, b is half the side length of the cube. For the three contact legs, the kinematic constraints need to be met in the whole state transition process. For the three swing legs, the kinematic constraints are only verified at the final time T .

Dynamic Model

The dynamic model represents the time-dependent aspects of a system, and we approximate it by Single Rigid Body Dynamics (SRBD) [Winkler, 2018]. Then, we can get the Newton-Euler equations of the hexapod robot, which is defined as the SRBD:

where m is the mass of the hexapod robot, g is the acceleration of gravity, ¨ rB is the linear acceleration of the CoM. I ∈ R3×3 is the moment of inertia.

The dynamic model of the hexapod robot can be modeled by an ordinary differential equation ˙x = F(x, f). The rates of the Euler angles ˙θ can be calculated by the optimized Euler angles θ and the angular velocities ˙ω:

Based on the SRBD, the dynamic model of the hexapod robot is independent of its joint state, but only related to the external forces on the contact legs.

Pushing Force

According to the physics, the force provided by the environment to the hexapod robot can only be thrust, and we set up the following constraint:

where n(pci ) is the normal vector of the environmental surface at coordinate pci .

Friction Cone

The friction follows from Coulomb's law pushing stronger into a surface allows exerting larger side-ways forces without slipping. Therefore, the resultant force on each contact leg is always in the interior of the friction cone. The linear approximation is as follows:

where t{1,2} (pci ) is the tangential vector of the environment at coordinate pci and µ is the friction coefficient.

3.4 DRL-basedMulti-contact Motion Planning

Based on the proposed transition feasibility model and the formulated MDP, an integrated DRL algorithm for multicontact motion planning of hexapod robots moving on uneven plum-blossom piles is shown as in Algorithm 1.

Given the current state st of the hexapod robot, we design a policy network πθ to map the state st to the action at. The architecture of πθ is shown in Fig. 2. This policy network is parameterized as a diagonal Gaussian distribution πθ (at | st) := N (at | µθ (st) , σ), while the mean µθ (st) is output by a neural network and the standard-deviation parameters σ are standalone parameters. The coordinates of all the plum-blossom piles Mp are input into a Graph Attention Network (GAT) [Veliˇckovi´c et al., 2018] with a multi-head attention:

where ⃗h is the input node feature. For the plum-blossom piles, the feature is its own normalized coordinate, ⃗h′ is the output node feature, σ is an activation function, K is the number of the independent attention mechanisms, Wk is the corresponding input linear transformation's weight matrix, Ni is the neighborhood of node i in the graph. In our work, Ni represents the 5 points closest to node i. αij is the normalized attention coefficient:

The attention coefficient eij can be calculated as

where ⃗aT is the weight vector and ‖ represents concatenation. We use the sparse matrix operations in GAT to reduce the storage complexity.

Algorithm 1 DRL for Multi-contact Motion Planning

• 1: Initialize policy parameters θ0 and value function param-
• eters φ0. 2: for episod k = 0, 1, 2, ...,M do 3: Randomly initialize s0 and ptarget. 4: for step t = 0, 1, 2, ..., T do 5: Run policy π(θk), get action at. 6: Take at, observe s′t+1 7: Input st, s′t+1 to the transition feasibility model and get Ftf . 8: if Ftf = 1 then 9: Get positive reward rt, st+1 = s′t+1 . 10: else 11: Get negative reward rt, st+1 = st. 12: end if 13: Store experience (st, at, rt, st+1) in Dk. 14: end for 15: Compute rewards-to-go ˆRt. 16: Compute ˆ At using GAE based on value function Vφk . 17: Update policy: θk+1 = argmaxθ La(θ). 18: Fit value function: φk+1 = argminφ Lc(φ).
• 19: end for

The output of GAT is concatenated with the remaining part of the state st and subsequently input into the FullyConnected Layer 1 (FC1). Since the posture of the hexapod robot at the next moment will affect the selection of the target foothold positions, we divide the output action into two parts. The action ∆rBt and ∆θt are output by the Output Layer 1 (O1) and then input into the FC4 with the output of FC3. And the action kt is output by O2. The hexapod robot executes the complete action at , and then observes the next state st+1 and the immediate reward rt.

To train the policy πθ, a variant of Proximal Policy Optimization (PPO) [Schulman et al., 2017] using clipped loss and a Generalized Advantage Estimation (GAE) critic is used. We compute the policy update via stochastic gradient ascent with Adam and we have

where

and rt(θ) is the probability ration defined as

The value function is update by regression on the meansquared error:

where

4 Experiments

In this section, the multi-contact motion policies of the hexapod robot is trained based on the proposed DRL algorithm, and the policies are tested in both simulation and real plumblossom pile environments. We assessed the performance of the trained policies through the Average Episode Rewards (AER), the Average Episode Steps (AES) and the Average Success Rate (ASR) in different environment settings. To illustrate the effect of the transition feasibility model, we trained two kinds of policies, one is trained with the proposed transition feasibility model and the other is trained with a normal kinematic model. We use the Average Transferable Rate (ATR) to assess the performance of the later policy.

4.1 Experimental Setup

In order to test our method, we build three different types of simulation plum-blossom pile environments E1, E2, E3 and a real plum-blossom pile environment E4 as shown in Fig. 4. All the simulation environments present a 1200 × 1200 mm2 square area consisting of Np plum-blossom piles, and the E4 is a simplified version of E3 in real world. In E1, we set plum-blossom piles with the same height but random coordinates in the x-axis and the y-axis, and limit the distance between piles to more than 300 mm. In E2, the height of the plum-blossom piles is sampled between -30 mm and 30 mm randomly on the basis of which in E1. And the plum-blossom piles in E3 are set as random stairs. In order to generate E3, we first set up a standard stair environment. Each step of the stair is 140 mm wide and 25 mm high. Then we project the plum-blossom piles in E1 onto the surface of the stair in the z-axis direction, and the height of the plum-blossom piles are the same as that of each step of the stair.

4.2 Training Setup

For a training process, we first set a random initial point and a random target area with radius of 100 mm. The goal of the hexapod robot is to move successfully from the initial point to the target area with the shortest path. At the beginning of the training process, we first reset the CoM of the hexapod robot to the initial point. Then, the plum-blossom pile nearest to the center of the ith cube pi is selected as the initial position of the ith contact leg. If the three initial contact legs violate the kinematic constraints, the initial point and the target area are resampled. The initial angle θ of the body is parallel to the plane formed by the three initial contact legs. So far, the reset of the hexapod robot is completed. Then, for each time-step t, the hexapod robot obtains the current state st, executes the action at from the policy network, receives a reward rt and obtains the next state st+1. When the CoM of the hexapod robot reaches the target area or the maximum number of steps in the current episode reaches 300, the current episode is terminated and a new episode is started. We repeat the above process until the end of the training.

To illustrate the effect of the transition feasibility model, we trained two kinds of policies, one is trained with the proposed transition feasibility model and the other is trained with a normal kinematic model. For the training process of the latter policy, we only check the kinematic constraints of the initial position and the target position at each time-step t, as is done in [Ding et al., 2020]. Moreover, we use a simplified kinematic model in three-dimensional space as described in subsection 3.3.

We train our policy network on a computer with an i7-7700 CPU and a Nvidia GTX 1060ti GPU. The RL algorithm is implemented using Pytorch 1 , and the transition feasibility model used in the training process is solved using CasADi 2 .

4.3 Experimental Results

According to the learning curve, as shown in Fig. 5, a total of 1 million time-steps are set for training and the whole training process takes about 12 hours in each environment. We tested the trained policies in E1, E2, E3 and E4 and we assumed that the environment information and the position of the robot are known quantities. We assessed the performance of our method through AER, AES and ASR in different environments as shown in Table 2. The fully trained policies can generate valid footholds and CoM sequences which lead the robot to the random target area with a ASR between 90% and 100%. The policies that trained without transition feasibility model can get a even higher ASR, but the ATR is low, which may results in a poor passibility in the real system. Since the algorithm verifies the transition feasibility at each time-step t in the training process, it can be observed in Fig. 6 that the rotation angles of coxae, which are constrained between -45◦ and +45◦ , always satisfy the kinematic constraints (Take E3 as an example). Another important observation is about the distribution of the legs in different environments. In cases of E1 and E2, the legs are relatively evenly distributed around the CoM. However, in the cases of E3 and E4, leg 2 and leg 5 are placed to the rear for better stability due to the transition feasibility model.

1 https://pytorch.org/

2 https://web.casadi.org/

5 Conclusion

In this paper, we presented an integrated DRL method for hexapod robots moving on uneven plum-blossom piles. The multi-contact motion planning problem is mathematically formulated as an MDP with the specified reward function. The promising properties of DRL enable the motion planning algorithm to be implemented in a high dimensional state and action space. The proposed transition feasibility model for hexapod robots ensures that the planned footholds and CoM sequences satisfy the kinematic and dynamic constraints. Both of the simulation and experimental results on physical systems demonstrate the feasibility and efficiency of the proposed method. Our future work will focus on obtaining more robust motion planning policies for dynamic environments and extending the proposed method to more complex real environments.

Acknowledgments

This work was supported in part by the National Key Research and Development Program of China (No. 2018AAA0101100), the State Key Laboratory of Computer Architecture (ICT, CAS) under Grant No. CARCHB202012 and the National Natural Science Foundation of China (No. 62073160).