Keywords

1 Introduction

Legged robots are advanced mobile robots that can traverse over a large range of tough terrains. Autonomous legged robot should be able to adjust their motions adaptively to overcome terrain variations. Such planning strategy should be good at building models of terrains and effectively propose respective motions to realize feasible and stable locomotion without much computational overhead. In this paper, the motion planning on a special type of tough terrain, the inclined plane, is investigated on our six-legged robot.

1.1 Problem Statement

For a legged robot that walks on an inclined plane or a slope, the key planning purpose is to let the robot stably move on the plane with feasible motions of mechanical joints. However, according to our experience, deadlock frequently occurs due to kinematic workspace limitations of the robot when the robot steps are as aggressive as those on flat plane. Other cases include falling over of the robot due to careless regulation of the robot COG (center of gravity). These are two main aspects when a legged robot fails to walk on an inclined plane. From the opposite perspective, some terrains are indeed too inclined that the robot could hardly step on. In this regard, to explore the maximum mobility of the robot on a given inclined plane is helpful and instructive.

Some insights could be gained after the above discussion of the problem. Yet, to solve the motion planning problem of a robot walking on a 3D spatial inclined terrain, efforts should be made in terms of robot mechanism design, gait planning, body pose regulation, etc. In this paper, we focus our study on the body pose regulation using a six-legged robot, taking both robot kinematics and stability limitations into account. We formulate the problem into a universal optimization problem and eventually obtain the optimal robot stance configuration on any given plane.

1.2 State of the Art

The problem of locomotion optimization for legged robot walking on inclined terrains has been addressed by Hong et al. [1] on humanoid robots applying a modifiable walking pattern generation method. Also, for humanoid robots, Kim et al. [2] presented an online control algorithm through which the robot could adapt to the floor conditions. Maneuvering on slopes of multi-legged robots are also discussed in [3] and [4] for hexapod and quadruped robot. More detailed stability evaluation was given in [5] and [6], in which different criteria of stability were compared and also the effect of the robot dynamics was proposed. Most of the above researches focused on the gait generation and the stability and of the legged robot. On the other hand, for more generous locomotion on rough terrain, leg kinematics played an important role and was investigated. Belter and Skrzypczyński [7] introduced an effective online posture optimization of a hexapod which could avoid deadlocks in most cases and enhance the robot mobility. In our study, we would like to consider the robot stability as well as the leg kinematics in one optimization scheme. Before that, we will firstly introduce our six-legged robot.

1.3 Robot Overview

The six-legged robot we employ has six legs symmetrically distributed (Fig. 1). All legs are in their standard stance positions. The ground reference coordinate system, body coordinate system and leg coordinate system are illustrated respectively. The default advancing direction of the robot is along its z axis. For the robot gait, we employed the most frequently used gait of the six-legged robot, the tripod gait. Two pairs of legs are numbered as (1, 3, 5) and (2, 6, 4), defined in an anti-clockwise order.

Fig. 1
figure 1

Different (ground, body, leg) coordinate systems and sequence of legs

Full size image

In terms of the control system, we utilize a Linux OS with a cross-compiled Xenomai real-time core. Onboard sensory system includes a Kinect 3D stereo vision system, six degree-of-freedom force/torque sensors mounted on each of the foot tips and an inertial measurement unit. Legs are actuated by eighteen 400 W motors. The command exchange in the actuation layer is through ethercat communication, with a frequency of 1 kHz.

2 Planning Strategy

In this paper, we propose to establish the explicit connection of the robot optimized pose and features of the inclined terrain. In this way, when the robot encounters any given terrains, the robot would be able to make fast response with a precomputed set of optimized control inputs as body translational offsets.

Unlike on other kinds of terrains with non-negligible obstacles, the difficulty of walking on inclined terrains mainly lies in the regulation of body pose, not the leg trajectories. Leg continuous motion could be generated applying simple trajectories such as the ellipsoid trajectory, rectangular trajectory, or their variations, etc.

What we would like to do is to generate a series of standard robot configuration primitives. More precisely, these are stance configuration primitives, with default position distributions of legs. Body poses of these configurations can cover a good range of robot motion on the given inclined terrain. In real world locomotion, we would then refer to these stored primitives and instruct for planning purpose. At the same time, feasible motions are restored into the table of primitives as a memorizing process to enrich the reference functionality.

