# Hierarchically Planning Static Gait for Quadruped Robot Walking on Rough Terrain

Authors: Xingdong Li; Hewei Gao; Jian Li; Yangwei Wang; Yanling Guo
Journal: Journal of Robotics
 (2019)

---

## Abstract

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, cost map 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.


---

## Body

## 1. 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–4]. Static gait [5–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 n possible states in the next step, and series of robot state for d steps are computed; then there are O(nd) 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. Section2 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.


## 2. Searching 2D Path of the Robot Center Heuristically

### 2.1. 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 Figure1, 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.Figure 1
2D projection of grid based elevation map.


#### 2.1.1. Estimating Slope of the Terrain

Let the grid(i,j) lie at center of the robot body, and radius of the circle is rcost which is related to the scale of the robot, for each grid (c,r) inside the circle; there is a 3D point related to such gird. Suppose there are e grids totally inside the circle; all the 3D points are represented with an ordered set which is shown in (1)PijC=p1Cijx1Cij,y1Cij,z1Cij,p2Cijx2Cij,y2Cij,z2Cij,…,peCijxeCij,yeCij,zeCij=pcrxcr,ycr,zcr∣∀gridc,r∈thecirclecenteredatthegridi,jPlane function is defined as (2), whose parameters are estimated from the set PijC using the least square method. (2)Ax+By+Cz+D=0For optimizing the parameters of the plane handily, the triangleM is constructed as (3) and the vector b as (4).(3)M=∑k=1exkCij2∑k=1exkCijykCij∑k=1exkCij∑k=1exkCijykCij∑k=1eykCij2∑k=1eykCij∑k=1exkCij∑k=1eykCije(4)b=∑k=1exkCijzkCij,∑k=1eykCijzkCij,∑k=1ezkCijTLet the intermediate variables be computed fromM and b as (5), and then the parameters of the plane are computed as (6).(5)A′,B′,D′T=M-1∗b(6)A,B,C,DT=-A′,-B′,1,-D′TSlope of the terrain centered at the grid(i,j) is estimated using (7), where nPlane is the normal of the plane, and nz=0,0,1T. (7)Slopei,j=arccosdotnPlane,nznPlane∗nz


#### 2.1.2. Estimating Roughness of the Terrain

The elevation of the grid(c,r) is represented by G(c,r) 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 (8)Roughnessi,j=StdVarGc,r∣∀gridc,r∈thecirclecenteredatthegridi,j


#### 2.1.3. Estimating Step Height of the Terrain

For each grid(c,r) inside the circle centered at the grid (i,j), 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 (c,r), which is shown as blue triangle in Figure 1. The greatest one is selected from all the maximum differences with respect to every grid (c,r) as the step height of the terrain centered at the grid (i,j). (c1,r1) is the grid with respect to the maximum difference of height for the grid (c,r), and noverCrit in Figure 2 is used to record the number of grids (c,r) satisfying that slope of the line between center point of (c,r) and that of (c1,r1). ncrit is the empirical value of noverCrit. 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.(9)step←minhtmp,noverCritncritFigure 2
The processing diagram for computing the step height.


#### 2.1.4. 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 δcritSteHeit, then the robot is considered not to traverse such terrain, and the cost with respect to the central grid (i,j) is set to ∞.(ii) Computing Cost of Grid ( i , j ). 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 (i,j) can be computed from three features: slope, roughness, and step height using (10)CBodyCeni,j=∞ifMtraversi,j=0λsSi,jsmax+λrRi,jrmax+λhHi,jhmaxifMtraversi,j=1In (10), Mtravers is sign of the traversability. S,R,H represent slope, roughness, and step height, respectively. λs,λr,λh are weight coefficients, which reflect contributions for the cost value, and quadratic sum of them is 1; i.e., λs2+λr2+λh2=1. smax,rmax,hmax are maximum feature values which are used to normalize the cost value.


### 2.2. Computing Average Terrain Cost Beneath 4 Feet

#### 2.2.1. Computing Cost for Selecting Foothold

There are 4 features of the terrain beneath foothold:(i)
f1: curvature of the terrain around the candidate foothold pc, it is used to recognize whether the terrain is flat or not, which is considered for less sliding of the foot.(ii)
f2: slope of the terrain around the candidate foothold pc, it is computed as the angle between the normal at pc and the horizontal surface.(iii)
f3: the feature for illustrating concave terrain or convex one, let p1,p1,…,p8 be 8 points with uniform distribution around the candidate foothold pc, and vi is the directed vector from pc to pi where i∈[1,8]; the intersection angle between vi and the normal of terrain at point pc 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)
f4: the feature for determining uphill terrain or downhill one, the vertical plane P is constructed from the motion direction of the robot d and the vertical upward axis Z, suppose n′ is the vector which is projected onto the plane P from the terrain normal n at the candidate foothold, the rotational angle from d to n′ 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,5mm and 10mm, 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 f1-f4 computed with the radius 5mm and 10mm, respectively.(11)ξ=λ1,λ2,…,λj,…,λ8TNow the cost function is defined as the weighted sum of feature values, which is represented formally as(12)Cfootξ=WTξ=∑k=18ωkλkW is weight vector as (13), which can be optimized using expert learning [19]. (13)W=ω1,ω2,…,ωj,…,ω8T


#### 2.2.2. 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 nlf is the number of footholds inside the triangle, ξklf is the feature vector of the kth foothold, and Wlf is the weight vector with respect to left front foot.(14)Clf=∑k=1nlfWlfTξklfnlfCrf=∑k=1nrfWrfTξkrfnrfClb=∑k=1nlbWlbTξklbnlbCrb=∑k=1nrbWrbTξkrbnrbSimilarly, let the center of the robot body lie in the grid(i,j), and the cost related to foot is computed by averaging 4 costs in (14), which is shown in (15)CAvgFeeti,j=Clf+Crf+Clb+Crb4


