Hierarchically Planning Static Gait for Quadruped Robot Walking on Rough Terrain

Quadruped robot has great potential to walk on rough terrain, in which static gait is preferred. A hierarchical structure based controlling algorithm is proposed in this paper, in which trajectory of robot center is searched, and then static gaits are generated along such trajectory. Firstly, costmap is constructed by computing terrain features under robot body and cost of selecting footholds at default positions, and then the trajectory of robot center in 2D space is searched using heuristic A∗ algorithm. Secondly, robot state is defined from foothold and robot pose, and then state series are searched recursively along the trajectory of robot center to generate static gaits, where a tree-like structure is used to store such states. Lastly, a classical model for quadruped robot is designed, and then the controlling algorithm proposed in the paper is demonstrated on such robot model for both structured terrain and complex unstructured terrain in a simulation environment.


Introduction
Legged robot offers great potentials in some complex and dangerous environment, because it has more flexibility and adaptability than that of wheeled counterparts.This potential has sparked a great deal of research on legged robot in recent years, especially on quadruped robot [1][2][3][4].Static gait [5][6][7][8] is mainly used to drive quadruped robot to walk on rough terrain, although there has also been work on adaptive gaits which make use of a biologically inspired CPG [9] to traverse rough terrain using both dynamic and static gait, where the terrain typically has only very small irregularities compared to the size of the robot.Static gait is that the quadruped robot maintains static stability, which keeps the center gravity of the robot inside supporting triangle composed of three standing feet.LittleDog [10] is a classical quadruped robot specifically designed to support the research applications as part of DARPA's Learning Locomotion program (DARPA BAA 05-25), and a great deal of good works [11,12] developed for quadruped robot on rough terrain are validated by the platform.
This paper is focused on designing hierarchical controlling system for quadruped.Research on controlling algorithm for quadruped robot has changed gradually to design static gait used on complex and dangerous terrain, such as excellent works [13,14].Bai S et al. [13] proposed concepts of main static gait and minor static gait; the main static gait is used for in general cases, and the minor static gait will be started when the main gait cannot control the robot.Space for searching possible gait is reduced greatly by employing such mixed gait, so it is very efficient to generate static gait for quadruped robot.Estremera J [14] introduced noncontinuous static gait for irregular terrain to increase efficiency of planning gait.S. Bai et al. proposed a controlling algorithm [15] for driving the quadruped robot to walk on structured terrain, where Best First Search (BFS) method was applied to search the valid walking path, but the searching space will be increased largely for some more rough terrain environment.Kolter et al. proposed a completed hierarchical frame [16] for controlling the little dog [10] to walk, in which three levels including high level planner, low level programming and low level controller are defined and realized.The high level planner extracts possible footholds which are ordered according to strategy of hierarchical apprenticeship learning (HAL) [17]; the low level planner generates trajectory of robot between two continuous states, and the low level controller is used to drive the robot along the trajectory.Although the algorithm is hierarchical, robot center is determined by the footholds which are searched in the whole space, so the efficiency can be optimized.This paper will provide an algorithm to generate series of robot states hierarchically, where series of robot center are firstly searched, and then footholds are estimated from robot center and terrain feature.Annett et al. employed the similar idea [18] to drive hexapod robot, but the trajectory of robot center is searched without considering the complexity of the terrain beneath the foothold.In aspect of space complexity, most of the algorithms aforementioned store robot state by a queue structure; suppose there are  possible states in the next step, and series of robot state for  steps are computed; then there are (  ) states at least in the queue, so the space complexity is very large, which will limit the scalability of the controlling algorithm.In this paper, the space complexity will be decreased by storing robot state in a tree-like structure.
The remainder of the paper is organized as follows.Section 2 provides algorithm for searching trajectory of robot body center; Section 3 describes algorithm for generating series of static gait along the trajectory of robot center; Section 4 defines a classical model of quadruped robot, and the whole algorithm proposed in this paper is validated for both structured terrain and complex unstructured terrain in simulation environment.Section 5 will conclude the whole paper.

