CMU 16-299 Final Project: Controlling a Hexapod

This webpage documents my project for the class 16-299: Making Decisions: Robot Control, Planning, and Learning in CMU. The project focuses on exploring the control mechanisms of a hexapod. It aimed to implement and analyze various functionalities crucial for autonomous operation of a hexapod in potentially complex environment.

Some core objectives achieved within this project encompass:

1. Kinematic Modeling: The foundation for the project is the development of a precise kinematic model. This was realized through a MATLAB-based script with essential geometric parameters such as joint articulation limits, coxal height, and femoral/tibial/tarsal segment lengths as inputs. This model served as the basis for calculating both forward and inverse kinematics.
2. Gait Implementation: Two locomotive gaits were successfully implemented and evaluated:
3. Tripod Gait: This gait involves synchronized movements of two sets of three legs, provides fast movement on stable surfaces.
4. Wave Gait: This gait involves sequential movements of individual legs, providing enhanced stability while sacrificing some speed.
5. Rotational Maneuvering: Using a similar logical framework for walking gaits, the hexapod is also capable of turning.
6. Dynamic Balancing: A significant portion of the project was dedicated to achieving dynamic stability. This involved the integration of an IMU to acquire real-time pitch and roll data. A complementary filter was employed for data processing, followed by an inverse kinematics algorithm to compute the requisite servo adjustments. A Proportional-Integral-Derivative (PID) controller then actuated the servos to counteract detected imbalances.
7. Obstacle Avoidance: Basic autonomous navigation was done using an ultrasonic sensor for object detection, which enables the hexapod to alter its moving path upon encountering any kind of obstacles.

While the primary objectives were largely realized, we can also highlight areas for future refinement, including enhancing kinematic model accuracy and optimizing load distribution across the robot's chassis. This webpage will elaborate on the methodologies employed, the results obtained, and the challenges encountered throughout the project.

For the main body of the hexapod, professor Chris Atkeson provided me with a T-HEX 4 DOF Hexapod (my sincerest gratitude here):

https://www.robotshop.com/products/lynxmotion-t-hex-4dof-hexapod-robot-kit-no-electronics?srsltid=AfmBOopMUVB4MEmq4n0W_kBe6aCJe72bbUW0FuUYaI6VP-UPCvURw2Eg

(Theoretically any kind of hexapod would work; for robots with a different DOF, it should be relatively easy to adapt any software provided below.)

Some main components/electronics I used on the body:

1. HS-645MG servos (*24)
2. 6.0 Volt Ni-MH 2800mAh Battery Pack (BAT-05)
3. WH-01 battery wire with a switch
4. SSC-32U Servo Controller Board
5. Arduino Nano ESP32

For modelling, we need:

1. Access to MATLAB
2. Geometric measuring tools (for measuring some parameters of the robot)

For walking and turning, we do not need additional supplies.

For self-balancing, we need:

1. MPU6050 IMU sensor

For obstacle avoiding, we need:

1. HC-SR04 Ultrasonic Distance Sensor

Above is a very rough list of key components needed. Additional (common) tools and supplies are not listed.

Before we could start implementing algorithms on the hexapod, we have some preliminary work to do first.

(1) Robot Assembly

For this step, please refer to the official instruction set provided by Lynxmotion:

https://wiki.lynxmotion.com/info/wiki/lynxmotion/view/ses-v1/ses-v1-robots/ses-v1-3-4-dof-hexapods/t-hex-4-dof-leg/

https://wiki.lynxmotion.com/info/wiki/lynxmotion/view/ses-v1/ses-v1-robots/ses-v1-3-4-dof-hexapods/t-hex-3-4-dof-body/

(2) Servo Calibration

After the robot is assembled, we need to calibrate the hexapod to a standard starting position. Please refer to the below guide:

https://wiki.lynxmotion.com/info/wiki/lynxmotion/view/ses-software/lynxterm/

https://wiki.lynxmotion.com/info/wiki/lynxmotion/view/servo-erector-set-system/ses-servos/ses-lynxmotion-rc-servo/rc-servo-mid-position-tutorial/

(3) Controller Test

Note that the controller board on the robot, SSC-32U, only understands serial command. To communicate with it, I used the UART pins on Arduino Nano ESP32 Board and sent commands through the Arduino IDE Serial Monitor. Also note that the Baud Rate can be changed by pressing the designated button on the board.

For a simple test on communication, type #3 P1600 T1000 into the serial monitor; this should move servo 3 to position 1600 in 1 second's time.

For a complete guide on command grammar for controller board, please refer to the SSC-32U manual attached.

(4) Optional: Start Position

It is highly suggested that we determine a start position for the hexapod. This allows us to ensure preciseness when the robot starts. My calibrated version is detailed below:

Note that RR denotes Right Rear leg (servos: 0, 1, 2, 3), RM denotes Right Middle leg, and RF for Right Front leg, etc.