### 2.3. Searching 2D 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 (i,j) in the grid based elevation map; then the whole cost value related to (i,j) is computed using (16), where CBodyCen(i,j) and CAvgFeet(i,j) are estimated by (10) and (15), respectively. λBodyCen,λAvgFeet are weight coefficients for two kinds of cost, and λBodyCen2+λAvgFeet2=1, which can guarantee the last cost value belongs to the range [0,1].(16)Ci,j=λBodyCenCBodyCeni,j+λAvgFeetCAvgFeeti,jC ( i , j ) in (16) represents the cost for robot body center moving across the grid (i,j) along some direction, in order to search optimal path base on the cost map C 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.(17)CEi,j,i′,j′=βdresoy2i-i′2+resox2j-j′2+βcCi′,j′( i , j ) , i ′ , j ′ are current and adjacent grids and resox,resoy are resolutions of grids in elevation map, βd,βc are weight coefficients which satisfy the constraints: βc>βd≥1, and the larger the βc is, the greater the tendency is taking a detour to find a safe path.The movement directionDi′,j′ for computing the whole cost of grid i′,j′ is determined by the relative positions between grid (i,j) and grid i′,j′, which is represented formally using (18)Di′,j′=j′-jresox,i′-iresoyTDesigning heuristic function is a key step to search optimal path using A∗ algorithm; in our case, the heuristic function is listed as (19), where (it,jt) is the terminal grid, and the heuristic function must satisfy the constraint shown in (20) for searching optimal path.(19)hi,j,it,jt=resoy2i-it2+resox2j-jt2(20)hi,j,it,jt≤CEi,j,i′,j′+hi′,j′,it,jtAccording 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.


## 2.1. 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 Figure1, 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.Figure 1
2D projection of grid based elevation map.


### 2.1.1. Estimating Slope of the Terrain

Let the grid(i,j) lie at center of the robot body, and radius of the circle is rcost which is related to the scale of the robot, for each grid (c,r) inside the circle; there is a 3D point related to such gird. Suppose there are e grids totally inside the circle; all the 3D points are represented with an ordered set which is shown in (1)PijC=p1Cijx1Cij,y1Cij,z1Cij,p2Cijx2Cij,y2Cij,z2Cij,…,peCijxeCij,yeCij,zeCij=pcrxcr,ycr,zcr∣∀gridc,r∈thecirclecenteredatthegridi,jPlane function is defined as (2), whose parameters are estimated from the set PijC using the least square method. (2)Ax+By+Cz+D=0For optimizing the parameters of the plane handily, the triangleM is constructed as (3) and the vector b as (4).(3)M=∑k=1exkCij2∑k=1exkCijykCij∑k=1exkCij∑k=1exkCijykCij∑k=1eykCij2∑k=1eykCij∑k=1exkCij∑k=1eykCije(4)b=∑k=1exkCijzkCij,∑k=1eykCijzkCij,∑k=1ezkCijTLet the intermediate variables be computed fromM and b as (5), and then the parameters of the plane are computed as (6).(5)A′,B′,D′T=M-1∗b(6)A,B,C,DT=-A′,-B′,1,-D′TSlope of the terrain centered at the grid(i,j) is estimated using (7), where nPlane is the normal of the plane, and nz=0,0,1T. (7)Slopei,j=arccosdotnPlane,nznPlane∗nz


### 2.1.2. Estimating Roughness of the Terrain

The elevation of the grid(c,r) is represented by G(c,r) 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 (8)Roughnessi,j=StdVarGc,r∣∀gridc,r∈thecirclecenteredatthegridi,j


### 2.1.3. Estimating Step Height of the Terrain

For each grid(c,r) inside the circle centered at the grid (i,j), 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 (c,r), which is shown as blue triangle in Figure 1. The greatest one is selected from all the maximum differences with respect to every grid (c,r) as the step height of the terrain centered at the grid (i,j). (c1,r1) is the grid with respect to the maximum difference of height for the grid (c,r), and noverCrit in Figure 2 is used to record the number of grids (c,r) satisfying that slope of the line between center point of (c,r) and that of (c1,r1). ncrit is the empirical value of noverCrit. 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.(9)step←minhtmp,noverCritncritFigure 2
The processing diagram for computing the step height.


### 2.1.4. 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 δcritSteHeit, then the robot is considered not to traverse such terrain, and the cost with respect to the central grid (i,j) is set to ∞.(ii) Computing Cost of Grid ( i , j ). 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 (i,j) can be computed from three features: slope, roughness, and step height using (10)CBodyCeni,j=∞ifMtraversi,j=0λsSi,jsmax+λrRi,jrmax+λhHi,jhmaxifMtraversi,j=1In (10), Mtravers is sign of the traversability. S,R,H represent slope, roughness, and step height, respectively. λs,λr,λh are weight coefficients, which reflect contributions for the cost value, and quadratic sum of them is 1; i.e., λs2+λr2+λh2=1. smax,rmax,hmax are maximum feature values which are used to normalize the cost value.


## 2.1.1. Estimating Slope of the Terrain