Searching 2D Path of the Robot Center Heuristically
. .Constructing Cost Map of Terrain under Robot Body.The cost map of terrain under robot body is computed from the following 3 terrain features: terrain slope, step height, and terrain roughness.In order to estimate those features, as shown in Figure 1, a circle on the grid based map is made around the robot center, whose radius is set large enough to cover the whole robot body, and the features are estimated based on the grids inside the circle.
. . .Estimating Slope of the Terrain.Let the grid (, ) lie at center of the robot body, and radius of the circle is   which is related to the scale of the robot, for each grid (, ) inside the circle; there is a 3D point related to such gird.Suppose there are  grids totally inside the circle; all the 3D points are represented with an ordered set which is shown in Plane function is defined as (2), whose parameters are estimated from the set    using the least square method.For optimizing the parameters of the plane handily, the triangle  is constructed as (3) and the vector  as (4).
Let the intermediate variables be computed from  and  as (5), and then the parameters of the plane are computed as (6).
Slope of the terrain centered at the grid (, ) is estimated using (7), where   is the normal of the plane, and   = [0, 0, 1] T .

S
. . .Estimating Roughness of the Terrain.The elevation of the grid (, ) is represented by (, ) and the roughness of the terrain under the robot body is defined as the standard deviation of the elevations related to the grids inside the green circle in Figure 1, which is written formally as  ℎ (, ) =  ({ (, ) | ∀grid (, ) ∈ the circle centered at the grid (, )})

. . . Estimating
Step Height of the Terrain.For each grid (, ) inside the circle centered at the grid (, ), maximum difference between the elevation of it and that of other girds around it.A sliding window is made to search adjacent grids of the grid (, ), which is shown as blue triangle in Figure 1.The greatest one is selected from all the maximum differences with respect to every grid (, ) as the step height of the terrain centered at the grid (, ).( 1 ,  1 ) is the grid with respect to the maximum difference of height for the grid (, ), and  V in Figure 2 is used to record the number of grids (, ) satisfying that slope of the line between center point of (, ) and that of ( 1 ,  1 ).  is the empirical value of  V .The step height is computed by (9), which combines the slope value and the height value, so it considers the case that some grid in the sliding window may lie outside the circle. .

. . Computing Cost of Terrain Centered at Robot Body Center
(i) Traversability Judgment.The step height of the terrain under the robot body is used to judge the traversability of the robot; if the step height is larger than some threshold   , then the robot is considered not to traverse such terrain, and the cost with respect to the central grid (, ) is set to ∞.
(ii) Computing Cost of Grid (, ).The traversability map justly indicates whether the robot can walk across the grid; in order to search optimized path, cost value must be assigned to each gird with respect to the center of the robot body.Considering the traversability, cost value of the grid (, ) can be computed from three features: slope, roughness, and step height using In (10),  V is sign of the traversability., ,  represent slope, roughness, and step height, respectively.  ,   ,  ℎ are weight coefficients, which reflect contributions for the cost value, and quadratic sum of them is (i)  1 : curvature of the terrain around the candidate foothold   , it is used to recognize whether the terrain is flat or not, which is considered for less sliding of the foot.
(ii)  2 : slope of the terrain around the candidate foothold   , it is computed as the angle between the normal at   and the horizontal surface.
(iii)  3 : the feature for illustrating concave terrain or convex one, let  1 ,  1 , . . .,  8 be 8 points with uniform distribution around the candidate foothold   , and   is the directed vector from   to   where  ∈ [1,8]; the intersection angle between   and the normal of terrain at point   is computed, and then all the 8 angles are averaged as the feature value.The concave terrain is better than the convex one, as long as the foot can not be stuck.
(iv)  4 : the feature for determining uphill terrain or downhill one, the vertical plane  is constructed from the motion direction of the robot  and the vertical upward axis , suppose   is the vector which is projected onto the plane  from the terrain normal  at the candidate foothold, the rotational angle from  to   is computed as the feature value.It is very important to judge that the terrain is uphill or downhill, because the front foot of the quadruped robot prefers uphill terrain, while the back foot prefers downhill terrain.
Both the normal and curvature are estimated from 3D points inside the sphere with a radius, so the terrain features are related to the radius.In order to get terrain information with different scales, two radiuses are employed, 5 and 10, respectively.As shown in (11),  is feature vector of the terrain beneath foothold, and  1 −  4 and  5 −  8 in the feature vector  are terrain features  1 −  4 computed with the radius 5 and 10, respectively. = [ 1 ,  2 , . . .,   , . . .,  8 ]  (11) Now the cost function is defined as the weighted sum of feature values, which is represented formally as is weight vector as (13), which can be optimized using expert learning [19].
(13) . . .Computing Cost of Terrain around Default Foothold.Default foothold is defined as the projection point of shoulder of the quadruped robot; a triangle is made around the default foothold which lies at center of it.There are 4 triangles related to default footholds, respectively.Average cost for each default foothold is computed from all the grids in the triangle.Equation ( 14) shows 4 average costs, where   is the number of footholds inside the triangle,    is the feature vector of the ℎ foothold, and   is the weight vector with respect to left front foot.
Similarly, let the center of the robot body lie in the grid (, ), and the cost related to foot is computed by averaging 4 costs in (14), which is shown in . .Searching D Path Based on Cost Map.Differing from the case of using traditional A * algorithm, cost map here is updated in real-time, because the cost map consists of terrain cost related both robot body and foot, and default foothold varies with the moving direction of the robot.Let the robot center be at the grid (, ) in the grid based elevation map; then the whole cost value related to (, ) is computed using (16), where   (, ) and  V (, ) are estimated by (10) and (15), respectively.  ,  V are weight coefficients for two kinds of cost, and √ 2  + (, ) in ( 16) represents the cost for robot body center moving across the grid (, ) along some direction, in order to search optimal path base on the cost map  using A * algorithm, the grid based map should be organized into a graph structure, where node is represented by the grid, and edge is represented by the moving cost from the grid to an adjacent grid.The edge of the graph is computed as (17), and it is the real cost value for search 2D path of robot body center.
(, ), (  ,   ) are current and adjacent grids and   ,   are resolutions of grids in elevation map,   ,   are weight coefficients which satisfy the constraints:   >   ≥ 1, and the larger the   is, the greater the tendency is taking a detour to find a safe path.
The movement direction  (  ,  ) for computing the whole cost of grid (  ,   ) is determined by the relative positions between grid (, ) and grid (  ,   ), which is represented formally using Designing heuristic function is a key step to search optimal path using A * algorithm; in our case, the heuristic function is listed as (19), where (  ,   ) is the terminal grid, and the heuristic function must satisfy the constraint shown in (20) for searching optimal path.
According to the theory of triangle inequality, the heuristic function here satisfies the constraint.The searched 2D path of robot body center includes both coordinates of the points and movement direction on the path.

