Milestone 2 — Mid-Point Technical Proof

Due: Apr 15     Weight: 10%     Team: Varad Jahagirdar, Dhiren Makwana, Sharat Mylavarapu
Table of Contents

1. Kinematics

1.1 Differential Drive Motion Model

The LoCoBot uses a Kobuki differential drive base. The robot’s state is defined as:

\[\mathbf{x} = [x, y, \theta]^T\]

where $x$, $y$ are the 2D position in the world frame and $\theta$ is the heading angle.

Given left and right wheel velocities $v_L$ and $v_R$, the linear and angular velocities of the robot are:

\[v = \frac{v_R + v_L}{2}, \qquad \omega = \frac{v_R - v_L}{L}\]

where $L = 0.23$ m is the wheel separation (track width of the Kobuki base).

The discrete-time state update equations are:

\[x_{t+1} = x_t + v \cos(\theta_t) \cdot \Delta t\] \[y_{t+1} = y_t + v \sin(\theta_t) \cdot \Delta t\] \[\theta_{t+1} = \theta_t + \omega \cdot \Delta t\]

This is the standard unicycle model for differential drive robots. In our ROS 2 implementation, this model is handled by the Gazebo Harmonic DiffDrive plugin which integrates these equations at every simulation timestep and publishes odometry on /odom.

1.2 Odometry State Estimation

The odometry is published as a nav_msgs/msg/Odometry message with:

\[\hat{\mathbf{x}}_t = \hat{\mathbf{x}}_{t-1} + \begin{bmatrix} v\cos\hat{\theta} \\ v\sin\hat{\theta} \\ \omega \end{bmatrix} \Delta t\]

The odom_tf_broadcaster node subscribes to /odom and re-publishes the transform odom → locobot/base_footprint to the TF tree using sim time, ensuring timestamp consistency with Nav2.

1.3 Beacon Trilateration Model

Given three beacons at known positions $(b_{x_1}, b_{y_1})$, $(b_{x_2}, b_{y_2})$, $(b_{x_3}, b_{y_3})$ with noisy range measurements $r_1, r_2, r_3$, the ball position $(x, y)$ satisfies:

\[(x - b_{x_i})^2 + (y - b_{y_i})^2 = r_i^2, \quad i = 1, 2, 3\]

Subtracting equation 1 from equations 2 and 3 linearizes the system:

\[2(b_{x_2} - b_{x_1})x + 2(b_{y_2} - b_{y_1})y = r_1^2 - r_2^2 - b_{x_1}^2 + b_{x_2}^2 - b_{y_1}^2 + b_{y_2}^2\] \[2(b_{x_3} - b_{x_1})x + 2(b_{y_3} - b_{y_1})y = r_1^2 - r_3^2 - b_{x_1}^2 + b_{x_3}^2 - b_{y_1}^2 + b_{y_3}^2\]

In matrix form $\mathbf{A}\mathbf{p} = \mathbf{b}$:

\[\mathbf{A} = \begin{bmatrix} 2(b_{x_2}-b_{x_1}) & 2(b_{y_2}-b_{y_1}) \\ 2(b_{x_3}-b_{x_1}) & 2(b_{y_3}-b_{y_1}) \end{bmatrix}, \quad \mathbf{p} = \begin{bmatrix} x \\ y \end{bmatrix}\] \[\mathbf{b} = \begin{bmatrix} r_1^2 - r_2^2 - b_{x_1}^2 + b_{x_2}^2 - b_{y_1}^2 + b_{y_2}^2 \\ r_1^2 - r_3^2 - b_{x_1}^2 + b_{x_3}^2 - b_{y_1}^2 + b_{y_3}^2 \end{bmatrix}\]

Solved via numpy.linalg.solve(A, b).

2. System Architecture

2.1 rqt_graph

rosgraph_page-0001

Figure 1: ROS 2 node graph showing all active nodes and topic connections during full system operation.

2.2 System Mermaid Diagram

Screenshot 2026-04-15 203706

2.3 Active Topics

Topic Message Type Publisher Subscriber
/odom nav_msgs/msg/Odometry gz_bridge odom_tf_broadcaster, Nav2
/cmd_vel geometry_msgs/msg/Twist Nav2 gz_bridge → Gazebo
/scan sensor_msgs/msg/LaserScan depthimage_to_laserscan Nav2 costmap
/camera/depth sensor_msgs/msg/Image depth_bridge depthimage_to_laserscan
/camera/camera_info sensor_msgs/msg/CameraInfo depth_bridge depthimage_to_laserscan
/robot_description std_msgs/msg/String robot_state_publisher Gazebo spawn
/joint_states sensor_msgs/msg/JointState joint_state_publisher robot_state_publisher
/tf tf2_msgs/msg/TFMessage gz_bridge, odom_tf_broadcaster, static_transform_publisher Nav2, RViz2
/ball_global_pose geometry_msgs/msg/PoseStamped beacon_localization Nav2 goal
/goal_pose geometry_msgs/msg/PoseStamped user/nav node Nav2 bt_navigator

