Hierarchical control architecture operates based on the sense-plan-act robot control methodology [17]. Initially, information about the surrounding environment of the robot is gathered through the sensors, then a motion plan of the next gait cycle is calculated in a goal oriented manner. Finally, the planned action is undertaken through actuators. Then the same procedure is repeated until the goal of the given task is achieved [17]. Figure 4.1 shows the block diagram of the hierarchical control architecture that is implemented in the hexapod robot. It is assumed that the terrain features are provided before the execution of the simulation.

Figure 4.1 Hierarchical Control Architecture

4.1.High Level Planner

4.1.1.Body Path Planner

For a given start point, A and a goal point, B, the shortest distance may be calculated using a map-based planning method. Note that, the shortest distance may have a higher movement cost, i.e. there might be a longer path with fewer obstacles and easier to navigate. In order to designate whether or not this is the case, the cost map of the terrain needs to be taken into account which indicates how easy the terrain is to navigate over [6]. In this report, the shortest path is assumed to be the one with the lowest movement cost.

Figure 4.2-a Uneven terrain and the 2D representation of terrain, adapted from [15]

Figure 4.2-a shows an uneven terrain where the regions that are higher than the reachable workspace of the robot leg, are represented as obstacles in the 2D grid that is shown on Figure 4.2-b. For example, Figure 4.3 shows a terrain height where the robot is capable of stepping over, hence in the 2D grid representation, also referred as occupancy grid, of this region is not an obstacle.

Figure 4.3 Collision map of a foot trajectory over an obstacle, adapted from [28]

The occupancy grid framework provides information about terrain regarding to the free spaces and obstacles where each grid is roughly equal to the size of the robot. [29] For the computation, the occupancy grid is represented as a multidimensional matrix where 0 denotes free space, Qfree, and 1 denotes obstacle, Qobs [14].

The path planning algorithm, A* (star), that was implemented to compute the body path, runs in discrete space, however the real‐world is continuous hence the map needs to be discretised which is done by using an approximate cell decomposition method, trapezoidal decomposition [30]. Figure 4.4 shows the occupancy grid that was used in the MapleSim simulations. Using the trapezoidal decomposition approach, it can be decomposed into 7 cells. The idea is to extend a vertical line in Qfree until it touches either a Qobs or reaches the borders of occupancy grid [31] [32]. There is a harder-to-compute decomposition method, named as exact cell decomposition, which covers the exact shape of Qobs using convex polygons, i.e. this method produces cells with irregular boundaries. However, since all the obstacles are chosen to be rectangular, trapezoidal decomposition is sufficient enough in this particular example.

Figure 4.4 Occupancy Grid Representation

Figure 4.5 Adjacency Map of the Occupancy Grid.

Then, an adjacency map is generated where each cell is represented as a node and the adjacent cells are connected with a line, shown in Figure 4.5. The adjacency map can be expressed in matrix form, where the adjacent cells are represented as 1 and the rest as 0, given by Equation 28.

A* (star) path planning algorithm determines the cost of the 8 surrounding nodes by using Equation 28,

where G is the distance from the start node and H is the estimated distance from the end node using heuristics [30]. G and H are calculated ignoring the obstacles, where the vertical and horizontal movements are counted as increments of 1 whereas the diagonal motions are counted in increments of √2. Table 3-1 shows an example of how G and H were calculated for Nodes A, B, and C that are shown on Figure 4.6. The nodes with the lowest cost, F, are given priority in the exploration process.

Table 3‑1 Example G, H, and F values of Nodes A, B, and C.

Figure 4.6 The body path found by the A* algorithm MATLAB Script 8-3 (in Appendix) was used to generate the path in Figure 4.6 which shows the generated body path between Start node and Goal node in grey and the obstacles are shown in black. It can be seen that, the path given in Figure 4.6 is not smooth. In order to obtain a smoother body path, a 2^nd order low pass Butterworth filter is applied and hence, the body path given in Figure 4.7 is generated. This path has been used in the MapleSim simulations as the test body path.Figure 4.7 The body path that is used in the MapleSim simulations