For servos, we have H for hip servos, V1 & V2 for vertical servos, and K for knee servos.

(5) Global Definitions for Servos

This projects uses both forward kinematics and inverse kinematics to achieve precise and controlled movements on the hexapod. We will focus on inverse kinematics here in this section, since forward kinematics is very straight forward and is easily implemented by transcribing geometric calculations depending on the setup of your robot.

Inverse Kinematics (IK) is, essentially, calculating the set of joint angles required to reach certain position given a desired target position for the foot in space. Thus we can use IK model to translate desired foot placements (e.g., for walking, balancing) into specific commands for the leg servos. A MATLAB script was developed to handle these geometric calculations, which proved extremely useful, especially for tasks like balancing.

We use a function called calculateLegIK_3DOF_Refined to implement our IK model.

Some key inputs to this function includes:

1. targetPos_Body: A 1x3 vector [X, Y, Z] representing the desired coordinates of the foot tip, relative to the center of the robot's body.
2. legIndex: An integer (0-5) specifying which leg is being calculated
3. params: A MATLAB structure containing essential geometric parameters of the robot.

Some key outputs:

1. PW_H, PW_V1, PW_V2, PW_K: The calculated pulse width values (ranging from 500 to 2500 microseconds) for the Horizontal (Coxa), Vertical1 (Femur), Vertical2 (Tibia), and Knee (Tarsus) servos of the specified leg. PW_K is generally fixed, as it does not contribute very much when the robot moves/balances. It is possible to relief this restriction, but sometimes it causes the model to display unwanted or eccentric behaviors.
2. angles_deg: Mainly a debugging output.
3. reachable: A boolean flag indicating whether the position is reachable physically.

Here is the params I measured from this specific robot I used:

It should serve as a good reference; however, I would like to point out this early, that due to the complexity of the structure of the robot, it is very hard to obtain an accurate measurement on the geometric information of this hexapod.

