Hexapod guidance and control for autonomous waypoint navigation over uneven terrain

. This paper presents a novel hexapod guidance and control system that enables an autonomous hexapod robot to walk over uneven terrain while navigating waypoints and keeping its central body level. The system uses a depth camera to determine the terrain heights at the horizontal locations where the hexapod plans to place its feet. The guidance and control software has been implemented in the Robotics Operating System and has been verified in simulation using the Gazebo simulator. A physical hexapod robot has been constructed and practical tests are currently being performed to validate the system.


Introduction
Legged robots are increasingly being used as service robots for surveillance, inspection, exploration, search and rescue, and mine clearance. The environments for these tasks are typically unstructured and uneven terrain [1]. Autonomous legged robots therefore require the ability to sense and traverse uneven terrain.
Previous research used either proprioceptive sensing or exteroceptive sensing to sense the terrain and adjust the leg motion accordingly. Proprioceptive sensing uses force, tactile or torque sensors to sense the contact forces between the hexapod legs and the terrain. Exteroceptive sensing uses external sensors, such as vision-based sensors, to observe and build a map of the terrain. The data from the proprioceptive and exteroceptive sensing is then used to adapt the robot's leg motion planning parameters to enable it to traverse the terrain. [2], [3], and [4] are examples of using proprioceptive sensing, while [5] and [6] are examples of using exteroceptive sensing.
In this paper we introduce a novel guidance and control system for hexapod robots that uses exteroceptive sensing to generate the planning parameters for the leg motions according to the terrain. Specifically, the system adjusts the step heights of the feet based on the sensed heights of the terrain. The system uses a depth camera to produce a minimal representation of the terrain in front of the robot. This representation consists of depth data that is used to calculate the terrain heights at the horizontal locations where the robot plans to place its feet. The system then uses the heights to plan the leg trajectories and executes it accordingly.
In addition, the system also uses a different exteroceptive sensing as input to a guidance controller for navigation between waypoints and proprioceptive sensing as input to a body tilt angle controller.
Unlike the work from [5], [6] and [7] that also uses a stereo camera to adapt their walking based on the images from the camera, what makes our contribution unique is how we use the stereo camera. Where [5] uses a computer vision algorithm to create a digital elevation model (DEM) from the data of the stereo camera and then they extract features from this model to get the step height for the hexapod in the terrain, [6] also uses a computer vision algorithm to create an elevation map, which they use to adapt their walking posture, and [7] uses the stereo camera to detect obstacles and navigate between them. We use the depth image from the stereo camera as is, to only calculate the height of the positions where the hexapod is going to place its feet in the next step.

Methodology
The guidance and control system consists of multiple hierarchical layers which include a guidance controller for waypoint navigation, a body tilt angle controller, a foot trajectory planner, and a leg motion controller. The guidance controller controls the hexapod to follow the ground track between two waypoints by commanding the speed and heading of the hexapod based on the position measurements from a local positioning system (LPS). The body tilt angle controller controls the tilt angle of the hexapod's body using the tilt angle measurements from an onboard inertial measurement unit (IMU). The foot trajectory planner plans the trajectories for the hexapod's feet using the images taken by the depth camera and adjusts the vertical heights of the foot final positions to equal the terrain heights at the locations where the feet will be placed. Finally, the leg motion controller uses the inverse leg kinematics to convert the planned foot trajectories into angle commands for the servomotors that actuate the hexapod's legs.
A simplified simulated model has been created for the Gazebo simulator. This model was created in Autodesk Fusion 360 and was exported to the Unified Robot Description Format (URDF), which is the format that is used by Gazebo and the Robotics Operating System (ROS) to describe the simulated robot. The simulated robot has been equipped with the necessary sensors that are required by the guidance and control system. The guidance and control system has been implemented with the help of ROS. ROS allows the different parts of the system to communicate with each other and allows the system to be used and tested in the Gazebo simulator. This enables the system to be implemented and tested as it is on the real robot.  A physical hexapod robot has been constructed, which can be seen in Fig. 1, and has been equipped with an NVIDIA Jetson Nano as the onboard computer, a Teensy microcontroller to control the servo actuators for the hexapod legs, a Marvelmind ultrasonic indoor positioning system sensor with onboard inertial measurement unit, and an Intel RealSense depth camera. The depth camera is to be mounted on top of the hexapod body and is angled downward to view the terrain in front of the robot.

