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.
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 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.
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 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.
2.1.1. 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 .
2.1.2. 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
2.1.3. 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 . is the grid with respect to the maximum difference of height for the grid , and in Figure 2 is used to record the number of grids satisfying that slope of the line between center point of and that of . is the empirical value of . 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.
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 , 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), 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.e., . 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): 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): slope of the terrain around the candidate foothold , it is computed as the angle between the normal at and the horizontal surface.(iii): the feature for illustrating concave terrain or convex one, let be 8 points with uniform distribution around the candidate foothold , and is the directed vector from to where ; 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): 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, and , respectively. As shown in (11), is feature vector of the terrain beneath foothold, and and in the feature vector are terrain features computed with the radius and , respectively.
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].
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 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
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 in the grid based elevation map; then the whole cost value related to is computed using (16), where and are estimated by (10) and (15), respectively. are weight coefficients for two kinds of cost, and , which can guarantee the last cost value belongs to the range .
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: , 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.
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). 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 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 .
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 is found from the plane in which there are 3 supporting points, and projection of onto the horizontal plane is .(ii) 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 .
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 Figure 5.
The robot is projected into 2D plane , and it walks along positive direction, coordinate of start point is , and that of end point is , 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.
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. 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 , key poses are listed in Table 2
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
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 is smallest and that of is largest.
(b) Set of Roll Angles. Ordered set of roll angles is shown in (24), where is the number of candidate roll angles, default roll angle is , and step length for adjusting roll angle is .
(c) Set of Pitch Angles. Similar to the set of roll angles, that of pitch angles is shown in (25), where is the number of candidate roll angles, default pitch angle is , and step length for adjusting pitch angle is .
(d) Set of Heights. Set of vertical distance between robot body center and terrain is shown in (26), where is the number of candidate heights, default height is , and step length for adjusting h is .
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
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.
3.3.2. 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 , which indicates that the th state is selected from candidates for th step. Let ordered set of candidate robot state for th step be as shown in
Let be the first state from the set that satisfies the following conditions:(i)Robot state will not violate kinematic constraint of both robot body and leg.(ii)No collision will occur when the robot is moving from state to state .
If is found from , then the robot state for th step is ; else no possible robot state can be continuously searched from state ; now backtracking must be carried out. Searching process restarts from the th candidate state of th step and state for next step 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 to , then the children states and 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 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. and are two virtual axes introduced for convenient representation, but there is no rotational degree of freedom actually. lies at the center of robot body, and it is supposed that and 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.
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 and width of ; height of the cuboid is randomized in the range . 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.
(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 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 , and 2.5D elevation map based on grid with respect to such terrain model is shown in Figure 11(a).
(a) 2.5D elevation map based on grid
(b) Roughness
(c) Slope
(d) Step height
The radius for computing terrain features (roughness, slope, and step height) is ; 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.
The larger some feature weight is, the greater its contribution to the cost function is. Taking slope feature as an example, if 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 is set lower in this paper. The proportion of these 3 parameters is
The computed cost map is shown in Figure 12, 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.
The cost map in Figure 12 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. in (17) are parameters for weighing moving distance and walking safety. Figure 13 shows path of body center searched with two kinds of parameters, and , respectively, where color stands for the cost value of the terrain.
Coordinate of the start point is or in grid unit, and coordinate of the end point is or in grid unit. Resolution of grids in both X and Y direction is . 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 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 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.
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.
Data Availability
The datasets generated during and/or analysed during the current study are available from the corresponding author upon reasonable request.
Conflicts of Interest
The authors declare that they have no conflicts of interest.
Acknowledgments
This work has been supported by Fundamental Research Funds for the Central Universities (Grant no. 2572017BB12); the State Key Laboratory of Robotics and System (HIT) Open Fund (Grant no. SKLRS-2017-KF-06); the National Natural Science Foundation of China (Grant no. 16051575097); the National Science Foundation for Postdoctoral Scientists of China (Grant nos. 2015M571381 and 2017T100217); and the Natural Science Foundation of Heilongjiang Province (Grant no. JJ2019LH2071).