System architecture, algorithms, conclusion & references

Series: Part 1 → Part 2

This part covers the technical implementation: the system architecture from UWB tags to trajectory generation, the planning and control logic, followed by the conclusion and references.

2. System architecture – from tags to trajectory

This section gives a more technical view of the components and data flows.

The robot used in this project is a Clearpath Dingo‑D platform running ROS Noetic and a custom Python navigation node developed by Md Al‑Naim. The key data sources are:

• Front and back Iiwari UWB tags – provide Kalman-filtered x, y positions at 10 Hz via the Iiwari cloud API, with typical noise around ±20 cm.

• Filtered odometry from ROS – provides the robot’s heading (yaw) based on wheel odometry and IMU.

• Floorplan image – a high‑resolution map of the environment, converted into a 1‑metre grid for path planning.

The navigation script brings these inputs together in a compact pipeline. To combine the two UWB tags into a single pose estimate, we use a small module called DualTagPoseEstimator. It takes the front and rear UWB tag positions and fuses them with the robot’s filtered odometry (yaw) to produce a stable estimate of (x, y, heading). The odometry callback provides the robot’s orientation, and the estimator averages the tag positions to compute the centre of the robot. This approach gives us a consistent and clean pose estimate without requiring long code listings in the article.

In addition to the filtering performed by the Iiwari system itself, the navigation node uses a lightweight extended Kalman filter to further stabilise the pose estimate before it is passed to the controller. The internal implementation details are beyond the scope of this article but contribute to smoother and more reliable motion.

A background thread polls the Iiwari API for both tags, passes their positions through DualTagPoseEstimator, and updates the current pose used by the controller. Another part of the node queries all Points of Interest (POIs) from Iiwari, orders them using a nearest‑neighbour heuristic, and plans a multi‑stop route: start → A → B → C.

Each leg of the route is planned on top of the floorplan grid using the classic A* pathfinding algorithm. The path is then simplified and smoothed before it is given to a Pure‑Pursuit controller that converts it into linear and angular velocity commands (cmd_vel). A small Flask‑based dashboard visualises the current pose, path and goals in real time in a browser.

The AI obstacle detection model – a lightweight Python/Pygame-based regression model developed by Premton Canamusa (5G:llä vauhtia digitalisaatioon – Pohjois-Savon liitto, REACT-EU EAKR: A77467) – runs on a server connected via Nokia 5G. When the quadruped robot appears in front of the Dingo, this model recognises the obstacle and sends a pause command through a small TCP interface. The robot stops, waits until the path is clear, and then continues along its planned trajectory.

3. Algorithms and control logic in more detail

This final section gives a closer look at the algorithms for readers who work with robotics, autonomous systems or applied AI.

Map processing and path planning

The floorplan is stored as a grayscale image and converted into a grid where each cell represents a one‑metre square. Dark pixels become obstacles and bright pixels remain free space. We use an 8‑connected A* planner (four cardinal and four diagonal moves) with a slightly higher cost for diagonal steps. If A* cannot find a path for a leg, that segment is skipped.

The raw A* output often contains many small zigzags. To reduce unnecessary waypoints, we apply a randomised pruning step that repeatedly removes intermediate points if a straight line between two points remains collision‑free. After pruning, we fit a Catmull–Rom-style spline using SciPy to generate a smooth, continuous trajectory in world coordinates. In practice this removes the visible “zig-zag” behaviour of following a raw grid-based path and makes the motion look more natural.

If the robot later deviates more than 2.5 metres from this path, the system automatically triggers online re-planning for the current leg. In normal runs this re-planning is rare and mainly occurs if some external factor, such as a person nudging the platform, moves the robot significantly off course. This keeps the robot robust against UWB noise, wheel slip and occasional disturbances.

Pure‑Pursuit controller and three‑stage docking

The heart of the motion control is a customised Pure‑Pursuit controller. Rather than following the path point by point, the controller always looks ahead along the path by a variable distance and aims the robot towards that look‑ahead point. In our implementation:

• The look‑ahead distance is reduced near the goal for precise docking.

• A three‑stage “docking” profile (around 2.0 m, 1.0 m and 0.5 m from the goal) gradually lowers speed and tightens the allowed turning rate.

• At the start of each leg, the robot performs an initial rotation to align itself with the first path segment before driving forward.

In code, each control cycle computes a new Twist:

cmd, target_point = self.controller.twist(

path=current_path,

pos=(x, y),

yaw=yaw

)

self.cmd_pub.publish(cmd)

Linear speed is capped at 0.35 m/s and angular speed at 0.9 rad/s. These values offered a good compromise between smooth motion and safety in the Savonia test environment.

Safety, 5G connectivity and logging

For safety and analysis, several additional features were added:

• A small TCP server lets us pause and resume motion from an external application.

• The script can log pose, target point and controller outputs to a file for later study and teaching.

• The AI obstacle model communicates over Nokia 5G, which provides low‑latency control. The same model running over Wi‑Fi was noticeably slower and caused visible delays in robot reactions.

Together, these building blocks form a UWB‑centric indoor navigation stack that is technically robust but still compact enough to be reused in future research and teaching.

Conclusion

This project demonstrates that Ultra-Wideband positioning can serve as a reliable foundation for indoor robot navigation, especially in environments where LiDAR-based SLAM is difficult to maintain across multiple floors or buildings. By combining Iiwari UWB localisation with lightweight path planning, Pure-Pursuit control, and 5G-connected AI obstacle detection, we were able to create a navigation pipeline that is both robust and easy to transfer between different spaces.

The results show that UWB provides stable, building-scale positioning, while 5G connectivity enables responsive safety behaviour even in dynamic environments. The system remains compact, understandable, and suitable for research and teaching, offering a strong basis for future work in autonomous robots, campus-wide asset tracking, and hybrid localisation strategies.

---

Authors info: Md Al-Naim, md.al-naim@savonia.fi, Asmo Jakorinne, asmo.jakorinne@savonia.fi

Supervised by: Premton Canamusa (Savonia UAS), Asmo Jakorinne (Savonia UAS), and Teemu Lätti (Iiwari Tracking Solutions)

Developed by: Md Al-Naim (Savonia DigiCenter North Savo)

---

References:

5G:llä vauhtia digitalisaatioon’, Pohjois-Savon liitto, REACT-EU EAKR: A77467.

Part 1 of 2 — Motivation and UWB rationale

Project: Automaatio- ja Tekoäly Tiedoksi -hanke (AuToTIE), Pohjois-Savon Liitto, EAKR

Robot Platform: Clearpath Dingo-D

Location: Savonia UAS DigiCenter North Savo

---