Let the grid(i,j) lie at center of the robot body, and radius of the circle is rcost which is related to the scale of the robot, for each grid (c,r) inside the circle; there is a 3D point related to such gird. Suppose there are e grids totally inside the circle; all the 3D points are represented with an ordered set which is shown in (1)PijC=p1Cijx1Cij,y1Cij,z1Cij,p2Cijx2Cij,y2Cij,z2Cij,…,peCijxeCij,yeCij,zeCij=pcrxcr,ycr,zcr∣∀gridc,r∈thecirclecenteredatthegridi,jPlane function is defined as (2), whose parameters are estimated from the set PijC using the least square method. (2)Ax+By+Cz+D=0For optimizing the parameters of the plane handily, the triangleM is constructed as (3) and the vector b as (4).(3)M=∑k=1exkCij2∑k=1exkCijykCij∑k=1exkCij∑k=1exkCijykCij∑k=1eykCij2∑k=1eykCij∑k=1exkCij∑k=1eykCije(4)b=∑k=1exkCijzkCij,∑k=1eykCijzkCij,∑k=1ezkCijTLet the intermediate variables be computed fromM and b as (5), and then the parameters of the plane are computed as (6).(5)A′,B′,D′T=M-1∗b(6)A,B,C,DT=-A′,-B′,1,-D′TSlope of the terrain centered at the grid(i,j) is estimated using (7), where nPlane is the normal of the plane, and nz=0,0,1T. (7)Slopei,j=arccosdotnPlane,nznPlane∗nz


## 2.1.2. Estimating Roughness of the Terrain

The elevation of the grid(c,r) is represented by G(c,r) 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 (8)Roughnessi,j=StdVarGc,r∣∀gridc,r∈thecirclecenteredatthegridi,j


## 2.1.3. Estimating Step Height of the Terrain

For each grid(c,r) inside the circle centered at the grid (i,j), 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 (c,r), which is shown as blue triangle in Figure 1. The greatest one is selected from all the maximum differences with respect to every grid (c,r) as the step height of the terrain centered at the grid (i,j). (c1,r1) is the grid with respect to the maximum difference of height for the grid (c,r), and noverCrit in Figure 2 is used to record the number of grids (c,r) satisfying that slope of the line between center point of (c,r) and that of (c1,r1). ncrit is the empirical value of noverCrit. 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.(9)step←minhtmp,noverCritncritFigure 2
The processing diagram for computing the step height.


## 2.1.4. 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 δcritSteHeit, then the robot is considered not to traverse such terrain, and the cost with respect to the central grid (i,j) is set to ∞.(ii) Computing Cost of Grid ( i , j ). 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 (i,j) can be computed from three features: slope, roughness, and step height using (10)CBodyCeni,j=∞ifMtraversi,j=0λsSi,jsmax+λrRi,jrmax+λhHi,jhmaxifMtraversi,j=1In (10), Mtravers is sign of the traversability. S,R,H represent slope, roughness, and step height, respectively. λs,λr,λh are weight coefficients, which reflect contributions for the cost value, and quadratic sum of them is 1; i.e., λs2+λr2+λh2=1. smax,rmax,hmax are maximum feature values which are used to normalize the cost value.


## 2.2. Computing Average Terrain Cost Beneath 4 Feet

### 2.2.1. Computing Cost for Selecting Foothold

There are 4 features of the terrain beneath foothold:(i)
f1: curvature of the terrain around the candidate foothold pc, it is used to recognize whether the terrain is flat or not, which is considered for less sliding of the foot.(ii)
f2: slope of the terrain around the candidate foothold pc, it is computed as the angle between the normal at pc and the horizontal surface.(iii)
f3: the feature for illustrating concave terrain or convex one, let p1,p1,…,p8 be 8 points with uniform distribution around the candidate foothold pc, and vi is the directed vector from pc to pi where i∈[1,8]; the intersection angle between vi and the normal of terrain at point pc 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)
f4: the feature for determining uphill terrain or downhill one, the vertical plane P is constructed from the motion direction of the robot d and the vertical upward axis Z, suppose n′ is the vector which is projected onto the plane P from the terrain normal n at the candidate foothold, the rotational angle from d to n′ 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,5mm and 10mm, 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 f1-f4 computed with the radius 5mm and 10mm, respectively.(11)ξ=λ1,λ2,…,λj,…,λ8TNow the cost function is defined as the weighted sum of feature values, which is represented formally as(12)Cfootξ=WTξ=∑k=18ωkλkW is weight vector as (13), which can be optimized using expert learning [19]. (13)W=ω1,ω2,…,ωj,…,ω8T


### 2.2.2. 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 nlf is the number of footholds inside the triangle, ξklf is the feature vector of the kth foothold, and Wlf is the weight vector with respect to left front foot.(14)Clf=∑k=1nlfWlfTξklfnlfCrf=∑k=1nrfWrfTξkrfnrfClb=∑k=1nlbWlbTξklbnlbCrb=∑k=1nrbWrbTξkrbnrbSimilarly, let the center of the robot body lie in the grid(i,j), and the cost related to foot is computed by averaging 4 costs in (14), which is shown in (15)CAvgFeeti,j=Clf+Crf+Clb+Crb4


## 2.2.1. Computing Cost for Selecting Foothold

There are 4 features of the terrain beneath foothold:(i)
f1: curvature of the terrain around the candidate foothold pc, it is used to recognize whether the terrain is flat or not, which is considered for less sliding of the foot.(ii)
f2: slope of the terrain around the candidate foothold pc, it is computed as the angle between the normal at pc and the horizontal surface.(iii)
f3: the feature for illustrating concave terrain or convex one, let p1,p1,…,p8 be 8 points with uniform distribution around the candidate foothold pc, and vi is the directed vector from pc to pi where i∈[1,8]; the intersection angle between vi and the normal of terrain at point pc 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)
f4: the feature for determining uphill terrain or downhill one, the vertical plane P is constructed from the motion direction of the robot d and the vertical upward axis Z, suppose n′ is the vector which is projected onto the plane P from the terrain normal n at the candidate foothold, the rotational angle from d to n′ 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,5mm and 10mm, 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 f1-f4 computed with the radius 5mm and 10mm, respectively.(11)ξ=λ1,λ2,…,λj,…,λ8TNow the cost function is defined as the weighted sum of feature values, which is represented formally as(12)Cfootξ=WTξ=∑k=18ωkλkW is weight vector as (13), which can be optimized using expert learning [19]. (13)W=ω1,ω2,…,ωj,…,ω8T