3. Module Descriptions

3.1 Module Declaration Table

Module Type Status Plan
Beacon Localization Custom ✅ Complete Gaussian noise + trilateration implemented
Vision-Based Ball Detection Custom 🔄 Pending M3 Final
Autonomous Navigation Library (Nav2) ✅ Complete MPPI controller configured
Base Alignment Custom 🔄 Pending M3 Final
Manipulation & Grasping Library (MoveIt2) 🔄 Pending M3 Final
CG Stability Controller Custom 🔄 Pending M3 Final
Vacuum Throw Release Custom 🔄 Pending M3 Final
Gazebo Simulation Library ✅ Complete World with beacons, obstacles, ball, throw target
Depth Camera + LaserScan Library ✅ Complete Added to URDF, bridged to ROS2
TF Tree Management Library ✅ Complete odom→base_footprint via gz_bridge

3.2 Nav2 — Autonomous Navigation Stack

Nav2 provides the full autonomous navigation pipeline. Key parameters tuned:

Parameter Value Purpose
inflation_radius 0.15 m Reduced to allow navigation through narrow gaps
robot_radius 0.15 m Robot footprint approximation
vx_max 0.8 m/s Maximum forward velocity
xy_goal_tolerance 0.15 m Goal reached threshold
controller_frequency 20 Hz MPPI controller update rate
scan_topic /scan LaserScan input from depth camera
global_frame map Planning frame
robot_base_frame locobot/base_footprint Robot frame

Planner: NavFn (Dijkstra-based global planner)
Controller: MPPI (Model Predictive Path Integral)
Config: nav2_params.yaml

3.3 depthimage_to_laserscan

Converts depth camera image to 2D LaserScan for Nav2 costmap updates.

Parameter Value Purpose
scan_height 10 pixels Rows used for scan
range_min 0.2 m Minimum valid range
range_max 10.0 m Maximum valid range
output_frame locobot/depth_camera_link Output frame matching TF tree
use_sim_time true Sync with Gazebo sim time

3.4 odom_tf_broadcaster

Source: odom_tf_broadcaster.py

Subscribes to /odom and broadcasts the odom → locobot/base_footprint TF transform using the odometry timestamp to ensure Nav2 receives transforms with correct sim time.

3.5 beacon_localization

Source: beacon_localization.py

Subscribes to Gazebo pose info, extracts ball position, injects Gaussian noise into beacon range measurements, solves trilateration, and publishes /ball_global_pose.

4. Experimental Analysis & Validation

4.1 Depth Camera Calibration

The depth camera is positioned at (0.1, 0, 0.4) m relative to locobot/base_link, facing forward with a 60° horizontal FOV. The depthimage_to_laserscan node extracts a 10-pixel horizontal strip from the depth image and converts it to a 2D LaserScan. Range is validated between 0.2 m and 10.0 m, publishing at ~8 Hz.

4.2 Beacon Noise & Uncertainty Analysis

The beacon_localization node acts as both the localization estimator and the noise injector. It corrupts clean range measurements from Gazebo with Gaussian noise before running trilateration, simulating real-world UWB/WiFi range sensor uncertainty.

Gaussian Noise Model

Each beacon-to-ball range measurement is independently corrupted with additive white Gaussian noise:

\[d_i^{\text{noisy}} = d_i^{\text{true}} + \epsilon_i, \quad \epsilon_i \sim \mathcal{N}(0, \sigma^2)\]

where $\sigma = 0.3$ m, giving variance $\sigma^2 = 0.09$ m².

The true distance from beacon $i$ at position $(b_{x_i}, b_{y_i})$ to the ball at $(x, y)$ is:

\[d_i^{\text{true}} = \sqrt{(x - b_{x_i})^2 + (y - b_{y_i})^2}\]

Noise Injector Code 

NOISE_STDDEV = 0.3  # meters
 
for name, (bx, by) in BEACON_POSITIONS.items():
    true_dist = np.sqrt((true_x - bx)**2 + (true_y - by)**2)
    noise = np.random.normal(0, NOISE_STDDEV)
    noisy_dist = max(0.01, true_dist + noise)
    noisy_distances[name] = noisy_dist

Each beacon measurement is independently corrupted with $\mathcal{N}(0, 0.09)$, creating a realistic multi-beacon noise scenario.

### Assumptions

  • Beacon positions are perfectly known (no anchor uncertainty)
  • Noise is independent and identically distributed (i.i.d.) across beacons
  • Ball is stationary during localization
  • Line-of-sight ranging (no multipath effects)

  • Noise is zero-mean (unbiased measurements)

Trilateration Under Noise

