Footholds optimization for legged robots walking on complex terrain

Abstract

This paper proposes a novel continuous footholds optimization method for legged robots to expand their walking ability on complex terrains. The algorithm can efficiently run onboard and online by using terrain perception information to protect the robot against slipping or tripping on the edge of obstacles, and to improve its stability and safety when walking on complex terrain. By relying on the depth camera installed on the robot and obtaining the terrain heightmap, the algorithm converts the discrete grid heightmap into a continuous costmap. Then, it constructs an optimization function combined with the robot’s state information to select the next footholds and generate the motion trajectory to control the robot’s locomotion. Compared with most existing footholds selection algorithms that rely on discrete enumeration search, as far as we know, the proposed algorithm is the first to use a continuous optimization method. We successfully implemented the algorithm on a hexapod robot, and verified its feasibility in a walking experiment on a complex terrain.

Key words： footholds optimization; legged robot; complex terrain adapting; hexapod robot; locomotion control

1 Introduction

Legged robots have more capacity in complex terrain than wheeled ones. The agile and dynamic control of legged robots in flat terrains has achieved great progress, but taking full advantage of the flexibility of legged robots in rough terrain remains a challenge. Because in the locomotion of a legged robot, its own motion balance and terrain perception should be considered at the same time, only then the corresponding footwork for adapting to complex terrain can be generated.

Heuristic walking gait [1,2] is a simple and effective blind locomotion strategy. Based on the model of spring-loaded inverted pendulum (SLIP), it considers the balance relationship between its own speed and foothold in the process of walking. However, in this strategy, the next foothold is related only to its body speed but not to the ground features. It is only suitable for flat road conditions and cannot effectively adapt to unstructured terrains. Moreover, there is an assumption that in the locomotion of legged robots, it has no relative sliding between the support legs and the ground. Nevertheless, the more uneven the ground is, the more difficult it is to satisfy this assumption. For example, when climbing stairs with a fixed gait, the robot often slips when stepping on the edge of a step, kicks to the eaves of a step, and even trips over.

If such a blind walking strategy is applied on complex terrains, the stability and safety of the robot cannot be guaranteed. Thus, it is necessary to introduce perceptive information of the terrain, to optimize the footholds selection, and avoid stepping on the edge of potholes or obstacles.

Researchers at LittleDog [3,4], HyQ [5,6], and ANYmal [7,8] proposed methods for footholds and trajectory optimization based on a one-time preprocessed costmap. This kind of methods is time consuming to plan the actions before the robot starts to move. After the action trajectories are determined, it runs open loop, which cannot be adjusted when environmental changes or error occurs. Then, onboard and online foothold optimization methods were proposed and applied in HyQ [9–11], ANYmal [12,13], Hexapod-III [14], Mini Cheetah [15], and Qingzhui [16–18]. They considered a similar approach (i.e., obtaining a grid heightmap of the terrain through a depth camera and generating a costmap), then conducted a random search in the batch area around the preplanned foothold. However, given that the costmap is discretized in grid cells, the optimization of the footholds can only enumerate a finite set of integers in the batch area, rather than a spatial continuous optimization. The distance of the searched footholds in the optimization process depends on the grid size. Hence, although the feasibility of a selected foothold can be guaranteed, it is not a global optimum. Later, HyQ researchers [19] utilized a convolutional neural network (CNN) to find the optimal foothold, whose input is also the batch area around the preplanned foothold. The CNN greatly improved the computational efficiency. ANYmal researchers [20,21] directly applied the reinforcement learning method to complete the autonomous movement of the robot in the field environment and offshore platform [22], which can be said to be a milestone achievement in the field of legged robots. However, these data-driven methods require considerable data for training, which increases the initial cost of algorithm deployment. In addition to such methods of constructing terrain maps, some researchers have extracted geometric features of known target structures through visual information [23–25], but these methods can only adapt to the terrain of preknown scenes.

Therefore, we need to develop a set of solutions that combine terrain awareness with the state constraints of the robot, do not rely on high-cost data training, and can operate efficiently onboard, to optimize the footholds, control the robot’s actions in a spatially continuous way, and expand the adaptability of the legged robot in complex terrain.

The contributions of this paper are summarized as follows:

a) A continuous and derivable method for converting discrete grid maps is proposed. Through this method, the grid terrain heightmap is converted into a smooth terrain costmap.

b) A gradient-based continuous optimization method is constructed using the terrain costmap and the motion state of the robot to select the robot’s footholds.

c) The body motion is corrected in accordance with the optimized footholds, and the swing leg trajectories are generated. Combined with the method of balance force control, an experiment is carried out on a hexapod robot passing through a complex terrain.

To our knowledge, this is the first footholds selection method for legged robots based on continuous optimization algorithm with no training data required, which can efficiently run online and onboard. We choose the continuous optimization method, for the reason that we want to realize optimal footholds in a continuous space. Given that it requires a continuous and derivable costmap and objective function, it can be efficiently deployed online. The remainder of this paper is structured as follows:

Section 2 provides the overview of the hexapod robot developed by Shanghai Jiao Tong University. Section 3 presents a novel and efficient method to make discrete heightmaps continuous and derivable, generalizing a continuous costmap from an elevation map obtained by a red, green, blue, depth (RGBD) camera. Section 4 formulates the cost function in consideration of the costmap above, along with the robot locomotion and kinematics, to instruct the footholds of the robot when walking. Section 5 explains how to use the optimized footholds on our robot to combine it with our control framework. Section 6 shows the results from the experiment of the hexapod walking on an irregular terrain with our method. Finally, conclusions are drawn in Section 7.

2 Robot overview

2.1 Hardware structure

LittleStrong is a bionic designed hexapod robot inspired by insects. Its six legs are symmetrically distributed on both sides of the body. To avoid legs’ collision with each other when walking, the middle legs are installed slightly farther from the sagittal plane.

There are 3 degrees of freedom for each leg, serially constructed as hip, thigh, and shank joints. Thus, LittleStrong has 18 joints in total, as shown in Fig.1. Each joint is driven by a brushless electric motor with a planetary gear set and an embedded encoder. The robot is about 0.4 m in height, 0.6 m in length, and 0.4 m in width when standing.

Fig.1 Photo of the hexapod robot and its coordinate systems. RGBD: red, green, blue, depth.

Full size|PPT slide

The coordinate systems of the robot are defined in Fig.1. The world coordinate system (WCS) is fixed to the ground, and the position where the robot is turned on is regarded as the origin. The robot locomotion control is mostly completed in the WCS. The body coordinate system (BCS) is fixed at the center of its body, and all variables in BCS will be marked with a left superscript {B}. The six legs of the robot have their own leg coordinate system (LCS), which is fixed with the BCS but takes the leg’s hip joint as the origin. All variables in LCS will be marked with a left superscript L, and the right subscript i represents the leg number.

The electrical system is shown in Fig.2. There are two onboard computers that cooperate with each other. The locomotion control computer runs the GNU/Linux patched with Xenomai [26] real-time kernel. It communicates with 18 drivers and an inertial measurement unit (IMU) through EtherCAT protocol. The drivers provide power for 18 actuators (brushless motors), along with 18 encoders feeding back the angle and angular velocity of each motor. IMU is installed at the center of the robot body and feeds back attitude, angular velocity, and acceleration. The depth camera is fixed on the head of the robot and connected to the navigation computer running the Robot Operating System. The navigation computer sends the locomotion commands to the locomotion control computer through ZeroMQ [27].

Fig.2 Electrical system of the robot. IMU: inertial measurement unit, RGBD: red, green, blue, depth.

Full size|PPT slide

2.2 Control framework

The overall control framework of the robot is shown in Fig.3. The orange blocks run on the navigation computer, which will be the main content of the footholds optimization in the later sections. The blue blocks run on the locomotion control computer. The locomotion control of the legged robot mainly includes two contents, namely, gait control and body balance control.

Fig.3 Control framework of the robot for footholds optimization.

Full size|PPT slide

For gait control, the robot employs a gait scheduler to generate step cycles. A legged robot needs to periodically control the alternating lifting and landing of different legs to let the body move. For a hexapod, there are many optional gait combinations. Considering that a trot gait is commonly used for quadrupeds, a tripod-trot gait [28] is adopted for hexapods. Tripod-trot gait refers to that the six legs of the robot are divided into two groups. In each step cycle, there are three legs in swing phase and the other three legs in support phase. The gait scheduler is shown in Fig.4, where the white areas represent the swing phase; the blue and red areas represent the support phase for different leg groups, corresponding to the lifting and landing of each leg, respectively.

Fig.4 Gait scheduler and tripod-trot gait for the robot.