## 2.2.2. 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 nlf is the number of footholds inside the triangle, ξklf is the feature vector of the kth foothold, and Wlf is the weight vector with respect to left front foot.(14)Clf=∑k=1nlfWlfTξklfnlfCrf=∑k=1nrfWrfTξkrfnrfClb=∑k=1nlbWlbTξklbnlbCrb=∑k=1nrbWrbTξkrbnrbSimilarly, let the center of the robot body lie in the grid(i,j), and the cost related to foot is computed by averaging 4 costs in (14), which is shown in (15)CAvgFeeti,j=Clf+Crf+Clb+Crb4


## 2.3. Searching 2D 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 (i,j) in the grid based elevation map; then the whole cost value related to (i,j) is computed using (16), where CBodyCen(i,j) and CAvgFeet(i,j) are estimated by (10) and (15), respectively. λBodyCen,λAvgFeet are weight coefficients for two kinds of cost, and λBodyCen2+λAvgFeet2=1, which can guarantee the last cost value belongs to the range [0,1].(16)Ci,j=λBodyCenCBodyCeni,j+λAvgFeetCAvgFeeti,jC ( i , j ) in (16) represents the cost for robot body center moving across the grid (i,j) along some direction, in order to search optimal path base on the cost map C 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.(17)CEi,j,i′,j′=βdresoy2i-i′2+resox2j-j′2+βcCi′,j′( i , j ) , i ′ , j ′ are current and adjacent grids and resox,resoy are resolutions of grids in elevation map, βd,βc are weight coefficients which satisfy the constraints: βc>βd≥1, and the larger the βc is, the greater the tendency is taking a detour to find a safe path.The movement directionDi′,j′ for computing the whole cost of grid i′,j′ is determined by the relative positions between grid (i,j) and grid i′,j′, which is represented formally using (18)Di′,j′=j′-jresox,i′-iresoyTDesigning heuristic function is a key step to search optimal path using A∗ algorithm; in our case, the heuristic function is listed as (19), where (it,jt) is the terminal grid, and the heuristic function must satisfy the constraint shown in (20) for searching optimal path.(19)hi,j,it,jt=resoy2i-it2+resox2j-jt2(20)hi,j,it,jt≤CEi,j,i′,j′+hi′,j′,it,jtAccording 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.


## 3. Searching Series of Static Gaits

### 3.1. 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). Figure3 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.Figure 3
Nominal static gait pattern of one circle.The star in Figure3 represents the projection of robot center, which lies in central line of interactional triangle of two supporting triangles. Let V(vx,vy) be the vertex of the interactional triangle and Pc(px,py) be mid-point of the faced edge, projected point of robot body gravity PG is computed using (21), where α is coefficient of proportionality with range 0,05. (21)PG=αPc-V+VIn 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 Plane, and a vertical line across the robot center interacts with the plane Plane at point pint; the distance between pint 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 PB is center of two projections with respect to back feet; PF is center with respect to front feet. The yaw angle is computed from the directed vector DYaw=PBPF→.Figure 4
Yaw angle computation.(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 pointPG3D is found from the plane in which there are 3 supporting points, and projection of PG3D onto the horizontal plane is PG.(ii)
PG3D is translated upward by height of the robot body center, which is used as the coordinate of robot body center.(iii)
Rotational transformationRYaw is computed from yaw angle.(iv)
Another rotational transformationRRollPitch is computed from roll and pitch angle.(v)
Rotational transformation of robot body is computed byRrobot=RYaw∗RRollPitch.


### 3.2. 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.


#### 3.2.1. 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 Figure5.Figure 5
Foot Trajectory.The robot is projected into 2D planeY=0, and it walks along X positive direction, X coordinate of start point is 0.1m, and that of end point is 0.17m, 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.Table 1
Key points in the foot trajectory.
1 2 3 4 5 6 7 X 0.100 0.100 0.100 0.135 0.170 0.170 0.170 Z -0.270 -0.235 -0.200 -0.200 -0.200 -0.235 -0.270


#### 3.2.2. 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. Figure6 shows the process of adjusting robot body, given that the foot is fixed (taking right front foot as an example).Figure 6
Moving trajectory of the robot body.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 planeY=0, key poses are listed in Table 2Table 2
Key poses in the body adjusting trajectory.
1 2 3 4 5 X 0 0.025 0.05 0.05 0.05 α Y 0 0 0 −7.5° −15°


### 3.3. Searching Series of Robot States

#### 3.3.1. Defining Robot State

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(22)s=foot,roll,pitch,heightNow 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).(23)Fcandi=f1,f2,…,fnfootIn (23), there are nfoot candidates in ordered set Fcandi; cost value of f1 is smallest and that of fnfoot is largest.(b) Set of Roll Angles.Ordered set of roll angles is shown in (24), where nroll is the number of candidate roll angles, default roll angle is r, and step length for adjusting roll angle is Δr. (24)Rcandi=r1,r2,…,rnroll=r,r+Δr,r-Δr,r+2Δr,r-2Δr,…(c) Set of Pitch Angles.Similar to the set of roll angles, that of pitch angles is shown in (25), where npitch is the number of candidate roll angles, default pitch angle is p, and step length for adjusting pitch angle is Δp. (25)Pcandi=p1,p2,…,pnpitch=p,p+Δp,p-Δp,p+2Δp,p-2Δp,…(d) Set of Heights.Set of vertical distance between robot body center and terrain is shown in (26), where nheight is the number of candidate heights, default height is h, and step length for adjusting h is Δh. (26)Hcandi=h1,h2,…,hnheight=h,h+Δh,h-Δh,h+2Δh,h-2Δh,…Let the number of states for next step ben, then it can be computed using (27)n=nfoot×nroll×npitch×nheightSet ofn states Scandi is ordered in terms of priority as (28); the first element in the set has the highest priority. (28)Scandi=s1,s2,…,sn=f1r1p1h1,f1r1p1h2,…,f1r1p2h1,…,f1r2p1h1,…,f2r1p1h1,…According to the order in setScandi, priority for adjusting 4 parameters is(29)Heightofrobotcenter≻Pitchangle≻Rollangle≻FootholdTerrain 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.


