Autonomous Navigator

Principal Authors:   Lucero Aguilar-Larios & Gian Fajardo

Faculty Advisor:   Dr. Shahnam Mirzaei

Table of Contents

• 1. Goals
• 2. Methods
• 3. Materials
• 4. Online Resources
  • 4.1. Things I've made to explain this stuff
  • 4.2. C library
  • 4.3. Hardware Development
  • 4.4. Sensor Fusion with MARG sensor
  • 4.5. Iterative Closest Point (ICP)
  • 4.6. GraphSLAM
  • 4.7. EKF-SLAM
  • 4.8. RRT*
• 5. Dynamic Window Approach (DWA) Algorithm
• 6. Checklist
  • 6.1. Task 1: 12 Dec 2025 (3 weeks)
  • 6.2. Task 2
  • 6.3. Ongoing Task

---

1. Goals

   The scope of this project is to explore the fundamentals of LiDAR-based 2D mapping and autonomous navigation. In a world reliant on more autonomous systems, these systems need to navigate effectively as they can enable efficient and precise movements in complex environments without human intervention. Furthermore, the system needs to prioritize the safety of the human agents around it. In part, it should traverse dangerous places that humans cannot survive. Exploration of our oceans, disasters created from war, and planets can and do benefit from having autonomous systems. To do this, Autonomous Systems need to identify its environment. From there, motion planning algorithms are applied.

   Whereas other work uses other autonomous systems like humanoids, which will have to adopt different mathematical models to describe their states, our project is different. To see if this plan is feasible, we will simplify the scope to make a two-wheeled robot navigator from the TI-RSLK chassis. This navigator is to be equipped with a LiDAR scanner, the RPLiDAR C1. The goal of this research project is to make an autonomous system that:

1. scan its environment through two different modes
   1. GraphSLAM method
   2. EKF-SLAM method

2. Methods

Here is how we achieve our goals:

• We will recreate its pose and its environment in-memory using the graph-based Simultaneous Localization and Mapping (GraphSLAM) algorithm and Kalman filter based using the multiple LiDAR ICs mentioned before.

• We perform sensor fusion via the extended Kalman Filter (EKF) which will hopefully gather a better estimate of its state without the expected drift from GraphSLAM alone. We intend to use additional sensors like a MARG sensor and a GPS receiver.

• For methods, we will consider using new approaches. I was considering investigating scan matching methods (ICP) as a second point of measurement for EKF-SLAM.

3. Materials

The materials involved include:

Amounts	Name	Description
1	TI-RSLK Chassis	Two-Wheeled Platform
1	RPLiDAR C1	2D LiDAR Scanner
1	ICM-20948	MARG Sensor
1	GPS Receiver	N/A
6	VL53L5CX	SparkFun ToF Imager

4. Online Resources

4.1. Things I've made to explain this stuff

• the CSUN-SPaRA presentation I spoke at to explain my progress

4.2. C library

• The GNU C Library Reference Manual - GNU.org

4.3. Hardware Development

• RPLiDAR C1 Support Page
  • RPLiDAR S&C Series Protocol v2.8
    • what NotebookLM says about the protocol

• Alongside this project, I have a self-made API that I've made in reaction to how the given SDK is confusing to read. At the time of writing (10 Dec 2025), this is a fully-functioning driver but I have high hopes that we can further improve this API.
  • there are no plans to make this platform-agnostic. sorry.
  • accompanying RPLiDAR C1 UART ISR-driven firmware - dnblvr

---

• This collection of links might be a very interesting use of firmware optimization using the already-existing ARM-v4 optimized CMSIS libraries
• PowerPoint Presentation - msp432.pdf
• SIMPLELINK-MSP432-SDK Software development kit (SDK) | TI.com
• using CMSIS-DSP for msp432 system - Google Search
• CMSIS DSP Software Library
• DSP Libraries — Real Time Digital Signal Processing B Term 2024 documentation

---

• This collection of links is in the event that I did want to transition to my own boards instead of the (kindly provided) CSUN-supplied MSP432P401R
• RainerKuemmerle/g2o: g2o: A General Framework for Graph Optimization
  • g2o: A General Framework for Graph Optimization - paper.pdf
    • G2o: A general framework for graph optimization | IEEE Conference Publication | IEEE Xplore
