Learning the Cost Function for Foothold Selection in a Quadruped Robot ^†

Abstract

This paper is focused on designing a cost function of selecting a foothold for a physical quadruped robot walking on rough terrain. The quadruped robot is modeled with Denavit–Hartenberg (DH) parameters, and then a default foothold is defined based on the model. Time of Flight (TOF) camera is used to perceive terrain information and construct a 2.5D elevation map, on which the terrain features are detected. The cost function is defined as the weighted sum of several elements including terrain features and some features on the relative pose between the default foothold and other candidates. It is nearly impossible to hand-code the weight vector of the function, so the weights are learned using Supporting Vector Machine (SVM) techniques, and the training data set is generated from the 2.5D elevation map of a real terrain under the guidance of experts. Four candidate footholds around the default foothold are randomly sampled, and the expert gives the order of such four candidates by rotating and scaling the view for seeing clearly. Lastly, the learned cost function is used to select a suitable foothold and drive the quadruped robot to walk autonomously across the rough terrain with wooden steps. Comparing to the approach with the original standard static gait, the proposed cost function shows better performance.

1. Introduction

To operate autonomously on a rough terrain, a quadruped robot has to perceive the environment, build the terrain model, localize itself and plan its path and gait. There are several types of terrain models that can be used for a mobile robot, such as raw point cloud, octree [1], multi-level surface (MLS) maps [2] and a 2.5D elevation map [3], etc. The 2.5D elevation map is chosen as the model for selecting footholds in this paper, and several frames from TOF camera are captured and registered [4,5,6] to generate the complete point cloud of the terrain, and then the elevation map is constructed based on the cloud. In order to improve the map, measuring error [7] of the TOF camera should be reduced as much as possible, and some new technologies [8] can be applied to increase efficiency of the map generation. Selecting a suitable foothold on the 2.5D elevation map for each foot is a key technology, which will take terrain features, robot trunk pose and collision between leg and terrain, etc. into consideration. It is nearly impossible to adjust the parameters of the foothold selection system, and using machine learning techniques seems unavoidable.

There have been so many impressive works [9,10,11,12,13,14] to autonomously plan a legged robot and select a proper foothold on rough terrains. Barasuol et al. [9] proposed an algorithm for reactive trotting with foot placement corrections through visual pattern classification, it only classifies the terrain, but it did not evaluate the terrain complexity for each candidate foothold because it is designed for quadruped trotting with the help of Motion Capture System. Fankhauser et al. [10] designed a foothold score function that is a linear of different quality measures including only terrain features, for example, the slope, curvature and roughness, and so on. Mastalli et al. [11] decomposed the quadruped robot walking problem into body action planning and footstep planning, in the part of footstep planning, a cost function is also defined that considered terrain features and robot body stability, although the default footholds were not employed in their approach. Central Pattern Generators (CPGs) [12] are often used to design a controlling system [13,14] for quadruped robots, which simulate the animal walking regularity. In general application of CPGs [14] on quadruped robots, CPGs is responsible for generating torque series to adjust the robot pose and leg. Another branch of the research on autonomously controlling quadruped robot on rough terrain is based on learning related techniques. A large number of excellent works [15,16,17] are spawned as a result of DARPA’s project [18] concentrating on developing an intelligent algorithm on learning locomotion with a LittleDog [19] platform. Many of these works assumed a known model of the environment, and nearly perfect pose estimation using a motion capture system. Only few works [20] plan the Littledog to walk on rough terrain in real time.

Let us be focused on the problem of selecting a foothold for the quadruped robot. In this paper, a cost function of selecting foothold is designed and learned for a real quadruped robot driven by hydraulic pressure. In fact, the idea of selecting footholds for quadruped robots has been introduced in our previous conference paper [21], which was published in the proceedings of the 2nd International Conference on Advanced Robotics and Mechatronics, but that paper mainly described the approach for computing terrain features in simulation environments. This paper is focused on applying the foothold selection idea on the real quadruped robot, including modelling the real robot and learning parameters for foothold selection on the real terrain.The parameters for selecting foothold are optimized using an expert based learning technique, and the training data set are acquired by constructing the 2.5D elevation map of real physical terrain using a TOF camera. There are some similar works that learn to select foothold for legged robot, such as Zico Kolter et al. [16], select the best foothold from several randomly sampled candidate footholds on the point cloud map to generate a training set for learning cost function, given that the terrain information is fully known. Belter et al. [22] learned a decision surface that can classify the terrain to distinguish between the good and the poor footholds, although the training set is generated only from a simulation environment. In fact, the idea of this paper comes from an excellent work [23], which also introduced the default foothold for selecting a suitable foothold. This idea was carried out in a simulation environment with a structured terrain, and only the candidate footholds on the front or back of the default foothold are selected, so some better candidate footholds on the right or left of the default foothold may be missed out.

The remainder of the paper is organized as follows: Section 2 presents the model of the physical quadruped robot, and introduces default foothold based on the model; Section 3 describes the algorithms to compute terrain features contributing the cost function; Section 4 defines the cost function of selecting a foothold, whose inputs include several types of features; Section 5 describes how to generate training data set, and the parameters of the cost function are learned using such a data set; Section 6 will apply the learned cost function to the real quadruped robot to navigate it across a rough terrain composed of several wooden steps; the last Section 7 will conclude the whole paper.

2. Modelling the Quadruped Robot

2.1. DH Parameters of the Model