#### 3.3.2. Searching Gaits Recursively

When the quadruped robot explores one step forwardly, there aren candidate states, and they are selected in turn by the element order in the set Scandi in (28). Several steps can be searched one time; robot state of kth step is si1i2...ik, which indicates that the ikth state is selected from n candidates for kth step. Let ordered set of candidate robot state for k+1th step be Scandik+1 as shown in (30)Scandik+1=s1k+1,s2k+1,…,snk+1Letsjk+1 be the first state from the set Scandik+1 that satisfies the following conditions:(i)
Robot statesjk+1 will not violate kinematic constraint of both robot body and leg.(ii)
No collision will occur when the robot is moving from statesi1i2...ik to state sjk+1.Ifsjk+1 is found from Scandik+1, then the robot state for k+1th step is si1i2...ikik+1=sjk+1; else no possible robot state can be continuously searched from state si1i2...ik; now backtracking must be carried out. Searching process restarts from the ik+1th candidate state of kth step and state for next step k+1 is searched until reaching to the target searching depth.The searching process with recursive backtracking is represented by a recursive tree shown in Figure7. For convenience, suppose there are only 2 candidate states for each searching step, gaits of 4 steps are illustrated by green line with arrow.Figure 7
Tree-style structure for recursively searching series of static gaits.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 froms12 to s121, then the children states s1211 and s1212 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.


## 3.1. 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). Figure3 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.Figure 3
Nominal static gait pattern of one circle.The star in Figure3 represents the projection of robot center, which lies in central line of interactional triangle of two supporting triangles. Let V(vx,vy) be the vertex of the interactional triangle and Pc(px,py) be mid-point of the faced edge, projected point of robot body gravity PG is computed using (21), where α is coefficient of proportionality with range 0,05. (21)PG=αPc-V+VIn 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 Plane, and a vertical line across the robot center interacts with the plane Plane at point pint; the distance between pint 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 PB is center of two projections with respect to back feet; PF is center with respect to front feet. The yaw angle is computed from the directed vector DYaw=PBPF→.Figure 4
Yaw angle computation.(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 pointPG3D is found from the plane in which there are 3 supporting points, and projection of PG3D onto the horizontal plane is PG.(ii)
PG3D is translated upward by height of the robot body center, which is used as the coordinate of robot body center.(iii)
Rotational transformationRYaw is computed from yaw angle.(iv)
Another rotational transformationRRollPitch is computed from roll and pitch angle.(v)
Rotational transformation of robot body is computed byRrobot=RYaw∗RRollPitch.


## 3.2. 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.


### 3.2.1. 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 Figure5.Figure 5
Foot Trajectory.The robot is projected into 2D planeY=0, and it walks along X positive direction, X coordinate of start point is 0.1m, and that of end point is 0.17m, 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.Table 1
Key points in the foot trajectory.
1 2 3 4 5 6 7 X 0.100 0.100 0.100 0.135 0.170 0.170 0.170 Z -0.270 -0.235 -0.200 -0.200 -0.200 -0.235 -0.270


### 3.2.2. 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. Figure6 shows the process of adjusting robot body, given that the foot is fixed (taking right front foot as an example).Figure 6
Moving trajectory of the robot body.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 planeY=0, key poses are listed in Table 2Table 2
Key poses in the body adjusting trajectory.
1 2 3 4 5 X 0 0.025 0.05 0.05 0.05 α Y 0 0 0 −7.5° −15°


## 3.2.1. 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 Figure5.Figure 5
Foot Trajectory.The robot is projected into 2D planeY=0, and it walks along X positive direction, X coordinate of start point is 0.1m, and that of end point is 0.17m, 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.Table 1
Key points in the foot trajectory.
1 2 3 4 5 6 7 X 0.100 0.100 0.100 0.135 0.170 0.170 0.170 Z -0.270 -0.235 -0.200 -0.200 -0.200 -0.235 -0.270


## 3.2.2. 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. Figure6 shows the process of adjusting robot body, given that the foot is fixed (taking right front foot as an example).Figure 6
Moving trajectory of the robot body.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 planeY=0, key poses are listed in Table 2Table 2
Key poses in the body adjusting trajectory.
1 2 3 4 5 X 0 0.025 0.05 0.05 0.05 α Y 0 0 0 −7.5° −15°


## 3.3. Searching Series of Robot States

### 3.3.1. Defining Robot State

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(22)s=foot,roll,pitch,heightNow 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).(23)Fcandi=f1,f2,…,fnfootIn (23), there are nfoot candidates in ordered set Fcandi; cost value of f1 is smallest and that of fnfoot is largest.(b) Set of Roll Angles.Ordered set of roll angles is shown in (24), where nroll is the number of candidate roll angles, default roll angle is r, and step length for adjusting roll angle is Δr. (24)Rcandi=r1,r2,…,rnroll=r,r+Δr,r-Δr,r+2Δr,r-2Δr,…(c) Set of Pitch Angles.Similar to the set of roll angles, that of pitch angles is shown in (25), where npitch is the number of candidate roll angles, default pitch angle is p, and step length for adjusting pitch angle is Δp. (25)Pcandi=p1,p2,…,pnpitch=p,p+Δp,p-Δp,p+2Δp,p-2Δp,…(d) Set of Heights.Set of vertical distance between robot body center and terrain is shown in (26), where nheight is the number of candidate heights, default height is h, and step length for adjusting h is Δh. (26)Hcandi=h1,h2,…,hnheight=h,h+Δh,h-Δh,h+2Δh,h-2Δh,…Let the number of states for next step ben, then it can be computed using (27)n=nfoot×nroll×npitch×nheightSet ofn states Scandi is ordered in terms of priority as (28); the first element in the set has the highest priority. (28)Scandi=s1,s2,…,sn=f1r1p1h1,f1r1p1h2,…,f1r1p2h1,…,f1r2p1h1,…,f2r1p1h1,…According to the order in setScandi, priority for adjusting 4 parameters is(29)Heightofrobotcenter≻Pitchangle≻Rollangle≻FootholdTerrain 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.