In this paper, the primary mission is to generate the most general set of robot poses, depending on the different terrain parameters. The first term to accomplish is to model the inclined terrain feature.

2.1 Inclined Plane and Body Orientation

An ideal inclined plane in the 3D space could be mathematically expressed as a plane equation with respect to the given coordinate system.

$$ {\text{Ax}} + {\text{By}} + {\text{Cz}} + D = 0 $$
(1)

in which the normal vector of the plane is given as (A, B, C) terr .

Theoretically, when the robot is in its stance phase, the six foot tips locate on the same inclined plane. However, this is the case when the plane is a perfect surface. In real world applications, terrain feature always involves irregularities such as small convexity or concavity on the ground, tiny variations of elevation, etc. Therefore, we compute the plane with the actual stance foot tip positions. The obtained plane is computed by minimizing the least squares of the distance between stance foot tips and the plane.

$$ {\text{J}} = \hbox{min} \sum\limits_{{{\text{i}} = 1}}^{6} {\left\| {{\text{Ax}}_{\text{i}} + {\text{By}}_{\text{i}} + {\text{Cz}}_{\text{i}} + {\text{D}}} \right\|^{2} } $$
(2)

A ground coordinate system is set on the absolute flat terrain as the reference coordinate system. To express the inclined plane, we set another coordinate system attached to itself, in which the y axis coincides with the normal vector of the plane, with its vertical component pointing upward (Fig. 2). Therefore,

$$ {}_{\text{terr}}^{\text{G}} {\mathbf{y}} = \text{sign}({\text{B}})\frac{{\left( {\begin{array}{*{20}c} {\text{A}} & {\text{B}} & {\text{C}} \\ \end{array} } \right)^{T} }}{{\sqrt {{\text{A}}^{2} + {\text{B}}^{2} + {\text{C}}^{2} } }} $$
(3)

After that, we would like to set the z axis of the plane as the advancing direction of the robot along the plane. That is to say, if we express a transformation between the plane and the reference frame with the yaw-pitch-roll Euler angles, the plane would have the same yaw angle α as the robot w.r.t. the reference coordinate system. It is the navigation angle of the robot, which is obtained from the navigation system or human instructions as the known variable. We can compute this coordinate system by the following steps.

Firstly, an auxiliary x axis is given, which is the pitch rotation axis during the Euler angle transformation after the yaw rotation,

$$ {}_{\text{terr}}^{\text{G}} {\mathbf{x}}_{\text{aux}} = {\text{roty}}({}^{\text{G}}\alpha_{\text{b}} ) \cdot \left( {\begin{array}{*{20}c} 1 \\ 0 \\ 0 \\ \end{array} } \right) = \left( {\begin{array}{*{20}c} {\cos {}^{\text{G}}\alpha_{\text{b}} } \\ 0 \\ { - \sin {}^{\text{G}}\alpha_{\text{b}} } \\ \end{array} } \right) $$
(4)

Then, the advancing direction, namely z axis,

$$ {}_{{\text{terr}}}^{\text{G}} {\mathbf{z}} = \frac{{{}_{{\text{terr}}}^{\text{G}} {\mathbf{x}}_{\text{aux}} \times {}_{{\text{terr}}}^{\text{G}} {\mathbf{y}}}}{{\left\| {{}_{\text{terr}}^{\text{G}} {\mathbf{x}}_{\text{aux}} \times {}_{\text{terr}}^{\text{G}} {\mathbf{y}}} \right\|}} $$
(5)

Lastly, x axis and the overall rotation matrix.

$$ {}_{{\text{terr}}}^{\text{G}} {\mathbf{x}} = {}_{{\text{terr}}}^{\text{G}} {\mathbf{y}} \times {}_{{\text{terr}}}^{\text{G}} {\mathbf{z}} $$
(6)
$$ {}_{{\text{terr}}}^{\text{G}} {\mathbf{R}} = \left( {{}_{{\text{terr}}}^{\text{G}} {\mathbf{x}},\,{}_{{\text{terr}}}^{\text{G}} {\mathbf{y}},\,{}_{{\text{terr}}}^{\text{G}} {\mathbf{z}}} \right) $$
(7)