4.1.2.Footstep Planner

The footstep planner, provides the initial configuration, , and final configuration, that are required by the foot trajectory planner to generate the trajectory that traverses from to where is the current foot position and is the position of the next footstep [16]. The footstep planner makes a projection of where the leg end effector needs to be positioned for the next step in order to follow the path that was given by the body path planner [18]. Figure 4.8 X-Y View of the possible Final Foothold Configurations.

Figure 4.8 illustrates the reachable area of the leg end effector in the increments of 10 degrees where the Z axis was kept constant at -1 in order to represent a level terrain. Since it is assumed that the robot only moves forward and sideways but not backwards, the possible final foothold configurations that can be chosen by the footstep planner are limited by the area that is given by Figure 4.9.

Figure 4.9 X-Y View of the possible Final Foothold Configurations in the 1^st Quadrant.

The procedure to compute the final configuration, qf is as follows. The derivative of the path signal that is given by the body path planner is computed with respect to time to obtain the slope, α(t) as a function of time. Then, the signal is amplified by a gain, G which was selected such that the maximum value of α(t)corresponds to π/2 where the robot moves vertically. Equations 29 and 30 are used to calculate the Cartesian coordinates, and of the next final foothold configuration such that the robot follows the given body path.

where R is a constant that specifies how much further the end effector will be positioned from the fixed frame of the individual leg. In the MapleSim simulations, R is chosen to be 1.2 due to the torque constraints on the joint actuators, hence the actual operational area of the leg end effector is the area that was enclosed by the black dashed line shown in Figure 4.9. Figure 4.10 shows 4 different leg configurations and the corresponding horizontal distances to the geometric centre of the body. Figure 4.10 Different leg configurations and corresponding R values of the MapleSim Model.

The Z coordinate of the qf is a matter of altitude which is obtained by finding the corresponding height of the point from the height map of the terrain for any given X and Y coordinates. Figure 4.12 shows the possible positions on an arbitrarily generated uneven terrain such that the chosen random variables lie within ±0.25m of the exponential function given by Figure 4.11 which represents a hill.

Figure 4.11 Curve that was used to generate the uneven terrain

Figure 4.12 3D View of the possible final foothold configurations on an uneven terrain

Figure 4.12 3D View of the possible final foothold configurations on an uneven terrain

Figure 3.13 shows the X-Y view of the positions of qf on an arbitrarily generated height map, it can be seen that the X axis was incremented by 0.1m in order to reduce the computation time. MATLAB Script 8-4 (in Appendix) was used to create the uneven terrain that is shown in Figure 4.12.

Figure 4.13 X-Y view of the final foothold configurations on an uneven terrain

4.2.Low Level Planner

4.2.1.Foot Trajectory Planner

The kinematic analysis of a robot manipulator is concerned with the geometry of the robot and the constraints due to the existence of an obstacle within the workspace of the robot leg are completely neglected. The possibility of collisions is taken into account by the foot trajectory planner and a collision free path is generated [16].

The foot trajectory planner computes the dynamics of the leg end effector motion during the swing phase as it traverses from an initial configuration, qo, to a given final configuration, qf [16]. Table 3-2 gives the notations that are used in this section of the report.

Figure 4.14 shows a foot trajectory which is decomposed into 3 sections labelled as 1, 2, and 3, respectively. During section 2, the leg moves faster whereas in sections 1 and 3, the leg moves slower to avoid an impact with the ground. Therefore, a minimum number of 4 constraints; initial configuration, qo, final configuration, qf, initial velocity, vo, and final velocity vf needs be taken into account.

Figure 4.14 Foot Trajectory between initial and final configurations.

MATLAB Script 8-5 (in Appendix), was used to calculate the position, velocity and acceleration of each joint of Leg 1. Figure 4.15 demonstrates the variation in position, velocity and acceleration of Joints 1, 2, and 3 during a complete leg cycle of 4 seconds.