Guidance and control system
The guidance and control system presented in this paper combines different control layers to improve the automation of the hexapod robot and enhance the locomotion over uneven terrain. A block diagram of the control elements of this system is given in Fig. 2. The guidance controller receives the position and orientation of the robot in the world from the LPS and IMU and sets the desired walking and turning velocities of the hexapod using a straight-line path following algorithm. Simultaneously, the height adaptation algorithm receives depth images from the stereo camera and adjusts the step height of the hexapod as it traverses the uneven terrain while navigating between the waypoints. The foot trajectory planner receives those desired velocities and the desired step heights and then calculates the positions of the feet and the frequency of the steps. The leg motion controller uses the feet positions to calculate the angles of the joints which are then used to command the servos. The step frequency is used to control the speed at which the feet move to the desired positions. In addition, the body tilt angle controller improves the stability of the hexapod while traversing terrain with slopes by keeping the body at a desired orientation.

Planner
The foot trajectory planner generates a path that the foot must follow to complete a single stride. A stride is the combination of swing and support phases. Hexapods move by coordinating the support and swing phases of each of the six legs. Different gait patterns are possible by performing different combinations of the support and swing phases. The different gaits have different duty cycles, = support / stride ,which describes what percentage of the time each leg spends in the support phase. stride = support + swing and support and swing are the time each leg spends in the support and swing phase respectively. The tripod gait is the gait that is used in this work and has a duty factor of = 0.5. Three legs are in the swing phase while the other three are in the support phase. From this the total velocity of the hexapod is given by = s stride (1) where s , and stride are the step size, duty factor and stride time, respectively. In this work the step size will remain constant while changing the step frequency to change the velocity. This allows for the step frequency to be calculated when the velocity is given.
The support and swing phases have a start, mid and end point that is first determined based on various parameters before the trajectory for each foot is calculated. Therefore, foot position planning is done before the foot trajectory is generated. This foot position planning was inspired by work done by Christopher et al. [8].
The various parameters that influence the position of the legs are the radial distance from the centre of the hexapod to the foot, the stride length, the step height, and the crab angle. The pitch and roll angles of the hexapod's body also influence the height position of the feet in reference to the body, but this adjustment is handled by the leg motion controller. The yaw of the hexapod is also handled by the leg motion controller that determines and adds the adjustment of the feet in the x and y positions.
The crab angle is the angle at which the robot is walking. This angle is defined from the positive x−axis of the body coordinate system. This angle, represented by ψ, can be seen in Fig. 3. The crab angle determines in which direction each leg must move while it is in the support phase to move the hexapod in that given direction. An example of the strides of each leg while the hexapod is walking with a crab angle of 30° and stride length of 100 mm is shown in Fig. 3. Fig. 4 shows the stride of one leg when the hexapod walks at a crab angle of ψ and with different heights at the start and end point of the stride. Using some simple trigonometric calculations shown in the diagrams of the stride, the full start, mid and end points can be calculated as follows.
Start point:

= − + Δℎ end
where α is the clockwise rotation relative to leg 1, Rd is the radial distance to the middle of the stride of each leg, H is the height of the hexapod body in the global coordinate system and Sh is the step height. Δhstart and Δhend are the changes in height at the start and end of a stride that allows the hexapod to step onto or off an obstacle in front of it.
The foot trajectory planner uses the start, mid and end points calculated from the foot position planner to determine the path of the foot. The planner determines the path that satisfies the required initial and final position, velocity, and acceleration conditions in each direction. The path planning technique used to generate the trajectory uses defined start and end points as well as any number of points in between the start and end points to determine a desired smooth path. The intermediate points allow for setting the height and shape of the path. These intermediate points can be used to change the path of the foot based on the environment and the obstacles in it.
The foot trajectory planner makes use of a polynomial function to generate a path that goes through the given points. Christopher et al. [8] also explored different order polynomials to obtain the best one. It was observed from this that the single sixth order polynomial gives the smoothest path without any spikes in acceleration. Thus, the single sixth order polynomial is used in the foot trajectory planner to generate the path for the feet.