These should be enough to initialize the model. We then attempt to recalculate and transform the target foot position (which is initially given relative to the robot's body center) to be relative the origin of the current leg's coxa joint (the hip).

We can thus calculate the horizontal hip angle using atan2:

Which gives the geometric angle of the leg in the horizontal plane.

We then project the 3d problem to a 2d plane to calculate the angles for the femur (V1) and tibia (V2) servos.

And all we left is using the law of cosines to find the geometric angles for the femur (angleV1_geom_deg) and the effective tibia+tarsus link (angleV2_geom_deg).

and reachability can be checked accordingly:

Then we can easily convert Geometric Angles to Servo Angles and Convert Servo Angles (0-180) to Pulse Widths. This should complete the IK model.

The model is very important, in the sense that it allows the robot to adjust its body posture for balancing on uneven terrain and perform coordinated movements involving multiple legs and do many other things. However, I must point out several sources of errors and areas of improvements for the above model we discussed:

1. As discussed earlier, such kinematics model will involve geometric calculations and will rely heavily on them. Even small errors can lead to noticeable inaccuracies in foot placement.
2. Mechanical imperfections always exists. Various issues like servo gear backlash, flexibility in joints, or slight misalignments during assembly can cause deviations from the ideal mathematical model.
3. Calibration was also a great source of error if not done carefully.

As discussed in the introduction, in this section we implement two locomotive gaits for walking: tripod gait and wave gait.

(1) Tripod gait

https://youtube.com/shorts/nB-qbEh2v_Y

The tripod gait offers a balance of speed and stability, making it effective for locomotion on relatively even surfaces. It involves dividing the hexapod's six legs into two groups (tripods) that alternate their support and swing phases:

1. Tripod A: Left Front (LF), Left Rear (LR), Right Middle (RM)
2. Tripod B: Right Front (RF), Right Rear (RR), Left Middle (LM)

The core logic for this gait is straightforward. We first lift Tripod A, where the V1 servos of LF, LR, and RM legs are commanded to new positions:

We can then swing Tripod A forward and Tripod B backward:

This completes phase 1. Using a similar logic, we can implement phase 2, where Tripod A lowers and moves fully forward and Tripod B shifts fully backward. (The rest of the code is omitted due to high similarity) These two phases are then repeated with the other set of tripod.

Note that we will need to tune the below parameters for this to work, depending on the specs of the particular robot.

(2) Wave gait

https://youtube.com/shorts/UmBqe7bx-uo

The wave gait is characterized by its high stability. This intuitively checks as only one leg is lifted from the ground at any time, leaving five to support the body. This makes it particularly suitable for navigating uneven terrain, though we will have to sacrifice some moving speed compared to Tripod Gait.

The implementation moves legs sequentially in a predefined order: Right Front (RF), Right Middle (RM), Right Rear (RR), Left Rear (LR), Left Middle (LM), and Left Front (LF). Each leg undergoes a two-phase movement.

We first lift RF up and move it half forward, while shifting the other legs halfway back:

We then lower RF and moves it fully forward, while shifting the other legs fully backwards:

And this repeats for the other legs (The rest of the code is omitted due to high similarity). Again, we still need to tune the below parameters:

https://youtube.com/shorts/cw7uGvB5KC8

The turning algorithm has a logic very similar to walking under tripod gait, so we do not overly elaborate on it. Again, we divide the legs into two groups of tripod:

1. Tripod A: Left Front (LF), Left Rear (LR), Right Middle (RM)
2. Tripod B: Right Front (RF), Right Rear (RR), Left Middle (LM)

and we take turnLeft as an example to illustrate how this is implemented.

In Phase 1, we lift tripod A and rotates the legs towards left, and shifts tripod B in the opposite direction.

Then in phase 2, we lower tripod A and shifts it fully backwards, while shifting Tripod B fully forward:

And this repeats for the other set of tripod. Repeating the entire process some finite number of times will be sufficient for turning left. For right turns, we simply reverse what we have done above.

https://youtube.com/shorts/JN-ZdIDZKJM

The self-balancing capability of the hexapod is a critical feature of this project; it allow the hexapod to maintain stability on uneven or tilted surfaces. This is managed by a closed-loop control system that continuously monitors the hexapod's orientation using an IMU sensor and makes corrective adjustments to the leg servos. The core of this section relies on a PID control algorithm to calculate the necessary corrections. The diagram attached above captures the key logic: the data collected by IMU will go through a complementary filter, and the processed data will be fed into the IK model discussed earlier (along with geometric calculations). The model will calculate expected servo PWMs, which is then sent to a PID controller to actuate the servos.

We first initialize the IMU, the servos and the PID controller; after that, we will need to calibrate the IMU:

Within the main loop, the function updateIMUAngles() is used to get the latest orientation data from the MPU6050. The function reads accelerometer and gyroscope data, and applies a complementary filter to combine them.

The filter equation is: angle = ALPHA * (angle + gyro_rate * dt) + (1.0 - ALPHA) * accel_angle where ALPHA is the filter coefficient.

The IMU data is then used as inputs to the IK model discussed earlier (geometric calculations omitted here). We can easily adapt the MATLAB code into Arduino, so we omit the discussion here for fluency.

We then implement the PID control logic. We have targetPitch and targetRoll set to 0.0 to maintain stability, so it follows that

A deadband is also implemented. If the error is within this deadband, no corrective action is taken, and the integral term is reset. This prevents minor oscillations and reduces unnecessary servo movements. The main PID logic is below:

The key outputs of the PID controller is thus pitchCorrection and rollCorrection.

Calculated correction will then be used to send commands to the servos:

As a user to this algorithm we have several parameters to tune:

1. ALPHA, the filter coefficient
2. Kp_pitch, Ki_pitch, Kd_pitch
3. Kp_roll, Ki_roll, Kd_roll
4. PITCH_EFFECT_FACTOR
5. ROLL_EFFECT_FACTOR
6. Z_ADJUST_TO_V1_PW_FACTOR
7. pitchDeadband
8. rollDeadband

And below are tuned paramters I am using, which serves as a reference:

The outcome, as shown in the videos, is satisfactory (though not perfect). Inaccuracies in the kinematic model and filter performance were challenges; and further tuning of the PID controller is an area of further improvement. We will discuss this more in the reflections section.

https://youtube.com/shorts/kaOB-5LXwxQ

A fundamental capability for autonomous mobile robots is the ability to navigate their environment by detecting and avoiding obstacles. This section details the implementation of an obstacle avoidance system for the hexapod using a HC-SR04 ultrasonic sensor. The walking and turning functions will be omitted since we have discussed them before.

The core logic is extremely simple. We make the robot 'see' whether there is anything in front of it with close proximity; once the obstacle is within a certain range of the distance we make the robot turn, until the obstacle is no longer in sight--at which point the robot will continue moving forward.

The core implementation is below:

And continuously checking for distance in the main loop will let us achieve the desired behavior.

This project is successful since I have completed all items on my list of goals. However, we could still point towards a few of the places that could have been improved:

(1) The kinematics model is not accurate enough. This can be improved by more sophisticated measurements of the geometrics of the hexapod.

(2) There is too much loading on the back legs of the hexapod, which caused imbalance during moving. This can be improved by redesigning some of the structure of the hexapod.

(3) In the balancing algorithm, we observed some slight disturbances and abnormalities in servo reactions. This is probably because the filter performance is not optimal; it can be improved by slightly adjusting the coefficients of the filter more--or even redesigning the entire filter.

(4) In the balancing algorithm, the PID controller is not tuned optimally. It behaves at a satisfactory standard, but could be tuned more to reduce the extent of oscillations even further.

It has been an inspiring journey working with robot. I would like to present my sincerest gratitude towards Professor Chris Atkeson and our best TA Krishna. This project would not have been finished successfully without your help!