### 3.3.2. Searching Gaits Recursively

When the quadruped robot explores one step forwardly, there aren candidate states, and they are selected in turn by the element order in the set Scandi in (28). Several steps can be searched one time; robot state of kth step is si1i2...ik, which indicates that the ikth state is selected from n candidates for kth step. Let ordered set of candidate robot state for k+1th step be Scandik+1 as shown in (30)Scandik+1=s1k+1,s2k+1,…,snk+1Letsjk+1 be the first state from the set Scandik+1 that satisfies the following conditions:(i)
Robot statesjk+1 will not violate kinematic constraint of both robot body and leg.(ii)
No collision will occur when the robot is moving from statesi1i2...ik to state sjk+1.Ifsjk+1 is found from Scandik+1, then the robot state for k+1th step is si1i2...ikik+1=sjk+1; else no possible robot state can be continuously searched from state si1i2...ik; now backtracking must be carried out. Searching process restarts from the ik+1th candidate state of kth step and state for next step k+1 is searched until reaching to the target searching depth.The searching process with recursive backtracking is represented by a recursive tree shown in Figure7. For convenience, suppose there are only 2 candidate states for each searching step, gaits of 4 steps are illustrated by green line with arrow.Figure 7
Tree-style structure for recursively searching series of static gaits.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 froms12 to s121, then the children states s1211 and s1212 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.


## 3.3.1. Defining Robot State

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(22)s=foot,roll,pitch,heightNow 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).(23)Fcandi=f1,f2,…,fnfootIn (23), there are nfoot candidates in ordered set Fcandi; cost value of f1 is smallest and that of fnfoot is largest.(b) Set of Roll Angles.Ordered set of roll angles is shown in (24), where nroll is the number of candidate roll angles, default roll angle is r, and step length for adjusting roll angle is Δr. (24)Rcandi=r1,r2,…,rnroll=r,r+Δr,r-Δr,r+2Δr,r-2Δr,…(c) Set of Pitch Angles.Similar to the set of roll angles, that of pitch angles is shown in (25), where npitch is the number of candidate roll angles, default pitch angle is p, and step length for adjusting pitch angle is Δp. (25)Pcandi=p1,p2,…,pnpitch=p,p+Δp,p-Δp,p+2Δp,p-2Δp,…(d) Set of Heights.Set of vertical distance between robot body center and terrain is shown in (26), where nheight is the number of candidate heights, default height is h, and step length for adjusting h is Δh. (26)Hcandi=h1,h2,…,hnheight=h,h+Δh,h-Δh,h+2Δh,h-2Δh,…Let the number of states for next step ben, then it can be computed using (27)n=nfoot×nroll×npitch×nheightSet ofn states Scandi is ordered in terms of priority as (28); the first element in the set has the highest priority. (28)Scandi=s1,s2,…,sn=f1r1p1h1,f1r1p1h2,…,f1r1p2h1,…,f1r2p1h1,…,f2r1p1h1,…According to the order in setScandi, priority for adjusting 4 parameters is(29)Heightofrobotcenter≻Pitchangle≻Rollangle≻FootholdTerrain 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.


## 3.3.2. Searching Gaits Recursively

When the quadruped robot explores one step forwardly, there aren candidate states, and they are selected in turn by the element order in the set Scandi in (28). Several steps can be searched one time; robot state of kth step is si1i2...ik, which indicates that the ikth state is selected from n candidates for kth step. Let ordered set of candidate robot state for k+1th step be Scandik+1 as shown in (30)Scandik+1=s1k+1,s2k+1,…,snk+1Letsjk+1 be the first state from the set Scandik+1 that satisfies the following conditions:(i)
Robot statesjk+1 will not violate kinematic constraint of both robot body and leg.(ii)
No collision will occur when the robot is moving from statesi1i2...ik to state sjk+1.Ifsjk+1 is found from Scandik+1, then the robot state for k+1th step is si1i2...ikik+1=sjk+1; else no possible robot state can be continuously searched from state si1i2...ik; now backtracking must be carried out. Searching process restarts from the ik+1th candidate state of kth step and state for next step k+1 is searched until reaching to the target searching depth.The searching process with recursive backtracking is represented by a recursive tree shown in Figure7. For convenience, suppose there are only 2 candidate states for each searching step, gaits of 4 steps are illustrated by green line with arrow.Figure 7
Tree-style structure for recursively searching series of static gaits.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 froms12 to s121, then the children states s1211 and s1212 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.


## 4. Simulation Result and Discussion