The velocity plots, in Figure 4.15, show that velocity of joints peaks at the centre of each one-second segment. In other words, for the majority of the time, the velocity is below its maximum value which is inefficient and hence undesirable. In order to solve this problem, linear segment with parabolic blend (LSPB) method was implemented which has increased the duration for which the joint operates at its maximum velocity [14]. Figure 4.16 shows the position, velocity and acceleration profiles calculated by LSPB method and the code is given in MATLAB Script 8-6 (in Appendix).

Figure 4.15 Position, Velocity, and Acceleration profiles for Quintic Polynomial

Figure 4.16 Position, Velocity, and Acceleration calculated by the LSPB Planner

It can be seen in Figure 4.17 that the trajectory of the complete leg cycle is linear since all the intermediate velocities between consecutive segments were set to 0ms-1. Using heuristics, the optimum intermediate velocities may be calculated given that only initial and final velocities are 0ms-1. Figure 4.18 shows the position, velocity, and acceleration calculated using heuristics, notice that the intermediate velocities are no longer equal to 0ms-1. Consequently, the generated foot trajectory, shown by Figure 4.19, is an arc instead of a straight line as it was shown in Figure 4.17. MATLAB Script 8-7 (in Appendix) was used to generate Figures 4.17 and 4.19.

Figure 4.17 The Foot Trajectory generated using the LSPB Planner

Figure 4.18 Position, velocity, and acceleration calculated using heuristics

Figure 4.19 The Foot Trajectory generated using heuristics

Figure 4.20 MapleSim implementation of the generated trajectory

Figure 4.20 shows the simulation of the foot trajectory which was obtained using quantic polynomial that is given by Equation 31. Notice that, during the swing phase, the leg end effector approaches the body in the direction of axis, shifting the mass towards the COG, reduces the mass distribution and hence minimises the angular momentum [33]. Due to the law of conservation of total momentum, the reduced angular momentum must be compensated by the speed of the motion i.e. the end effector pursues on the given trajectory at a higher velocity [33].

When the robot is navigating on an uneven terrain, a path planner algorithm needs to be implemented in order to compute the Cartesian coordinates of the end effector for a given number of intermediate segments within the trajectory. These Cartesian coordinates are then connected and a collision-free trajectory is obtained.

As it was mentioned in the literature review, artificial potential fields, probabilistic roadmap method (PRM) and rapidly exploring random trees (RRT) algorithms are widely used to generate a point-to-point trajectory [12] [16] [7]. Using artificial potential fields may be quite problematic since the existence of a local minima in the field causes the algorithm to get trapped [16]. As a result, random motion algorithms appear to be better alternatives since these algorithms are often quicker in terms of computation [16]. The PRM algorithm is more advantageous over RRT which spreads omni-directionally, mainly because relatively fewer points need to be tested in order to generate the trajectory between initial configuration, qo, and final configuration, qf, which reduces the computation time. Computation time needs to be taken into account since this process is repeated in every footstep and hence an algorithm that creates less computational burden was chosen [14].

Figure 4.21 Arbitrarily generated obstacle to test the PRM algorithm

Figure 4.21 shows an obstacle that is located within the configuration space of the robot,. An occupancy grid matrix is generated in a similar manner to the body path planner where the is discretised by representing the obstacle region, Qobs, as 1 and free configuration space, Qfree, as 0.

Equation 38 illustrates the occupancy grid of the obstacle shown in Figure 4.21 using a 10 by 10 matrix, the matrix dimension may be extended to 100 by 100 to improve the accuracy however the computational burden on the processor would be increased as a trade-off. Notice that the matrix given by Equation 38 is vertically inverted since the 1^st row and 1^st column corresponds to the origin (0,0) of the Cartesian coordinate system.