Feet position
The system only returns the height of the positions where the hexapod places its feet, thus it is necessary for the height adaptation algorithm to know the positions of the feet in the world before the hexapod steps there. Therefore, the system uses the inputs of step size, crab angle and yaw angle to determine the horizontal location where the next step will be. The effects of the step size, crab angle and yaw angle are taken into account in the foot trajectory planner and leg motion controller which gives the positions of the feet relative to the body. If the position of where the body will be was known, the positions of where the feet will be could easily be calculated. But where the body will be right when the hexapod takes its next step is not available. What is known is where each foot will be relative to the body when the hexapod takes a step and where the support feet are in the world. This can be used to calculate where each foot will be relative to another foot and then where the swinging feet will be placed in the world. An auxiliary coordinate system is used to help with finding and defining the position of another foot relative to the chosen foot. Two of the feet that are on the floor are used to define the auxiliary coordinate system. The auxiliary coordinate system when feet 1, 3 and 5 are on the floor can be seen in Fig. 5. In the case that is shown in the figure, feet 3 and 5 are used to define the coordinate system. The orientation of this auxiliary coordinate system relative to the body can be calculated using the positions of feet 3 and 5 in the body coordinates when they are at their last position in the support phase.
where Δx and Δy is the difference between the x position and the y position of the two feet in the body coordinate system, respectively. In this case foot 3 is the origin of the auxiliary coordinate system. The step position of the swinging feet in the auxiliary coordinate system is found by transforming the difference between the swinging feet and foot 3 with a rotation matrix from the body coordinate system to the auxiliary coordinate system, where xk and yk are the position coordinates of foot k in the auxiliary coordinate system, Δxk and Δyk are the displacements of foot k relative to foot 3 in the body coordinate system, and k = 2, 4 and 6 for the case in Fig. 5.
With the position of the swinging feet in the auxiliary coordinate system known, the world position of the feet can be calculated. This is done by firstly finding the angle between the auxiliary coordinate system and the world coordinate system. tan ( LW ) = Δ w Δ w (8) where Δxw and Δyw is the difference between the x-position and the y-position of the two support feet in the world coordinate system, respectively. Then, the world coordinate offset between the origin foot of the axillary coordinate system and the swinging feet is determined by transforming the step position of the swinging feet in the auxiliary coordinate system to the world coordinate system using a rotation matrix.
Using the world coordinate offset, Δxk,w and Δyk,w, the step position of the swinging feet in the world can be obtained by ,w = Δ ,w + origin,w ,w = Δ ,w + origin,w where xorigin,w and yorigin,w are the x and y position of the auxiliary coordinate system's origin in world coordinates. In Fig. 5, the origin is foot 3, and the k = 2, 4 and 6. The step positions of the swinging feet in world coordinates are where these feet will be positioned when they become the support feet. The above calculations then restart for the case where foot 2, 4 and 6 are the supporting legs. The auxiliary coordinate system is then defined with foot 2 and 6, where foot 2 is the origin and the k = 1, 3 and 5. It is possible that the hexapod feet may not step exactly on the planned step positions, due to model uncertainty or external disturbances. This may cause the actual foot positions to drift from the expected foot positions over time. This is corrected by calculating the actual position of the swinging feet as soon as they become supporting feet, by using the position of the body in the world obtained from the LPS and the position of the feet relative to the body obtained from the foot trajectory planner. The corrected positions of the supporting feet are then used for the next iteration of the algorithm above.