Searching Series of Static Gaits
. .Planning Standard Static Gait of Quadruped Robot.In terms of energy savings and robot stability, standard order of switching flying foot is left front foot(LF)→ right back foot(RB)→ right front foot(RF)→ left back foot(LB).Figure 3 shows one circle of standard gaits, black solid dots are supporting footholds, solid circles are feet before flying, and dotted circles are targets of flying feet.Projection of robot center gravity must fall into the supporting triangle which is determined by the supporting feet.The star in Figure 3 represents the projection of robot center, which lies in central line of interactional triangle of two supporting triangles.Let (V  , V  ) be the vertex of the interactional triangle and   (  ,   ) be mid-point of the faced edge, projected point of robot body gravity   is computed using (21), where  is coefficient of proportionality with range [0, 05].
In addition to selecting proper foothold, adjusting pose of the robot body is an important step to drive the quadruped robot to walk.The pose is generally determined by the parameters: height of robot body center, angle of yaw, angle of roll, and angle of pitch.The following part will briefly introduce how to get such parameters.
(a) Height of Robot Body Center.Three supporting feet are used to fit a plane   , and a vertical line across the robot center interacts with the plane   at point   ; the distance between   and the robot center is computed as the height of robot body center.
(b) Angle of Yaw.The yaw is determined by 3 supporting feet and target of the flying foot, which is shown in Figure 4. Taking the case that left front foot is flying as an example; foot is firstly projected to the horizontal plane, and   is center of two projections with respect to back feet;   is center with respect to front feet.The yaw angle is computed from the directed vector   =   →     .
(c) Angle of Raw and Pitch.The interaction angle between robot body and horizontal plane is varied with both raw and pitch angle of the robot; in terms of robot stabilization, robot body should be parallel with the terrain plane in which 3 supporting feet lie.
To sum up, there are 5 steps to compute the pose of robot body, given 3 supporting feet and the target of the flying foot.
(i) 3D point  3   is found from the plane in which there are 3 supporting points, and projection of  3   onto the horizontal plane is   .
(ii)  3   is translated upward by height of the robot body center, which is used as the coordinate of robot body center.
(iii) Rotational transformation   is computed from yaw angle.
(iv) Another rotational transformation   ℎ is computed from roll and pitch angle.
(v) Rotational transformation of robot body is computed by   =   *   ℎ . .