### 4.1. 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 Figure8, and the foot can be reached anywhere within its motion space.Figure 8
DH parameters based model of the quadruped robot.In Figure8, a is the half length of robot body, b is the half width of robot body, c is the width of shoulder, d is the length of thigh, and e 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. Z0 and Z4 are two virtual axes introduced for convenient representation, but there is no rotational degree of freedom actually. Z0 lies at the center of robot body, and it is supposed that X0 and X1 share the same line.Variable DH parameters of the model above are shown in Tables3 and 4. θs in the tables above are related to the joint angles directly, whose ranges are shown in Table 5.Table 3
The DH parameters for the left legs.
LF LB i a i d i θ i α i a i d i θ i α i 1 b 0 0 90° b 0 0 9 0 ° 2 0 a θ 2 L F 9 0 ° 0 - a θ 2 L B 9 0 ° 3 - d c θ 3 L F 0 - d c θ 2 L B 0 4 - e 0 θ 4 L F 0 - e 0 θ 2 L B 0Table 4
The DH parameters for the right legs.
RF RB i a i d i θ i α i a i d i θ i α i 1 - b 0 0 90° - b 0 0 9 0 ° 2 0 a θ 2 R F 9 0 ° 0 - a θ 2 R B 9 0 ° 3 - d - c θ 3 R F 0 - d - c θ 3 R B 0 4 - e 0 θ 4 R F 0 - e 0 θ 4 R B 0Table 5
Range of the joint angle.
θ 2 L F , θ 2 R F , θ 2 L B , θ 2 R B θ3LF,θ3RF,θ3LB,θ3RB θ4LF,θ4RF θ4LB,θ4RB  7 5 ° , 10 5 ° - 4 5 ° , 4 5 ° - 18 0 ° , 0 ° [ 0 ° , 18 0 ° ]Constant parameters are shown in Table6.Table 6
Constant parameters of the model.
a b c d e 0.15m 0.07m 0.02m 0.09m 0.1m


### 4.2. 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.4m and width of 0.04m; height of the cuboid is randomized in the range [0m,0.06m]. 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.Table 7
Parameters for generating candidate body pose.
r Δ r p Δ p h Δ h n r o l l n p i t c h n h e i g h t 0 ° 5 ° 0 ° 5 ° 0.14 m 0.015 m 5 5 5Standard static gaits for two circles are shown in Figure9. 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.Figure 9
Series of standard static gaits for two circles on structured terrain.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 Figure9. 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 Figure10; 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.Figure 10
Different searching results by looking one and four steps ahead, respectively.
(a)
Looking one step ahead (recursive depth=1) (b)
Looking four steps ahead (recursive depth=4)


### 4.3. Walking on Complex Unstructured Terrain

Simulation in Section4.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 is1.4m×1.2m, and 2.5D elevation map based on grid with respect to such terrain model is shown in Figure 11(a).Figure 11
Geometric features of 3D terrain under the robot body.
(a)
2.5D elevation map based on grid (b)
Roughness (c)
Slope (d)
Step heightThe radius for computing terrain features (roughness, slope, and step height) isrcost=0.1m; maps of their feature values are shown in Figures 11(b), 11(c), and 11(d), respectively, where color represents feature value.Cost map is constructed from three feature maps using (10), where parameters are set as Table 8.Table 8
Parameters for computing cost value.
λ s λ r λ h s m a x r m a x h m a x 0.667 0.667 0.333 1.5708 R a d 0.02 m 0.035 mThe larger some feature weight is, the greater its contribution to the cost function is. Taking slope feature as an example, ifλs is larger, then the robot will walk along a path far from the point with deep slope, although that path may be longer than that crossing such point. In our simulation environment, step height is relatively low, and the robot can walk across nearly all the steps, so λh is set lower in this paper. The proportion of these 3 parameters is (31)λh:λr:λs=1:2:2The computed cost map is shown in Figure12, where the cost value is represented by the color. According to simulation result in the figure, the cost value computed from the approach proposed in this paper can measure the complexity of the terrain accurately.Figure 12
Cost map of the 3D terrain.The cost map in Figure12 is projected into 2D space, in which 2D path of the robot body center is heuristically searched using the updated A∗ algorithm discussed in Section 2.3. βd,βc in (17) are parameters for weighing moving distance and walking safety. Figure 13 shows path of body center searched with two kinds of parameters, βd=1,βc=5 and βd=1,βc=10, respectively, where color stands for the cost value of the terrain.Figure 13
Trajectory of 2D body center.Coordinate of the start point is(-0.05m,0.05m) or [1,71] in grid unit, and coordinate of the end point is (0.65m,-0.075m) or [141,46] in grid unit. Resolution of grids in both X and Y direction is 5mm. Point in the search path is so dense that only one point among three points is shown in Figure 13. According to the simulation result in the figure, if the parameter βc is set larger, then the robot will avoid the region with higher cost terrain, though it may walk for a longer distance.The 2D path of body center with parameterβd=1,βc=5 is selected, and the quadruped robot is planned to walk along such 2D path using the algorithm proposed in the paper; the real walking trajectory is shown in Figure 14. For showing clearly, 2.5D elevation map is projected into 2D space, where the elevation value is represented by the color. According to the simulation result in the figure, the projected point of robot body center moves along the searched 2D path, and none of the feet touches the terrain with higher cost, which can demonstrate the effectiveness of the proposed planning algorithm for driving the quadruped robot walking on complex terrain.Figure 14
Motion trajectory of the quadruped.