Height adaptation
The height adaptation algorithm uses a stereo camera to derive a representation of the terrain in front of the hexapod. This algorithm also uses an IMU to align the images at the correct orientation with the world. Based on these depth images, the algorithm sets suitable leg heights in the foot trajectory planner, eq. 2 and eq. 4, that allows the hexapod to move over traversable obstacles. From the two equations it can be observed that both the starting height and end height of the step can be adjusted. The end height is the one adjusted to allow the hexapod to step onto or off an obstacle. When the end height of the next step is greater than the starting height, the starting height is set equal to the end height, to stop the hexapod's leg from colliding with the obstacle. If the end height is lower than the starting height, the starting height is kept the same until the hexapod finishes the step. The starting height is then set equal to the end height, to keep the hexapod from tipping over. The step height in eq. 3 is also adjusted by the observed height to allow the hexapod to lift its foot high enough.
Using the stereo disparity information from the camera a depth image of the upcoming terrain is generated. This depth image is an array that holds the depth readings of the pixels in the image. The size of the array is the size of the resolution of the depth camera. If the camera has a resolution of 1280 x 720, the array has 720 rows and 1280 columns. An element in the array corresponds to a pixel. The depth readings in this array are used to determine the height of the terrain in front of the hexapod.
The stereo camera cannot see the terrain right beneath the hexapod, which means that the current height that the rear legs need to step cannot be sensed. The camera is only able to see where the rear legs should step three gait cycles from where it is standing. It is therefore necessary to "remember" a view of the terrain a few steps back. Thus, a snapshot is taken of the terrain in front of the hexapod each time that the hexapod places its feet after a step was made. The snapshots are stored in a multidimensional array. The newest snapshot is at the end of the array and the oldest snapshot is in the beginning. The elements in the array are shifted to the left if a new snapshot is taken. A total number of six snapshots are needed to have a view of where the rear legs will be placed in three gait cycles. The snapshots are saved with the oldest one being replaced once there are six snapshots, and a new snapshot is taken.
Together with the snapshots being saved when the hexapod takes a step, the position and orientation of the hexapod's body are also saved at the same time. This position and orientation are used to align the snapshots with the world at the time they were taken.
The height adaptation algorithm receives the end position, in world coordinates, of the feet that is in the swing phase next just before the hexapod is about to take its next step. These positions are translated from the world coordinate system to the body coordinate system using the position and orientation of the body that were saved with the snapshots. With this, the position of the feet in the body coordinates can be obtained by using the following transformations where Xworld and Yworld is the position of a foot in the world, snap body and snap body is the position of the body in the world in the snapshots and ψsnap is the yaw of the body in the snapshots. The positions of the feet need to be translated from the body to the camera coordinate system so that the depth value from the camera to that point can be obtained in order to calculate the height at that point. where LI is the offset of the left imager from the centre of the camera, θ cam is the angle the camera is tilted at, h is the height at the point, = √ 2 + 2 and HL = tan −1 . The slope HL and angle θHL is determined from Fig. 6.
However, as it can be seen in eq. 12, to translate the position of the feet from the body to the camera coordinate system, the height at that position is required, but the height is also unknown. To solve this problem a possible range of heights is inserted into eq. 12, this yields a range of y camera positions and depths at a certain x camera position. The intrinsic parameters of the camera are used to find the corresponding pixel coordinates of these range of camera points. These pixel coordinates are used to lookup what the depth values are in a depth image snapshot. If a depth value obtained from the depth image is equal to a depth value calculated from eq. 12, it means that the calculated depth value exists in the image and that the height used to calculate that depth value also exists. The height in the body coordinate system that corresponds to the existing depth value is then obtained by converting the depth value from the camera coordinates to the height in the body coordinates with where θcam, HL and θHL are the same as in eq. 12, zc is the existing depth value and yc is the y position in the camera coordinates that corresponds to that depth position at that height. From this, the height in the world coordinate system is calculated by where snap body is the height of the body in the world in the snapshots.