The chosen path planner algorithm, PRM, generates a set of random configurations within Q then the end effector configurations that lie within Qobs are neglected and the configurations that are located within the borders of the Qfree are connected by straight lines to form pairs of configurations given that the lines do not violate the Qobs [16]. The lines that introduce a collision, i.e. violate the Qobs are also neglected. Then, the Cartesian coordinates of the configurations that are located on the shortest roadmap connecting the qo and qf are used by the trajectory planner to generate the collision free foot trajectory that lands on the position projected by the footstep planner. Figure 4.22 shows the set of configurations that needs to be followed to prevent a collision with the arbitrarily generated obstacle, where the randomly chosen configurations are denoted by blue nodes and the configurations that lie on the shortest roadmap are denoted by green nodes. In this particular example, the qo and qf are chosen as (2,1) and (9,1), respectively, whereas in the simulation the initial and final configurations are provided by the footstep planner.

Figure 4.22 The intermediate end effector positions that connect qo and qf.

The PRM planner needs to be specified with a safety margin, i.e. minimum distance that the green nodes can be positioned. An example is shown in Figure 4.23. In practice, a zone surrounding the obstacle is formed which the planner treats as an expanded Qobs and hence it will not be violated. To illustrate, the node that is located on (4,5) is not chosen by the planner since its location lies within the specified safety margin of 0.3m.

Figure 4.23 [6] Safety margin.

Figure 4.24 The complete foot trajectory

Figure 4.24 shows the generated foot trajectory by using the intermediate leg configurations that were given by the PRM Planner. This trajectory is used to compute the 3 joint angles, θ1, θ2, and θ3 by using the inverse kinematics equations to make the leg step over the obstacle and land to the projected .

Figure 4.45 shows the simulation result that was generated in the MapleSim environment. The black line shows the path trace of the motion and it can be seen that the leg steps over the obstacle as expected. The same method is used by the hexapod robot to compute the trajectory over an obstacle while navigating on an uneven terrain.

Figure 4.25 MapleSim implementation of the generated foot trajectory

4.2.2.Body Trajectory Planner

As it was stated in Section 3.2., the adapted walking gait and dynamic stability approaches are used by the MapleSim model to maintain the stability of robot when the terrain becomes steeper or the locomotion speed is increased so that the static stability may no longer be preserved. The results showed that the dynamic stability approach appears to be more advantageous over adaptive walking gait since it improves the locomotion speed of the robot while maintaining its stability.

Adaptive Walking Gait Selector

In the MapleSim simulation, the SM of the motion, which is an indication of how close the robot is to be statically unstable, was used to alter the walking gait depending on the slope of the terrain that the robot is located on. For instance, the walking gait is switched from tripod gait to wave gait in order to increase the area of the support polygon and hence the stability of the robot.

Figure 4.26 shows the support polygon that is formed between the ground contact points during wave gait where a single leg is in the swing phase at a time. It can be seen that the wave gait is intrinsically more stable since the area of the support polygon is greater comparing to tripod gait. Figure 4.27 shows the transition from tripod gait to wave gait to improve the stability. However, note that, this transition gave rise to a trade-off between locomotion speed and stability as it is shown in Figure 3.23-c. The MapleSim code that was used to generate the adaptive walking gait is shown in Figure 8.2 (in Appendix).

Figure 4.26: Bottom view of the Hexapod Robot during wave gait

Figure 4.27 The transition from tripod gait to wave gait.

Figure 4.28 The variation in SM when the walking gait is changed.

Figure 4.28 shows that the SM has increased when the walking gait is changed to wave gait, indicating that the robot is less expected to lose its stability.

Dynamic Stability

The static stability criterion that was covered in Section 3.2 does not take the effect of external forces into account which are caused by the inertial acceleration of the robot. For a legged robot that is navigating on an uneven terrain, the vertical projection of the COG is expected to be located outside or on the border of the support polygon [20].