Full size|PPT slide

The tripod-trot gait can furnish a triangular stable region underneath its supported legs in each gait cycle, as shown in Fig.4. This can be considered a natural advantage of hexapods over quadrupeds, such that we can focus on the working task itself without wasting much attention on the balance control of the robot. For a swing leg, a suitable foothold must be selected to satisfy the speed and balance requirements of body movement when it becomes a supporting leg in the next step cycle. Additionally, a suitable swing leg trajectory is also needed to be designed. This will be further explained in Section 4 and Subsection 5.1.

The robot employs a centroid balance control (CBC) algorithm to generate support legs’ control signals through the state feedback of the whole robot body. In the calculation process, we apply a simplified model of robot dynamics. The details are elucidated in Subsection 5.2.

The locomotion controller composed of the above two contents can satisfy the adaptive locomotion on general terrain. However, when in complex terrain, it can only pass through by blind walking. A gait optimizer should be introduced in combination with visual information to avoid pits and obstacles. The details will be elaborated below.

3 Costmap processing

The key idea for a legged robot adapting to complex terrains is to select suitable footholds for the next step. The selection of footholds integrates two parts: terrain perception and body state. This section focuses on terrain perception. To obtain spatial continuous optimal and apply continuous optimization algorithms, this section proposes a new method to transform the discrete grid heightmap into a continuous costmap. Subsection 3.1 proposes a new method to transform the discrete grid map into a second-order continuously derivable costmap. Note that this subsection is purely a theoretical approach to discrete grid continuation, and the examples used are only representative and simple grid maps. Subsection 3.2 introduces how to apply this method to convert the terrain point cloud obtained by the depth camera into a terrain costmap for robot locomotion optimization in the next section.

3.1 Grid map continuation

Convolution is a commonly used method in digital image processing [29]. In the process of image convolution, each element of the image is added to its local neighbors, weighted by the kernel. As shown in Fig.5, the Hadamard product (i.e., multiplication of corresponding elements) is performed between the four pixels in the red box in the left original image and the yellow convolution kernel matrix. The elements of the generated intermediate matrix are summed to obtain the pixels in the red box in the processed image on the right. The lower part of the figure shows a 3D view of the process. Given that the convolution operation synthesizes the information between the pixels, the influence of image noise is well avoided. However, in this process, the convolution kernel operates on the image pixel by pixel and moves discretely. To obtain spatial continuously changing values, interpolation methods can be adopted.

Fig.5 Example of image convolution in digital image processing.

Full size|PPT slide

In our grid map, each pixel (grid cell) represents the average height of the ground in a specific area (2 cm × 2 cm). To obtain the height between cells, continuous height values must be obtained through 2D interpolation. Considering that the direct interpolation will be affected by random noise, the convolution interpolation (CI) is used for continuous smoothing of the grid map on the basis of the image convolution technique. As shown in Fig.6, in the CI process, different from the image convolution, the kernel (red box) is a mask-plate that moves smoothly on the grid map. The overlapping area (yellow box in dash line) between the mask-plate and the cell is defined as its weight. The sum of weighted cells is used as the center (point c) value of the mask-plate. In this way, a continuous heightmap can be obtained. Moreover, in the image convolution mentioned above, adjusting the size of its kernel, so called mask-matrix, can yield different filtering results. Similarly, in CI, the number of cells involved in convolution can be adjusted by altering the size of the mask-plate to obtain similar effects.

Fig.6 Convolution interpolation for transforming a pixel into a continuous heightmap.

Full size|PPT slide

Although CI can obtain continuous costmaps, its results are not differentiable. As shown in Fig.6, there will be obvious bending at the boundary of the grid, which will have a fatal impact on the optimization solution in the next section. To solve this problem, we propose a pyramid convolution interpolation (PCI) smoothing algorithm. In PCI, the kernel is no longer the mask-matrix used for image convolution nor the mask-plate mentioned in CI but a mask-pyramid (projected as red box) shown in Fig.7. Similarly, the mask-pyramid moves smoothly on the grid map. The overlapping volume (projected as yellow box) above the cell, which is covered by the projection of the pyramid, is used as the weight of this cell. The sum of weighted cells is used as the center point value of the pyramid. From Fig.7, the continuous heightmap obtained in this way is smoother and derivable than that in Fig.6. The number of cells involved in convolution can also be adjusted by changing the size of the pyramid’s bottom quadrilateral.

Fig.7 Pyramid convolution interpolation for transforming a pixel into a continuous heightmap.

Full size|PPT slide

However, in the actual computing process, the overlapping volume of the pyramid and the cell is difficult to calculate; especially after the size of different bottom surfaces of the pyramid is adjusted, the volume shape of a block is more complex and difficult to calculate. This will have a significant impact on the calculation efficiency in the optimization iterative calculation process in the next section.

Therefore, this paper proposes a new convolution method, Normalized Derivable Convolution Interpolation (NDCI) algorithm. In PCI, the second-order continuity can be obtained because in the weighting process, with the smooth movement of the pyramid, the cell’s weight gradually increases from zero, along with its second-order continuous. When the center of the convolution kernel coincides with the center of the grid, the weight value reaches its maximum. Then, to make the costmap differentiable, we only need to find the weight function with similar expression. Obviously, this characteristic can be obtained by a cosine function. The algorithm formula is as follows:

(1)J\left({\mathit{p}}_{\mathrm{h}}\right)=\sum _{i=0}^m\sum _{j=0}^n{\left(\cos \frac{\text{π}\left(x_{\mathrm{h}}-x_{i,j}\right)}{2{\sigma }_x}\cos \frac{\text{π}\left(y_{\mathrm{h}}-y_{i,j}\right)}{2{\sigma }_y}\right)}^2,

where {\mathit{p}}_{\mathrm{h}}=[\begin{array}{cc}{x}_{\mathrm{h}}& y_{\mathrm{h}}\end{array}{]}^{\mathrm{T}} is a horizontal coordinate, representing the position of the selected point on the whole map, x_{\mathrm{h}} and y_{\mathrm{h}} are x and y components of the horizontal coordinate for the position of the selected point on the heightmap, respectively, {\mathit{c}}_{i,j}=[\begin{array}{cc}{x}_{i,j}& y_{i,j}\end{array}{]}^{\mathrm{T}} is the coordinate of the center of the pixel (i, j) on the map, x_{i,j} and y_{i,j} are x and y components of the horizontal coordinate for the position of the selected point on the heightmap, respectively, {m, n} are the set numbers of pixels that need to be involved, similar to the size of the kernel in image convolution, \left\{{\sigma }_x,{\sigma }_y\right\} are the smoothing factors in two directions, which can eliminate the local minimum to a certain extent and keep the continuity of the function. Briefly, they ensure that the variable value of the cosine function is kept between \left[-{\displaystyle \frac{\text{π}}{2}},{\displaystyle \frac{\text{π}}{2}}\right]. Therefore, \left\{{\sigma }_x,{\sigma }_y\right\} are related to {m, n}. In addition, on the basis of the chain rule, its partial derivative \mathrm{\nabla }J \left({\mathit{p}}_{\mathrm{h}}\right) can also be easily obtained:

(2)\mathrm{\nabla }J\left({\mathit{p}}_{\mathrm{h}}\right)= \left[\begin{array}{c}{\displaystyle \frac{\mathrm{\partial }J\left({\mathit{p}}_{\mathrm{h}}\right)}{\mathrm{\partial }{x}_{\mathrm{h}}}}\\ {\displaystyle \frac{\mathrm{\partial }J\left({\mathit{p}}_{\mathrm{h}}\right)}{\mathrm{\partial }{y}_{\mathrm{h}}}}\end{array}\right]= \left[\begin{array}{c}{\displaystyle \sum _{i=0}^m{\displaystyle \sum _{j=0}^n\left(-{\displaystyle \frac{\text{π}}{{\sigma }_x}}\cos {\displaystyle \frac{\text{π}\left(x_{\mathrm{h}}-x_{i,j}\right)}{2{\sigma }_x}}\sin {\displaystyle \frac{\text{π}\left(x_{\mathrm{h}}-x_{i,j}\right)}{2{\sigma }_x}}{\cos }^2{\displaystyle \frac{\text{π}\left(y_{\mathrm{h}}-y_{i,j}\right)}{2{\sigma }_y}}\right)}}\\ {\displaystyle \sum _{i=0}^m{\displaystyle \sum _{j=0}^n\left(-{\displaystyle \frac{\text{π}}{{\sigma }_y}}{\cos }^2{\displaystyle \frac{\text{π}\left(x_{\mathrm{h}}-x_{i,j}\right)}{2{\sigma }_x}}\cos {\displaystyle \frac{\text{π}\left(y_{\mathrm{h}}-y_{i,j}\right)}{2{\sigma }_y}}\sin {\displaystyle \frac{\text{π}\left(y_{\mathrm{h}}-y_{i,j}\right)}{2{\sigma }_y}}\right)}}\end{array}\right].