Leg motion controller
The leg motion controller uses the position for each leg and determines the joint angles needed to achieve that position. The positions determined by the foot trajectory planner takes all the input parameters into account except for the pitch, roll, and heading (yaw) angles. The effect of these angles on the positions are added by the leg motion controller. Before the joint angles are determined, the adjusted points on the path of the foot trajectory are interpolated with time. This interpolation with time is what allows the speed to be controlled. The inverse kinematics of the legs are then used to obtain the joint angles from those positions and then send to the robot to actuate the joints. Inverse kinematics is the process of finding the joint angles and displacements that relates to the position of the end effector, where in this case the end effector is the foot of each leg. The technique used by Christopher [8] to determine the inverse kinematics of a hexapod leg was used to obtain the following inverse kinematics equations for joint 1 θ1, joint 2 θ2 and joint 3 θ3 of a leg

Guidance controller
The walking path that the hexapod is to follow is defined as a series of straight-line path segments between predefined waypoints. The straight-line segment between two consecutive waypoints is called the ground track. The purpose of the guidance controller is to guide the hexapod onto the ground track by controlling the cross-track position error to zero. The hexapod's cross track error y and its in-track distance x from the source waypoint is shown in Fig. 8. The cross track error is the perpendicular distance from the hexapod's position to the ground track. The in-track distance along the track, is the distance of the hexapod's projection onto the track, from the source waypoint.
The hexapod is following the ground track when its heading ψ equals the ground track heading ψtrack, and the cross track error is zero. The guidance controller is used to control this cross track error to zero. The hexapod has reached the destination waypoint when its in-track distance equals the length of the ground track Ltrack and at this point the new destination waypoint is selected.
The guidance controller works by generating a heading command for the hexapod that is proportional to the cross track error. This heading command then produces a rate of change in the cross track error ̇ that is proportional to the heading of the hexapod relative to the ground track heading ψ − ψtrack, and the ground speed of the hexapod V. The cross track error is thus reduced which guides the hexapod onto the track with the desired heading as the hexapod walks to the destination.

Body tilt angle controller
The body tilt angle controller generates a roll and pitch command, based on the measurements from the IMU, for the hexapod to keep the body at a desired orientation, horizontal in this case, while the hexapod walks on slopes or steps on obstacles. With this the hexapod's centre of mass is shifted which increases the stability of the hexapod on slopes. The roll and pitch commands are used in the leg motion controllers to set offset heights for the feet which will allow the hexapod to stay horizontal.