The noisy distances are fused using the linear trilateration system solved via numpy.linalg.solve. The system linearizes the range equations around the three beacon positions to produce a direct estimate of ball position. Since each of the three beacon measurements carries independent noise, the position error propagates through the matrix inversion and typically results in a localization error slightly above the single-beacon noise standard deviation.

Observed Performance

Run True Position Estimated Position Error (m)
1 (2.34, 2.98) (2.51, 3.21) 0.29
2 (3.12, 2.70) (2.89, 2.45) 0.34
3 (1.68, 2.29) (1.42, 2.58) 0.38
4 (3.26, 2.86) (3.54, 2.71) 0.31
5 (2.12, 3.22) (1.93, 3.47) 0.32

The average localization error of 0.33 m is consistent with the injected noise standard deviation of 0.3 m. The small additional error above 0.3 m is expected due to noise amplification through the trilateration matrix inversion — a known characteristic of geometric multilateration under noisy measurements.

Beacon Positions in World

Beacon World Position Color
beacon_ne (4.5, 4.5) Red
beacon_se (4.5, -4.5) Blue
beacon_sw (-4.5, -4.5) Yellow

4.3 Run-Time Issues & System Behaviors

The following run-time issues were observed and resolved during development and testing of the autonomous navigation system:

Issue 1: TF Tree Disconnection

Observed Behavior: Nav2 repeatedly logged:

Could not find a connection between 'odom' and 'locobot/base_footprint'
because they are not part of the same tree.

Root Cause: The Gazebo-ROS2 bridge was publishing odometry on /odom but not forwarding the corresponding TF transform to the ROS2 /tf topic.

Resolution: Added /model/locobot/tf to the gz_bridge with a remapping to /tf, ensuring the odom → locobot/base_footprint transform reaches Nav2 with correct simulation timestamps.

Issue 2: Scan Frame Timestamp Mismatch

Observed Behavior: Nav2 costmap continuously dropped scan messages:

Message Filter dropping message: frame 'camera_depth_frame'
at time X for reason 'timestamp earlier than transform cache'

Root Cause: The depthimage_to_laserscan node published the LaserScan with a default frame camera_depth_frame which did not exist in the TF tree.

Resolution: Set the output_frame parameter to locobot/depth_camera_link which is the actual TF frame of the depth camera in the robot URDF.

Issue 3: Robot Spawning at Previous Position

Observed Behavior: After restarting the simulation, the robot appeared at its last position from the previous run, causing odometry to start from a non-zero position and breaking Nav2’s localization.

Root Cause: Gazebo Harmonic caches the last known model state between runs.

Resolution: Added the -r flag to gz_sim to auto-reset the world on launch, and added a 5-second TimerAction delay before robot spawn to ensure Gazebo fully initializes first.

Issue 4: Robot Stuck Near Obstacles

Observed Behavior: The robot stopped moving near clusters of obstacles even when a navigable path existed nearby. It would remain stuck for extended periods before the recovery behavior triggered.

Root Cause: The Nav2 costmap inflation radius was set to 0.55 m, causing obstacles to appear artificially large and blocking all available paths through the cluttered environment.

Resolution: Reduced inflation parameters in nav2_params.yaml:

  • inflation_radius: 0.55 m → 0.15 m
  • robot_radius: 0.22 m → 0.15 m

This allowed the planner to find paths through the gaps between obstacles.

5. Milestone Video

Navigation Demo: https://youtu.be/GvLrjGdQ7WQ

The video demonstrates the robot autonomously navigating from its spawn position to the ball at (3.0, 3.0) in Gazebo, with the depth camera LaserScan visible in RViz2 and obstacle avoidance active throughout.

6. Instructor Feedback Integration (M1 → M2)

M1 Feedback Technical Action Taken
Missing module intents and safety Added full Module Declaration Table with Intent, Library, Safety for all modules
Index page placeholder Updated index.md with team info and project description
Flowchart \n characters Fixed Mermaid diagram syntax
Git repo not found Created ros2_ws/ directory with all ROS2 packages
Beacon noise? Implemented Gaussian noise $\tilde{d}_i = d_i + \mathcal{N}(0, \sigma^2)$ in beacon_localization.py
Why Gazebo? Evaluated Gazebo vs Webots vs Isaac Sim — selected for ROS2 Jazzy compatibility
Which robot platform? Committed to LoCoBot (Kobuki base + WidowX 250s arm)
Base velocity during throw Incorporated into throw design: $v_{release} = v_{arm} + v_{base}$

7. Individual Contribution

Team Member Role Key Commits Files
Dhiren Makwana Simulation & Navigation 3828c66 435758f f054235 locobot_gazebo.launch.py nav2_params.yaml locobot_kobuki.urdf.xacro
Varad Jahagirdar Perception & Localization bd8437f cfed6b9 bab74f0 beacon_localization.py locobot_world.sdf
Sharat Mylavarapu Documentation & Integration c637cb7 20c1733 report2.md
This site uses Just the Docs, a documentation theme for Jekyll.