Fig.8 shows the outcome heightmap of NDCI, whose shape is similar to the PCI result in Fig.7 but greatly reduces the difficulty of calculation. The lower part of Fig.8 shows the gradient in the horizontal directions, respectively.

Fig.8 NDCI for transforming a pixel into a continuous heightmap, with its gradients. NDCI: normalized derivable convolution interpolation.

Full size|PPT slide

3.2 Costmap generation

In the previous subsection, we proposed a method for continuous smoothing of grid maps. This subsection will take the above approach and describes the process of generating a costmap from a complete heightmap obtained by the depth camera.

First, a discrete heightmap of the terrain around the robot is generated. The robot only has a depth camera on its head. To obtain complete terrain information around the robot, the point cloud data obtained by the depth camera during the movement of the robot should be recorded. Then, the point cloud data, combined with the robot’s position and attitude information, are spliced and filtered. The spliced point cloud data are compressed, sampled, and rasterized, so we can obtain a heightmap of the terrain around the robot. The heightmap has (100 × 50) pixels, each pixel corresponds to an area (2 cm × 2 cm) projected on the horizontal plane, and each pixel’s value (grayscale) represents a different height.

Secondly, the heightmap is transformed into a discrete height difference map. The places with large height difference are usually the edges of obstacles, which are the areas that the robot does not want to step on. The places with small height difference are flat areas, which can be safely stepped on with confidence. The edge detection algorithm commonly used in digital image processing is employed.

Lastly, the method mentioned in Subsection 3.1 is applied to convert the discrete height difference map into a continuous costmap. This costmap will be used as one of the items for optimization in the next section. Fig.9 shows our experimental environment, which will be further discussed in Section 6, as well as the processed discrete heightmap and continuous costmap.

Fig.9 Experimental environment and the postprocessing: (a) photograph, (b) point cloud, (c) heightmap, (d) height difference map, and (e) costmap.

Full size|PPT slide

The processing and splicing of point cloud data are updated in real time at the frequency of 30 Hz. The heightmap is sampled at once before each step cycle of the robot. After the discrete height difference map is obtained, the continuous costmap does not need to be calculated completely at one time, but is calculated iteratively in accordance with different points p_h in the form of a cost function in the optimization process in the next section.

4 Locomotion optimization

As mentioned earlier, footholds optimization includes two parts: terrain perception and the robot’s own state. In the previous section, the cost function based on terrain perception was generated. In consideration of the robot state, this section will build a continuous and derivable objective function to obtain optimized footholds. Afterward, the body trajectory is corrected in accordance with the selected foothold, and the motion control in Section 5 can be performed.

4.1 Foothold preplanning

The robot adopts the stable walking mode of tripod-trot gait. As described in Subsection 2.2, during the alternating swing of the two leg groups, when the ith leg is in the swing phase, according to the heuristic algorithm [2] inspired by the SLIP model, the preplanning of the target foothold is as follows:

(3)\begin{array}{rl}& {\mathit{p}}_{\mathrm{p}\mathrm{r}\mathrm{e},i}={\mathit{p}}_{\mathrm{b}}+\mathit{R}\cdot {}^{\mathrm{B}}{\mathit{p}}_{\mathrm{h}\mathrm{i}\mathrm{p},i}+\frac{T_{\mathrm{s}\mathrm{t}}}{2}\mathit{v}+\left(T_{\mathrm{s}\mathrm{w}}-t_{\mathrm{s}\mathrm{w},i}\right){\mathit{v}}_{\mathrm{d}},\\ & \mathit{R}={\mathit{R}}_{\mathit{\text{z}}}\left(\psi \right){\mathit{R}}_y\left(\theta \right){\mathit{R}}_x\left(\varphi \right),\end{array}

where {\mathit{p}}_{\mathrm{p}\mathrm{r}\mathrm{e},i} is the preplanning of the target foothold, p_b is the current position of the body, R∈SO(3) is the current attitude matrix, calculated from the Euler angles, roll (\varphi), pitch (θ), and yaw (ψ) measured by the IMU, ^Bp_hip,i is the position of the ith leg’s hip joint in the BCS, v is the current velocity of the body, v_d is the desired velocity of the body, T_st is the duration of the standing phase, T_sw is the duration of the swing phase, and t_sw,i is the time when the ith leg enters the swing phase.

After the preplanned foothold is obtained, in consideration of the terrain information, calculating an adjustment Δp_i relative to the preplanned foothold is necessary. The addition of them is the optimized foothold position.

(4){\mathit{p}}_i={\mathit{p}}_{\mathrm{p}\mathrm{r}\mathrm{e},i}+\mathrm{\Delta }{\mathit{p}}_i.

The next subsection will find the optimized foothold p_i.

4.2 Footholds optimization

With the tripod-trot gait, there are only three legs in swing phase at every footstep. Therefore, the legs can be divided into two groups (leg groups A and B) in accordance with which are in swing phase, and only one group needs to be optimized at each footstep. Only the horizontal components of the footholds are considered because the height component is directly obtained from the heightmap in accordance with the coordinates of the horizontal components.

The horizontal components of the foothold point {\mathit{p}}_i\in {\mathbb{R}}^3 are extracted and reorganized into {\mathit{h}}_i\in {\mathbb{R}}^2 as