Results and discussions
As mentioned in the methodology, the hexapod guidance and control system has been verified in simulation by implementing and creating a simulation model of the hexapod in the Gazebo simulator. Although a simplified model of the hexapod was created, the software implemented on the simulation model is not a reduced or simplified version as it can be implemented as is on the physical hexapod. This is made possible by the use of the ROS environment. The ROS environment allows different parts of the system to be implemented separately and still be able to communicate with each other. The ROS environment is also used to connect the software of the hexapod to the model in Gazebo. The Gazebo model subscribes to the data send from the software and this allows the model to move as commanded. At the same time the Gazebo model sends information back to the software. This information is position and orientation information, and also the depth camera stream.
The guidance controller and the body tilt angle controller were tested in the simulation but the results of these controllers are not shown here as the focus is on the foot trajectory planner that uses the depth camera to plan the motion of the feet accordingly. However, the body tilt angle controller was enabled while generating the results for the foot trajectory planner. The results from the foot trajectory planner experiment are also used to verify that the leg motion controllers work properly.
The results are generated by tracking the position of the feet in the world and how the position changes as the hexapod walks and how the height is adjusted when there is an object in front of the hexapod. The position of the feet is not directly obtainable from the Gazebo simulation; however, the position of the feet can be calculated because the angles of the joints of each leg and the position of the body in the world are available. The forward kinematics of the legs are used to determine where the feet are in the body coordinates based on the joint angles obtained from the simulation and then these positions are transformed to world positions by using the positions of the body in the world. Work from Christopher et al. [8] was used to obtain the forward kinematics equations for a hexapod.
The first results generated is to show how well the foot trajectory planner can determine where the hexapod will place its feet for the next step based on the planning parameters. In this test only the odd numbered feet were tracked to make is easier the see which point is for which foot. The hexapod was only given a forward velocity to the right with a step size of a 100 mm. Fig. 9 shows a snippet of this test that was done in the Gazebo simulation. The circles on the floor indicates where the hexapod will place feet 1, 3 and 5. The circles to the left of the hexapod are the calculated feet positions that the hexapod already took while walking to the right. From Fig. 9 it can be seen that the hexapod's actual feet positions are close to the calculated positions for the current step in the snippet. The calculated feet positions and the actual feet positions in the snippet were saved to a csv file and this was used to make a point graph. The point graph can be seen in Fig. 10. From this figure it can be observed that the actual positions are quite close to the calculated positions, which shows that the foot trajectory planner can accurately determine where the hexapod places its feet next. The heights at the hexapod's next foot placements are the next thing that is determined by the foot trajectory planner. Thus, the next results generated are to show that the hexapod adjusts the heights when the feet are placed at the next positions with an elevated surface. Fig. 11 shows the hexapod at its seventh step of the even numbered feet (2, 4 and 6). The circles still show the calculated feet positions but this time of feet 2, 4 and 6. The circles only show the world x and y positions of the feet and therefore they are still on the ground although the hexapod feet are stepping at different heights. Fig. 12 shows the height of leg 6 (the front left leg) versus the x position of the leg. This figure shows that the height for leg 6 was adjusted for its third step to allow the leg to step onto the elevated surface and from Fig. 11 it can be observed that this adjustment was done at the correct step. It is thus observed from Fig. 11 and Fig. 12 that the hexapod still places its feet at the calculated positions and that the heights of the feet are adjusted at the positions accordingly.   Fig. 14 also show results for the height adaptation algorithm for the foot trajectory planner. The hexapod was given a forward velocity with two obstacles in front of it, each with a different height. With the higher obstacle being slightly to the right and the other one slightly to the left. Fig. 13 shows a snippet of this experiment, where leg 6 (front left leg) and leg 2 (front right leg) already stepped onto the two different obstacles. Fig. 14 shows the height of legs 1 (front leg), 2 and 3 (back right leg) over time for a portion of time during the experiment in Fig. 13. Leg 2 is out of phase with leg 1 and 3 and therefore leg 2 is on the ground while the other two are in the air and vice versa. From Fig. 14 it can be observed that leg 2 stepped onto the obstacle first, thereafter leg 3 stepped onto the obstacle two steps later, while leg 1 kept stepping on the ground which corresponds to what can be observed in Fig. 13. This shows that those two legs were able to adjust their foot heights as needed. From all of the simulation results shown above it is evident that the hexapod is able to use the depth data from a stereo camera to determine the height of the terrain in front of the hexapod while it is autonomously navigating between waypoints. This enables the hexapod to adjust the height of the feet to appropriate heights depending on the terrain in front of the hexapod. The results are also an indication that the foot trajectory planner is capable of generating a path for a foot to follow based on the inputs it receives, as the feet were able to step and move the robot in the desired direction. It is also clear from the results that the leg motion controllers are able to take the positions of the feet and calculate correct joint angles to actuate the joints. The body of the hexapod was horizontal for the whole duration of the experiments while the hexapod traversed the obstacles which is an indication that the body tilt angle controller is functioning properly.
Preliminary results from the physical hexapod also verifies that the trajectory planner is capable of generating suitable paths from the control parameters and that the leg motion controllers are able to control the motors with the correct joint angles to move the feet to the desired positions.

Conclusion
This paper described a guidance and control system consisting of multiple hierarchical layers that allows the hexapod to walk between waypoints while adapting its feet height based on the height observed by a stereo camera. This included a detailed description of how the trajectories for the feet are generated and how those trajectories are adjusted to adapt to the height changes. The paper also included a description of the technical background of the other control layers as well as results to verify the system.
The simulation results show that the proposed hexapod guidance and control system shall be able to control an autonomous hexapod to traverse uneven terrain by adjusting the foot heights to step onto or of obstacles and by keeping its body levelled while traversing slopes and the uneven terrain. The preliminary results with the physical hexapod robot are also promising.
Future work will include fully implementing the proposed system on the physical hexapod and testing it in an indoor multi-terrain test environment.