Therefore, dynamic stability criterion needs to be implemented which is based on computing the optimum acceleration profile of the COG that keeps the zero moment point (ZMP) within the borders of the support polygon during a complete gait cycle [34] [18]. In other words, even though the vertical projection of the COG is located outside the support polygon, the robot is balanced by the external forces that are created during motion. ZMP is defined by [35] as "the point on the ground surface about which the horizontal component of the moment of ground reaction force is zero.” The position of the ZMP, coincides with the position of the COG when the robot is stationary where COG is assumed to be located at the geometric centre of the robot [34] [30].

The body trajectory planner, say in tripod gait, takes the estimated positions of the next 3 footholds which were calculated by the foot step planner and computes a COG trajectory given that ZMP of the robot is kept within the support polygon [18]. The relationships between the position of the ZMP along the X axis, x_ZMP and the Y axis, y_ZMP are given by Equations 39 and 40, respectively.

where xm, ym and zm are the X, Y and Z coordinates of the COG and g is the gravitational acceleration [18].

Figure 4.29 The trajectory of COG and ZMP during tripod gait, adapted from [18]

Figure 4.29 shows the trajectory that the COG traverses from previous support polygon to next support polygon while ZMP is kept within the current support polygon which is shown by the black triangle [18].

4.3.Low Level Controller

In real life, the planned footstep placement and foot trajectories may not be executed as anticipated due to a number of affects such as friction, jamming, slippage, and meshing. Consequently, the stability of the robot is jeopardized and hence the robot locomotion is no longer accurate. In order to overcome the mentioned problems, 2 low level controllers are implemented [18].

4.3.1.Inverse Dynamics

Inverse dynamics is a nonlinear control technique that provides a better trajectory tracking by calculating the required joint actuator torques that need to be generated by the motors to achieve a given trajectory [25] [16].

As it is given in Section 3.4.1, the matrix form of the dynamics of a robot leg is given by Equation 41 where the F and J terms are neglected [25].

For a given q0 and v0 the nonlinearity of the system can be vanished, making the system linear and hence the exact amount of torque that needs to be generated by the joint actuator to overcome the inertia of the actuator can be calculated. If the q0 and v0 match with the desired q0 and v0 then the projected trajectory will be tracked as expected [35].

Equation 42 shows the control law expression where is a^q new input to the system. The numeric value of a^q is calculated by Equation 43 where q_d is the desired trajectory, q is the actual trajectory, and Kp and Kd are position and velocity gains, respectively [16].

Since the mass matrix, M, is invertible, equating Equation 41 to Equation 42 gives the expression in Equation 44 which shows that the new system is linear as it is given by Equation 45.

Figure 4.30 demonstrates the inner loop and outer loop control architecture where the output of the system yields the joint space signal which enables a better trajectory tracking given that the feedback gains Kp and Kd are tuned as appropriate.

Figure 4.30: Inner Loop – Outer Loop Linearized Control, adapted from [16]

4.3.2.Stability Detection and Recovery

Figure 4.31: The foot positions that are likely to cause a slippage, adapted from [2]

Figure 4.31 shows the foothold positions that the robot leg may slip during the stance phase. Figure 4.31-b has a higher chance of slippage than the foot positions that are shown in Figure 4.31-a. For such cases, a recovery controller needs to be implemented for real life applications where conditions are not ideal, this controller is particularly suitable for the cases in which the robot slips or an unexpected obstacle happens to be present within the workspace of the robot leg. The recovery controller provides a reflex based reaction to the 3 cases that are shown in Figure 4.32.

• In Figure 4.32-a, the leg slips from position 1 to position 2 then the recovery controller takes action and leg is moved to position 3 to minimise the slippage.

• In Figure 4.32-b, the leg encounters an unexpected obstacle at position 2 and leg is raised higher to overcome the obstacle and positioned to position 3.

• In Figure 3.32-c, no contact is detected at the expected location 2 and the leg seeks for another foothold and eventually detects position 3.

Figure 4.32: Recovery reflexes, adapted from [4]