The rotation matrix could be also expressed by yaw-pitch-roll (213) Euler angles,

$$ {}_{{\text{terr}}}^{\text{G}} {\mathbf{R}} = {\text{roty}}\left( {{}^{\text{G}}\alpha_{\text{terr}} } \right) \cdot \text{rotx}\left( {{}^{\text{G}}\beta_{\text{terr}} } \right) \cdot rotz\left( {{}^{\text{G}}\gamma_{{\text{terr}}} } \right) $$
(8)

where G α terr is the same as the robot yaw angle G α b and that leads to two determinant variables, the pitch G β terr and roll G γ terr angle of the plane. These two variables describe the fore-and-aft inclination and the lateral inclination when the robot is moving along a certain given direction.

Fig. 2
figure 2

The six-legged robot on inclined plane. Yaw angle of the robot is given by G α terr. The plane normal vector is marked as (A, B, C) terr

Full size image

As for the body orientation regulation, we choose to fix the body orientation with that of the inclined plane as a coarse optimization measure. Such handling of the body orientation is reasonable because first of all, the robot body is redundant with regard to the six stance legs and we prefer to coarsely adjust the body orientation and focus the precise optimization on body displacement. In this way, the problem variable space would have a lower dimension, which is efficient to solve. In addition, body orientation in parallel with the plane leaves each leg a considerably good kinematic margin. More importantly, as will be explained in the following chapters, the involvement of body orientation would violate the convexity of the overall optimization problem. We therefore fix it to strictly guarantee a feasible solution of the body displacement. The final body transformation matrix w.r.t. the reference frame is,

$$ {}_{\text{B}}^{\text{G}} {\mathbf{T}} = \left( {\begin{array}{*{20}c} {{}_{{\text{terr}}}^{\text{G}} {\mathbf{R}}} & {{}^{\text{G}}{\mathbf{p}}_{\text{b}} } \\ {0_{{\left( {1 \times 3} \right)}} } & 1 \\ \end{array} } \right) $$
(9)

where the rotation matrix is obtained from the stance leg positions and only the robot position G p b serves as the undetermined variable, T being a linear transformation. The linearity of T is of great importance to relate variables in the body and the ground reference frames through an affine transformation, which guards the convexity of the optimization overall problem.

2.2 Leg Workspace Approximation

For each leg, there are three sub-chains of prismatic actuation to achieve three degrees of freedom for leg translation in space. Each prismatic actuation has upper and lower bounds of travel limits due to the mechanical constraint of the leading screw. Because of these bounds, the workspace of each leg is a non-convex 3D space (Fig. 3). As a main aspect of the planning strategy, the kinematic margin of the leg is hard to evaluate in such irregular workspace. Out of this consideration, we propose an approximation of the workspace applying a convex subset of the workspace, formed by the intersection of a convex cone and the half space split by a spatial plane (Fig. 4). The convex subset only sacrifices some narrow workspace which is hardly reached in walking tasks. Moreover, the convex subset facilitates the formulation of the overall optimization problem.

Fig. 3
figure 3

Leg workspace boundary point cloud with respect to the leg coordinate system

Full size image
Fig. 4
figure 4

Leg workspace and convex subspace viewed in the x-z plane

Full size image

2.3 Convex Optimization Problem Formulation

The convex optimization formulation was adapted by various researches [8, 9] in robot trajectory and path planning. Generally, when any problem is formulated into a strict convex optimization problem, it could be resolved with efficient off-the-shelf algorithms. Algorithm implementation with software such as Mosek [10] often facilitates the computation process while only requires for the proper definition of problem variables, constraints and objective.

In our planning problem, with the plane parameters pitch and roll given, legs constraint in respective convex workspaces and body orientation fixed in parallel with the plane, the computation of optimized body position could be handled through the formulation of a general convex optimization problem. All leg and body variables are linearly related because transformations between different coordinate systems are already guaranteed as linear.

Apart from the body position, we assign another set of variables to express the possible reachable areas of the swing legs (Table 1).

Table 1 Parameters of the reachable areas of swing legs with respect to the body frame
Full size table

The above three variables defines a box space w.r.t. the standard swing leg position. The vertices of the box are:

$$ \begin{aligned} \begin{array}{l} ({\text{X}}_{\text{step}} / 2,{\text{H}}_{\text{step}} ,{\text{Z}}_{\text{step}} / 2),({\text{X}}_{\text{step}} / 2,0,{\text{Z}}_{\text{step}} / 2),\\( - {\text{X}}_{\text{step}} / 2,{\text{H}}_{\text{step}} ,{\text{Z}}_{\text{step}} /2),( - {\text{X}}_{\text{step}} /2, \, 0,{\text{ Z}}_{\text{step}} / 2), \\ ({\text{X}}_{\text{step}} / 2,{\text{H}}_{\text{step}} , - {\text{Z}}_{\text{step}} / 2),({\text{X}}_{\text{step}} / 2,0, - {\text{Z}}_{\text{step}} / 2),\\( - {\text{X}}_{\text{step}} / 2,{\text{H}}_{\text{step}} , - {\text{Z}}_{\text{step}} / 2), ( - {\text{X}}_{\text{step}} / 2,0, - {\text{Z}}_{\text{step}} / 2). \\ \end{array}\end{aligned} $$

These coordinates indicate synchronous motions corresponding to the swing leg w.r.t. the body coordinate system. That is to say, they are possible step motions of swing legs from the standard stance configuration along the inclined plane. In the formulation of the problem, we seek for the optimized body position satisfying that the box space of the swing leg is reachable with the least dimension of 0.2 m*0.2 m*0.08 m. The convexity of the box guarantees that all positions in the box are feasible as long as all eight motions of the vertices are feasible.

  • Constants

    1. (a)

      Robot mechanical dimensions.

    2. (b)

      Inclined terrain parameters (pitch and roll).

    3. (c)

      Lower bounds of dimensional parameters of the swing leg box workspace.

  • Variables

    1. (a)

      Body position (Xb, Yb, Zb).

    2. (b)

      Dimensional parameters of the swing leg box workspace (Xstep, Hstep, Zstep).

  • Convex/Affine Constraints

    1. (a)

      All legs in the standard stance configuration are bounded in the respective convex workspace.

    2. (b)

      All legs in the eight extreme configurations are bounded in the respective convex workspace.

    3. (c)

      The robot center of gravity lies inside the support polygon (triangle) formed by the support legs with a good stability margin, for all aforementioned configurations.

  • Objective

    $$ \hbox{max} \frac{{Z_{step} - Z_{\hbox{min} } }}{{Z_{\hbox{max} } - Z_{\hbox{min} } }} $$
    (10)

There could be various objective definitions of a convex optimization problem. However, in our study, we would like to reflect the relationship between the maximum mobility of the robot and the inclined terrain feature. Therefore we describe the mobility of the robot by the value of the motion Zstep which is along the advancement direction z axis and try to maximize the mobility. Zmax is the maximum step with zero inclination of the plane.

So far, the formulation of the general convex optimization problem is finished. A simple index is proposed to express robot mobility. In the next section, some computation results will be given.

3 Results and Discussion

It is illustrated in Fig. 5 that the robot mobility indeed varies according to the variation of inclination of the plane. The stability margin is given as 0.15 m. For our robot, because of the symmetric structure, the symmetric inclination results in identical body mobility. Also, it could be observed that, when the plane pitch and roll angles are small, the mobility would not alter violently. This indicates that small inclination does not influence much on the robot stability and motion performance.

Fig. 5
figure 5

Mobility index variation in terms of the terrain pitch and roll angles. Red spots represents infeasible inclinations of the terrain for the robot to walk

Full size image

In addition, body position adjustment with respect to the standard origin is given corresponding to the different terrain features (Table 2). Symmetric features are also observed.

Table 2 Body position adjustment with respect to the standard original coordinate system
Full size table

In fact, the reachable workspace box could be modified according to the different motion preferences. In that way, body position adjustment could be more task dependent.

To be conclusive, the proposed optimization based method did work effectively. The planner compromised both the robot stability and leg kinematics to gain a series of feasible configuration primitives on inclined planes. The planning method as well as the obtained results will offer more instructive help to the planning strategy in real world test. Future study includes real world terrain mapping and body pose estimation with the help of sensory systems. More dynamic motion planning and trajectory smoothing will also play an important role in future study.