The cost of selecting a foothold is a function whose inputs include relative pose between default foothold and that candidate foothold, in order to compute default foothold, the model of the quadruped robot shown in Figure 1 is represented with DH parameters. There are three degrees for each leg of the robot, which are driven by three hydraulic cylinders, respectively.

A kinematic model of the robot is shown in Figure 2, and $Z_1,Z_1,Z_3$ are the rotational axes for three joint angles of the right back leg. a is the half length of robot body, b is the half width of robot body, c indicates the length of the connecting rod between the body and thigh, d is the length of thigh and e is the length of shank, these constant parameters are listed in

Table 1. Constant parameters of the model.

$\mathit{a}\left(\mathbf{m}\right)$	$\mathit{b}\left(\mathbf{m}\right)$	$\mathit{c}\left(\mathbf{m}\right)$	$\mathit{d}\left(\mathbf{m}\right)$	$\mathit{e}\left(\mathbf{m}\right)$
$0.5007$	$0.175$	$0.0852$	$0.31$	$0.3557$

. Taking the right back leg as an example, the red arrow line represents a rotation axis (Z), and the green arrow line represents a common vertical line between two rotational axises, whose direction is equal to that of the axis X. $Z_0$ and $Z_4$ are two virtual axes introduced for convenient representation, but there is no rotational degree of freedom actually. $Z_0$ lies at the center of robot body, and it is supposed that $X_0$ and $X_1$ share the same line.

Taking the right back leg as the example, let ${\alpha }^{RB},{\beta }^{RB},{\gamma }^{RB}$ represent degrees w.r.t. side-sway joint (around $Z_1$ axis), hip joint and knee joint, respectively. The sign of the angle is determined in terms of the right-hand screw rule, and their ranges are listed in

Table 2. Ranges for the joint angle of the right back leg.

${\mathit{\alpha }}^{\mathit{RB}}\left({}^{\circ }\right)$	${\mathit{\beta }}^{\mathit{RB}}\left({}^{\circ }\right)$	${\mathit{\gamma }}^{\mathit{RB}}\left({}^{\circ }\right)$
$[-42.6426,31.0461]$	$[14.1359,120.3034]$	$[81.5307,180.7252]$

.

DH parameters of the model will be computed for resolving inverse kinematic of the foot, all the parameters are shown in

Table 3. DH parameters for the right back leg.

i	${\mathit{a}}_{\mathit{i}}\left(\mathbf{m}\right)$	${\mathit{d}}_{\mathit{i}}\left(\mathbf{m}\right)$	${\mathit{\theta }}_{\mathit{i}}\left({}^{\circ }\right)$	${\mathit{\alpha }}_{\mathit{i}}\left({}^{\circ }\right)$
1	$-b$	0	0	90
2	$-c$	$-a$	${\theta }_2=90+{\alpha }^{RB}$	90,
3	$-d$	0	${\theta }_3=-90+{\beta }^{RB}$	0,
4	$-e$	0	${\theta }_4=180-{\gamma }^{RB}$	0.

. Kinematics equation of right the back foot is represented as Equation (1).