• Prepare build with Eigen release v5.0.0 · RainerKuemmerle/g2o@ef80e64
• i.MX RT106x Crossover Processors for EdgeReady™ Off-The-Shelf ML/AI IoT Edge Compute Solutions - IMXRT1060CEC_SUPPLEMENT.pdf
• IMXRT1060CEC_Rev_4 - IMXRT1060CEC.pdf

4.4. Sensor Fusion with MARG sensor

• SparkFun_ICM-20948_ArduinoLibrary/examples/PortableC/Example999_Portable/Example999_Portable.ino at main · sparkfun/SparkFun_ICM-20948_ArduinoLibrary

4.5. Iterative Closest Point (ICP)

• Iterative Closest Point (ICP) - 5 Minutes with Cyrill - YouTube
• Singular value decomposition - Wikipedia

4.6. GraphSLAM

• Understanding SLAM Using Pose Graph Optimization | Autonomous Navigation, Part 3 - YouTube
• A brief introduction to GraphSLAM | by Shiva Chandrachary | Medium
  • Application of Maximum Likelihood Estimation in GraphSLAM | by Shiva Chandrachary | Medium
  • An application of Numerical Solution to Maximum Likelihood Estimation in GraphSLAM | by Shiva Chandrachary | Medium
• Graph SLAM: From Theory to Implementation | Federico Sarrocco

• The GraphSLAM Algorithm with Applications to Large-Scale Mapping of Urban Structures - Thrun et. al.

• A Tutorial on Graph-Based SLAM - Grisetti et. al.

  • loading times are longer than usual so please be patient!

4.7. EKF-SLAM

• The Extended Kalman Filter: An Interactive Tutorial
• EKF-SLAM A Very Quick Guide - SLAM course.pdf

4.8. RRT*

• Path Planning with A* and RRT | Autonomous Navigation, Part 4 - YouTube
• path finding - rapid exploring random trees - Stack Overflow

5. Dynamic Window Approach (DWA) Algorithm

• An Improved Dynamic Window Approach for Mobile Robot Dynamic Path Planning | IEEE Conference Publication | IEEE Xplore
• I built a visual and interactive DWA path planner in 2D with Pygame – supports obstacle avoidance, real-time replanning, and click-to-set goals : r/robotics
  • DWA_Algorithm_PYTHON/DWA.py at master · EricChen0104/DWA_Algorithm_PYTHON
• AtsushiSakai/PythonRobotics: Python sample codes and textbook for robotics algorithms.
  • Welcome to PythonRobotics’s documentation! — PythonRobotics documentation
• PythonRobotics/PathPlanning/DynamicWindowApproach at master · AtsushiSakai/PythonRobotics
  • PythonRobotics/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py at master · AtsushiSakai/PythonRobotics
    • PythonRobotics/PathPlanning/ClosedLoopRRTStar at master · AtsushiSakai/PythonRobotics
• A Hybrid DWA-MPC Framework for Coordinated Path Planning and Collision Avoidance in Articulated Steering Vehicles | MDPI

6. Checklist

In general, for all tasks, they should follow the same guidelines:

• make the code happen in any IDE or simulator
• simulate it in normal C code in Visual Studio Code, MATLAB, or any other IDE, if possible
  • if simulating in MATLAB, use its C-code converter
• optimize the functions once done, if applicable

6.1. Task 1: 12 Dec 2025 (3 weeks)

• improve motor control and motor tachometer functions
  • perhaps incorporate the ICM-20948 (exclusively I^2^C) to supplement the tachometer-only motion model
• improve GraphSLAM
  • loop closure detection
    • develop auto-steering/local planning using the Dynamic Window Approach (DWA) to prevent direct steering
  • perhaps explore other methods like HECTOR SLAM (will provide links)

6.2. Task 2

• explore EKF-SLAM

6.3. Ongoing Task

• meet every Monday to check on our weekly progress

Monday	Task
14 Dec	1^st^ progress report
21 Dec	2^nd^ progress report give time to decide to stick by the project
28 Dec	3^rd^ progress report