Emad Razavi

System overview

The stack is intentionally lightweight: Spot uses a 2D LiDAR for mapping/localization, a RealSense T265 for visual-inertial odometry, and a RealSense D435 for RGB D detections. SLAM Toolbox builds the pre-run map; during navigation, AMCL localizes on the fixed map and Nav2 sends goals through the ROS 2 bridge.

• Two-stage design: navigation relies on a fixed map and a recorded semantic database instead of rebuilding the world online.
• Confirmed objects only: detections are saved after repeated support over time, with gating and duplicate merging.
• Inspectable behavior: the map, object list, selected target, generated goal, and Nav2 plans can be checked in RViz.

Demo 1: Semantic mapping

During the pre run, D435 RGB detections are projected into 3D using depth and transformed into the map frame. The semantic layer keeps temporary observations separate from confirmed objects, which reduces noisy entries in the final database.

• Proposal memory: temporary observations that must survive repeated checks.
• Static memory: confirmed object instances, merged when nearby detections describe the same object.
• Recorder output: a human readable YAML database used later for object-goal navigation.

Demo 2: Voice based object goal navigation

In the second stage, Spot loads the saved map and object database, then receives a target request by voice. Whisper transcribes the command, embedding matching selects the closest recorded object instance, and the object pose is converted into a safe standoff goal for Nav2.

• Goal generation: targets are not placed inside objects; they are offset with a clear facing direction.
• Execution: Nav2 receives a standard NavigateToPose goal with map, LiDAR obstacle, and inflation costmaps.
• Safety: Spot's native safety layer remains active under the ROS 2 navigation stack.

Implementation and evaluation

The implemented system includes a confirmed-only semantic layer, an object-to-goal interface for Nav2, onboard Spot deployment, RViz/rosbag tooling, and repeated-run supervision. Evaluation covered three real indoor environments: a church, the DLS lab, and a large IIT test room.

The same pipeline produced usable 2D maps across different layouts and kept the semantic database compact by focusing on a small set of object classes. The main observed limitations were depth alignment bias in object placement and localization inconsistency causing early goal acceptance in edge cases. Both failure modes were visible in logs and RViz, making the system inspectable and technically actionable.

Overview

This project investigates PID tuning with Ant Colony Optimization for a second-order control system, while explicitly modeling a delayed baseline to study how time delay affects the transient response. The goal is to minimize the Integral of Time-weighted Absolute Error (ITAE) and obtain a faster, cleaner response with reduced oscillation and smaller tracking error.

We implemented the project in MATLAB and combined classical transfer-function modeling, heuristic optimization, and result visualization. We first build the delayed reference model, then search for PID gains that improve closed-loop performance through repeated simulation and pheromone-based updates.

System definition and delay modeling

We define the plant as G = tf([1], [1 2 3]), which corresponds to a second-order transfer function. A pure delay is then introduced through exp(-1 * s). Since a pure delay is not directly convenient for rational transfer-function analysis, we use a first-order Pade approximation to construct the delayed model used for the baseline step response.

• Plant model: the base system is a stable second-order transfer function with denominator coefficients [1 2 3].
• Delay model: we approximate exp(-s) with pade(delay, 1) to obtain a rational delayed model.
• Baseline response: the delayed model is closed with unity feedback as Gf = feedback(Gt, 1) and plotted before controller tuning.

Controller objective

The controller follows the standard PID structure with proportional, integral, and derivative gains. The performance target is ITAE, which weights the tracking error by time and therefore penalizes not only large deviations but also errors that persist for too long.

One implementation detail is worth stating clearly: in our current implementation, the delayed model is used for the initial baseline plot, while the ACO loop evaluates PID candidates on the nominal plant through feedback(Gc * G, 1). This page reflects that implementation exactly.

Why these methods

• Time delay modeling: delays often degrade damping and tracking performance, so they should be represented explicitly during analysis.
• Pade approximation: it converts the exponential delay into a rational form that can be simulated with standard transfer-function tools.
• ACO: it offers a practical derivative-free search strategy for coupled tuning parameters.
• ITAE: it encourages fast error reduction and typically leads to responses with better settling and lower oscillation.

Code structure and workflow

Our MATLAB workflow is organized around four stages: system definition, ACO initialization, iterative search, and result visualization. We define the nominal and delayed models, allocate the pheromone tables and candidate arrays, evaluate sampled PID gains in closed loop, and store the best cost value across iterations.

• System setup: defines G, builds the delayed model with pade(delay, 1), and plots the unity-feedback delayed response.
• ACO initialization: sets the number of ants, decay factor, scaling factor, iterations, and candidate gain structure.
• Main loop: computes selection probabilities, samples gains, evaluates each controller, records the best solution, and updates pheromones.
• Visualization: generates the delayed baseline response, the optimized closed-loop response, and the ITAE history over iterations.

Results

Figure 1. Step response of the original delayed system. The uncompensated response shows overshoot and oscillatory behavior, highlighting the performance degradation introduced by delay.

Figure 2. Step response after tuning the PID gains with ACO. The optimized controller improves settling behavior and produces a noticeably cleaner transient response.

Figure 3. ITAE across optimization iterations. The downward trend indicates convergence toward more effective PID gains.

How to run the code

The project is implemented in MATLAB with the Control System Toolbox. Running the main file builds the delayed baseline model, executes the ACO-based PID search, reports the best gains and minimum ITAE in the command window, and generates the three figures used in the thesis.

• Open the MATLAB file such as PID_ACO.m.
• Run it in MATLAB.
• Inspect the reported values for kp, ki, kd, and Min_ITAE.
• Review the three generated figures for baseline behavior, optimized response, and cost convergence.

The experiment can also be repeated with different ACO settings. The most direct parameters to adjust are the number of ants, pheromone decay factor, scaling factor, number of iterations, and the lower bound, upper bound, and step size of the PID gain search space.

Conclusion

This thesis shows that Ant Colony Optimization can be used effectively to tune a PID controller for a second-order control problem with a delayed reference model. By minimizing ITAE, the optimized controller achieves a cleaner transient response and better settling behavior than the uncontrolled baseline.

It also demonstrates the value of combining classical control tools with heuristic optimization in a compact, reproducible MATLAB workflow. The project was a solid first experience in algorithmic controller tuning and technical evaluation during my undergraduate studies.