$$\left\{\begin{array}{c}{\displaystyle X_{RB}=dcos\left({\beta }^{RB}\right)-ecos\left({\gamma }^{RB}-{\beta }^{RB}\right),}\hfill \\ {\displaystyle Y_{RB}=\left(dsin\left({\beta }^{RB}\right)+esin\left({\gamma }^{RB}-{\beta }^{RB}\right)+c\right)sin\left({\alpha }^{RB}\right),}\hfill \\ {\displaystyle Z_{RB}=-\left(dsin\left({\beta }^{RB}\right)+esin\left({\gamma }^{RB}-{\beta }^{RB}\right)+c\right)cos\left({\alpha }^{RB}\right).}\hfill \end{array}\right.$$

(1)

2.2. Default Foothold

The default foothold in the 2D plane is computed based on the body center of the robot and the movement direction, which is the default position of the foot before walking on flat terrain with a standard static gait. The model in Figure 2 is projected onto a 2D plane, which is shown in Figure 3, and the green point represents the default position of feet. LF, RF, LB and RB stand for the default positions of left front foot, right front foot, left back foot and right back foot, respectively. Two default footholds on the same side are exactly symmetric, the distance between two left footholds is $d_l$, and the distance of that on the right is $d_l$. Two distances must satisfy the following inequality constraint in Equation (2); in the following experiment, $d_l$ and $d_r$ is set to 0.96 m and 1.02 m, respectively.

$$0<d_l\le d_r\le 2a.$$

(2)

In order to increase the stability of the robot, the default footholds are selected outside the robot body coverage area by controlling the rotation angle around the axis $Z_1$ in Figure 2, which can increase the supporting triangle area. The distance between the default footholds from two sides is larger than the width of the robot body, which is measured by $2\Delta b$ whose range is represented by Equation (3), where h is the default height of the robot body from the horizontal ground, and ${42.6426}^{\circ }$ is the maximized angle around the $Z_1$ axis. $\Delta b$ is set to 0.125 m in the experiment:

$$0<\Delta b\le h\times tan\left({42.6426}^{\circ }\right).$$

(3)

Default footholds are computed using the Equation (4), where ${\mathit{R}}_{2d}\left(\alpha \right)$ is the rotational matrix around the vertical axis in 2D space shown in Equation (5):

$$\left\{\begin{array}{c}{\mathit{F}}_{lf}={\mathit{O}}_{\mathit{c}}+\sqrt{{d_l}^2/4+{\left(b+\Delta b\right)}^2}{\mathit{R}}_{2d}\left({\alpha }_{lf}\right)\mathit{D},\hfill \\ {\mathit{F}}_{rf}={\mathit{O}}_{\mathit{c}}+\sqrt{{d_r}^2/4+{\left(b+\Delta b\right)}^2}{\mathit{R}}_{2d}\left({\alpha }_{rf}\right)\mathit{D},\hfill \\ {\mathit{F}}_{lb}={\mathit{O}}_{\mathit{c}}+\sqrt{{d_l}^2/4+{\left(b+\Delta b\right)}^2}{\mathit{R}}_{2d}\left({\alpha }_{lb}\right)\mathit{D},\hfill \\ {\mathit{F}}_{rb}={\mathit{O}}_{\mathit{c}}+\sqrt{{d_r}^2/4+{\left(b+\Delta b\right)}^2}{\mathit{R}}_{2d}\left({\alpha }_{rb}\right)\mathit{D},\hfill \end{array}\right.$$

(4)

$${\mathit{R}}_{2d}\left(\alpha \right)=\left[\begin{array}{cc}{\displaystyle cos\left(\alpha \right)}& -sin\left(\alpha \right)\\ {\displaystyle sin\left(\alpha \right)}& cos\left(\alpha \right)\end{array}\right].$$

(5)

3. Computing the Terrain Features

Suppose there has been a complete point cloud map that can be constructed with the Simultaneous Localization and Mapping (SLAM) algorithms [4,5,6]. In our robot, two data frames are transformed to the same coordinate system and then they are combined [4], and poses of over two frames must be optimized for decreasing the combining error [5,6]; a 2.5D elevation map is firstly constructed from the point cloud map, and then terrain features around the foothold are extracted [21].

3.1. Constructing 2.5D Elevation Map Based on Grids

After 3D point clouds are generated from several TOF frames using such SLAM algorithms [4,5,6], all of the 3D points in the cloud are transformed into the specified referencing coordinate system whose Z-axis is antiparallel to the gravity direction. The coordinate system is called $O_L-X_L{Y}_L{Z}_L$ shown in Figure 4, and then the elevation map is constructed in this coordinate system, and the smallest unit for selecting foothold is one grid.

$G(i,j)$ is the elevation at the grid $(i,j)$, $r_{normal}$ is the radius for searching 3D points around the grid center (the points inside the dotted yellow circle), normal and curvature of the terrain are computed from such resulting 3D points, which are used to evaluate the cost of selecting the foothold centered at the grid $G(i,j)$. $r_{normal}$ should be adjusted properly according to the foot size.

Let ${\mathit{P}}_L$ be the set of the points located in the coordinate system $O_L-X_L{Y}_L{Z}_L$:

$${\mathit{P}}_L=\left\{{\mathit{p}}_1\left(x_1,y_1,z_1\right),{\mathit{p}}_2\left(x_2,y_2,z_2\right),\dots ,{\mathit{p}}_n\left(x_n,y_n,z_n\right)\right\}.$$

(6)

The points in ${\mathit{P}}_L$ are projected and partitioned into the 2D grid according to the $(x,y)$ coordinate of the point, the start point of the grid is the origin of 2D coordinate system $O_G-X_G{Y}_G$. $O_L-X_L{Y}_L$ is transformed to $O_G-X_G{Y}_G$ by the translation vector ${\mathit{t}}_{L-G}(t_x,t_y,{\delta }_z)$. The elevation of the grid is computed by referencing the plane $O_G-X_G{Y}_G$. ${\delta }_z$ represents the translation in the vertical direction. $t_x,t_y$ in the vector ${\mathit{t}}_{L-G}$ are the minimum values of x and y in the point set ${\mathit{P}}_L$:

$$\left\{\begin{array}{c}{\displaystyle t_x=x_{min}=min\{x_1,x_2,\dots ,x_n\},}\hfill \\ {\displaystyle t_y=y_{min}=min\{y_1,y_2,\dots ,y_n\}.}\hfill \end{array}\right.$$

(7)

The resolution of the grid map is $({\Delta }_x,{\Delta }_y)$; suppose there are m points located inside the grid $(i,j)$, constituting the subset ${\mathit{P}}_L^{ij}\subset {\mathit{P}}_L$:

$${\mathit{P}}_L^{ij}=\left\{{{\mathit{p}}_k}_{{}_1},{{\mathit{p}}_k}_{{}_2},\dots ,{\mathit{p}}_{k_m}\right\}.$$

(8)

For any point in the subset ${\mathit{P}}_L^{ij}$, $\forall {{\mathit{p}}_k}_{{}_r}\in {\mathit{P}}_L^{ij}$, the coordinate w.r.t both x and y dimensions must satisfy the following constraints:

$$\left\{\begin{array}{c}{\displaystyle x_{min}+(i-1)\times {\Delta }_x\le {x_k}_{{}_r}<x_{min}+i\times {\Delta }_x,}\hfill \\ {\displaystyle y_{min}+(j-1)\times {\Delta }_y\le {y_k}_{{}_r}<y_{min}+j\times {\Delta }_y.}\hfill \end{array}\right.$$

(9)

Elevation of the grid $(i,j)$ is computed by averaging Z coordinates of all the points in ${\mathit{P}}_L^{ij}$; supposing that there are m points falling into such a grid, then the elevation can be computed using the following equation:

$$\mathit{G}(i,j)=\frac{{\displaystyle \sum _{r=1}^m}{z}_{k_r}}{m}.$$

(10)

The flag bit is introduced to indicate whether there is valid point in the grid or not, and the bit is represented by ${\mathit{V}}_G$:

$${\mathit{V}}_G(i,j)=\left\{\begin{array}{c}{\displaystyle 1,\mathrm{If}|{\mathit{P}}_L^{ij}|>0,}\hfill \\ {\displaystyle 0,\mathrm{If}|{\mathit{P}}_L^{ij}|=0.}\hfill \end{array}\right.$$

(11)

3.2. Computing Normal and Curvature

Normal and curvature are computed directly from 3D points. In order to get these two terrain features around the given grid in the 2.5D elevation map, it is necessary to search all the points inside the sphere centered at that grid. Flow chart of the searching algorithm is shown in Figure 5.

The algorithm searches the grids around the grid $G(i,j)$ according to the radius $r_{normal}$, and the points belonging to such grids are added to the resulting point cloud ${\mathit{P}}_{ij}^{normal}$ step by step. Let ${\mathit{P}}_{ij}^{normal}$ be an ordered set composed of w points:

$${\mathit{P}}_{ij}^{normal}=\left\{{\mathit{p}}_1^{ij}\left(x_1^{ij},y_1^{ij},z_1^{ij}\right),{\mathit{p}}_2^{ij}\left(x_2^{ij},y_2^{ij},z_2^{ij}\right),\dots ,{\mathit{p}}_w^{ij}\left(x_w^{ij},y_w^{ij},z_w^{ij}\right)\right\}.$$

(12)

Principal component analysis is used to compute the normal and curvature from the set ${\mathit{P}}_{ij}^{normal}$, a medial matrix $\mathit{A}$ with the size $w\times 3$ is firstly constructed:

$$\mathit{A}=\left[\begin{array}{ccc}{\displaystyle x_1^{ij}-\overline{x^{ij}}}& y_1^{ij}-\overline{y^{ij}}& z_1^{ij}-\overline{z^{ij}},\\ {\displaystyle x_2^{ij}-\overline{x^{ij}}}& y_2^{ij}-\overline{y^{ij}}& z_2^{ij}-\overline{z^{ij}},\\ {\displaystyle ⋮}& ⋮& ⋮\\ {\displaystyle x_w^{ij}-\overline{x^{ij}}}& y_w^{ij}-\overline{y^{ij}}& z_w^{ij}-\overline{z^{ij}}\end{array}\right].$$

(13)

In Equation (13), the average value of 3D coordinates is computed as the following equation:

$$\left\{\begin{array}{c}{\displaystyle \overline{x^{ij}}=\frac{1}{w}\times \left(x_1^{ij}+x_2^{ij}+\dots +x_w^{ij}\right),}\hfill \\ {\displaystyle \overline{y^{ij}}=\frac{1}{w}\times \left(y_1^{ij}+y_2^{ij}+\dots +x_w^{ij}\right),}\hfill \\ {\displaystyle \overline{x^{ij}}=\frac{1}{w}\times \left(x_1^{ij}+x_2^{ij}+\dots +x_w^{ij}\right).}\hfill \end{array}\right.$$

(14)

Now, the covariance matrix ${\mathit{C}}_{ij}^{normal}$ of the 3D point cloud ${\mathit{P}}_{ij}^{normal}$ can be computed from the medial matrix $\mathit{A}$:

$${\mathit{C}}_{ij}^{normal}={\mathit{A}}^T\times \mathit{A}.$$

(15)

${\mathit{C}}_{ij}^{normal}$ can be decomposed by eigenvalue:

$${\mathit{C}}_{ij}^{normal}\times \left[{\mathit{\alpha }}_1,{\mathit{\alpha }}_2,{\mathit{\alpha }}_3\right]=\left[\begin{array}{ccc}{\displaystyle {\lambda }_1}& & \\ {\displaystyle }& {\lambda }_2& \\ {\displaystyle }& & {\lambda }_3\end{array}\right]\times \left[{\mathit{\alpha }}_1,{\mathit{\alpha }}_2,{\mathit{\alpha }}_3\right].$$

(16)

${\lambda }_i$ is the eigenvalue of the covariance matrix ${\mathit{C}}_{ij}^{normal}$, and ${\mathit{\alpha }}_i$ is the eigenvector with respect to the eigenvalue ${\lambda }_i$. Suppose that the eigenvalues satisfy the ordering constraint: ${\lambda }_1<{\lambda }_2<{\lambda }_3$, according to the theory of principal component analysis, ${\mathit{\alpha }}_1$ is the normal vector of the terrain at grid $G(i,j)$, as shown in Equation (17):

$${\mathit{n}}^{ij}={\left[n_x^{ij},n_y^{ij},n_z^{ij}\right]}^T={\mathit{\alpha }}_1.$$

(17)

The curvature is represented by the normalization of minimum eigenvalue shown as Equation (18):

$${\mathit{C}}_{urv}\left(i,j\right)=\frac{{\lambda }_1}{\sqrt{{{\lambda }_1}^2+{{\lambda }_2}^2+{{\lambda }_3}^2}}.$$

(18)

4. Designing the Cost Function of Selecting a Foothold

4.1. Rules for Selecting a Foothold

The best foothold selected should satisfy the following rules as much as possible:

(1) Foot slip should be avoided or minimized.

(2) Walking speed should be maximized. For two candidate footholds, one lies at the front of the default foothold, and the other lies at the back; the priority of the front foothold should be larger than that of the back one.

(3) The distance between the candidate foothold and default foothold should be minimized, according to the law of quadruped bionic movement.

(4) The area of the supporting triangle should be maximized for increasing the stability of robot walking.

It is impossible to meet all requirements simultaneously. Even some rules are contradictory, it is therefore necessary to weigh such rules into consideration. It is not possible to weigh such rules by adjusting parameters manually, so expert learning strategy is employed to consider those rules comprehensively.

In order to avoid sliding of the foot from the ground as much as possible, some classical contact models between the foot and ground are listed, which are shown in Figure 6. Front and back legs are configured differently in the figure, in terms of the view used in Figure 1.

In the first three cases (a)–(c), the normal of the terrain is vertical to the horizontal plane, but there is some angle between the normal of the terrain and the horizontal plane in the cases (d) and (e), with uphill and downhill slope, respectively. For the first three cases, priority of model (b) is the highest, no matter which leg it is, because the curvature of the terrain in model (b) is larger than that of (a). Priority of model (c) is lower than that of (a) because the curvature of the terrain in model (c) is too large to lift the foot smoothly. In our experiment, the foothold with curvature larger than 0.16 must be excluded from the candidate set. For the last two models (d) and (e), selection characteristics of front legs are different that of back legs, which is determined by the robot structure; feet of the front legs are used to step down forwardly, while that of the back leg is used to step down backwardly. Thus, the terrain with an uphill slope is more suitable for the front foot, while the terrain with a downhill slope is more suitable for the back foot.

4.2. Cost Function Definition

Only the candidate footholds around the default foothold are comparable. Suppose there are n candidate footholds in the rectangle centered at the default foothold, as shown in Equation (19), in which ${\mathit{\xi }}_i$ is the feature vector of the ith candidate foothold:

$${\mathit{F}}_c=\{{\mathit{\xi }}_1,{\mathit{\xi }}_2,\dots ,{\mathit{\xi }}_i,\dots ,{\mathit{\xi }}_n\}.$$

(19)

The cost of selecting one foothold whose feature vector belongs to the set ${\mathit{F}}_c$ will be computed using the cost function, and the foothold with the lowest cost would be selected. The feature vector of each foothold is 11-dimensional. In Equation (20), ${\lambda }_j$ is the jth element of the feature vector:

$$\mathit{\xi }={\left[{\lambda }_1,{\lambda }_2,\dots ,{\lambda }_j,\dots ,{\lambda }_{11}\right]}^T.$$

(20)

The feature vector is composed of both the terrain features and positional relationship between the candidate foothold ${\mathit{p}}_c$ and the default foothold ${\mathit{p}}_d$.

The terrain features are listed below:

(1) $f_1^t$: The curvature of the terrain around the candidate foothold ${\mathit{p}}_c$ is used to judge whether the terrain is flat or not, which is considered for decreasing the foot slip.

(2) $f_2^t$: The slope of the terrain around the candidate foothold ${\mathit{p}}_c$ is computed as the angle between the normal of the terrain centered at ${\mathit{p}}_c$ and the horizontal surface.

(3) $f_3^t$: The feature for characterizing the terrain is concave or convex. Let ${\mathit{p}}_1,{\mathit{p}}_1,\dots ,{\mathit{p}}_8$ have eight points with uniform distribution around the candidate foothold ${\mathit{p}}_c$, and ${\mathit{v}}_i$ is the directed vector from ${\mathit{p}}_c$ to ${\mathit{p}}_i$, where $i\in [1,8]$. The intersection angle between ${\mathit{v}}_i$ and the normal of the terrain centered at the point ${\mathit{p}}_c$ is computed, and then all eight of the 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.

(4) $f_4^t$: The feature for characterizing the terrain is uphill or downhill. The vertical plane $\mathit{P}$ is constructed based on the walking direction of the robot $\mathit{D}$ and the vertical upward axis $\mathit{Z}$. Suppose ${\mathit{n}}^{\prime }$ is the vector which is projected onto the plane $\mathit{P}$ from the terrain normal $\mathit{n}$ at the candidate foothold. The rotational angle from $\mathit{D}$ to ${\mathit{n}}^{\prime }$ 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 based on the 3D points inside the sphere with a radius $r_{normal}$ as shown in Figure 4, so the terrain features will change each time the radius is adjusted. In order to get terrain information with different scales, two radiuses are employed, 2 cm and 5 cm, respectively. ${\lambda }_1-{\lambda }_4$ and ${\lambda }_5-{\lambda }_8$ in the feature vector $\mathit{\xi }$ are terrain features $f_{t1}-f_{t4}$ computed with the radius 2 cm and 5 cm, respectively.

Position related features of the vector:

(1) $f_1^p$: The feature for representing the distance between the candidate foothold and the default foothold, which will make the quadruped robot select the foothold closer to the default foothold as much as possible.

(2) $f_2^p$: The feature for characterizing that the candidate foothold ${\mathit{p}}_c$ lies on the left side of the default foothold or the right side. Let us define the rectangular coordinate system ${\mathit{O}}_d$, its original point is ${\mathit{p}}_d$, which is the projection of the default foothold. The $\mathit{X}$-axis direction is the same as $\mathit{D}$, which is the walking direction of the robot, the $\mathit{Y}$-axis is vertical with $\mathit{X}$ and lies at the left side of the axis $\mathit{X}$, the coordinate value of ${\mathit{p}}_c$ w.r.t $\mathit{Y}$ is computed as the feature value.

(3) $f_3^p$: The feature for illustrating that the candidate foothold ${\mathit{p}}_c$ lies on front of the default foothold or back of it. The coordinate value of ${\mathit{p}}_c$ w.r.t $\mathit{X}$ in system ${\mathit{O}}_d$ is computed as the feature value.

In fact, the positional relationship $f_1^p-f_3^p$ represents a polar coordinate in the coordinate system, in which the pole is the point ${\mathit{p}}_d$, and the polar axis is the moving direction of the robot. These three features are used for weighing the last three rules of selecting the foothold. ${\lambda }_9-{\lambda }_{11}$ in the feature vector $\mathit{\xi }$ is the positional relationship $f_1^p-f_3^p$.

The weight vector $\mathit{W}$ is defined in Equation (21), and the dimensions of the vector are the same as that of the feature vector:

$$\mathit{W}={\left[{\omega }_1,{\omega }_2,\dots ,{\omega }_j,\dots ,{\omega }_{11}\right]}^T.$$

(21)

Now, the cost function is defined as the weighted sum of feature values:

$$C_{foot}\left(\mathit{\xi }\right)={\mathit{W}}^T\mathit{\xi }={\displaystyle \sum _{k=1}^{11}}{\omega }_k{\lambda }_k.$$

(22)

5. Learning the Weight Vector of the Cost Function

In order to get the cost function $C_{foot}\left(\mathit{\xi }\right)$, it is a key step to estimate the weight vector as Equation (21) w.r.t some quadruped robot and terrain environment. The weight vector is also called parameters of the cost function.

5.1. Training Data Collection

The learned cost function will be applied to navigate the real quadruped robot, so the training data set for learning the weight vector of the cost function must be collected in some type of terrain, on which the quadruped robot will walk.

Given the real terrain shown in Figure 7a, its 2.5D elevation map as Figure 7b is constructed from the point cloud generated by applying TOF camera, the colorbar in Figure 7b indicates the elevation value, and then the training data set is collected based on the elevation map. A simulated quadruped robot with the same kinematic model as the real robot is placed randomly on the map. Four candidate footholds are generated randomly around the default foothold. The expert sorts four such candidate footholds, allowing feature vectors of the four footholds shown in Equation (23). Suppose that the first foothold is the best one, the fourth foothold is the worst one, and the second foothold is better than the third one. The smaller the value of cost function, the better the foothold is. Sorted data like Equation (24) will be collected as the training data, where $\mathit{W}$ is the weight vector to be learned and ${\mathit{\xi }}_i$ is the feature vector of the ith foothold.

$${\mathit{F}}_c^4=\{{\mathit{\xi }}_1,{\mathit{\xi }}_2,{\mathit{\xi }}_3,{\mathit{\xi }}_4\},$$

(23)

$${\mathit{W}}^T{\mathit{\xi }}_1\le {\mathit{W}}^T{\mathit{\xi }}_2\le {\mathit{W}}^T{\mathit{\xi }}_3\le {\mathit{W}}^T{\mathit{\xi }}_4.$$

(24)

When the expert gives ranks of these footholds, several rules should be obeyed:

(1) Priority of the candidate foothold with a shadow pit is higher;

(2) Priority of the candidate foothold with a deep slope is lower;

(3) Priority of the candidate foothold will lead to the collision between the leg and terrain being lower;

(4) Priority of the candidate foothold at the edge of the terrain is lower;

(5) The candidate foothold with the possibility of sliding is preferred rather than that with the danger of the leg sticking;

(6) The five rules above an elaborate influence of the terrain to foothold selection, for the two candidate footholds with similar terrain features, the one with a closer distance from the default foothold has higher priority;

(7) The candidate foothold lying at the front of the default foothold has higher priority; suppose that the six principles above are similar for two candidate footholds;

(8) The candidate foothold maximizing the next supporting triangle has higher priority; suppose that the seven principles above are similar for two candidate footholds.

In order to explain the rules above vividly, Figure 8 and Figure 9 show the views of the default footholds and candidate footholds for left front, and left back, respectively. The two cases shown in Figure 8 and Figure 9 are typical because collision is prone to happen when the front foot moves down a step, and that the back foot moving up a step is also prone to colliding. For each case, several views can be shifted by experts to sort candidate footholds accurately; here, only two views are shown for saving paper space. The blue star lies at the center of the robot body, the black sphere on the map stands for the default foothold, the black line with arrows indicates the moving direction. Four sampled candidate footholds are represented by spheres with green, red, yellow and purple colors, respectively.

For the case in Figure 8, the priority order of these four candidate footholds is represented using the color of foothold: $red⪰yellow⪰purple⪰green$. The red foothold has the highest priority because it lies in the left front side of the default foothold, which will make the triangle area larger and the walking faster. The worst foothold is the green one, due to collisions between legs and terrain. The yellow foothold is better than the purple one because the yellow foothold is closer to the default foothold, and the purple foothold lies at the back of the default foothold, which will decrease the walking speed.

For the case in Figure 9, priority order of these four candidate footholds is represented using the color of foothold: $red⪰purple⪰green⪰yellow$. Priorities of the red and purple footholds are higher than that of green and yellow footholds because the former two footholds lie at the front of the default foothold. The yellow foothold is the worst one due to collisions between legs and terrain. The red footholds lie on the left side of the default foothold, which will increase triangle area, but the purple one lies on the right side, so the red foothold is better than the purple one.

5.2. Weight Vector Computation

The front foot and the back foot do not prefer the candidate foothold with the same feature vector, according to Figure 6. The left foot and the right foot also have different preferences for considering stability of the robot. Thus, the cost functions w.r.t. four feet are different, and the weight vector needs to be optimized individually.

Once four candidate footholds are generated on the elevation map, the order of the footholds is given by the expert, and then the order, the terrain feature and status of the robot are recorded simultaneously. $n(=1000)$ groups of data are recorded for one foot, and that of other three feet are recorded using the same approach. The training data sets are represented by ${\mathit{D}}_{train}^{LF},{\mathit{D}}_{train}^{RF},{\mathit{D}}_{train}^{LB},{\mathit{D}}_{train}^{RB}$ for four feet, respectively. The weight vector is learned based on the training data set using a support vector machine for each foot, and four vectors are represented by ${\mathit{W}}_{LF},{\mathit{W}}_{RF},{\mathit{W}}_{LB},{\mathit{W}}_{RB}$, respectively, which are shown in

Table 4. Weight vector table.

Index	${\mathit{W}}_{\mathit{LF}}$	${\mathit{W}}_{\mathit{RF}}$	${\mathit{W}}_{\mathit{LB}}$	${\mathit{W}}_{\mathit{RB}}$
1	$3.78\times {10}^{-5}$	$1.77\times {10}^{-5}$	$3.86\times {10}^{-6}$	$7.87\times {10}^{-6}$
2	$3.46\times {10}^{-4}$	$2.90\times {10}^{-4}$	$4.60\times {10}^{-4}$	$4.67\times {10}^{-4}$
3	$-1.91\times {10}^{-4}$	$-1.95\times {10}^{-4}$	$4.83\times {10}^{-5}$	$9.17\times {10}^{-6}$
4	$1.46\times {10}^{-4}$	$-3.89\times {10}^{-5}$	$-1.65\times {10}^{-4}$	$1.37\times {10}^{-4}$
5	$1.88\times {10}^{-5}$	$2.99\times {10}^{-5}$	$2.63\times {10}^{-5}$	$2.58\times {10}^{-5}$
6	$3.39\times {10}^{-4}$	$3.88\times {10}^{-4}$	$3.68\times {10}^{-4}$	$4.65\times {10}^{-4}$
7	$-0.98\times {10}^{-4}$	$-2.17\times {10}^{-4}$	$-2.37\times {10}^{-5}$	$-8.65\times {10}^{-6}$
8	$9.56\times {10}^{-5}$	$3.25\times {10}^{-5}$	$-1.29\times {10}^{-4}$	$-1.48\times {10}^{-4}$
9	$1.68\times {10}^{-5}$	$3.76\times {10}^{-6}$	$1.58\times {10}^{-5}$	$-8.64\times {10}^{-6}$
10	$1.67\times {10}^{-6}$	$-1.13\times {10}^{-5}$	$-6.89\times {10}^{-6}$	$-1.66\times {10}^{-5}$
11	$1.70\times {10}^{-6}$	$4.67\times {10}^{-6}$	$5.77\times {10}^{-6}$	$1.33\times {10}^{-5}$

.

In order to validate the reasonableness of the learned weight vector, it is used to reorder each group of candidate footholds in the training data set. There are $C_4^2$ ordered pairs of candidate footholds in each group, so $1000C_4^2$ ordered constraints can be provided by the expert for each foot. If m ordered foothold pairs are computed from the cost function of candidate footholds using Equation (22), then the accuracy of the weight vector is $m/\left(1000C_4^2\right)$, shown in

Table 5. The accuracy of weight vector on training data.

${\mathit{W}}_{\mathit{LF}}$	${\mathit{W}}_{\mathit{RF}}$	${\mathit{W}}_{\mathit{LB}}$	${\mathit{W}}_{\mathit{RB}}$
92.98541%	93.8%	91.145633%	94.24623%

for each foot.

6. Experimental Results and Discussion

In this section, semi physical and physical experiments are both carried out to validate the effectiveness of the proposed cost function. The cost function is applied on the offline elevation map, which is used to learn the parameters of function in the semi physical experiment. Furthermore, in the physical experiment, the quadruped robot selects a suitable and safe foothold using the cost function on the elevation map constructed in real time.

6.1. Semi Physical Experiment

A body center point is selected on the 2.5D elevation map shown in Figure 7b, and then four default footholds are computed using the Equation (4), given that the quadruped robot walks along the the forward direction of the X axis. For viewing clearly, the elevation map and the footholds are projected into 2D space, which is shown in Figure 10. Elevation value is represented by the color, the green circle is the robot center, the red circle dots are default footholds, the green arrow stands for the walking direction and candidate footholds lie inside the red rectangle. Coordinates of the default footholds are listed in

Table 6. Coordinates and grids of the default foothold in the real world.

$\mathit{Format}$	${\mathit{F}}_{\mathit{lf}}$	${\mathit{F}}_{\mathit{rf}}$	${\mathit{F}}_{\mathit{lb}}$	${\mathit{F}}_{\mathit{rb}}$
$Grids$	$\left(198,173\right)$	$\left(201,113\right)$	$\left(102,173\right)$	$\left(99,113\right)$
$Coordinates$	(1.98 m, 1.73 m)	(2.01 m, 1.13 m)	(1.02 m, 1.73 m)	(0.99 m, 1.13 m)

in two units: m and $grid$. The candidate region on the front of the default foothold is larger than that on the back, which ensures the robot walking forward, to some extent.

For each grid inside the red rectangle region, cost is computed using the cost function shown in Equation (22), given the learned weight vector in Table 4, the cost value is represented by the color, which is shown in Figure 11. For the candidate footholds outside all the rectangles, their costs are set to the maximized value 1 because only the footholds inside the same rectangle are comparable for the cost value. According to the experimental results shown in Figure 11, the cost of the candidate foothold lying at the edge of the terrain is larger, which can be clearly viewed by referencing the 2D map in Figure 11b. For the flat region inside the lower right rectangle in Figure 11b, which is the candidate region for the front right foot of the quadruped robot, the costs of the grids around the default foothold are lower than that far from the default foothold. For the two grids with the same distance from the default foothold, the grid lying in the lower right has a smaller cost because it can make the supporting triangle larger, which is carried out by obeying the rules for selecting a foothold presented in Section 4. The semi experimental result demonstrates the effectiveness of the cost function for indicating the complexity of the terrain. In order to improve the fault tolerance of selecting a proper foothold for the quadruped robot, the average cost of the grids inside the circle centered at some grid is computed, and the grid with the lowest average cost is selected as the foothold in the experiment.

6.2. Experiments for Walking across a Step

Cost function is used to select footholds in real time for the quadruped robot (shown in Figure 1) walking across a wooden step. A target of the experiment is that the robot can walk across the step autonomously, and the width of the step is 30 cm, the height is 10 cm. The robot is controlled to walk based on the learned model of foothold selection, maintaining the robot body level up. The TOF camera is the only sensor to construct the map and position it on the map.

Figure 12 and Figure 13 show the cases of walking across the step for right front foot and right back foot, respectively. In each figure, the right panel is the video screenshot of walking up the step and down the step, and the left panel shows the real elevation map and the skeleton of the robot at that time. From two figures, we can see that the robot can select a suitable foothold when crossing the step, which demonstrates the effectiveness of the cost function learned from a real terrain model. Only key screenshots are shown for saving paper space, but the robot is really controlled autonomously to cross the step from start point to end point.

In the experiment, the robot is set to maintain the robot body level, and Figure 14 shows the pitch and roll angle of the robot. The angle belongs to the range $[-2^{\circ },2^{\circ }]$, so the robot body is kept level more or less, which indicates the effectiveness of the controlling algorithm based on the TOF camera.

When the robot is walking on the ground, body height is set to 0.62 m; suppose the coordinate system of the robot body is located at zero w.r.t Z-axis, and then Z-coordinate values of four feet are rendered along the walking process which are shown in Figure 15. The whole time axis is partitioned manually using four green dotted lines. All four feet step on the ground for the time $[0,t_1)$; two front feet are crossing the step, while the two back feet step on the ground for the time $[t_1,t_2)$; all four feet step on the ground for the time $[t_2,t_3)$, although the step lies beneath the robot body in this time; the two back feet are crossing the step, while the two front feet step on the ground for the time $[t_3,t_4]$.

In order to validate the effectiveness of the proposed algorithm for selecting a foothold, the previous experiment of a quadruped robot walking across the wooden step is carried out repeatedly, and three groups of the experiments are conducted 100 times, respectively. The wooden step is randomly placed in front of the robot in each experiment. In the first group of experiments, the standard static gait is applied, and the TOF camera is also used to map the wooden step and terrain in front of the robot and locate the robot on the map, which can keep walking with a horizontal robot body. In the second and third group of experiments, the learned foothold selection model is applied, and the candidate footholds come from the whole rect w.r.t. some default foothold in the third group, but, in the second group of experiments, the candidate footholds come only from the line crossing the default foothold and having the same direction with the walking direction $\mathit{D}$. The last two groups demonstrate that the candidate foothold on the lateral side of the default foothold will be better. The walking failure rate of the first group of experiment is $69\%$, the second group is $28\%$, and the last group is merely $5\%$, which shows the great advantage of the proposed model for selecting footholds. In general, the main results for failing to walk across the step include: (1) stepping on the edge of the step, which will lead to the foot sliding; and (2) collisions between the leg and step. The detailed comparison results are represented in Figure 16.

From Figure 16, it is easy to see that the probability of stepping on the edge is much lower than that of collisions between the leg and step, which is obvious because there are many more candidate footholds that can lead to collision. There are five failures walking across the wooden step. Even if the proposed model for selecting a better foothold is integrated into the controlling system, which is caused by the error of the model itself, the error of the TOF sensing and the foot sliding is due to the instability of the robot body, etc. The error of the model can be further reduced by increasing the training data.

7. Conclusions

This article presents a cost function for foothold selection in a quadruped robot that is equipped with a TOF camera. Through experimental results, we can see that the cost function is very effective for the robot to select a suitable and safe foothold. The cost function is defined as the weighted sum of the elements in the feature vector including both terrain features and other features related to the default foothold. The reasons for the excellent performance of the cost function include:

(1) The default foothold is introduced to the cost function, only candidate footholds around the default foothold are selected, and the candidate that makes larger supporting region area is given higher priority, which follows the walking regularity of the four-footed animal.

(2) The weight vector of the cost function is learned under the guidance of the expert, and the training data are collected on the 2.5D elevation map of a real terrain with several steps, so the quadruped robot always selects a proper foothold for the whole process crossing the terrain with steps.

The cost function can be extended to the quadruped robot walking a more rough terrain, as long as training data set with respect to such type of terrain is collected, on which our future research will be the focus.