. Collision Detection.
There are two cases that may produce collision between robot (body or leg) and terrain: one is flying foot from original point to target point, the other is adjusting pose of robot body.
. . .Movement of Foot.Foot is planned to avoid the collision between leg and terrain; a simple method is employed to generate foot trajectory; foot is moving along rectangular box.Take right front foot as an example, which is shown in Figure 5.
The robot is projected into 2D plane  = 0, and it walks along  positive direction,  coordinate of start point is 0.1, and that of end point is 0.17, rectangle between the two points represents obstacle.If no collision is produced at any key positions of foot, then the process of flying foot is safe.The key position is shown in Table 1.
. . .Adjusting Pose of Robot Body.Before flying foot, gravity of robot body should be adjusted into the supporting triangle, which may produce collision between leg and terrain.Figure 6 shows the process of adjusting robot body, given that the foot is fixed (taking right front foot as an example).
There are 5 key poses of robot; if none of these poses produce collision, then it is safe to adjust pose of robot body when fixing standing feet.Similarly, after projecting the robot onto 2D plane  = 0, key poses are listed in Table 2 Now the problem is changed into how to select such 4 elements.
(a) Set of Footholds.A rectangle is made to be centered at default target of foothold; several candidate footholds are included in the rectangle, which are ordered by the cost value computed using (12).
In ( 23), there are   candidates in ordered set   ; cost value of  1 is smallest and that of    is largest.( Let the number of states for next step be , then it can be computed using Set of  states   is ordered in terms of priority as (28); the first element in the set has the highest priority.
According to the order in set   , priority for adjusting 4 parameters is Height of robot center ≻ Pitch angle ≻ Roll angle Terrain complexity has a great influence on the stability of robot, so foothold should be selected with the lowest cost as much as possible.Comparing to translation, the rotation has greater impact on the stability of robot, so the height parameter is firstly adjusted.According to the model of quadruped robot, it is relatively safe to adjust pitch angle firstly.
. . .Searching Gaits Recursively.When the quadruped robot explores one step forwardly, there are  candidate states, and they are selected in turn by the element order in the set   in (28).Several steps can be searched one time; robot state of th step is   1  2 ...  , which indicates that the   th state is selected from  candidates for th step.Let ordered set of candidate robot state for  + 1th step be  +1   as shown in Let  +1  be the first state from the set  +1  that satisfies the following conditions: (i) Robot state  +1  will not violate kinematic constraint of both robot body and leg.
(ii) No collision will occur when the robot is moving from state   1  2 ...  to state  +1  .
If  +1  is found from  +1  , then the robot state for  + 1th step is   1  2 ...   +1 =  +1  ; else no possible robot state can be continuously searched from state   1  2 ...  ; now backtracking must be carried out.Searching process restarts from the   + 1th candidate state of th step and state for next step  + 1 is searched until reaching to the target searching depth.
The searching process with recursive backtracking is represented by a recursive tree shown in Figure 7.For convenience, suppose there are only 2 candidate states for each searching step, gaits of 4 steps are illustrated by green line with arrow.
None of the searching paths on left and below of green line can reach the 4th step, because some switches of robot states would make collision.States on right and above green line have not been searched yet.In this searching process, only 5 states in the green line are restored.All of the states below the green line may be constructed in the worst case, but if there is no state transformation from  12 to  121 , then the children states  1211 and  1212 will not need to be considered.
The following heuristic rules can be used to prune some unnecessary branches.
(i) If the kinematic constraint can not be satisfied for current height value, then the larger height value needs not be considered.(ii) If collision happens between rear leg of robot and terrain for current pitch angle, then the smaller pitch angle needs not be considered.(iii) If collision happens between left leg of robot and terrain for current roll angle, then the larger roll angle needs not be considered.
The more steps the robot searches forwardly, the more optimized the searched series of static gaits.

Simulation Result and Discussion
. .Model of the Quadruped Robot.The model of the quadruped robot in this paper is simplified as long as there are three degrees in one leg.A simple model with DH parameters is shown in Figure 8, and the foot can be reached anywhere within its motion space.
In Figure 8,  is the half length of robot body,  is the half width of robot body,  is the width of shoulder,  is the length of thigh, and  is the length of shank.Taking right back leg as an example, the red arrow line represents rotation axis (Z), and the green arrow line represents common vertical line between two rotational axes, whose direction is equal to that of the axis X.  0 and  4 are two virtual axes introduced for convenient representation, but there is no rotational degree of freedom actually. 0 lies at the center of robot body, and it is supposed that  0 and  1 share the same line.
Variable DH parameters of the model above are shown in Tables 3 and 4. s in the tables above are related to the joint angles directly, whose ranges are shown in Table 5.
Constant parameters are shown in Table 6.
. .Walking on Structured Terrain.In order to validate the effectiveness of the algorithm for planning standard static gait, a simple simulation is carried out.The quadruped robot is driven along some straight line, and the footholds are divided into two categories: valid foothold at flat terrain and invalid foothold at terrain with edge.
Structured terrain model is generated in OpenRAVE [20] simulation environment, which is composed of several    cuboids with length of 0.4 and width of 0.04; height of the cuboid is randomized in the range [0, 0.06].The points at the edge are invalid foothold, and, in terms of priority, all the valid footholds around the default footholds are ordered according to the distance between default foothold and candidate foothold.The distance is closer; then the priority of the foothold is higher.In order to compute the pose of the robot body, parameters in the equation ( 24)-( 27) are set by the value in Table 7.
Standard static gaits for two circles are shown in Figure 9.All the candidate footholds belong to either set of valid footholds or set of invalid footholds and only the point at the edge of the terrain is invalid foothold.All of the valid footholds are ordered in terms of the distance to the default foothold.Priority of the foothold nearer to the default foothold is high.
The strategy with recursive backtracking is used to search gait series for the quadruped robot, recursive depth which is the gait circles for searching forwardly once and has great impact to the searching result, and the depth is 2 for the searching process shown in Figure 9.The recursive depth has great impact on the gait series searched, which is demonstrated by the simulation.In general, when the depth is not deep, the searching efficiency is satisfied, but the searching process may fall into local minima; then the result will not be optimized, even though there is no valid result.
Two cases are shown in Figure 10; the first case is that the algorithm searches forwardly only one step, and gait series after the first step are not considered, so collision may occur by some step as shown in Figure 10(a), in which the left back leg will be touched with the terrain when preparing for the next gait step.The collision mentioned above would not occur if the feasibility of such gait step is considered as shown in Figure 10(b), in which the recursive depth is 4.
. .Walking on Complex Unstructured Terrain.Simulation in Section 4.2 demonstrates the effectiveness of the algorithm for searching gait series on the structured terrain, which also validates the logistic correctness of the controlling algorithm, and in this section the complexity of the terrain will be increased, where algorithms for both searching 2D path of body center and searching gait series are validated comprehensively.
Another terrain model is generated in simulation environment, whose size is 1.4 × 1.2, and 2.5D elevation map based on grid with respect to such terrain model is shown in Figure 11(a).
The radius for computing terrain features (roughness, slope, and step height) is   = 0.1; maps of their feature

Figure 1 :
Figure 1: 2D projection of grid based elevation map.

Figure 2 :
Figure 2: The processing diagram for computing the step height.

1 Figure 6 :
Figure 6: Moving trajectory of the robot body.

Figure 7 :Figure 8 :
Figure 7: Tree-style structure for recursively searching series of static gaits.

Figure 10 :
Figure 10: Different searching results by looking one and four steps ahead, respectively.

Figure 11 :
Figure 11: Geometric features of 3D terrain under the robot body.

Table 1 :
Key points in the foot trajectory.

Table 2 :
Key poses in the body adjusting trajectory.State after taking next step of robot is defined from 4 elements: foothold, height of center of robot body, pitch angle, and roll angle.It is formally represented as  = [  ,   ,  ℎ , ℎ ℎ ] . .Searching Series of Robot States . . .Defining Robot State.

Table 3 :
The DH parameters for the left legs.Figure 9: Series of standard static gaits for two circles on structured terrain.

Table 4 :
The DH parameters for the right legs.

Table 5 :
Range of the joint angle.

Table 6 :
Constant parameters of the model.

Table 7 :
Parameters for generating candidate body pose.

Table 8 :
Parameters for computing cost value.