(5)\{\begin{array}{rl}& {\mathit{p}}_i={\left[\begin{array}{ccc}{p}_{i,x}& p_{i,y}& p_{i,\mathit{\text{z}}}\end{array}\right]}^{\mathrm{T}},\\ & {\mathit{h}}_i={\left[\begin{array}{cc}{p}_{i,x}& p_{i,y}\end{array}\right]}^{\mathrm{T}}.\end{array}

Then, the grouped footholds are merged into one vector h as

(6)\mathit{h}=\{\begin{array}{c}{\left[\begin{array}{ccc}{\mathit{h}}_0^{\mathrm{T}}& {\mathit{h}}_2^{\mathrm{T}}& {\mathit{h}}_4^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}},\mathrm{l}\mathrm{e}\mathrm{g}\mathrm{g}\mathrm{r}\mathrm{o}\mathrm{u}\mathrm{p}\mathrm{A},\\ {\left[\begin{array}{ccc}{\mathit{h}}_1^{\mathrm{T}}& {\mathit{h}}_3^{\mathrm{T}}& {\mathit{h}}_5^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}},\mathrm{l}\mathrm{e}\mathrm{g}\mathrm{g}\mathrm{r}\mathrm{o}\mathrm{u}\mathrm{p}\mathrm{B}.\end{array}

Similarly, the preplanned foothold p_pre,i is extracted as h_pre,i, and the grouped preplanned footholds are merged into a vector h_pre,

(7){\mathit{h}}_{\mathrm{p}\mathrm{r}\mathrm{e},i}={\left[\begin{array}{cc}{p}_{\mathrm{p}\mathrm{r}\mathrm{e},i,x}& p_{\mathrm{p}\mathrm{r}\mathrm{e},i,y}\end{array}\right]}^{\mathrm{T}},

(8){\mathit{h}}_{\mathrm{p}\mathrm{r}\mathrm{e}}=\{\begin{array}{c}{\left[\begin{array}{ccc}{\mathit{h}}_{\mathrm{p}\mathrm{r}\mathrm{e},0}^{\mathrm{T}}& {\mathit{h}}_{\mathrm{p}\mathrm{r}\mathrm{e},2}^{\mathrm{T}}& {\mathit{h}}_{\mathrm{p}\mathrm{r}\mathrm{e},4}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}},\mathrm{l}\mathrm{e}\mathrm{g}\mathrm{g}\mathrm{r}\mathrm{o}\mathrm{u}\mathrm{p}\mathrm{A},\\ {\left[\begin{array}{ccc}{\mathit{h}}_{\mathrm{p}\mathrm{r}\mathrm{e},1}^{\mathrm{T}}& {\mathit{h}}_{\mathrm{p}\mathrm{r}\mathrm{e},3}^{\mathrm{T}}& {\mathit{h}}_{\mathrm{p}\mathrm{r}\mathrm{e},5}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}},\mathrm{l}\mathrm{e}\mathrm{g}\mathrm{g}\mathrm{r}\mathrm{o}\mathrm{u}\mathrm{p}\mathrm{B}.\end{array}

Finally, the footholds optimization equation is established as

(9)\begin{array}{c}{\mathit{h}}^{\ast }=\arg \underset{\mathit{h}\in {\mathbb{R}}^6}{min}\left(\alpha \sum _{i}J\left({\mathit{h}}_i\right)+{\left(\mathit{h}-{\mathit{h}}_{\mathrm{p}\mathrm{r}\mathrm{e}}\right)}^{\mathrm{T}}\mathit{W}\left(\mathit{h}-{\mathit{h}}_{\mathrm{p}\mathrm{r}\mathrm{e}}\right)\right),\\ i\in \{\begin{array}{c}{\left[\begin{array}{ccc}0& 2& 4\end{array}\right]}^{\mathrm{T}},\mathrm{l}\mathrm{e}\mathrm{g}\mathrm{g}\mathrm{r}\mathrm{o}\mathrm{u}\mathrm{p}\mathrm{A},\\ {\left[\begin{array}{ccc}1& 3& 5\end{array}\right]}^{\mathrm{T}},\mathrm{l}\mathrm{e}\mathrm{g}\mathrm{g}\mathrm{r}\mathrm{o}\mathrm{u}\mathrm{p}\mathrm{B},\end{array}\\ \mathrm{s}.\mathrm{t}.-\mathrm{\Delta }{\mathit{h}}_{\mathrm{l}\mathrm{i}\mathrm{m}}⩽\mathit{h}-{\mathit{h}}_{\mathrm{p}\mathrm{r}\mathrm{e}}⩽\mathrm{\Delta }{\mathit{h}}_{\mathrm{l}\mathrm{i}\mathrm{m}},\mathrm{\Delta }{\mathit{h}}_{\mathrm{l}\mathrm{i}\mathrm{m}}>0,\end{array}

where {\mathit{h}}^{\ast } is optimized horizontal coordinate of the foothold.

The first item of the optimization objective J(h_i) is the terrain cost shown in Section 3, as to keep away from the edge of potholes and obstacles, to prevent footsteps from slipping, and to maintain the predictive balance of the robot. α is the weighting factor of this cost. The second item is the balance cost of the robot state. The weighting matrix W = βD + γQ is a symmetric matrix, as shown below, where the diagonal matrix D indicates that the optimized footholds cannot be too far away from the preplanned footholds h_pre, and the symmetrical matrix Q indicates that the grouped footholds can be adjusted in the same direction. {β, γ} are their weights. The constraint Δh_lim represents the maximum amount of adjustment Δp_i allowed to prevent exceeding the leg workspace.

(10)\begin{array}{rl}\mathit{W}=& \beta \underset{\mathit{D}}{\underbrace{\left[\begin{array}{cccccc}1& & & & & \\ & 1& & & & \\ & & 1& & & \\ & & & 1& & \\ & & & & 1& \\ & & & & & 1\end{array}\right]}}\\ & +\gamma \underset{\mathit{Q}}{\underbrace{\left[\begin{array}{cccccc}2& 0& -1& 0& -1& 0\\ 0& 2& 0& -1& 0& -1\\ -1& 0& 2& 0& -1& 0\\ 0& -1& 0& 2& 0& -1\\ -1& 0& -1& 0& 2& 0\\ 0& -1& 0& -1& 0& 2\end{array}\right]}}.\end{array}

The designation of the symmetric matrix Q is intuitive. Let Δh = h − h_pre, which is an expansion of the following quadratic formula:

(11)\begin{array}{rl}\mathrm{\Delta }{\mathit{h}}^{\mathrm{T}}\mathit{Q}\mathrm{\Delta }\mathit{h}=& {\left({}_0^{}\mathrm{\Delta }\mathit{h}-{}_2^{}\mathrm{\Delta }\mathit{h}\right)}^2+{\left({}_0^{}\mathrm{\Delta }\mathit{h}-{}_4^{}\mathrm{\Delta }\mathit{h}\right)}^2\\ & +{\left({}_2^{}\mathrm{\Delta }\mathit{h}-{}_4^{}\mathrm{\Delta }\mathit{h}\right)}^2+{\left({}_1^{}\mathrm{\Delta }\mathit{h}-{}_3^{}\mathrm{\Delta }\mathit{h}\right)}^2\\ & +{\left({}_1^{}\mathrm{\Delta }\mathit{h}-{}_5^{}\mathrm{\Delta }\mathit{h}\right)}^2+{\left({}_3^{}\mathrm{\Delta }\mathit{h}-{}_5^{}\mathrm{\Delta }\mathit{h}\right)}^2.\end{array}

To distinguish from the right subscript of h_i representing different legs, the left subscript of _nΔh represents the nth element of Δh. When it is used as a cost function, it indicates that the adjustments of the footholds of different legs are encouraged to be biased in the same direction, to prevent different legs from moving away from or approaching each other, which would impair the body balance.

In the objective function Eq. (9), the gradient \mathrm{\nabla }J of the first term J(h_i) is given by Eq. (2). The second term is a quadratic type, whose gradient can be easily obtained. In this way, the objective function can be solved via the Levenberg–Marquardt method [30].

After the optimized horizontal coordinate {\mathit{h}}_i^{\ast }\in {\mathbb{R}}^2 of the foothold is obtained, it is easy to find the height components {\mathit{p}}_{i,\mathit{\text{z}}}^{\ast } at the corresponding position on the heightmap. Then, they are combined into a 3D point {\mathit{p}}_i^{\ast }={\left[\begin{array}{ccc}{h}_{i,x}^{\ast }& h_{i,y}^{\ast }& p_{i,\mathit{\text{z}}}^{\ast }\end{array}\right]}^{\mathrm{T}}\in {\mathbb{R}}^3, which is the final optimized position of the foothold for i-leg.

4.3 Body trajectory correction

The preplanned footholds are obtained on the basis of the heuristic algorithm, which conform to the basic SLIP balance model. After new optimized footholds are generated, this balance may be broken to some extent. Given that the optimized footholds include the costs of adapting to the terrain, the body trajectory should be corrected in accordance with the optimized footholds.

(12)\{\begin{array}{c}\mathrm{\Delta }{\mathit{p}}_{\mathrm{b}}^{\ast }=\sum _i\left({\mathit{p}}_i^{\ast }-{\mathit{p}}_{\mathrm{p}\mathrm{r}\mathrm{e},i}\right),i\in \{\begin{array}{c}{\left[\begin{array}{ccc}0& 2& 4\end{array}\right]}^{\mathrm{T}},\mathrm{l}\mathrm{e}\mathrm{g}\mathrm{g}\mathrm{r}\mathrm{o}\mathrm{u}\mathrm{p}\mathrm{A},\\ {\left[\begin{array}{ccc}1& 3& 5\end{array}\right]}^{\mathrm{T}},\mathrm{l}\mathrm{e}\mathrm{g}\mathrm{g}\mathrm{r}\mathrm{o}\mathrm{u}\mathrm{p}\mathrm{B},\end{array}\\ \mathrm{\Delta }{\mathit{v}}_{\mathrm{b}}^{\ast }=0,\end{array}

where \mathrm{\Delta }{\mathit{p}}_{\mathrm{b}}^{\ast } is the correction amount on the original body trajectory. The speed correction \mathrm{\Delta }{\mathit{v}}_{\mathrm{b}}^{\ast } is zero. Then, according to the gait cycle, within the current swing phase T_sw, which refers to the position and velocity startpoint and endpoint conditions, the body trajectory correction is subjected to cubic interpolation.

To further improve the accuracy, there is a discussion that is it possible to run the footholds optimization repeatedly in accordance with the corrected body trajectory? In fact, it is not necessary. The reason is that, in Eq. (10), the balance cost of the second term, the quadratic matrix Q already contains the inspiration of the grouped legs adjusting to the same direction. In the experiment, we have tried to iterate the optimization many times but found little improvement.

5 Control strategy

Subsection 2.1 described the basic gait scheduler that directs the lift-up and touch-down of each leg. Section 4 introduced the optimization method for the footholds and the correction of the body trajectory. However, these two parts are not enough to control a robot.

For a legged robot, animal-like walking is realized by swinging and touching-down its legs alternately. Therefore, the legs are divided into two groups, namely, swing and support. During the walking process, the swing trajectories of swing legs should be controlled to step on the desired foothold point of each leg. The support legs should also be controlled to maintain body stability and its movement. These actions rely on the cooperation of each joint of the robot, which means that the motors need to provide the corresponding torque. This section will go through the remaining part of the control framework shown in Fig.3.

5.1 Swing leg control

In the WCS, assuming there is no relative sliding between the supporting legs and the ground, the current tip of the supporting leg is fixed to the ground. The optimal footholds are also calculated in the WCS, which will be the support points for the next gait cycle of each leg. Therefore, when the leg enters the swing phase, the current support point and the optimized foothold must be connected through a smooth curve in combination with the gait scheduler. This curve is the leg swing trajectory.

The current tip position of the leg is obtained by forward kinematics and state estimation [31]. This study adopts a spatial composite trajectory based on cycloid. The expression is as follows:

(13)\begin{array}{rl}{\mathit{p}}_{{\xi }_i}\left(t_{\mathrm{s}\mathrm{w},i}\right)=& \left[\begin{array}{c}{p}_{x,{\xi }_i}\\ p_{y,{\xi }_i}\\ p_{\mathit{\text{z}},{\xi }_i}\end{array}\right]=\frac{{\mathit{p}}_i^{\ast }-{\mathit{p}}_{{\xi }_i=0}}{2\text{π}}\left({\xi }_i-\mathrm{s}\mathrm{i}\mathrm{n}\left({\xi }_i\right)\right)\\ & +\left[\begin{array}{c}0\\ 0\\ {\displaystyle \frac{h_{\mathit{\text{z}}}}{2}}\end{array}\right]\left(1-\mathrm{c}\mathrm{o}\mathrm{s}\left({\xi }_i\right)\right),\\ {\xi }_i=& 2\text{π}\frac{t_{\mathrm{s}\mathrm{w},i}}{T_{\mathrm{s}\mathrm{w}}}\in \left[0,2\text{π}\right].\end{array}

The trajectory {\mathit{p}}_{{\xi }_i}\in {\mathbb{R}}^3 is a function of the ith leg’s swing time t_sw,i, with {\xi }_i for the swing phase. {\mathit{p}}_i^{\ast } is the target foothold optimized in Section 4, {\mathit{p}}_{{\xi }_i=0} is the tip position before leg lifting (when t_sw,i= 0), h_z is the step height that needs to be raised in the middle of the swing process. This trajectory is continuous and derivable, and the derivative of the trajectory is the velocity of the tip. Fig.10 shows a step-walk sequence. The red solid line represents the swing leg, and the blue solid line is the support leg. The red dotted line is the swing trajectory, and the yellow dotted line is the swing trajectory when the height component of {\mathit{p}}_i^{\ast } is equal to 0.

Fig.10 Swing trajectory based on cycloid spatial composite trajectory.

Full size|PPT slide

After the swing trajectory is determined, the target position of the tip in the LCS {}_{}^{\mathrm{L}}{\mathit{l}}_i^{\ast } is obtained as follows:

(14){}_{}^{\mathrm{L}}{\mathit{l}}_i^{\ast }={\mathit{R}}^{\mathrm{T}}\left({\mathit{p}}_{{\xi }_i}-{\mathit{p}}_{\mathrm{b}}\right)- {}_{}^{\mathrm{B}}{\mathit{p}}_{\mathrm{h}\mathrm{i}\mathrm{p},i}.

Then, the virtual force {}_{}^{\mathrm{L}}{\mathit{f}}_{\mathrm{s}\mathrm{w},i}^{\ast } of the swing leg in LCS is generated by tracking the swing trajectory through the virtual model impedance control. The virtual model is shown in Fig.11.

Fig.11 Leg virtual model of impedance control.

Full size|PPT slide

(15){}_{}^{\mathrm{L}}{\mathit{f}}_{\mathrm{s}\mathrm{w},i}^{\ast }= {}_{}^{\mathrm{L}}{\mathit{K}}_{\mathrm{P}} \left({}_{}^{\mathrm{L}}{\mathit{l}}_i^{\ast }-{}_{}^{\mathrm{L}}{\mathit{l}}_i\right)+{}_{}^{\mathrm{L}} {\mathit{K}}_{\mathrm{D}} \left({}_{}^{\mathrm{L}}{\dot{\mathit{l}}}_i^{\ast }-{}_{}^{\mathrm{L}}{\dot{\mathit{l}}}_i\right).

The inertia term is not included in the impedance model formulation because we assume that the leg has a negligibly small mass. The notations with a subscript *, i.e., {}_{}^{\mathrm{L}}{\mathit{l}}_i^{\ast } and its derivative {}_{}^{\mathrm{L}} {\dot{\mathit{l}}}_i^{\ast }, are the target position and velocity of the ith leg in the LCS, respectively, {}_{}^{\mathrm{L}} {\mathit{l}}_i and its derivative {}_{}^{\mathrm{L}}{\dot{\mathit{l}}}_i are the position and velocity of the ith leg obtained by the forward kinematics, respectively. Diagonal matrices \left\{{}_{}^{\mathrm{L}}{\mathit{K}}_{\mathrm{P}},{}_{}^{\mathrm{L}}{\mathit{K}}_{\mathrm{D}}\right\}\in {\mathbb{R}}^{3\times 3} represent the three-dimensional stiffness and damping of the leg, respectively. Through this virtual model, the feedforward force {}_{}^{\mathrm{L}}{\mathit{f}}_{\mathrm{s}\mathrm{w},i}^{\ast } of the swing leg can be obtained.

5.2 Body balance control

A CBC is used to generate the virtual force (or called ground reaction force, GRF [2,32]) of the supporting legs. The states that need to be controlled for body balance include the desired position {\mathit{p}}_{\mathrm{b}}^{\ast }\in {\mathbb{R}}^3 and velocity {\dot{\mathit{p}}}_{\mathrm{b}}^{\ast }\in {\mathbb{R}}^3 of the center of mass (CoM), along with the attitude {\mathit{R}}^{\ast }\in SO\left(3\right) and angular velocity {\text{ω}}_{\mathrm{b}}^{\ast }\in {\mathbb{R}}^3 of the body. The feedback control regulation can be described by the following formula:

(16)\begin{array}{rl}& {\ddot{\mathit{p}}}_{\mathrm{b}}^{\ast }={\mathit{K}}_{\mathrm{P},\mathrm{p}}\left({\mathit{p}}_{\mathrm{b}}^{\ast }-{\hat{\mathit{p}}}_{\mathrm{b}}\right)+{\mathit{K}}_{\mathrm{D},\mathrm{p}}\left({\dot{\mathit{p}}}_{\mathrm{b}}^{\ast }-{\widehat{\dot{\mathit{p}}}}_{\mathrm{b}}\right),\\ & {\dot{\text{ω}}}_{\mathrm{b}}^{\ast }={\mathit{K}}_{\mathrm{P},\mathrm{a}}\left(\mathrm{l}\mathrm{o}\mathrm{g}\left({\mathit{R}}^{\ast }{\hat{\mathit{R}}}^{\mathrm{T}}\right)\right)+{\mathit{K}}_{\mathrm{D},\mathrm{a}}\left({\text{ω}}_{\mathrm{b}}^{\ast }-{\widehat{\text{ω}}}_{\mathrm{b}}\right),\end{array}

where \left\{{\hat{\mathit{p}}}_{\mathrm{b}},{\widehat{\dot{\mathit{p}}}}_{\mathrm{b}},{\widehat{\mathbf{\text{ω}}}}_{\mathrm{b}}\right\}\in {\mathbb{R}}^3 and \hat{\mathit{R}}\in SO\left(3\right) are the estimated states of the current position, velocity, angular velocity, and attitude of the robot. The diagonal matrices \left\{{\mathit{K}}_{\mathrm{P},\mathrm{p}},{\mathit{K}}_{\mathrm{D},\mathrm{p}},{\mathit{K}}_{\mathrm{P},\mathrm{a}},{\mathit{K}}_{\mathrm{D},\mathrm{a}}\right\}\in {\mathbb{R}}^{3\times 3} are the stiffness (subscript with P) and damping (subscript with D) of position (subscript with ,p) and attitude (subscript with ,a). log(*) is the mapping from a rotation matrix (SO(3)) to a rotation vector ({\mathbb{R}}^3). After the above PD control regulation, the control values of the CoM’s acceleration {\ddot{\mathit{p}}}_{\mathrm{b}}^{\ast }\in {\mathbb{R}}^3 and angular acceleration {\dot{\text{ω}}}_{\mathrm{b}}^{\ast }\in {\mathbb{R}}^3 are obtained.

The position acceleration and angular acceleration need to be provided by the GRFs of the support legs. Here, the mass of legs is assumed to be negligible compared with the mass of body, and the body mass is concentrated on the CoM, ignoring the influence of the Coriolis force and centrifugal force. Then, the simplified dynamics of the robot satisfies

(17)\{\begin{array}{c}m\left({\ddot{\mathit{p}}}_{\mathrm{b}}^{\ast }+\mathit{g}\right)={\sum }_1^{\mu }{\mathit{f}}_i,\\ \hat{\mathit{R}}\cdot {}_{}^{\mathrm{B}}\mathit{I}\cdot {\hat{\mathit{R}}}^{\mathrm{T}}{\dot{\text{ω}}}_{\mathrm{b}}^{\ast }\approx {\sum }_1^{\mu }\hat{\mathit{R}}\cdot {}_{}^{\mathrm{B}}{\mathit{l}}_6\times {\mathit{f}}_i,\end{array}

where m is the mass of CoM, \mathit{g}\in {\mathbb{R}}^3 is the local gravitational acceleration, and {}_{}^{\mathrm{B}}\mathit{I}\in {\mathbb{R}}^{3\times 3} is the inertia matrix of the body in the BCS. μ means the leg number in contact, ^Bl_i is the tip position in the BCS, and f_i is the GRF of the ith leg. Equation (17) is rewritten in matrix form as

(18)\underset{\mathit{A}}{\underbrace{\left[\begin{array}{ccc}\mathit{E}& \dots & \mathit{E}\\ \left[\hat{\mathit{R}}\cdot {}_{}^{\mathrm{B}}{\mathit{l}}_1\right]\times & \dots & \left[\hat{\mathit{R}}\cdot {}_{}^{\mathrm{B}}{\mathit{l}}_6\right]\times \end{array}\right]}}\underset{\mathit{f}}{\underbrace{\left[\begin{array}{c}{\mathit{f}}_1\\ ⋮\\ {\mathit{f}}_6\end{array}\right]}}=\underset{\mathit{b}}{\underbrace{\left[\begin{array}{c}m\left({\ddot{\mathit{p}}}_{\mathrm{b}}^{\ast }+\mathit{g}\right)\\ \hat{\mathit{R}}\cdot {}_{}^{\mathrm{B}}\mathit{I}\cdot {\hat{\mathit{R}}}^{\mathrm{T}}{\dot{\text{ω}}}_{\mathrm{b}}^{\ast }\end{array}\right]}},

where \mathit{E}\in {\mathbb{R}}^{3\times 3} is the identity matrix. The symbol [*]× maps from a vector ({\mathbb{R}}^3) to an antisymmetric matrix ({\mathbb{R}}^{3\times 3}). A, f, and b are the merged matrix and vectors defined in Eq. (18). The legs in the support phase form an over-restraint system for the body. Therefore, the virtual force of each support leg should be obtained through the following optimization equation:

(19)

The diagonal matrix \mathit{S}\in {\mathbb{R}}^{18\times 18} is the selection matrix, and its elements will be zeros if the corresponding leg is in the swing phase, \delta is the weighting factor for CBC, Matrix C and vector f_lim ensure that the optimized GRFs satisfy the friction cone and upper and lower bound constraints, {\mathit{f}}_{\mathrm{s}\mathrm{t}}^{\ast } is the virtual force of the supporting legs and {\mathit{f}}_{\mathrm{s}\mathrm{t},i}^{\ast } is the virtual force of the supporting legs. The virtual force of the supporting leg can be optimized by the balance optimizer model above:

(20){\mathit{f}}_{\mathrm{s}\mathrm{t}}^{\ast }={\left[\begin{array}{ccc}{\mathit{f}}_{\mathrm{s}\mathrm{t},1}^{\ast }& \dots & {\mathit{f}}_{\mathrm{s}\mathrm{t},6}^{\ast }\end{array}\right]}^{\mathrm{T}}.

Then, the feedforward force {}_{}{}^{\mathrm{L}} \mathit{f}{}_{\mathrm{s}\mathrm{t},\mathrm{i}}^{\ast } in the LCS is obtained as follows:

(21){}_{}^{\mathrm{L}}{\mathit{f}}_{\mathrm{s}\mathrm{t},i}^{\ast }= {\hat{\mathit{R}}}^{\mathrm{T}}{\mathit{f}}_{\mathrm{s}\mathrm{t},i}^{\ast }.

5.3 Joint PD control

The virtual force obtained above is transformed into joint space as feedforward torque, and the final control torque of the actuator is generated through joint feedback control.

(22){}_{}^{\mathrm{L}}{\mathit{f}}_i^{\ast }=\{\begin{array}{c}{}_{}^{\mathrm{L}}{\mathit{f}}_{\mathrm{s}\mathrm{w},i}^{\ast },\mathrm{s}\mathrm{w}\mathrm{i}\mathrm{n}\mathrm{g}\mathrm{l}\mathrm{e}\mathrm{g},\\ {}_{}^{\mathrm{L}}{\mathit{f}}_{\mathrm{s}\mathrm{t},i}^{\ast },\mathrm{s}\mathrm{u}\mathrm{p}\mathrm{p}\mathrm{o}\mathrm{r}\mathrm{t}\mathrm{l}\mathrm{e}\mathrm{g},\end{array}

(23){\mathit{\tau }}_i^{\ast }={\mathit{J}}_i^{\mathrm{T}}\cdot {}_{}^{\mathrm{L}} {\mathit{f}}_i^{\ast }+{\mathit{K}}_{\mathrm{P},\mathrm{j}}\left({\mathit{q}}_i^{\ast }-{\mathit{q}}_i\right)+{\mathit{K}}_{\mathrm{D},\mathrm{j}}\left({\dot{\mathit{q}}}_i^{\ast }-{\dot{\mathit{q}}}_i\right),

where {}_{}^{\mathrm{L}} {\mathit{f}}_i^{\ast } is the virtual feedforward force of the ith leg in the LCS, {\mathit{J}}_i\in {\mathbb{R}}^{3\times 3} is the Jacobian matrix of the ith leg, and the diagonal matrices \left\{{\mathit{K}}_{\mathrm{P},\mathrm{j}},{\mathit{K}}_{\mathrm{D},\mathrm{j}}\right\}\in {\mathbb{R}}^{3\times 3} are the joint stiffness and damping coefficients of each leg. \left\{{\mathit{q}}_i^{\ast },{\dot{\mathit{q}}}_i^{\ast }\right\}\in {\mathbb{R}}^3 are the target angle and angular velocity for each joint of the leg, respectively, which are obtained by the inverse kinematic solution. \left\{{\mathit{q}}_i,{\dot{\mathit{q}}}_i\right\}\in {\mathbb{R}}^3 are the current angle and angular velocity read by the encoder installed on each joint. Finally, the control torque {\mathit{\tau }}_i^{\ast } for the joint of each leg is obtained and sent to the motor driver.

So far, the entire robot control framework is accomplished.

6 Contrast experiments and analysis

The approach mentioned above is tested on a hexapod robot, LittleStrong. We conduct experiments on a terrain composed of irregularly placed strips. As shown in Fig.12, each strip is about 0.2 m wide and 0.12 m high. To compare our foothold optimization algorithm, we set up a control group, comparing the results of the hexapod passing through the terrain without and with using the foothold optimization method proposed in Subsection 6.1. The comparison of the experiments is analyzed in Subsection 6.2.

Fig.12 Experiment setup for footholds optimization with the hexapod robot.

Full size|PPT slide

6.1 Contrast experiments

(1) Without the presented foothold optimization algorithm

First, the robot walks blindly, adopting a heuristic method with fixed gait cycle and step length. In the experiment, the robot slips when stepping on the edge of the strip. The process is demonstrated in the attached video; a screenshot of the process is shown in Fig.13.

Fig.13 Screenshots of blind walking. Slips are circled in red.

Full size|PPT slide

When there is a sliding step, the robot exhibits instantaneous shaking. Fig.14 shows the graph of the Euler angles of robot attitude, which indicates that when slips occur, the robot loses its balance, and the Euler angles have large peaks. Hence, in the case of irregular terrain, the balance of the robot, when facing obstacles, is difficult to ensure when a blind walking gait is adopted.

Fig.14 Euler angles during blind walk.

Full size|PPT slide

(2) With the presented footholds optimization algorithm

After our online foothold optimization method is deployed, experiments are carried out under the same scenario mentioned above. The process is also shown in the attached video; Fig.15 shows the screenshots of it. The robot adopts a variable step length, and each step is stepped on a position that is relatively safe from the edge of the strip, and no sliding occurs. Fig.16 shows the point cloud data processing, and the outstanding points in it are the swing leg tips before touching down.

Fig.15 Screenshots of footholds optimization.

Full size|PPT slide

Fig.16 Screenshots of point cloud processing.

Full size|PPT slide

Fig.17 depicts the footholds optimization results from the corresponding experiment. The six thinner curves are the step length of each leg. In the tripod-trot gait, the back-right, middle-left, and front-right legs are in one group; the back-left, front-left, and middle-right legs are in another group. Accordingly, the bold blue curve is the average step length of the leg group coming into the standing phase. From the attached video, a smaller step length is carried out before the strip to avoid stepping on the edge of it. Meanwhile, a larger step length is used when leaving the strip.

Fig.17 Step length with footholds optimization.

Full size|PPT slide

With the elimination of the interference of sliding and falling under the feet, the balance of the robot has been greatly improved. As shown in Fig.18, compared with Fig.14, the Euler angles are well smoothed, and no peaks occur.

Fig.18 Euler angles when using foothold optimization.

Full size|PPT slide

6.2 Analysis of the contrast experiments

We have conducted 10 comparative experiments, as shown above. From Tab.1, when the blind walking algorithm is used, the foot slides for a total of 47 times, and the Euler angle fluctuation variance of the body is 5.71×10⁻⁴ rad² for roll and 5.40×10⁻⁴ rad² for pitch. After the foothold optimization algorithm is applied, sliding occurs 2 times (caused by the failure of point cloud splicing in one experiment, which is beyond the discussion of this paper). The fluctuation variance of the body Euler angle is reduced to 2.72×10⁻⁴ rad² for roll and 3.21×10⁻⁴ rad² for pitch, indicating that our algorithm significantly improves the stability of the robot.

Tab.1 Contrast between blind walking and footholds optimized walking

Experiment	Slip number	Variance of Euler angle/(10−4 rad2)
		Roll	Pitch
Blind walking	47	5.71	5.40
Footholds optimized	2	2.72	3.21

7 Conclusions

In this paper, we propose a novel smooth and derivable method for generating a costmap from a terrain grid map. Moreover, we propose an algorithm for online optimization of footholds, combined with the robot’s heuristic gait. The algorithm is fast and stable, and has a clear physical meaning and optimization objective, which can be directly deployed on the onboard computer. We verify this method on a hexapod robot. On an irregular terrain, the robot using blind walking will stumble. Now, with the proposed algorithm, combined with real-time visual information, the robot can pass through the terrain gracefully. The real-time property and feasibility of the algorithm are proved.

Nomenclature

Abbreviations
BCS	Body coordinate system
CBC	Centroid balance control
CI	Convolution interpolation
CNN	Convolutional neural network
CoM	Center of mass
GRF	Ground reaction force
IMU	Inertial measurement unit
LCS	Leg coordinate system
NDCI	Normalized derivable convolution interpolation
PCI	Pyramid convolution interpolation
RGBD	Red, green, blue, depth
SLIP	Spring-loaded inverted pendulum
WCS	World coordinate system
Variables
A	Merged matrix for CBC
b, f	Merged vectors for CBC
ci, j	Center of the pixel (i, j) on the map
C	Matrix of friction cone and upper and lower bound constraints for GRF
E	Identity matrix
fi	GRF of the ith leg
{}_{}{}^{\mathrm{L}}\mathit{f}{}_i^{\ast }	Virtual feedforward force of the ith leg in the LCS
flim	Friction cone and upper and lower bound constraints for GRF
{\mathit{f}}_{\mathrm{s}\mathrm{t}}^{\ast }	Virtual force of the supporting legs
{\mathit{f}}_{\mathrm{s}\mathrm{t},i}^{\ast }	Virtual force of the ith supporting leg
{}_{}^L{\mathit{f}}_{\mathrm{s}\mathrm{t},i}^{\ast }, {}_{}^{\mathrm{L}}{\mathit{f}}_{\mathrm{s}\mathrm{w},i}^{\ast }	Virtual feedforward forces of the ith supporting leg and swing leg in the LCS, respectivley
g	Local gravitational acceleration
hz	Step height that needs to be raised in the middle of the swing process
h	Merged vector of hi
h*	Horizontal component of optimized footholds
hi	Horizontal component of pi
{\mathit{h}}_i^{\ast }	Optimized horizontal coordinate of the foothold
hpre	Merged vector of hpre,i
hpre,i	Horizontal component of preplanned foothold
Δh	Intermediate variable of the difference between the optimized footholds and the preplanned footholds
Δhlim	Maximum amount of adjustment Δpi allowed
nΔh	nth element of Δh
i	Index for the item, such as the ith leg, or the (i, j) pixel
BI	Inertia matrix of the body in the BCS
j	Index for the item, such as the (i, j) pixel
Ji	Jacobian matrix of the ith leg
J (hi)	Terrain cost at the position of hi
J (ph)	Cost function for NDCI
\mathrm{\nabla }J\left({\mathit{p}}_{\mathrm{h}}\right)	Partial derivative of the cost function for NDCI
{\mathit{K}}_{\mathrm{D},\mathrm{j}}, {\mathit{K}}_{\mathrm{P},\mathrm{j}}	Joint damping and stiffness matrices, respectively
{\mathit{K}}_{\mathrm{D},\mathrm{p}}, {\mathit{K}}_{\mathrm{P},\mathrm{p}}	Damping and stiffness of body position, respectively
{\mathit{K}}_{\mathrm{D},\mathrm{a}}, {\mathit{K}}_{\mathrm{P},\mathrm{a}}	Damping and stiffness of body attitude, respectively
{}_{}^{\mathrm{L}}{\mathit{K}}_{\mathrm{D}}, {}_{}^{\mathrm{L}}{\mathit{K}}_{\mathrm{P}}	Damping and stiffness of the leg, respectively
Bli	Tip position in the BCS
{}_{}^{\mathrm{L}}{\mathit{l}}_i, {}_{}^{\mathrm{L}}{\dot{\mathit{l}}}_i	Position and velocity of the ith leg in the LCS, respectively
{}_{}^{\mathrm{L}}{\mathit{l}}_i^{\ast }, {}_{}^{\mathrm{L}}{\dot{\mathit{l}}}_i^{\ast }	Target position and velocity of the ith leg in the LCS, respectively
m, n	Set numbers of pixels that need to be involved
{\mathit{p}}_{\mathrm{b}}	Position of the robot body
{\mathit{p}}_{\mathrm{b}}^{\ast }, {\dot{\mathit{p}}}_{\mathrm{b}}^{\ast }, {\ddot{\mathit{p}}}_{\mathrm{b}}^{\ast }	Target position, velocity, and acceleration of the robot’s CoM, respectively
ph	Horizontal coordinate of the position of the selected point on the whole map
{\mathit{p}}_i	Foothold point of the ith leg
p_{i,x}, p_{i,y}, p_{i,\mathit{\text{z}}}	x, y, and z components of the foothold {\mathit{p}}_i, respectively
{\mathit{p}}_i^{\ast }	Optimized footholds
{\mathit{p}}_{i,\mathit{\text{z}}}^{\ast }	Height components at the position {\mathit{p}}_i^{\ast } on the heightmap
{\mathit{p}}_{\mathrm{p}\mathrm{r}\mathrm{e},i}	Preplanning of the target foothold
{\mathit{p}}_{{\xi }_i}	Swing trajectory of the ith leg
{\hat{\mathit{p}}}_{\mathrm{b}}, {\widehat{\dot{\mathit{p}}}}_{\mathrm{b}}	Estimated position and velocity of the robot, respectively
\mathrm{\Delta }{\mathit{p}}_{\mathrm{b}}^{\ast }	Correction amount on the original body trajectory
Δpi	Foothold adjustment for the ith leg
Bphip,i	Position of the ith leg’s hip joint in the BCS
{\mathit{q}}_i, {\dot{\mathit{q}}}_i	Current angle and angular velocity read by the ith encoder, respectively
{\mathit{q}}_i^{\ast }, {\dot{\mathit{q}}}_i^{\ast }	Target angle and angular velocity of the ith joint, respectively
R	Current attitude matrix
{\mathit{R}}^{\ast }	Desired attitude of the body
\hat{\mathit{R}}	Estimated current attitude of the robot
\mathbb{R}	Set of real number
S	Selection matrix for CBC
tsw,i	Time when the ith leg enters the swing phase
Tst, Tsw	Duration of the standing and swing phase, respectively
v	Velocity of the body
vd	Desired velocity of the body
\mathrm{\Delta }{\mathit{v}}_{\mathrm{b}}^{\ast }	Speed correction
xh	x component of the horizontal coordinate of the position of the selected point on the heightmap
xi,j	x component of the center of the pixel (i, j) on the map
yh	y component of the horizontal coordinate of the position of the selected point on the heightmap
yi,j	y component of the center of the pixel (i, j) on the map
W, D, Q	Weighting matrices for footholds optimization
α, β, γ,	Weighting factors for footholds optimization
δ	Weighting factor for CBC
σx, σy	Smoothing factor in x and y directions, respectively
ϕ, θ, ψ	Euler angle roll, pitch, and yaw measured by the IMU
μ	Number of legs in contact
{\mathit{\tau }}_i^{\ast }	Control torque of the ith joint
ξi	Swing phase
{\widehat{\mathbf{\text{ω}}}}_{\mathrm{b}}	Angular velocity of the body
{\text{ω}}_{\mathrm{b}}^{\ast }, {\dot{\text{ω}}}_{\mathrm{b}}^{\ast }	Target angular velocity and acceleration of the robot, respectively
*	Symbol maps from a vector ({\mathbb{R}}^3) to an antisymmetric matrix ({\mathbb{R}}^{3\times 3})

Acknowledgements

This work was supported by the National Key R&D Program of China (Grant No. 2021YFF0306202).

References

1 Raibert M H, Tello E R. Legged robots that balance. IEEE Expert, 1986, 1(4): 89

2 BledtG, Powell M J, KatzB, Di CarloJ, Wensing P M, KimS. MIT Cheetah 3: design and control of a robust, dynamic quadruped robot. In: Proceedings of 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems. Madrid: IEEE, 2018

3 VernazaP, Likhachev M, BhattacharyaS, ChittaS, Kushleyev A, Lee D D. Search-based planning for a legged robot over rough terrain. In: Proceedings of 2009 IEEE International Conference on Robotics & Automation. Kobe: IEEE, 2009, 2380–2387

4 KalakrishnanM, BuchliJ, PastorP, MistryM, Schaal S. Fast, robust quadruped locomotion over challenging terrain. In: Proceedings of 2010 IEEE International Conference on Robotics & Automation. Anchorage: IEEE, 2010, 2665–2670

5 Semini C, Tsagarakis N G, Guglielmino E, Focchi M, Cannella F, Caldwell D G. Design of HyQ—a hydraulically and electrically actuated quadruped robot. Proceedings of the Institution of Mechanical Engineers, Part I: Journal of Systems and Control Engineering, 2011, 225(6): 831–849

6 WinklerA, Havoutis I, BazeilleS, OrtizJ, Focchi M, Dillmann R, Caldwell D, SeminiC. Path planning with force-based foothold adaptation and virtual model control for torque controlled quadruped robots. In: Proceedings of 2014 IEEE International Conference on Robotics & Automation (ICRA). Hong Kong: IEEE, 2014, 6476–6482

7 HutterM, Gehring C, JudD, LauberA, Bellicoso C D, TsounisV, HwangboJ, BodieK, FankhauserP, Bloesch M, DiethelmR, BachmannS, MelzerA, HoepflingerM. ANYmal—a highly mobile and dynamic quadrupedal robot. In: Proceedings of 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Daejeon: IEEE, 2016, 38–44

8 Winkler A W, Bellicoso C D, Hutter M, Buchli J. Gait and trajectory optimization for legged systems through phase-based end-effector parameterization. IEEE Robotics and Automation Letters, 2018, 3(3): 1560–1567

9 MastalliC, Focchi M, HavoutisI, RadulescuA, Calinon S, Buchli J, Caldwell D G, SeminiC. Trajectory and foothold optimization using low-dimensional models for rough terrain locomotion. In: Proceedings of 2017 IEEE International Conference on Robotics & Automation (ICRA). Singapore: IEEE, 2017, 1096–1103

10 Mastalli C, Havoutis I, Focchi M, Caldwell D G, Semini C. Motion planning for quadrupedal locomotion: coupled planning, terrain mapping and whole-body control. IEEE Transactions on Robotics, 2020, 36(6): 1635–1648

11 MastalliC, Havoutis I, WinklerA W, CaldwellD G, SeminiC. On-line and on-board planning and perception for quadrupedal locomotion. In: Proceedings of 2015 IEEE International Conference on Technologies for Practical Robot Applications (TePRA). Woburn: IEEE, 2019, 1–7

12 FankhauserP, Bjelonic M, BellicosoC D, MikiT, HutterM. Robust rough-terrain locomotion with a quadrupedal robot. In: Proceedings of 2018 IEEE International Conference on Robotics and Automation (ICRA). Brisbane: IEEE, 2018, 5761–5768

13 Jenelten F, Miki T, Vijayan A E, Bjelonic M, Hutter M. Perceptive locomotion in rough terrain—online foothold optimization. IEEE Robotics and Automation Letters, 2020, 5(4): 5370–5376

14 ChaiX, Gao F, XuY L. Perception-based gait planning for a hexapod robot walking on typical structured terrain. In: Zhang X M, Wang N F, Huang Y J, eds. Mechanism and Machine Science. Singapore: Springer, 2017, 169–181

15 KimD, Carballo D, Di CarloJ, KatzB, Bledt G, Lim B, Kim S. Vision aided dynamic exploration of unstructured terrain with a small-scale quadruped robot. In: Proceedings of 2020 IEEE International Conference on Robotics and Automation (ICRA). Paris: IEEE, 2020, 2464–2470

16 Mao L H, Tian Y, Gao F, Zhao Y. Novel method of gait switching in six-legged robot walking on continuous-nondifferentiable terrain by utilizing stability and interference criteria. Science China Technological Sciences, 2020, 63(12): 2527–2540

17 Mao L H, Gao F, Tian Y, Zhao Y. Novel method for preventing shin-collisions in six-legged robots by utilising a robot–terrain interference model. Mechanism and Machine Theory, 2020, 151: 103897

18 ZhaoY, Gao F, YinY P. Obstacle avoidance and terrain identification for a hexapod robot. Research Square, 2020, preprint

19 Magaña O A V, Barasuol V, Camurri M, Franceschi L, Focchi M, Pontil M, Caldwell D G, Semini C. Fast and continuous foothold adaptation for dynamic locomotion through CNNs. IEEE Robotics and Automation Letters, 2019, 4(2): 2140–2147

20 Miki T, Lee J, Hwangbo J, Wellhausen L, Koltun V, Hutter M. Learning robust perceptive locomotion for quadrupedal robots in the wild. Science Robotics, 2022, 7(62): eabk2822

21 Tsounis V, Alge M, Lee J, Farshidian F, Hutter M. DeepGait: planning and control of quadrupedal gaits using deep reinforcement learning. IEEE Robotics and Automation Letters, 2020, 5(2): 3699–3706

22 GehringC, Fankhauser P, IslerL, DiethelmR, Bachmann S, PotzM, GerstenbergL, HutterM. ANYmal in the field: solving industrial inspection of an offshore HVDC platform with a quadrupedal robot. In: Ishigami G, Yoshida K, eds. Field and Service Robotics. Singapore: Springer, 2021, 247–260

23 LiuW, GaoY, GaoF, LiS Y. Trajectory adaptation and safety control via control barrier functions for legged robots. In: Proceedings of 2021 China Automation Congress (CAC). Beijing: IEEE, 2021, 5571–5576

24 Park H W, Wensing P M, Kim S. Jumping over obstacles with MIT Cheetah 2. Robotics and Autonomous Systems, 2021, 136: 103703

25 QiS H, Lin W C, HongZ J, ChenH, ZhangW. Perceptive autonomous stair climbing for quadrupedal robots. In: Proceedings of 2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Prague: IEEE, 2021, 2313–2320

26 GerumP. Xenomai—Implementing a RTOS emulation framework on GNU/Linux. White Paper, 2004, 81

27 HintjensP. ZeroMQ: Messaging for Many Applications. O’Reilly Media, 2013

28 Sun Q, Gao F, Chen X B. Towards dynamic alternating tripod trotting of a pony-sized hexapod robot for disaster rescuing based on multi-modal impedance control. Robotica, 2018, 36(7): 1048–1076

29 EkstromM P. Digital Image Processing Techniques. 2nd ed. Academic Press, 2012

30 Ranganathan A.. The Levenberg‒Marquardt algorithm. Tutoral on LM Algorithm, 2004, 11(1): 101–110

31 BlöschMHutterMHöpflinger M ALeuteneggerSGehringCRemyC D SiegwartR Y. State estimation for legged robots—consistent fusion of leg kinematics and IMU. In: Proceedings of Robotics: Science and Systems Conference (RSS 2012). Sidney: Robotics: Science and Systems Conference, 2012

32 Focchi M, Del Prete A, Havoutis I, Featherstone R, Caldwell D G, Semini C. High-slope terrain locomotion for torque-controlled quadruped robots. Autonomous Robots, 2017, 41(1): 259–272