## 4.1. 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 Figure8, and the foot can be reached anywhere within its motion space.Figure 8
DH parameters based model of the quadruped robot.In Figure8, a is the half length of robot body, b is the half width of robot body, c is the width of shoulder, d is the length of thigh, and e 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. Z0 and Z4 are two virtual axes introduced for convenient representation, but there is no rotational degree of freedom actually. Z0 lies at the center of robot body, and it is supposed that X0 and X1 share the same line.Variable DH parameters of the model above are shown in Tables3 and 4. θs in the tables above are related to the joint angles directly, whose ranges are shown in Table 5.Table 3
The DH parameters for the left legs.
LF LB i a i d i θ i α i a i d i θ i α i 1 b 0 0 90° b 0 0 9 0 ° 2 0 a θ 2 L F 9 0 ° 0 - a θ 2 L B 9 0 ° 3 - d c θ 3 L F 0 - d c θ 2 L B 0 4 - e 0 θ 4 L F 0 - e 0 θ 2 L B 0Table 4
The DH parameters for the right legs.
RF RB i a i d i θ i α i a i d i θ i α i 1 - b 0 0 90° - b 0 0 9 0 ° 2 0 a θ 2 R F 9 0 ° 0 - a θ 2 R B 9 0 ° 3 - d - c θ 3 R F 0 - d - c θ 3 R B 0 4 - e 0 θ 4 R F 0 - e 0 θ 4 R B 0Table 5
Range of the joint angle.
θ 2 L F , θ 2 R F , θ 2 L B , θ 2 R B θ3LF,θ3RF,θ3LB,θ3RB θ4LF,θ4RF θ4LB,θ4RB  7 5 ° , 10 5 ° - 4 5 ° , 4 5 ° - 18 0 ° , 0 ° [ 0 ° , 18 0 ° ]Constant parameters are shown in Table6.Table 6
Constant parameters of the model.
a b c d e 0.15m 0.07m 0.02m 0.09m 0.1m


## 4.2. 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.4m and width of 0.04m; height of the cuboid is randomized in the range [0m,0.06m]. 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.Table 7
Parameters for generating candidate body pose.
r Δ r p Δ p h Δ h n r o l l n p i t c h n h e i g h t 0 ° 5 ° 0 ° 5 ° 0.14 m 0.015 m 5 5 5Standard static gaits for two circles are shown in Figure9. 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.Figure 9
Series of standard static gaits for two circles on structured terrain.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 Figure9. 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 Figure10; 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.Figure 10
Different searching results by looking one and four steps ahead, respectively.
(a)
Looking one step ahead (recursive depth=1) (b)
Looking four steps ahead (recursive depth=4)


## 4.3. Walking on Complex Unstructured Terrain

Simulation in Section4.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 is1.4m×1.2m, and 2.5D elevation map based on grid with respect to such terrain model is shown in Figure 11(a).Figure 11
Geometric features of 3D terrain under the robot body.
(a)
2.5D elevation map based on grid (b)
Roughness (c)
Slope (d)
Step heightThe radius for computing terrain features (roughness, slope, and step height) isrcost=0.1m; maps of their feature values are shown in Figures 11(b), 11(c), and 11(d), respectively, where color represents feature value.Cost map is constructed from three feature maps using (10), where parameters are set as Table 8.Table 8
Parameters for computing cost value.
λ s λ r λ h s m a x r m a x h m a x 0.667 0.667 0.333 1.5708 R a d 0.02 m 0.035 mThe larger some feature weight is, the greater its contribution to the cost function is. Taking slope feature as an example, ifλs is larger, then the robot will walk along a path far from the point with deep slope, although that path may be longer than that crossing such point. In our simulation environment, step height is relatively low, and the robot can walk across nearly all the steps, so λh is set lower in this paper. The proportion of these 3 parameters is (31)λh:λr:λs=1:2:2The computed cost map is shown in Figure12, where the cost value is represented by the color. According to simulation result in the figure, the cost value computed from the approach proposed in this paper can measure the complexity of the terrain accurately.Figure 12
Cost map of the 3D terrain.The cost map in Figure12 is projected into 2D space, in which 2D path of the robot body center is heuristically searched using the updated A∗ algorithm discussed in Section 2.3. βd,βc in (17) are parameters for weighing moving distance and walking safety. Figure 13 shows path of body center searched with two kinds of parameters, βd=1,βc=5 and βd=1,βc=10, respectively, where color stands for the cost value of the terrain.Figure 13
Trajectory of 2D body center.Coordinate of the start point is(-0.05m,0.05m) or [1,71] in grid unit, and coordinate of the end point is (0.65m,-0.075m) or [141,46] in grid unit. Resolution of grids in both X and Y direction is 5mm. Point in the search path is so dense that only one point among three points is shown in Figure 13. According to the simulation result in the figure, if the parameter βc is set larger, then the robot will avoid the region with higher cost terrain, though it may walk for a longer distance.The 2D path of body center with parameterβd=1,βc=5 is selected, and the quadruped robot is planned to walk along such 2D path using the algorithm proposed in the paper; the real walking trajectory is shown in Figure 14. For showing clearly, 2.5D elevation map is projected into 2D space, where the elevation value is represented by the color. According to the simulation result in the figure, the projected point of robot body center moves along the searched 2D path, and none of the feet touches the terrain with higher cost, which can demonstrate the effectiveness of the proposed planning algorithm for driving the quadruped robot walking on complex terrain.Figure 14
Motion trajectory of the quadruped.


## 5. Conclusions

In this paper, a hierarchical algorithm for planning quadruped robot walking on rough terrain is proposed. There are two steps to execute the algorithm; the first step is searching the trajectory of robot center using heuristic algorithm A∗, in which the terrain feature beneath the foothold is also considered; the second step is searching series of static gait along the trajectory of robot center, and the searching space is shrunk greatly, so the time complexity of the algorithm is reduced clearly. In aspect of space complexity, robot state is introduced for representing node of static gait, and tree-like structure is constructed to store such states for saving memory space overhead. Lastly, a classical model of quadruped robot is defined, and then the whole algorithm is validated in simulation environment.



---

*Source: 3153195-2019-05-02.xml*