betaFeedback

WorksComputer Science

Technical Report of Auriga's 1:10 Scale Autonomous Vehicle Robot

Abstract

This report presents the design and implementation of Auriga Robotics' 1:10 scale autonomous vehicle, developed at Shahid Beheshti University's Robotics and Intelligent Automation Laboratory. The platform integrates a custom mechanical chassis, a distributed embedded system based on ESP32 microcontrollers, and a Jetson Orin NX module for high-level perception and control. The embedded layer manages sensor interfacing, real-time data acquisition, and actuator control, while the Jetson handles environment perception through deep-learning–based lane, sign, and traffic light detection, as well as decision-making and trajectory control. System communication is achieved via structured UART links, ensuring synchronized operation between perception and control subsystems. The vehicle's software stack incorporates ROS, modular firmware design, and multiple control algorithms—Stanley, PID, and Pure Pursuit—to achieve stable autonomous navigation in structured driving scenarios. The overall system demonstrates robust performance and reliability in urban-like environments, serving as a modular research platform for future autonomous driving studies.

Introduction

Autonomous driving represents one of the most advanced integrations of control theory and computer vision, and is of high importance in self-driving cars in the current age. To explore these concepts in a manageable and scalable way, Auriga Robotics at Shahid Beheshti University developed a 1:10 scale autonomous vehicle designed to perform perception, decision-making, and control in real time. The project was carried out as part of the team’s ongoing research in intelligent robotics and autonomous navigation systems, with the goal of building a modular platform suitable for academic experimentation. An image of the robot is attached in Figure 1.

Auriga’s 1:10 scale autonomous vehicle

Figure 1. Auriga’s 1:10 scale autonomous vehicle.

The primary objective of the project was to design a complete autonomous driving stack, that includes hardware, embedded firmware, and high-level perception algorithms. The vehicle was required to detect and follow lanes, recognize traffic signs and lights, and maintain stable longitudinal and lateral control under various driving scenarios. To achieve this, the system was divided into three processing layers: low-level sensor integration and actuation was handled by two ESP32 microcontrollers (called CONTROL and ZERO), and high-level perception and decision-making executed on an NVIDIA Jetson Orin NX mini-computer, in which all three units maintained stable communication with each other. On the software side, implemented on the Jetson Orin NX, the architecture was built around ROS (Robot Operating System) [4] to allow modular algorithm development, parallel debugging, and integration of computer vision and control pipelines.

The remainder of this report is organized as follows. § 2 presents the hardware design, including the mechanical structure, sensors, and electronics. § 3 details the embedded and high-level software architecture, covering sensor integration, communication between the processing units, and the Robot Operating System. § 4 discusses the perception and computer-vision algorithms, together with the training strategy and experimental validation of the lane-detection model. § 5 covers vehicle localization, navigation, and the control algorithms used for lane-keeping, decision-making, parking, and intersection handling.

This report describes the 2025 platform of Team Auriga, which builds and improves on our earlier 2024 vehicle [1].

Hardware Description

Mechanical Design

The mechanical structure of the autonomous vehicle was fully custom-designed to balance robustness, modularity, and scalability. It consists of three main parts: the chassis, the steering and drivetrain system, and the vehicle body. Each component was designed and fabricated to ensure reliable operation, efficient integration with electronic systems, and easy maintenance.

Car Chassis. The chassis of the vehicle is designed at a 1:10 scale relative to a full-sized car. It consists of an aluminum base plate and a 3D-printed plastic upper layer. The chassis layout was designed to accommodate the DC motor, servo motor, rotary encoder, electronic control boards, and other system components. The primary motivation behind developing a custom chassis, rather than using a prebuilt model, was to integrate specific mechanical features required for improved performance. These include the addition of a more powerful DC motor for greater torque and speed, an increased ground clearance to allow traversal over small obstacles, and custom mounting points for sensors and electronics. The vehicle dimensions are mentioned in Table 1.

DimensionValue (cm)
Wheelbase LL (front axle to rear axle)25.0±1.025.0 \pm1.0
Vehicle width21.5±0.521.5 \pm0.5
Overall length (front bumper to rear bumper)39.5±0.539.5 \pm0.5
Height29.5±0.529.5 \pm0.5

Table 1. Vehicle dimensions.

Steering and Drivetrain System. The vehicle employs a single differential rear-wheel drive system, where propulsion is provided by a DC motor mounted on the rear axle. The steering mechanism follows an Ackermann configuration [25] and is actuated by a servo motor. This arrangement enables the vehicle to achieve appropriate turning angles and efficiently cover a wide range of trajectories.

Car Body. The robot’s body consists of two main parts: the front bumper, which houses the headlights and front distance sensors, and the rear section, which includes the taillights and the camera mount along with the rear distance sensors. Both parts were designed and fabricated using 3D printing to ensure precision fitting and lightweight structure.

Electrical Design

The electrical system of Auriga’s autonomous vehicle is centered around a main control board and three auxiliary side boards. Two of these side boards are positioned at the front and roof of the vehicle, while the third board is dedicated to servo motor and DC motor data acquisition. This separation minimizes high-current noise coupling from the motor driver into sensitive digital communication lines. The side boards facilitate organized routing of sensor wires, ensuring efficient connections and reduced signal interference. Power is supplied by a 5000 mAh 4-cell lithium-polymer (Li-Po) battery, which delivers energy to both the drive and electronic subsystems, maintaining stable operation during extended runs. Voltage regulation is achieved through a combination of switching and linear regulators that maintain all subsystem voltages within safe operational limits. Over-voltage and over-current protection circuits are included to enhance reliability and prevent component damage. Several indicator LEDs are placed around the board to provide visual confirmation of correct voltage levels and regulator status, simplifying debugging and system validation.

Control Board. As shown in Figure 2, the main control board integrates two ESP32 microcontrollers [2]—ZERO and CONTROL—that communicate through a UART link. ZERO MCU handles sensor data acquisition and low-level control tasks, while CONTROL MCU manages high-level communication with the onboard computer (NVIDIA Jetson Orin NX [3]) and actuation, enabling synchronized data exchange and command handling.

Top layer of the control board

(a) Top layer of the control board.

Bottom layer of the control board

(b) Bottom layer of the control board.

Figure 2. Control board PCB layout showing (a) the top layer and (b) the bottom layer.

Power for the control board and peripherals is provided by both switching and linear regulators: two XL4016 switching regulators generate 12 V and 5 V outputs for the Jetson Orin NX, servo motor, encoder, and sensors, while two AMS1117-3.3 V and one AMS1117-5 V linear regulators provide stable voltage for logic-level components.

DC Motor Driver Board. For DC motor actuation, an L298 dual H-bridge driver [9] is employed, delivering sufficient current capacity for precise speed control. The printed circuit board (PCB) layout and assembly were designed and fabricated by the team, resulting in a compact and reliable architecture that integrates all power and control subsystems efficiently. The PCB design for the driver board is shown in Figure 3.

DC motor driver board PCB layout showing the top (left) and bottom (right) layers

Figure 3. DC motor driver board PCB layout showing the top (left) and bottom (right) layers.

Sensors. Range measurement is handled by VL53L1X time-of-flight (ToF) laser-ranging sensors [6] mounted on the front, rear, and sides of the vehicle, with a total of 12 ToF sensors. To avoid I²C [10] interference across these sensors, they are all multiplexed and connected to the ZERO MCU. These sensors provide high-speed and accurate distance measurements for obstacle detection, complementing camera-based perception for enhanced situational awareness.

Orientation is measured by a single compass module—the CMPS14 [8]—which provides a tilt-compensated heading for real-time localization. An MPU-9250 [7] was developed and tested separately on a breadboard and was not used on the robot. The CMPS14 is read by the ZERO MCU via the I²C protocol.

The DC motor includes an optical rotary encoder that provides wheel speed and position feedback, enabling precise closed-loop control of longitudinal motion.

To enhance the visual feedback of the autonomous vehicle and provide a more intuitive representation of its actions, a string of WS2812B Neopixel LEDs is integrated into the system. These LEDs are used to indicate various behaviors of the vehicle, such as turning left or right, scanning the surrounding environment, and detecting obstacles.

Embedded Design, High-Level Software Architecture, and Robot Operating System (ROS)

To achieve the required computational performance and maintain synchronized control across all subsystems, the vehicle’s architecture incorporates three distinct processing units: two ESP32-based embedded controllers, named ZERO and CONTROL, and an NVIDIA Jetson Orin NX module. Each unit is assigned specific responsibilities within the perception–control hierarchy to ensure modularity, determinism, and real-time responsiveness. The Jetson Orin NX serves as the high-level computing platform, running perception algorithms, decision-making routines, and overall control logic. The CONTROL ESP32 functions as the intermediary layer, managing actuator commands, multiplexing, and structured communication with both the Jetson and the ZERO module. The ZERO ESP32 handles all low-level sensor acquisition tasks, including ToF, compass, and encoder data collection, before transmitting the processed data to the CONTROL unit for integration and forwarding to the Jetson.

UnitProcessorPrimary Function
Jetson Orin NXARM Cortex-A78AE + GPUPerception and control logic
CONTROL ESP32Xtensa LX6 Dual-CoreIntermediary communication layer, actuator interfacing
ZERO ESP32Xtensa LX6 Dual-CoreSensor acquisition, low-level actuation

Table 2. Overview of the three processing units and their primary responsibilities.

Embedded System Software Architecture

The embedded subsystem forms the low-level operational layer of Auriga’s autonomous vehicle. It is responsible for acquiring sensor data, executing actuator commands, and maintaining synchronized communication between all processing units. The subsystem consists of two ESP32 microcontrollers, referred to as CONTROL and ZERO, that cooperate with the Jetson Orin NX computer. The CONTROL unit interfaces directly with the Jetson, relaying processed sensor data from ZERO and applying actuator commands such as steering and motor control. The ZERO unit manages all time-critical sensor measurements and DC motor control, providing a continuous data stream to CONTROL over UART. Together, these two microcontrollers form a distributed real-time embedded network bridging the vehicle’s physical layer and the Jetson’s high-level control layer.

Firmware Architecture

Both ESP32 units run a FreeRTOS-based firmware [5] divided into concurrent tasks, each handling an independent subsystem. On the ZERO module, tasks include ToF sensor polling, compass and encoder sampling, and UART data transmission. CONTROL executes complementary tasks for UART reception, servo actuation, lighting and signaling control (NeoPixels and buzzer), and I²C multiplexer channel selection. Inter-task communication relies on FreeRTOS queues to ensure deterministic timing and non-blocking operation. Both cores of each ESP32 are utilized, dedicating time-critical acquisition and actuation tasks to one core while communication and housekeeping tasks run on the other. Figure 4 illustrates the steady-state organization of dataflow and task scheduling among CONTROL, ZERO, and Jetson. Task core numbers have been labeled in the form TASK-{core#}/task_name, and every arrow from one node to the other represents the data being transmitted, and the direction of transmission. The Robot Operating System architecture for perception and control has been abstracted away as perception pipeline and control in the figure, and will be covered in depth in § 3.2.

Embedded firmware steady-state architecture

Figure 4. Embedded firmware steady-state architecture.

MCUTaskCorePrio.Function / period
ZEROSensor read and transmit010Reads all sensor data, builds and sends packet p1.
ZEROInstruction receiver010Parses p3 (motor command, ToF bitmask); 1-tick polling
ZEROToF reader113Sweeps the 12 ToF sensors every ∼41\sim41 ms; pushes results to a queue
ZEROButton monitor115Debounces the motor cut-off button (ISR-flagged) (this task was not included in Figure 4 but is implemented).
CONTROLZERO→Jetson forwarder010Receives p1 from ZERO, checks its integrity and forwards it to Jetson
CONTROLNeoPixel handler110Applies the light mode; 5-tick polling, 500 ms blink toggle
CONTROLJetson receiver114Validates p2, drives the servo and queues NeoPixel data to NeoPixel handler task, relays p3 to ZERO

Table 3. FreeRTOS tasks on the two ESP32 units (priority: higher runs first).

Three queues carry data between tasks (ToF values, ToF bitmask, light mode), and a mutex serialises access to the shared I²C bus between the ToF reader and the compass read. Time-critical events are handled in interrupt service routines placed in IRAM: encoder edges (ZERO), the Jetson over-current signal (ZERO), the motor cut-off button (ZERO), and the ToF multiplexer step (CONTROL).

Sensor and Actuator Integration

As shown in Figure 4, during steady-state operation the embedded layer continuously acquires and distributes data from fifteen data channels: twelve VL53L1X time-of-flight (ToF) sensors arranged around the vehicle perimeter, one rotary encoder for wheel speed feedback, a CMPS14 tilt-compensated magnetic compass, and the time elapsed since Jetson handshake was made.

To prevent I2^2C address conflicts, the VL53L1X modules are multiplexed through a hardware selector controlled by the CONTROL ESP32. The ZERO ESP32 requests channel selection and sequentially samples the active sensor, packaging the range data for transmission. The CMPS14 provides a direct heading measurement, while the encoder delivers rotational velocity used for odometry and closed-loop motor control.

All twelve VL53L1X sensors share the default I2^2C address, so only one is connected to the bus at a time. After each reading, ZERO sends a 100 µs pulse on a dedicated line; an interrupt on CONTROL advances a channel counter (modulo 12) and drives the four multiplexer select lines. The sensors run in long-distance mode with a 33 ms timing budget and a 40 ms inter-measurement period, giving a full-sweep of about half a second. Because packets are sent faster than sweeps complete, the last ToF values are repeated until a new sweep arrives. The Jetson can disable individual sensors through a 12-bit bitmask in p2; disabled or failed sensors are reported as −1.

The rotary encoder is decoded in x4 mode: an interrupt on every edge of both channels reads both pin levels and updates a signed position counter, giving 1600 counts per revolution. Speed is obtained from the count difference between consecutive packets and the measured time step.

The steering servo is driven at 50 Hz with a pulse width of 1000–1815 µs, mapped linearly from the 8-bit command u∈[0,255]u \in[0, 255] (3.2 µs per step, using a 16-bit PWM timer). The DC motor driver receives an enable line, a direction line, and a 500 Hz, 10-bit PWM signal; the sign of the motor command sets the direction.

All sensor outputs are collected and structured into a unified serial data packet, transmitted from ZERO to CONTROL via UART. CONTROL verifies packet integrity, forwards the data to the Jetson Orin NX, and awaits actuator commands. These commands are returned to CONTROL, containing servo steering angles, motor drive values, and LED states. CONTROL applies its own actuator commands locally and relays motor control commands to ZERO, which drives the DC motor accordingly. The details of packet structure and synchronization protocol are discussed in section 3.1.5.

Sensor Fusion for Heading Estimation

Note: the MPU-9250 was not used on the 2025 vehicle. Due to integration deadlines, the robot used only the CMPS14 heading during live track testing. The sensor fusion described below was developed and validated offline on a breadboard after the laboratory closures, is independent of the robot platform, and is the intended architecture for future platforms.

Accurate heading estimation is required for stable vehicle control and localization. The test platform was designed to use two complementary modules: an MPU-9250 IMU (3-axis accelerometer, 3-axis gyroscope, 3-axis magnetometer) and a CMPS14 tilt-compensated compass (BNO080-based) which provides a fused, tilt-compensated heading output. The relevant module specifications and internal fusion capability are documented in the manufacturers’ datasheets.

A two-stage fusion pipeline was implemented: (1) internal Kalman filtering [11, 12] on MPU-9250 measurements to produce a drift-compensated yaw estimate ψMPU\psi_{\mathrm{MPU}} and (2) a rate-dependent blend between ψMPU\psi_{\mathrm{MPU}} and the CMPS14 heading ψCMPS\psi_{\mathrm{CMPS}}.

Stage 1 — Heading estimation using the MPU-9250 data. The MPU-9250 sensor only provides raw vector readings for the relative acceleration a⃗\vec{a}, magnetic field strength m⃗\vec{m}, and the angular velocity ω⃗\vec{\omega}. Here, we combine the three to get an estimate on ψ\psi, the heading angle.

Definitions. Define the world orthonormal vector basis BW={x^W,y^W,z^W}\mathcal{B}_{W} = \{\hat{x}_{W}, \hat{y}_{W}, \hat{z}_{W}\}, where x^W\hat{x}_{W} points towards the north magnetic pole, y^W\hat{y}_{W} points to the magnetic west, and z^W\hat{z}_{W} points upwards from the horizontal plane. This basis is universal and fixed. Parallel to the fixed basis, define the time-varying body orthonormal basis BB={x^B,y^B,z^B}\mathcal{B}_{B} = \{\hat{x}_{B}, \hat{y}_{B}, \hat{z}_{B}\}, where x^B\hat{x}_{B} points to the front of the vehicle, y^B\hat{y}_{B} points to the left of the vehicle, and z^B\hat{z}_{B} points upwards with respect to the vehicle. The basis system is shown in Figure 5.

World $\mathcal{B}_W$ and body $\mathcal{B}_B$ coordinate systems

Figure 5. World BW\mathcal{B}_W and body BB\mathcal{B}_B coordinate systems.

For any pure vector v⃗\vec{v}, let the representation in the world and body coordinates be v⃗W\vec{v}^{W} and v⃗B\vec{v}^{B} respectively. These two representations are related to each other with a rotation matrix RWB\mathbf{R}_{WB}, such that

v⃗W=RWBv⃗B,v⃗B=RWBTv⃗W.(1)\vec{v}^{W} = \mathbf{R}_{WB}\vec{v}^{B}, \qquad\vec{v}^{B} = \mathbf{R}_{WB}^{T}\vec{v}^{W}. \tag*{(1)}

Where RWB\mathbf{R}_{WB} encodes the full orientation of the robot. Here, since the robot only moves in the horizontal plane, we only care about the heading angle and a full orientation state of the robot is not computed. The heading angle ψ\psi is defined as the angle between the horizontal projection (projection onto the plane spanned by {x^W,y^W}\{\hat{x}_W,\hat{y}_W\}) of x^B\hat{x}_B and the magnetic north axis x^W\hat{x}_W, measured clockwise about z^W\hat{z}_W:

ψ=atan2⁡(−x^B⋅y^W,x^B⋅x^W)=atan2⁡(−[RWB]21,[RWB]11),ψ∈[0,2π),(2)\psi= \operatorname{atan2}(-\hat{x}_B \cdot\hat{y}_W,\hat{x}_B \cdot\hat{x}_W) = \operatorname{atan2}(-[\mathbf{R}_{WB}]_{21},[\mathbf{R}_{WB}]_{11}), \qquad\psi\in[0,2\pi), \tag*{(2)}

Additionally, we define the operators PH\mathbf{P}_{\mathcal{H}} for projecting any vector to the horizontal plane H=span⁡(x^W,y^W)\mathcal{H} = \operatorname{span}(\hat{x}_W,\hat{y}_W) in both coordinate systems:

PHB=1−z^WB(z^WB)T,PHW=1−z^WW(z^WW)T.(3)\mathbf{P}_{\mathcal{H}}^{B} = \mathbf{1} - \hat{z}_W^{B}(\hat{z}_W^{B})^{T}, \qquad\mathbf{P}_{\mathcal{H}}^{W} = \mathbf{1} - \hat{z}_W^{W}(\hat{z}_W^{W})^{T}. \tag*{(3)}

Where 1\mathbf{1} is the identity matrix. Therefore for any vector v⃗\vec{v}, let v⃗H=PHv⃗\vec{v}_{\mathcal{H}} = \mathbf{P}_{\mathcal{H}}\vec{v} be the vector projected onto the horizontal plane.

Accelerometer. The accelerometer on the MPU-9250 measures the relative acceleration vector a⃗m=a⃗real−g⃗\vec{a}_m = \vec{a}_{\mathrm{real}} - \vec{g} in the body coordinate (the subscript mm denotes the measured result: the final value the sensor gives), where g⃗\vec{g} is the gravitational acceleration. Therefore, the ideal readings are of the form

a⃗mB=RWBTa⃗realW−g⃗B.(4)\vec{a}_m^{B} = \mathbf{R}_{WB}^{T}\vec{a}_{\mathrm{real}}^{W} - \vec{g}^{B}. \tag*{(4)}

For the stationary robot, from Eq. (4) we get a⃗mB≈−g⃗B\vec{a}_m^{B} \approx-\vec{g}^{B}. The real acceleration vector a⃗real(t)\vec{a}_{\mathrm{real}}(t) is fast-changing in time, hence for the discretized model of the accelerometer a⃗mB[n]\vec{a}_m^{B}[n], a low-pass filter is implemented to arrive at an estimate of g⃗B\vec{g}^{B} in the form

−g⃗B[n]=−(1−α)g⃗B[n−1]+αa⃗mB[n],α∈[0,1].(5)-\vec{g}^{B}[n] = -(1-\alpha)\vec{g}^{B}[n-1] + \alpha\vec{a}_m^{B}[n], \qquad\alpha\in[0,1]. \tag*{(5)}

The value α=0.05\alpha= 0.05 was chosen for this platform. From Eq. (5), we get an estimate of the universal up direction z^W\hat{z}_W, expressed in the body coordinates (Eq. (6)).

z^WB=−g⃗B∥g⃗B∥(6)\hat{z}_W^{B} = \frac{-\vec{g}^{B}}{\lVert\vec{g}^{B}\rVert} \tag*{(6)}

From this, we can find the horizontal plane in the body coordinates. Define the forward direction unit vector f^\hat{\mathbf f}, which is the projection of the body front unit vector x^B\hat{\mathbf x}_B onto the horizontal plane, calculated as

f⃗B=x^BB−(x^BB⋅z^WB)z^WB=[1−(zW,xB)2−zW,xBzW,yB−zW,xBzW,zB]Body,f^B=f⃗B∥f⃗B∥,(7)\vec{f}^{B}=\hat{x}^{B}_{B}-\left(\hat{x}^{B}_{B}\cdot\hat{z}^{B}_{W}\right)\hat{z}^{B}_{W}=\left[\begin{array}{c}1-\left(z^{B}_{W,x}\right)^2\\-z^{B}_{W,x}z^{B}_{W,y}\\-z^{B}_{W,x}z^{B}_{W,z}\end{array}\right]_{\mathrm{Body}},\qquad\hat{f}^{B}=\frac{\vec{f}^{B}}{\lVert\vec{f}^{B}\rVert}, \tag*{(7)}

and the left direction unit vector in the horizontal plane is then simply obtained by

l^B=z^WB×f^B=[zW,yBfzB−zW,zBfyBzW,zBfxB−zW,xBfzBzW,xBfyB−zW,yBfxB]Body.(8)\hat{l}^{B}=\hat{z}^{B}_{W}\times\hat{f}^{B}=\left[\begin{array}{c}z^{B}_{W,y}f^{B}_{z}-z^{B}_{W,z}f^{B}_{y}\\z^{B}_{W,z}f^{B}_{x}-z^{B}_{W,x}f^{B}_{z}\\z^{B}_{W,x}f^{B}_{y}-z^{B}_{W,y}f^{B}_{x}\end{array}\right]_{\mathrm{Body}}. \tag*{(8)}

Gyroscope. The gyroscope measures the angular velocity vector ω⃗B\vec{\omega}^{B} in the body coordinates. The heading rate change is

−dψdt=ω⃗W⋅z^WW=(ω⃗B)TRWBTRWBz^WB=ω⃗B⋅z^WB.(9)-\frac{d\psi}{dt}=\vec{\omega}^{W}\cdot\hat{z}^{W}_{W}=\left(\vec{\omega}^{B}\right)^{T}R^{T}_{WB}R_{WB}\hat{z}^{B}_{W}=\vec{\omega}^{B}\cdot\hat{z}^{B}_{W}. \tag*{(9)}

Projecting onto the estimated vertical rather than the board’s up axis (z^B\hat{z}_{B}) keeps the yaw rate correct when the vehicle pitches or rolls.

The chip gives a 16-bit integer per direction. The conversion factor, specified in the datasheet, is S=65.5S=65.5 LSB/(∘^\circ/s). Discretizing Eq. (9), the gyroscope output is the noisy, biased measurement

ω⃗mB[n]=ω⃗B[n]+b⃗B+η⃗gB[n],(10)\vec{\omega}^{B}_{m}[n]=\vec{\omega}^{B}[n]+\vec{b}^{B}+\vec{\eta}^{B}_{g}[n], \tag*{(10)}

with slowly varying bias b⃗\vec{b} and white noise η⃗g\vec{\eta}_{g}. A startup average b⃗B[0]\vec{b}^{B}[0] of 200 stationary samples is subtracted, giving ω⃗mB[n]−b⃗B[0]\vec{\omega}^{B}_{m}[n]-\vec{b}^{B}[0] as the measured angular velocity. The measured heading rate is therefore

ψ˙gyro[n]=−(ω⃗B[n]−b⃗B[0])⋅z^WB[n],(11)\dot{\psi}_{\mathrm{gyro}}[n]=-\left(\vec{\omega}^{B}[n]-\vec{b}^{B}[0]\right)\cdot\hat{z}^{B}_{W}[n], \tag*{(11)}

for small nn near the robot startup (the update rule for b⃗\vec{b} is discussed in Stage 2). Here, z^WB[n]\hat{z}^{B}_{W}[n] comes from Eq. (6). Integrating with the measured step Δt[n]\Delta t[n] (about 10 ms, calculated in each iteration) gives

ψ[n]=wrap⁡[0,2π)(ψ[n−1]+ψ˙gyro[n]Δt[n]),(12)\psi[n]=\operatorname{wrap}_{[0,2\pi)}\left(\psi[n-1]+\dot{\psi}_{\mathrm{gyro}}[n]\Delta t[n]\right), \tag*{(12)}

Where wrap⁡I(⋅)\operatorname{wrap}_{I}(\cdot) maps any angle into the principal interval II. Hence, the gyroscope gives a final noisy and biased estimate ψgyro\psi_{\mathrm{gyro}}.

Magnetometer. The magnetometer sensor is useful for measuring Earth’s magnetic field, which provides a universally fixed coordinate system. The magnetometer measurement model is

m⃗mB=ΓM⃗⊕B−O⃗B,(13)\vec{m}^{B}_{m}=\Gamma\vec{M}^{B}_{\oplus}-\vec{O}^{B}, \tag*{(13)}

where M⃗⊕\vec{M}_{\oplus} is the Earth’s magnetic field, O⃗\vec{O} is the magnetic offset vector, and Γ\Gamma is the magnetic field distortion matrix. The methods required to extract M⃗⊕\vec{M}_{\oplus} from m⃗m\vec{m}_{m} are discussed in Stage 4. For the ideal magnetometer with m⃗m=M⃗⊕\vec{m}_{m}=\vec{M}_{\oplus}, we can get a direct estimate on the heading angle.

Decomposing Earth’s magnetic field as M⃗⊕W=M⃗⊕,HW+MzWz^WW\vec{M}^{W}_{\oplus}=\vec{M}^{W}_{\oplus,H}+M^{W}_{z}\hat{z}^{W}_{W} (Eq. (3)), we have

M⃗⊕,HW=PHWM⃗⊕=PHWRWBm⃗mB,(14)\vec{M}^{W}_{\oplus,H}=P^{W}_{H}\vec{M}_{\oplus}=P^{W}_{H}R_{WB}\vec{m}^{B}_{m}, \tag*{(14)}

and since PHWRWB=RWBPHBP^{W}_{H}R_{WB}=R_{WB}P^{B}_{H}, we get

M⃗⊕,HW=RWBm⃗m,HB.(15)\vec{M}^{W}_{\oplus,H}=R_{WB}\vec{m}^{B}_{m,H}. \tag*{(15)}

This is the equation for tilt-compensation. Since, by definition,

M⃗⊕,HW∥M⃗⊕,HW∥=x^WW.(16)\frac{\vec{M}^{W}_{\oplus,H}}{\lVert\vec{M}^{W}_{\oplus,H}\rVert}=\hat{x}^{W}_{W}. \tag*{(16)}

and by definition for the heading angle, since

f^W=cos⁡ψx^WW−sin⁡ψy^WW,(17)\hat{\mathbf{f}}^{W} = \cos\psi\hat{\mathbf{x}}^{W}_{W} - \sin\psi\hat{\mathbf{y}}^{W}_{W}, \tag*{(17)}

we conclude that the heading angle is given by

ψ=atan2⁡(mm,HB⋅i^B,mm,HB⋅f^B),(18)\psi= \operatorname{atan2}\left(\mathbf{m}^{B}_{m,\mathcal{H}} \cdot\hat{\mathbf{i}}^{B}, \mathbf{m}^{B}_{m,\mathcal{H}} \cdot\hat{\mathbf{f}}^{B}\right), \tag*{(18)}

where mm,HB=PHBmmB\mathbf{m}^{B}_{m,\mathcal{H}} = P^{B}_{\mathcal{H}}\mathbf{m}^{B}_{m}. Hence, we get a direct estimate on the heading angle ψmag\psi_{\mathrm{mag}}.

Stage 2 — Kalman filtering. Integrating the measured heading rate alone (Eq. (12)) is dead reckoning: the white noise ηgB\boldsymbol{\eta}^{B}_{g} accumulates as a random walk in ψ\psi, and any bias left over after subtracting bB[0]\mathbf{b}^{B}[0] accumulates as a linear drift. The magnetometer heading ψmag\psi_{\mathrm{mag}} (Eq. (18)) has the opposite property: it does not drift, but it is noisy and is disturbed by steel and motor currents. Since the two errors are complementary, the two heading sources are fused with a Kalman filter [11, 12, 13], which is closely related to a complementary filter, except that the gain is computed from the noise statistics instead of being fixed.

State and model. The state is

x⃗[n]=[ψ[n]bψ[n]],(19)\vec{x}[n] = \begin{bmatrix} \psi[n] \\ b_{\psi}[n] \end{bmatrix}, \tag*{(19)}

where bψ[n]b_{\psi}[n] is the residual heading-rate bias that remains after the startup average b⃗B[0]\vec{b}^{B}[0] has been removed in Eq. (11). The true heading rate is therefore ψ˙gyro[n]−bψ[n]\dot{\psi}_{\mathrm{gyro}}[n] - b_{\psi}[n], where ψ˙gyro[n]\dot{\psi}_{\mathrm{gyro}}[n] is the measured heading rate of Eq. (11). The measured ψ˙gyro[n]\dot{\psi}_{\mathrm{gyro}}[n] acts as a known input to the model, and the residual bias is modeled as a random walk:

x⃗[n]=[1−Δt[n]01]⏟F[n]x⃗[n−1]+[Δt[n]0]⏟G[n]ψ˙gyro[n]+w⃗[n],w⃗[n]∼N(0⃗,Q[n]).(20)\vec{x}[n] = \underbrace{\begin{bmatrix} 1 & -\Delta t[n] \\ 0 & 1 \end{bmatrix}}_{F[n]} \vec{x}[n-1] + \underbrace{\begin{bmatrix} \Delta t[n] \\ 0 \end{bmatrix}}_{G[n]} \dot{\psi}_{\mathrm{gyro}}[n] + \vec{w}[n], \qquad\vec{w}[n] \sim\mathcal{N}\left(\vec{0},Q[n]\right). \tag*{(20)}

These correspond to the continuous-time update rules of ψ˙(t)=ψ˙gyro(t)−b(t)+wψ(t)\dot{\psi}(t) = \dot{\psi}_{\mathrm{gyro}}(t) - b(t) + w_{\psi}(t) and b˙(t)=wbψ(t)\dot{b}(t) = w_{b_{\psi}}(t). The bias is only affected by noise in this model. The first row of Eq. (20) reads ψ[n]=ψ[n−1]+(ψ˙gyro[n]−bψ[n−1])Δt[n]\psi[n] = \psi[n-1] + (\dot{\psi}_{\mathrm{gyro}}[n] - b_{\psi}[n-1])\Delta t[n], which is Eq. (12) with the bias removed, and the second row says bψb_{\psi} stays constant between steps. The process noise is Q[n]=diag⁡(qψΔt[n], qbΔt[n])Q[n] = \operatorname{diag}\left(q_{\psi}\Delta t[n],\ q_{b}\Delta t[n]\right), where qψq_{\psi} accounts for the gyroscope noise and integration error in ψ\psi, and qbq_{b} is the rate at which the bias wanders. The magnetometer heading is the measurement:

ψmag[n]=Hx⃗[n]+v[n],H=[10],v[n]∼N(0,R[n]).(21)\psi_{\mathrm{mag}}[n] = H\vec{x}[n] + v[n], \qquad H = \begin{bmatrix} 1 & 0 \end{bmatrix}, \qquad v[n] \sim\mathcal{N}\left(0,R[n]\right). \tag*{(21)}

Here ψmag[n]\psi_{\mathrm{mag}}[n] is Eq. (18) evaluated with the calibrated magnetic vector m⃗cB\vec{m}^{B}_{c} of Stage 4 in place of m⃗mB\vec{m}^{B}_{m}. The accelerometer is neither an input nor a measurement of the filter. It only provides z^WB\hat{\mathbf{z}}^{B}_{W} (Eq. (6)), which is used in ψ˙gyro[n]\dot{\psi}_{\mathrm{gyro}}[n] and in the tilt compensation of ψmag[n]\psi_{\mathrm{mag}}[n].

From here, for any quantity uu (vector or not) let u^[n∣n−1]\hat{u}[n|n-1] be the quantity estimated at step nn using the data up to n−1n-1 (the prediction section), and let u^[n∣n]\hat{u}[n|n] be the updated prediction after acquiring the magnetometer heading estimate, ψmag\psi_{\mathrm{mag}}. Furthermore, if uu is the true reading, define u~=u−u^\tilde{u} = u - \hat{u} as the error in the quantity uu.

Prediction. Let P=E[x⃗~x⃗~T]P = \mathbb{E}\left[\tilde{\vec{x}}\tilde{\vec{x}}^{T}\right] be the 2×22 \times2 covariance matrix. The prediction runs once per gyroscope sample, whether or not a magnetometer sample is available:

x⃗^[n∣n−1]=F[n]x⃗^[n−1∣n−1]+G[n]ψ˙gyro[n],(22)\hat{\vec{x}}[n|n-1] = F[n]\hat{\vec{x}}[n-1|n-1] + G[n]\dot{\psi}_{\mathrm{gyro}}[n], \tag*{(22)}
P[n∣n−1]=F[n]P[n−1∣n−1]F[n]T+Q[n],(23)P[n|n-1] = F[n]P[n-1|n-1]F[n]^{T} + Q[n], \tag*{(23)}

followed by ψ^[n∣n−1]←wrap⁡[0,2π)(ψ^[n∣n−1])\hat{\psi}[n|n-1] \leftarrow\operatorname{wrap}_{[0,2\pi)}\left(\hat{\psi}[n|n-1]\right). Here, P[n∣n−1]=E[x⃗~[n∣n−1]x⃗~T[n∣n−1]]P[n|n-1] = \mathbb{E}\left[\tilde{\vec{x}}[n|n-1]\tilde{\vec{x}}^{T}[n|n-1]\right]. Without magnetometer samples this reduces to dead reckoning, and PP grows. This is fixed when calculating P[n∣n]P[n|n] in the update section.

Update. When a valid magnetometer heading ψmag[n]\psi_{\mathrm{mag}}[n] is available, the innovation

y[n]=wrap⁡(−π,π](ψmag[n]−ψ^[n∣n−1])(24)y[n] = \operatorname{wrap}_{(-\pi,\pi]}\left(\psi_{\mathrm{mag}}[n] - \hat{\psi}[n|n-1]\right) \tag*{(24)}

is calculated as the difference between the predicted value and the directly measured value. Then, the update algorithm updates the estimated values as below.

σy2[n]=HP[n∣n−1]HT+R[n],(25)\sigma_y^2[n] = H P[n|n-1]H^T + R[n], \tag*{(25)}
K[n]=P[n∣n−1]HT/σy2[n],(26)K[n] = P[n|n-1]H^T/\sigma_y^2[n], \tag*{(26)}
x^[n∣n]=x^[n∣n−1]+K[n] y[n],(27)\hat{\boldsymbol{x}}[n|n] = \hat{\boldsymbol{x}}[n|n-1] + K[n]\,y[n], \tag*{(27)}
P[n∣n]=(1−K[n]H)P[n∣n−1].(28)P[n|n] = (1-K[n]H)P[n|n-1]. \tag*{(28)}

The innovation y[n]y[n] uses the signed wrap so that 359° →\to 1° counts as +2° instead of −358°. The gain K[n]K[n] splits the correction between the two states according to their uncertainties: a large R[n]R[n] makes the filter trust the gyroscope, and a large P00P_{00} makes it trust the magnetometer. Afterwards, ψ^\hat{\psi} is wrapped to [0,2π)[0,2\pi) and PP is symmetrized, to avoid asymmetry in single-precision arithmetic.

H\boldsymbol{H} observes only ψ\psi, but the pair (F,H)(F,H) is observable, since

∣HHF∣=[101−Δt[n]]\begin{vmatrix} H\\ HF \end{vmatrix} = \begin{bmatrix} 1 & 0\\ 1 & -\Delta t[n] \end{bmatrix}

has rank two. In practice, if the gyroscope has a residual bias, the innovation y[n]y[n] keeps the same sign. The cross-covariance P10P_{10} links this to bψb_\psi, so the second gain component K1=P10/σy2K_1=P_{10}/\sigma_y^2 moves b^ψ\hat{b}_\psi until the innovation averages to zero. The bias is therefore observable only through the magnetometer.

Adaptive measurement noise. The Earth’s field strength B0=∥M⃗⊕∥B_0=\lVert\vec{M}_{\oplus}\rVert is constant at a fixed location, so a measured strength that deviates from B0B_0 indicates a disturbance (motors, nearby steel). Define

δB[n]=∣∥m⃗c,HB[n]∥B0−1∣,R[n]=R0[1+(δB[n]δ0)2],(29)\delta_B[n] = \left|\frac{\lVert\vec{m}^{B}_{c,\mathcal{H}}[n]\rVert}{B_0}-1\right|,\qquad R[n] = R_0\left[1+\left(\frac{\delta_B[n]}{\delta_0}\right)^2\right], \tag*{(29)}

where m⃗c,HB=PHBm⃗cB\vec{m}^{B}_{c,\mathcal{H}}=P^{B}_{\mathcal{H}}\vec{m}^{B}_{c} is the horizontal part of the calibrated field (in the 3D calibration mode of Stage 4, the full vector m⃗cB\vec{m}^{B}_{c} is used instead). A disturbed field therefore makes the magnetometer less trusted, and if δB[n]>0.3\delta_B[n]>0.3 the sample is discarded and only the prediction step is run.

Initialization. The first call of the update loop only stores a time reference, and Δt[n]\Delta t[n] is measured from a hardware timer at every iteration and limited to 0.2 s. Before the first accepted magnetometer sample, the heading is unknown. At that sample, the filter is initialized as ψ^=ψmag\hat{\psi}=\psi_{\mathrm{mag}}, P00=R0P_{00}=R_0, P01=P10=0P_{01}=P_{10}=0, P11=Pb,0P_{11}=P_{b,0} and b^ψ=0\hat{b}_{\psi}=0. The same re-initialization occurs if the magnetometer calibration becomes valid for the first time or becomes invalid. If the startup average b⃗B[0]\vec{b}^{B}[0] was rejected because the vehicle moved during the measurement (spread larger than 3°/s), it is set to zero, and bψb_\psi then has to absorb the full gyroscope bias.

ParameterMeaningValue
qψq_\psiheading process noise10−5 rad2/s10^{-5}\ \mathrm{rad}^2/\mathrm{s}
qbq_bresidual-bias random-walk noise10−7 rad2/s310^{-7}\ \mathrm{rad}^2/\mathrm{s}^3
R0R_0magnetometer heading noise3×10−3 rad23\times10^{-3}\ \mathrm{rad}^2 (≈ 3°)
δ0\delta_0field-deviation scale in Eq. (29)0.1
P00P_{00} before first updateinitial heading variance3 rad23\ \mathrm{rad}^2
Pb,0P_{b,0}initial residual-bias variance10−2 (rad/s)210^{-2}\ (\mathrm{rad}/\mathrm{s})^2

Table 4. Kalman filter parameters.

Approximating the steady state with the continuous-time algebraic Riccati equation, with the measurement noise intensity R0ΔtR_0\Delta t for Δt=10\Delta t=10 ms, gives a heading standard deviation of about 0.26°, a residual-bias standard deviation of about 0.06°/s and a heading gain K0≈7×10−3K_0\approx7\times10^{-3} per update. The heading therefore converges to the magnetometer with a time constant of about Δt/K0≈1.5\Delta t/K_0\approx1.5 s, while the gyroscope carries the heading between updates.

Stage 3 — Rate-dependent CMPS14/MPU blend. The CMPS14 provides a direct tilt-compensated heading ψCMPS\psi_{\mathrm{CMPS}}. We expected this sensor to drift considerably more at standstill than the Kalman-filtered MPU-9250; our tests did not confirm this (§ 3.1.4). To exploit the fast reaction of the onboard fusion while retaining the low-noise, bias-corrected estimate from the MPU Kalman filter at low dynamics, we blend the two headings with a scalar weight w(∣ωz∣)w(\lvert\omega_z\rvert) that is a monotonic function of the yaw rate magnitude:

ψfused=w(∣ωz∣)ψCMPS+[1−w(∣ωz∣)]ψMPU.(30)\psi_{\mathrm{fused}} = w(\lvert\omega_z\rvert)\psi_{\mathrm{CMPS}} + [1-w(\lvert\omega_z\rvert)]\psi_{\mathrm{MPU}}. \tag*{(30)}

The weight is implemented as a smooth piecewise linear function in our baseline:

w(∣ωz∣)={0,∣ωz∣≤ω1,∣ωz∣−ω1ω2−ω1,ω1<∣ωz∣<ω2,1,∣ωz∣≥ω2.(31)w(\lvert\omega_z\rvert) = \begin{cases} 0, & \lvert\omega_z\rvert\le\omega_1,\\ \frac{\lvert\omega_z\rvert-\omega_1}{\omega_2-\omega_1}, & \omega_1 < \lvert\omega_z\rvert< \omega_2,\\ 1, & \lvert\omega_z\rvert\ge\omega_2. \end{cases} \tag*{(31)}

Thresholds ω1,ω2\omega_1,\omega_2 are design parameters and must be tuned to the vehicle dynamics; the angular velocity thresholds were empirically selected for our platform. The two sensors do not share the same zero reference, so a constant offset measured with the board at rest is subtracted from ψCMPS\psi_{\mathrm{CMPS}}, and the difference is wrapped to (−π,π](-\pi,\pi] before blending. In our tests the blend did not measurably reduce stationary drift compared with using the CMPS14 alone (§ 3.1.4).

Stage 4 — Magnetometer calibration and startup. So far the magnetometer was assumed ideal. In reality the measured vector follows Eq. (13), m⃗mB=ΓM⃗⊕B−O⃗B\vec{m}_m^B = \boldsymbol{\Gamma}\vec{M}_{\oplus}^B-\vec{O}^B, where Γ\boldsymbol{\Gamma} (soft iron: scaling, cross-axis coupling) and O⃗\vec{O} (hard iron: constant offset) depend on the vehicle’s wiring, batteries and mounting. They are therefore estimated on the vehicle itself, from the sensor readings only [14, 15].

Raw data. The AK8963 magnetometer is read at 100 Hz in 16-bit continuous mode through the I2^2C bypass of the MPU-9250. Each chip axis is scaled as mAK,i=raw⁡iaim_{\mathrm{AK},i}=\operatorname{raw}_i a_i (0.15 μ\muT), where ai=(ASAi−128)/256+1a_i=(\mathrm{ASA}_i-128)/256+1 is a factory sensitivity adjustment read once from the chip’s fuse ROM [18]. The chip axes are not aligned with the gyroscope and accelerometer (its xx and yy are swapped and zz is inverted), so m⃗mB=[mAK,y,mAK,x,−mAK,z]T\vec{m}_m^B=[m_{\mathrm{AK},y},m_{\mathrm{AK},x},-m_{\mathrm{AK},z}]^T.

Earth’s magnetic field. At a fixed location ∥M⃗⊕∥=B0\lVert\vec{M}_{\oplus}\rVert=B_0 is constant, so if the vehicle is rotated, M⃗⊕B\vec{M}_{\oplus}^B traces a sphere of radius B0B_0 in the body frame. Eq. (13) turns this sphere into an ellipsoid: the measured vectors satisfy

(m⃗mB+O⃗B)TA(m⃗mB+O⃗B)=1,A=Γ−TΓ−1B02,(32)(\vec{m}_m^B+\vec{O}^B)^T A(\vec{m}_m^B+\vec{O}^B)=1,\qquad A=\frac{\boldsymbol{\Gamma}^{-T}\boldsymbol{\Gamma}^{-1}}{B_0^2}, \tag*{(32)}

so −O⃗B-\vec{O}^B is the centre of the ellipsoid, and AA describes its shape. The calibration fits this ellipsoid to recorded samples and then maps it back to a sphere:

m⃗cB=W(m⃗mB+O⃗B),W=B0A1/2=(ΓΓT)−1/2,(33)\vec{m}_c^B=W(\vec{m}_m^B+\vec{O}^B),\qquad W=B_0A^{1/2}=(\boldsymbol{\Gamma}\boldsymbol{\Gamma}^T)^{-1/2}, \tag*{(33)}

where A1/2A^{1/2} is the symmetric square root of AA. The product WΓW\boldsymbol{\Gamma} is orthogonal, so m⃗cB\vec{m}_c^B is the Earth’s field M⃗⊕B\vec{M}_{\oplus}^B up to a rotation, with ∥m⃗cB∥=B0\lVert\vec{m}_c^B\rVert=B_0. This is the vector used in Eq. (18) and Eq. (29). Two limits follow. First, B0B_0 is the mean size of the fitted ellipsoid (the geometric mean of its semi-axes), not an independent measurement of the local field. Second, an ellipsoid fit cannot observe a rotation of the magnetometer axes relative to the body frame, which would appear as a constant heading error.

Least-squares fit. Let m⃗i\vec{m}_i, i=1,…,Ni=1,\ldots,N, be the stored samples with centroid m⃗\vec{m}. They are centred and scaled, p⃗i=(m⃗i−m⃗)/s\vec{p}_i=(\vec{m}_i-\vec{m})/s with s=50 μs=50\ \muT. Centring puts the origin inside the ellipsoid, and scaling keeps the equations well conditioned. Writing the ellipsoid as p⃗ TCp⃗+d⃗ Tp⃗=1\vec{p}^{\,T}C\vec{p}+\vec{d}^{\,T}\vec{p}=1, the unknowns enter linearly,

ϕ⃗i Tξ⃗=1,(34)\vec{\phi}_i^{\,T}\vec{\xi}=1, \tag*{(34)}
ϕ⃗i=[px2,py2,pz2,2pxpy,2pxpz,2pypz,px,py,pz]iT,(35)\vec{\phi}_i=[p_x^2,p_y^2,p_z^2,2p_xp_y,2p_xp_z,2p_yp_z,p_x,p_y,p_z]_i^T, \tag*{(35)}
ξ⃗=[Cxx,Cyy,Czz,Cxy,Cxz,Cyz,dx,dy,dz]T,(36)\vec{\xi}=[C_{xx},C_{yy},C_{zz},C_{xy},C_{xz},C_{yz},d_x,d_y,d_z]^T, \tag*{(36)}

and are obtained from the regularized normal equations (∑iϕ⃗iϕ⃗iT+ϵ1)ξ⃗=∑iϕ⃗i\left(\sum_i \vec{\phi}_i\vec{\phi}_i^T+\epsilon\mathbf{1}\right)\vec{\xi}=\sum_i\vec{\phi}_i with ϵ=10−9N\epsilon=10^{-9}N, solved by Gaussian elimination with partial pivoting [17]. Completing the square gives the centre p⃗0=−12C−1d⃗\vec{p}_0=-\frac{1}{2}C^{-1}\vec{d} and the shape matrix As=C/γA_s=C/\gamma with γ=1+p⃗0TCp⃗0\gamma=1+\vec{p}_0^T C\vec{p}_0. The hard-iron vector follows from −O⃗B=m⃗+sp⃗0-\vec{O}^{B}=\vec{m}+s\vec{p}_0. With the eigendecomposition As=Vdiag⁡(wk)VTA_s=V\operatorname{diag}(w_k)V^T (Jacobi method [17]),

reff=(det⁡As)−1/6,B0=s reff,W=reffVdiag⁡(wk)VT.(37)r_{\mathrm{eff}}=(\det A_s)^{-1/6},\qquad B_0=s\,r_{\mathrm{eff}},\qquad W=r_{\mathrm{eff}}V\operatorname{diag}(\sqrt{w_k})V^T. \tag*{(37)}

This is an unconstrained algebraic fit, as opposed to the ellipse-specific constrained fit of [16], so the validity of the result is enforced by the acceptance tests below. The computation is done in double precision (software-emulated on the ESP32), which is why it runs in a separate low-priority task.

2D and 3D modes. A ground vehicle only yaws, so the zz component of the field is not excited and cannot be calibrated. By default, only the xx–yy components are fitted, with the five unknowns CxxC_{xx}, CyyC_{yy}, CxyC_{xy}, dxd_x, dyd_y and reff=(det⁡As,xy)−1/4r_{\mathrm{eff}}=(\det A_{s,xy})^{-1/4}. In this mode, WW has no coupling with zz and Wzz=1W_{zz}=1, the zz component of O⃗B\vec{O}^{B} keeps its previous value, and the field strength in Eq. (29) is evaluated on the horizontal part only. The full 3D fit takes over when at least 80 samples are stored and their zz span is at least half of the mean xx–yy span, which only happens if the vehicle has been tilted. In the 2D mode, an error in the zz offset leaks into the tilt-compensated heading and grows with the tilt angle, so it is small as long as the vehicle stays level.

On-the-fly operation.

  1. Every valid m⃗mB\vec{m}^{B}_{m} sample is stored in a 128-sample circular buffer only if it lies farther than 1.5 μT1.5\ \mu\mathrm{T} from all stored samples. A stationary vehicle therefore adds nothing, and the oldest samples are replaced when the buffer is full.

  1. Once per second a background task copies the buffer and attempts a fit, provided there are at least 30 samples and they cover at least 11 of 12 equal angular sectors of the xx–yy plane around their centroid (the vehicle has turned nearly a full circle).

  1. The fit is rejected if AsA_s is not positive definite, if the ratio of the largest to the smallest semi-axis exceeds 2, if B0∉[10,120] μTB_0\notin[10,120]\ \mu\mathrm{T}, or if the RMS relative error

ϵ=1N∑i(∥W(m⃗i+O⃗B)∥B0−1)2(38)\epsilon=\sqrt{\frac{1}{N}\sum_i\left(\frac{\lVert W(\vec{m}_i+\vec{O}^{B})\rVert}{B_0}-1\right)^2} \tag*{(38)}

exceeds 6% (horizontal norm in the 2D mode).

  1. A new fit replaces the current calibration only if its ϵ\epsilon on the current buffer is lower than that of the current one. It is then blended in, O⃗B,W,B0←old+0.5(new−old)\vec{O}^{B},W,B_0\leftarrow\text{old}+0.5\left(\text{new}-\text{old}\right), so that the heading does not jump. The result is handed to the filter through a mutex-protected buffer and is picked up at the next iteration.

Startup. At power-up, the MPU-9250 is reset and identified, the AK8963 is enabled through the bypass mode and its ASAI values are read, the gyroscope bias b⃗[0] b\vec{b}^{\,b}_{[0]} is averaged over 200 samples (about 1 s, with the vehicle standing still), and the calibration task is started. The vehicle must then be rotated in place by about one full turn, so that the sector-coverage test is passed. Until the first valid calibration exists, no magnetometer update can be made and the heading is flagged invalid. At that moment the Kalman filter is initialized from the first accepted ψmag\psi_{\mathrm{mag}} (Stage 2). From then on, the calibration keeps refining itself while the vehicle drives. The firmware can also be seeded with a stored calibration.

Limitations. The calibration is static: field changes that depend on the motor current are not modeled and are only handled by the field-strength gate of Eq. (29). The heading is relative to magnetic north, which is distorted by indoor steel and varies between locations. If the vehicle is relocated, the calibration re-adapts as old samples are replaced in the buffer. Furthermore, in preliminary unlogged tests the 3D mode appeared to output much higher bias than the 2D mode, which we suspect is contributed to by the inaccuracy of the up direction estimation in Eq. (6).

Experimental Validation of the Heading Estimator

The estimator was tested on a breadboard rather than on the vehicle, with an MPU-9250 and a CMPS14 taped side by side on one board and logged over UART at 20 Hz. All processing was done offline. No external angle reference was available, so the experiments measure agreement between the two sensors and stability at rest, not absolute accuracy. The alignment offset between the two sensors was not captured on-line in the reported run. It was instead computed offline as the circular mean of the CMPS14–MPU difference over the first 23 min of stationary data (about −139∘-139^\circ, with a peak-to-peak variation of about 2∘2^\circ), and the aligned CMPS14 heading and the blended heading were recomputed from it. This is exact for the blended heading, because the blend weight depends only on the MPU yaw rate.

Figure 6 shows the magnetometer samples held in the calibration buffer for the three conditions tested, reconstructed offline from the logged raw samples with the same storage rule as the firmware (128-sample circular buffer, minimum spacing of 1.5 μT1.5\ \mu\mathrm{T}). The fit error and field radius of the last accepted calibration of each segment are given in the panel titles. Without magnets the raw samples form an offset ring, and the corrected samples are centred on the origin. The two magnet conditions are shown in Figure 7. The weak magnets leave the horizontal field close to the undisturbed value of about 38 μT38\ \mu\mathrm{T}, whereas the strong magnets raise it to about 215 μT215\ \mu\mathrm{T}. The corrected weak-magnet samples did not form a circle, while those of the other two cases did. The reason is not yet known.

Magnetometer calibration buffers showing raw and corrected samples for no magnet, weak magnets, and strong magnets

Figure 6. Magnetometer calibration buffers. Blue: raw samples; orange: samples corrected with the last accepted calibration of each segment; the black cross marks the fitted hard-iron centre.

Breadboard setups for weak and strong magnets

Figure 7. (a) Weak magnets. (b) Strong magnets. Breadboard setup of the two magnetic disturbance tests.

Figure 8 shows 23 min with the board untouched (the last minutes of the recording, when the operator returned to the desk, are excluded). The MPU Kalman heading had a slope of −0.001∘-0.001^\circ/min, a standard deviation of 0.17∘0.17^\circ and a peak-to-peak variation of 1.65∘1.65^\circ. The CMPS14 had a slope of −0.005∘-0.005^\circ/min and a peak-to-peak variation of 0.10∘0.10^\circ, which is its output resolution. At rest the blend weight is zero, so the blended heading is identical to the MPU heading. Our design assumed that the CMPS14 drifts considerably more than the filtered MPU at standstill, and the blend gives the MPU full weight at low rates for that reason. This run does not support the assumption. In a second 14 min run (during which a cable was briefly touched) the CMPS14 again varied by 0.1∘0.1^\circ against about 1.9∘1.9^\circ for the MPU. In both recordings the CMPS14 was at least as stable as the MPU, so the blend gave no measurable benefit at rest. A further 30 min run showed about 9∘9^\circ of CMPS14 drift, so CMPS14 stability seems to vary between runs. The measured MPU standard deviation is of the same order as the 0.26∘0.26^\circ predicted in Stage 2, although the two quantities are not strictly the same (the prediction is an estimation error with respect to the true heading, which was not measured here).

Stationary heading change of the CMPS14, the MPU Kalman estimate and the blended heading over 23 min

Figure 8. Stationary heading change of the CMPS14, the MPU Kalman estimate and the blended heading over 23 min. The blended heading coincides with the MPU estimate because the blend weight is zero at rest.

Figure 9 shows a 29 s rotation by hand with yaw rates up to about 350∘350^\circ/s, using blend thresholds of 3∘3^\circ/s and 15∘15^\circ/s. After alignment the two headings stay within about ±25∘\pm25^\circ of each other and mostly within 10∘10^\circ, and the difference drifts slowly toward −10∘-10^\circ. The blend weight follows the rate as designed, so the blended heading follows the MPU at low rates and the CMPS14 at high rates. Without a ground-truth angle we cannot say which sensor is closer to the true heading, only that they agree. The experiment therefore does not show that the blend improves on either sensor alone.

Manual rotation: aligned CMPS14 and MPU headings, blended heading, their difference, the yaw rate, and the blend weight $w$.

Figure 9. Manual rotation: aligned CMPS14 and MPU headings, blended heading, their difference, the yaw rate, and the blend weight ww.

The magnet tests are summarised in Figure 10. Placing the magnets near the board leaves the MPU heading uncorrected until the calibration is refitted: in both cases the CMPS14–MPU difference grew almost linearly, by about 100100–130∘130^\circ in 35–45 s (≈3∘/s\approx3^\circ/\mathrm{s}) while the board stood still, and returned to a few degrees as soon as a new calibration became valid after a manual reset. We believe the magnetometer samples were being rejected by the field-strength gate during these intervals, so the filter was integrating the gyroscope alone, but we did not verify this directly. With the strong magnets and a new calibration the MPU held its heading within about 1∘1^\circ over 22 s at rest and followed the CMPS14 within a few degrees during rotation. A plausible reason is that magnets fixed to the board act as an additional constant hard-iron offset, which the calibration removes. The calibration does not recover by itself, since the old buffer still holds samples from the undisturbed field and a new fit is accepted only after a full rotation. In the present implementation a lasting disturbance therefore requires a manual reset.

Horizontal field strength and CMPS14–MPU difference across the magnet tests

Figure 10. Horizontal field strength (top) and CMPS14–MPU difference (bottom) across the magnet tests. The vertical lines mark the start of the weak-magnet test, the start of the strong-magnet phase (the magnets were introduced about 45 s later), the strong-magnet stationary test and the strong-magnet rotation.

These results come from a single board and a small number of runs, without an external reference, and only the planar (2D) calibration mode was exercised. They should be read as a first check of the implementation and not as a characterisation of its accuracy.

Communication Protocol and Data Packet Structure

Data transfer between processing units is implemented over two UART channels, following a structured packet-based protocol. ZERO periodically sends data packets (p1) containing ToF, compass, and encoder readings to CONTROL, which checks packet integrity (using checksum methods) and forwards them as packet p1 to Jetson. CONTROL also receives control and lighting commands (p2) from Jetson, which upon arrival extracts data, actuates NeoPixel and Servo Motor directly, and sends DC motor speed as packet p3 to ZERO.

Each communication packet follows a fixed structure designed for robustness and low latency. The format is summarized as:

$d1!d2!…dn!chk8#…\text{\$}\mathbf{d}_1!\mathbf{d}_2!\ldots\mathbf{d}_n!\mathbf{chk8}\#\ldots

where $ is the start byte (synchronization marker), did_i are the data fields separated by !, chk8 is an 8-bit additive checksum for data integrity, # is the end byte (termination marker), and . denotes padding characters used to maintain fixed payload length.

This structure ensures byte-level synchronization and error detection even under variable serial latency conditions. Message acknowledgment is intentionally minimized to reduce round-trip delay, while periodic redundancy provides fault tolerance.

Both UART links use 115200 baud, 8N1. Table 5 lists the packets. A receiver waits for the start byte, reads a fixed-length frame, and then flushes its RX buffer, so only the most recent frame is processed. The integrity field is an 8-bit additive checksum (sum of the numeric fields modulo 256). The sensor packet p1 is verified on the Jetson; p2 and p3 are verified by their receivers (CONTROL and ZERO).

PacketDirectionLengthWire timeFields
p1ZERO→CONTROL→Jetson85 B7.4 ms/hop12 ToF distances (mm), compass bearing (0.1°), encoder count, speed, motor-enable flag, time elapsed, checksum
p2Jetson→CONTROL25 B2.2 mssteering command (0–255), buzzer, motor command (±1023\pm1023), light mode, ToF bitmask, checksum
p3CONTROL→ZERO18 B1.6 msmotor command, ToF bitmask, checksum

Table 5. Packet formats. Wire time is for a fixed-length frame at 115200 baud, 8N1 (10 bits per byte).

Forward latency (time passed between a ZERO packet transmission and a Jetson packet reception) was measured to have mean 18.2 ms with standard deviation of a 4.0 ms. The backward latency (time passed between a Jetson packet transmission and a ZERO packet reception) was shorter, owing to the smaller packet size. The mean was measured to be around 9.2 ms, with standard deviation of 3.4 ms. These measurements were performed by simultaneously hardware interrupting all three devices, and setting the interrupted time to zero, and continuously polling data transmission and reception time across the three devices.

Startup Sequences, Edge Cases, and Fault Protection. At startup, CONTROL initiates a handshake with ZERO to verify connectivity and sensor readiness before enabling motor actuation. All sensors are sequentially initialized and validated before control commands are accepted. Emergency stop and fail-safe routines are integrated into CONTROL’s firmware: if serial communication from Jetson is lost for more than 500 ms, or if a fault flag is raised by ZERO, CONTROL immediately halts motor drive signals and centers the steering servo. These measures ensure safe degradation of operation during transient or fault conditions.

Robot Operating System (ROS) Architecture

The autonomous vehicle software stack is implemented using the Robot Operating System (ROS) Noetic [4], running on Ubuntu 20.04 LTS on the Jetson Orin NX onboard computer. ROS provides a modular, distributed middleware that enables perception, decision-making, and actuation to run in parallel while exchanging data through topics, shared memory, and global parameters.

System Overview

The system consists of seven ROS nodes: Camera, Sign, Network, Postprocess, Crosswalk, Control, and Listener. Each node executes as an independent process but participates in a unified computation graph, visualized in Figure 11. Large data objects such as RGB and depth frames are communicated through shared memory to minimize copying overhead, while status, events, and processed features are sent through ROS topics and parameters.

Graph of ROS nodes and topic-level publish–subscribe relationships

Figure 11. Graph of ROS nodes and topic-level publish–subscribe relationships.

NodeInputsPrimary Function / Outputs
CameraIntel RealSense D455Captures synchronized RGB and depth frames at 848×480848 \times480 and writes them to shared memory. Publishes a startup-ready parameter after the first valid frame.
SignShared memory framesPerforms traffic sign detection. Publishes sign_topic (sign type and distance) and cross_input to trigger crosswalk processing.
NetworkShared memory RGB frameRuns lane segmentation model and writes a (32×64×7)(32 \times64 \times7) tensor to shared memory.
PostprocessNetwork output + RGB frameExtracts lane offset and heading angle, published on line_topic.
CrosswalkRGB frame + cross_inputComputes crosswalk angle and position, and receives the intersection parameter from Control.
ControlPerception topics + uart_topicComputes steering, throttle, and mode, publishing serial data to the Listener.
Listenerchat topicConverts control outputs to serial packets and republishes hardware feedback on uart_topic.

Table 6. Summary of ROS nodes and their functions

Node-Level Data Flow

The Camera node initializes the Intel D455 at 848×480848 \times480, the operating point providing the highest depth accuracy. It writes RGB and depth frames directly to shared memory, and after the first valid capture sets a ROS parameter indicating that dependent nodes may start processing. This prevents premature access to uninitialized memory.

The Sign node reads the current frames from shared memory and generates two outputs: (1) sign type and distance on sign_topic, and (2) a binary crosswalk trigger on cross_input.

The Network node receives the latest RGB frame and performs neural inference, producing a (32×64×7)(32 \times64 \times7) feature tensor stored in shared memory for downstream use.

The Postprocess node extracts lane geometry from the tensor: lateral offset and heading angle. These are published to line_topic, consumed by the Control layer.

The Crosswalk node uses shared memory frames and cross_input to compute crosswalk angle and position. Additionally, it receives a runtime parameter named intersection from the Control node indicating whether the vehicle is navigating a junction.

The Control node subscribes to line_topic, sign_topic, crosswalk outputs, and hardware feedback from uart_topic. It computes high-level actuation commands, which are published as chat messages.

The Listener node interfaces directly with the microcontrollers, translating chat into serial commands and publishing returning sensor feedback back onto uart_topic. The Listener node waits for the camera-ready parameter, opens the Jetson UART (/dev/ttyTHS0, 115200 baud), and sends the latest command packet to CONTROL on every loop iteration after clamping each field to its valid range. Incoming p1 packets (§ 3.1.5) are checked against their checksum and republished as 17 integers on uart_topic; if corrupted bytes are detected, the port is closed, reopened, and flushed.

The shared-memory relationships are shown in Figure 12.

Shared memory and data exchange structure among ROS nodes

Figure 12. Shared memory and data exchange structure among ROS nodes.

Launch Configuration and Fault Handling

All nodes are executed through a unified launch file in launch/. The configuration defines output behavior, restart rules, and global parameter initialization.

Because the Camera node allocates the shared memory region, if it restarts, the memory pointer changes. To avoid inconsistent pointers, the system is configured so that a Camera crash triggers a complete system restart, while crashes in other nodes cause only those nodes to restart individually.

Global shared parameters (calibration constants, visualization toggles, and processing modes) are propagated through the same launch configuration and are accessible system-wide, as shown in Figure 13.

Global parameter exchange for cross-node synchronization

Figure 13. Global parameter exchange for cross-node synchronization.

Timing, Synchronization, and Performance

Real-time operation requires matching node execution rates. The lane network (Network) runs at approximately 13 Hz and is therefore the bottleneck for downstream modules. Nodes such as Postprocess and Control can reach 30 Hz, but first verify whether new data is available before computing. If the network output has not changed, they defer processing, preventing redundant work and improving CPU efficiency.

Conversely, the Camera must operate faster than all dependent nodes to minimize data age and reduce perception-to-actuation latency.

This combined scheduling strategy produces deterministic timing, low redundant computation, and predictable behavior across the full perception and control pipeline.

Computer Vision and Perception

The perception stack serves as the primary sensory input for the autonomous vehicle, mapping the drivable area and enabling adherence to traffic rules. We designed a custom vision pipeline centered around a modified Point Instance Network (PINet) [20], optimized specifically for the sharp curvature and unique lighting conditions of the competition track. The stack is entirely vision-driven: a single Intel RealSense D455 provides synchronized RGB and depth, and three cooperating perception paths—lane detection, traffic sign and signal recognition, and crosswalk detection—run concurrently and publish their state to the controller through a zero-copy shared-memory bus.

This section is organized to follow a single camera frame through the full pipeline. We first describe acquisition, then the lane-detection network architecture and its mathematical training objective, the deterministic geometric post-processing that converts network outputs into physical control variables, the sign and crosswalk perception paths, the inter-process communication layer that binds them, and finally the experimental validation of the trained model. Throughout, we emphasize the engineering rationale behind each design decision, since the dominant constraint—real-time inference on an embedded Jetson Orin at 30 FPS—shaped nearly every choice in the stack.

Acquisition System

Visual data is acquired via an Intel RealSense D455 depth camera [19], selected for its global-shutter RGB sensor and its wide-baseline active stereo depth module. The global shutter is a deliberate choice for a moving platform: at competition speeds a rolling-shutter sensor introduces skew artifacts on vertical lane markings during yaw motion, which would corrupt the sub-pixel key point regression discussed in § 4.3. The camera streams synchronized 480×848480 \times848 RGB and depth frames at 30 FPS, with the depth stream hardware-aligned to the color optical frame so that any RGB pixel can be dereferenced directly into a metric depth value. This alignment is what later enables the depth-gated sign validation of § 4.7 without a separate extrinsic calibration step.

Unlike traditional pipelines that crop the input to a Region of Interest (ROI), we process the full sensor resolution (resized to the network input of 512×256512 \times256). We deliberately avoided ROI cropping to maintain the maximum possible field of view. This ensures the network can utilize peripheral visual cues—essential for detecting the entry points of sharp hairpin turns—which would otherwise be discarded by aggressive cropping. The trade-off is a larger input tensor and therefore a heavier front-end; we absorb this cost in the aggressive stride-2 stem described in Table 7 rather than by discarding field of view.

Block NameTypeInput Res.Out Ch.Engineering Rationale
StemConv2D + PReLU512×256512 \times25664Aggressive downsampling to reduce memory footprint.
HG-Block 1Standard Hourglass64×3264 \times32128Initial coarse feature extraction.
HG-Block 2Standard Hourglass64×3264 \times32128Refinement of features with skip connections.
HG-Block 3Dilated Hourglass64×3264 \times32128Custom: Uses dilation rates (2, 4) to expand receptive field for sharp turns.
HeadsConv 1×11 \times164×3264 \times327Projects features into: 1x Confidence, 2x Offsets, 4x Embeddings.

Table 7. Layer-wise Complexity Analysis.

Figure 14 shows a representative raw RGB frame captured on the competition track. The scene is intentionally cluttered—lane markings, a pedestrian crosswalk, a mandatory-direction sign, and a bank of distant traffic signs are all simultaneously visible—illustrating the range of cues the perception stack must disentangle from a single monocular frame. The lower portion of the frame is partially occluded by the vehicle’s own chassis and wiring harness; because we do not crop, the network must (and does) learn to ignore this static foreground rather than having it removed by a hand-tuned mask.

Raw RGB frame captured from the RealSense D455 on the competition track

Figure 14. Raw RGB frame captured from the RealSense D455 on the competition track. The full, uncropped field of view retains peripheral cues (distant signs, crosswalk, hairpin markings) at the cost of a static foreground occlusion from the chassis, which the network learns to suppress.

Lane Detection Architecture

We utilized a deep learning approach based on the Stacked Hourglass architecture. Standard lane detectors often struggle with the tight turning radii (<1< 1 m) found in 1:10 scale tracks. To address this, we engineered specific modifications into the PINet backbone.

Layer-wise Complexity Analysis

The summary below details the structural evolution of the feature maps as they propagate through the network. The Stem layer performs aggressive downsampling to reduce the memory footprint. The architecture then transitions into feature extraction, where HG-Block 3 introduces specific dilated convolutions. Finally, the Heads project these deep features into the specific 7-channel output.

Network Design: The ``Dilated'' 3-Stack Hourglass

Our model consists of three stacked hourglass modules. While the original PINet implementation uses standard convolutions, we engineered the bottleneck layers to utilize dilated convolutions [21]. The overall data flow is shown in Figure 15.

Top-Down Network Data Flow

Figure 15. Top-Down Network Data Flow. The high-resolution input (512×256512 \times256) is first processed by a Resizing Network to reduce spatial dimensions to 64×3264 \times32, optimizing inference speed on the Jetson Orin. The core backbone consists of three Stacked Hourglass modules. The final module (Hourglass 3) is engineered with a Dilated Bottleneck.

The Engineering Challenge: Standard convolutions in a compact network often lack the receptive field necessary to “see” the curvature of a sharp turn before the vehicle enters it.

The Solution: By applying dilation rates of 2 and 4 in the bottleneck layers, we expanded the network’s receptive field without increasing the parameter count. This allows the model to capture the global context of the lane geometry (e.g., a sharp U-turn) while retaining local spatial resolution for precise localization.

Reliability Strategy: We opted to train a 3-Hourglass network from scratch rather than distilling a larger 4-Hourglass teacher network. This “direct training” approach ensured that the validation metrics observed during training perfectly matched the inference behavior on the embedded hardware.

Mathematical Formulation of the Loss Function

To ensure precise key point estimation and instance segmentation, we implemented a multi-task loss function. This function is applied to the three output branches (Confidence, Offset, and Embedding) of each hourglass module.

Confidence Loss. The confidence branch predicts the probability of a key point existing in a given grid cell. We utilize a split loss formulation to handle the class imbalance. The Existence Loss LexistL_{\mathrm{exist}} penalizes errors only in cells where key points actually exist (GeG_e):

Lexist=1Ne∑ce∈Ge(ce∗−ce)2(39)L_{\mathrm{exist}} = \frac{1}{N_e} \sum_{c_e \in G_e} (c_e^* - c_e)^2 \tag*{(39)}

The Non-Existence Loss (Lnon_existL_{\mathrm{non\_exist}}) minimizes false positives in background cells GnG_n with a regularization term to prevent overfitting to zero:

Lnon_exist=1Nn∑cc∈Gn(cc∗−cc)2+0.00001⋅∑cc∈Gncc2(40)L_{\mathrm{non\_exist}} = \frac{1}{N_n} \sum_{c_c \in G_n} (c_c^* - c_c)^2 + 0.00001 \cdot\sum_{c_c \in G_n} c_c^2 \tag*{(40)}

Offset Loss. To mitigate quantization error, the network predicts normalized offsets in the range 0–1. The loss is calculated only for cells containing key points:

Loffset=1Ne∑cx∈Ge(cx∗−cx)2+1Ne∑cy∈Ge(cy∗−cy)2(41)L_{\mathrm{offset}} = \frac{1}{N_e} \sum_{c_x \in G_e} (c_x^* - c_x)^2 + \frac{1}{N_e} \sum_{c_y \in G_e} (c_y^* - c_y)^2 \tag*{(41)}

Embedding Feature Loss. To distinguish between multiple lane instances, we employ a discriminative loss function [22]. This pulls embeddings of the same instance (Iij=1I_{ij} = 1) closer together and pushes embeddings of different instances (Iij=0I_{ij} = 0) apart by a margin KK:

Lfeature=1Ne2∑iNe∑jNel(i,j)(42)L_{\mathrm{feature}} = \frac{1}{N_e^2} \sum_i^{N_e} \sum_j^{N_e} l(i,j) \tag*{(42)}

Where the pairwise distance term l(i,j)l(i,j) is defined as:

l(i,j)={∥f⃗i−f⃗j∥2if Iij=1max⁡(0,K−∥f⃗i−f⃗j∥2)if Iij=0(43)l(i,j) = \begin{cases} \lVert\vec{f}_i - \vec{f}_j \rVert_2 & \text{if } I_{ij} = 1 \\ \max(0, K - \lVert\vec{f}_i - \vec{f}_j \rVert_2) & \text{if } I_{ij} = 0 \end{cases} \tag*{(43)}

Total Optimization Objective. The final objective function is a weighted sum of these components:

Ltotal=γeLexist+γnLnon_exist+γoLoffset+γfLfeature(44)L_{\mathrm{total}} = \gamma_e L_{\mathrm{exist}} + \gamma_n L_{\mathrm{non\_exist}} + \gamma_o L_{\mathrm{offset}} + \gamma_f L_{\mathrm{feature}} \tag*{(44)}

Data Curation and Augmentation Pipeline

We developed a proprietary semi-automated data pipeline to solve the lack of labeled data.

Manual Annotation: We manually annotated 5,000 key frames.

The Augmentation Engine: We developed custom software to procedurally generate a dataset of 33,000 frames. The software applied randomized perturbations including geometric (random zooming, cropping, and horizontal flipping), photometric (HSV shifting to simulate different time-of-day lighting), and noise injection (Gaussian noise addition to simulate low-light sensor grain) transformations.

Training Strategy

Training was performed in two distinct stages.

Transfer Learning: Pre-trained for 20 epochs on the TuSimple public dataset [23].

Domain Adaptation with Manual Scheduling: Fine-tuned on our custom augmented dataset. We stepped the learning rate down (from 5×10−45 \times10^{-4} to 1×10−81 \times10^{-8}) specifically when the loss value dropped below 1.00.

Figure 16 summarizes the two training stages.

Phase 1 and Phase 2 Training Diagram

Figure 16. Phase 1 and Phase 2 Training Diagram. A public dataset provided the base feature extraction weights, while aggressive domain adaptation using synthetic expansion tuned the model to the physical constraints of the competition track.

Lane Geometry Extraction and Post-Processing

The raw outputs of the PINet network exist in perspective image space, which are non-linear and unsuitable for direct lateral control. To bridge the gap between perception and path planning, we engineered a deterministic post-processing pipeline that extracts physical state variables: Cross-Track Error (distance), Heading Error (degree), and Curvature.

Inverse Perspective Mapping (IPM)

To accurately measure distances and angles, the raw clustered lane points are projected into a Bird’s Eye View (BEV) using a highly calibrated Homography matrix (HIPMH_{\mathrm{IPM}}) [24]. Let p⃗i=[u,v,1]T\vec{p}_i = [u, v, 1]^{\mathsf{T}} represent a point in image space. The corresponding physical ground plane point p⃗w=[x,y,1]T\vec{p}_w = [x, y, 1]^{\mathsf{T}} is obtained via:

p⃗w=HIPM⋅p⃗i(45)\vec{p}_w = H_{\mathrm{IPM}} \cdot\vec{p}_i \tag*{(45)}

By executing this affine transformation after network inference rather than before, we preserve the far-field visual horizon for the neural network while achieving geometrically accurate measurements in the near-field for the controller. The pipeline in fact maintains two parallel realizations of this transform: a full-frame cv2.warpPerspective used only when a rasterized BEV image is required for visualization or the debug stream, and a lightweight point-domain realization that applies the homogeneous matrix product directly to the sparse cluster centroids. The latter avoids the cost of interpolated image resampling entirely, which matters when the transform is evaluated every frame at 30 FPS on embedded hardware.

Polynomial Path Fitting

Filtered BEV points are isolated into left and right lane boundaries. We model the lane geometry by fitting a second-degree polynomial to the xx and yy coordinates of the lane boundaries:

x(y)=a2y2+a1y+a0(46)x(y) = a_{2}y^{2} + a_{1}y + a_{0} \tag*{(46)}

If both lane lines are visible, the algorithm computes a dynamic centerline trajectory by averaging the polynomial coefficients: amid=aleft+aright2a_{\mathrm{mid}} = \frac{a_{\mathrm{left}} + a_{\mathrm{right}}}{2}.

Control Variable Generation

With the centerline modeled mathematically, critical control variables are extracted dynamically relative to a look-ahead point (yrefy_{\mathrm{ref}}). The Heading Error (θ\theta) is computed from the derivative of the polynomial:

θ=arctan⁡(2a2yref+a1)(47)\theta= \arctan(2a_{2}y_{\mathrm{ref}} + a_{1}) \tag*{(47)}

The Curvature (κ\kappa) is extracted using the standard formula for the radius of curvature:

κ=∣2a2∣(1+(2a2yref+a1)2)3/2(48)\kappa= \frac{\lvert2a_{2} \rvert}{\left(1 + (2a_{2}y_{\mathrm{ref}} + a_{1})^{2}\right)^{3/2}} \tag*{(48)}

To maximize stability, the final reported Cross-Track Error and Heading Error are not sampled from a single look-ahead point, but rather derived from a weighted spatial average spanning y∈[220,300]y \in[220, 300], heavily penalizing variations close to the vehicle chassis.

Embedding-Guided Instance Clustering

Raw detections emerging from the confidence, offset, and embedding heads arrive as an unordered point cloud; before any geometric fitting can occur, points must first be grouped into discrete lane instances. Rather than committing to a fixed-count clustering algorithm such as kk-means, we implemented a greedy, similarity-threshold clustering scheme that mirrors the training-time discriminative loss (Eq. (42)) directly at inference time: each candidate point is compared, in embedding space, against every cluster centroid formed so far, and is merged into the nearest cluster whenever the L2L_{2} distance falls under a fixed affinity threshold τinst\tau_{\mathrm{inst}}; otherwise it seeds a new cluster, bounded to a maximum of 12 concurrent instances to guarantee bounded worst-case runtime. Cluster centroids are updated incrementally as a running mean rather than recomputed from scratch, keeping the per-frame cost linear in the number of detected points.

Algorithm 1 Greedy Embedding-Affinity Clustering

Require: Points {(xi,yi)}\{(x_i,y_i)\} passing the confidence threshold; embeddings {f⃗i}\{\vec{f}_i\}; affinity threshold τinst\tau_{\mathrm{inst}}; instance cap Kmax⁡=12K_{\max}=12

Ensure: Instance-labeled point sets {Xk,Yk}\{X_k,Y_k\}

1: C←∅C \leftarrow\varnothing 2: for each point ii do 3: &nbsp;&nbsp;&nbsp;&nbsp;if C=∅C = \varnothing then 4: &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;create a new cluster with centroid f⃗←f⃗i\vec{f} \leftarrow\vec{f}_i 5: &nbsp;&nbsp;&nbsp;&nbsp;else 6: &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;k∗←arg⁡min⁡k∈C∥f⃗i−f⃗k∥2k^{*} \leftarrow\underset{k \in C}{\arg\min}\lVert\vec{f}_i - \vec{f}_k \rVert_2 7: &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;if ∥f⃗i−f⃗k∗∥2≤τinst\lVert\vec{f}_i - \vec{f}_{k^{*}} \rVert_2 \leq\tau_{\mathrm{inst}} then 8: &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;assign point ii to cluster k∗k^{*} 9: f⃗k∗←f⃗k∗⋅nk∗+f⃗ink∗+1\vec{f}_{k^*} \leftarrow\frac{\vec{f}_{k^*} \cdot n_{k^*} + \vec{f}_i}{n_{k^*}+1} ▹ incremental running mean

10: else if ∣C∣<Kmax⁡|C| < K_{\max} then

11: create a new cluster with centroid f⃗←f⃗i\vec{f} \leftarrow\vec{f}_i

12: end if

13: end if

14: end for

Two further filters run around this clustering stage. First, any cluster retaining three points or fewer is discarded as sensor noise prior to fitting, since a two-point set cannot support a statistically meaningful regression. Second, a region-of-interest mask, enforced via a calibrated polygon test against the drivable corridor, rejects points that fall outside the physically relevant track area both before and after clustering — suppressing off-track clutter and adjacent-lane bleed-through at the image periphery. A dedicated far-field suppression rule additionally discards any cluster whose points lie overwhelmingly (≥90%\ge90\%) above a fixed BEV row threshold, since such clusters typically correspond to unstable horizon-line artifacts rather than the near-field lane segment relevant to control.

Adaptive-Order Fitting and Nearest-Boundary Selection

Rather than committing every detected cluster to the fixed second-degree model of Eq. (46), the pipeline selects the polynomial order according to point count: sparse clusters (6–16 points) receive a first-degree (linear) fit, while well-populated clusters (> 16 points) receive the full second-degree fit. This prevents an under-constrained quadratic from chasing detector noise on short, partially-occluded lane segments — a frequent occurrence on a 1:10 scale track where motion blur and occlusion routinely truncate the visible lane length.

Because the network can, and does, emit several simultaneous line-cluster candidates per frame — occluded segments, opposing-straightaway bleed-through on hairpin exits, or spurious far-field fragments that survive the filters above — the pipeline does not naively take the two most confident clusters as “left” and “right.” Instead, every surviving fit is evaluated at the vehicle’s projected center row, and the pipeline independently selects the fit whose lateral position is nearest to the center line on the left and on the right, discarding all remaining candidates as scene clutter. This center-adjacency heuristic is what keeps the controller stable even in frames where the network reports four or five simultaneous line candidates.

When only one boundary survives this selection — for example, at a hairpin apex where the outer line briefly leaves the field of view — the centerline is reconstructed by offsetting the single available fit using a lane-width estimate. This width is tracked dynamically from the most recent frames in which both boundaries were visible, clamped to a physically plausible band of 60–100 px in BEV space, and falls back to a fixed default of 70 px whenever the running estimate leaves this band. This graceful-degradation path is what prevents a momentary single-line detection from producing a discontinuous jump in the reported centerline.

Finally, the pixel-domain lateral offset (Eq. (46)) and heading angle (Eq. (47)) are not physically meaningful on their own: both are converted to real-world units by calibration constants obtained from the same homography used for the IPM warp in § 4.6, before being packed and transmitted over the 30 Hz ROS control topic served by the shared-memory bus described in § 4.9.

Figure 17 shows a representative output of the full post-processing chain. After Inverse Perspective Mapping rectifies the perspective distortion, the greedy embedding-affinity clustering (Alg. 1) resolves the scene into discrete instances, and the adaptive-order polynomial fitter produces the three curves used by the controller: the left boundary (blue), the right boundary (red, dashed), and the synthesized centerline (green) computed by averaging the boundary coefficients. In the rectified BEV domain the physically parallel lane boundaries appear vertical and near-parallel, confirming that the homography is well calibrated; residual convergence at the top of the frame is the expected far-field noise that the weighted spatial averaging over y∈[220,300]y \in[220, 300] is designed to attenuate.

Bird’s Eye View output of the post-processing pipeline

Figure 17. Bird’s Eye View output of the post-processing pipeline. Clustered lane points are rectified via the homography HIPM\mathbf{H}_{\mathrm{IPM}} and fitted with adaptive-order polynomials: left boundary (blue), right boundary (red, dashed), and the synthesized centerline (green). The near-vertical, parallel boundaries validate the IPM calibration.

Traffic Sign and Signal Perception

Due to the scarcity of high-quality annotated data for 1:10 scale traffic signs within the target environment, relying strictly on single-frame neural network classification yielded unacceptable variance (flickering). To guarantee robust perception, we implemented a hybrid depth-gated approach coupled with an aggressive temporal smoothing architecture.

Depth-Gated Validation. The RealSense D455 provides aligned depth and RGB maps. Instead of relying purely on 2D bounding box dimensions to estimate sign proximity, we sample the raw depth tensor (scaled by 0.001 meters/unit) at the centroid of the detected sign. Sign bounding boxes reporting distances beyond a physical limit of 0.85 meters are immediately rejected. This depth-gating prevents the vehicle from reacting prematurely to signs in adjacent lanes or far down the track.

Temporal Voting Filter (FIFO Buffer). To overcome classification instability from the sparse training data, we treat the detection stream as a noisy time-series signal. We engineered a sliding-window First-In-First-Out (FIFO) buffer of length N=5N = 5.

Let CtC_t be the raw classification at time tt. The final published classification OtO_t is dictated by a strict majority voting threshold:

Ot={Ciif ∑k=0N−1δ(B[k],Ci)>⌊N/2⌋⊥ (Null)otherwise(49)O_t = \begin{cases} C_i & \text{if } \sum_{k=0}^{N-1}\delta(B[k], C_i) > \lfloor N/2 \rfloor\\ \bot\ \text{(Null)} & \text{otherwise} \end{cases} \tag*{(49)}

This hysteresis loop guarantees that temporary occlusions or motion blur do not trigger an erroneous state change, dramatically smoothing the behavioral transitions of the vehicle. Figure 18 illustrates the filter.

FIFO Temporal Filter

Figure 18. The FIFO Temporal Filter mitigates single-frame detection errors, translating noisy, data-starved classifier outputs into robust, state-machine-ready signals.

Crosswalk Detection

Pedestrian crosswalks require a fundamentally different treatment from the parametric lane model. A crosswalk is not a continuous boundary to be tracked but a discrete, safety-critical region that must trigger a deterministic stop-and-yield behavior. We therefore run crosswalk detection as an independent perception path rather than folding it into the PINet lane head, so that a crosswalk can be flagged even in frames where the lane geometry is momentarily ambiguous.

Detection and Geometric Validation. The detector operates on the rectified Bird’s Eye View, where the characteristic ladder pattern of a crosswalk becomes a set of near-rectangular, regularly spaced stripes of consistent orientation and area. Candidate stripes are extracted and then subjected to a geometric consistency test: an isolated bright rectangle is rejected, whereas a group of stripes sharing a common orientation, comparable area, and uniform inter-stripe spacing is accepted as a crosswalk. This structural voting scheme is deliberately conservative—demanding a repeating pattern rather than a single marking—so that the isolated dashes of a lane boundary or a specular reflection on the floor cannot masquerade as a crossing. Figure 19 shows a representative detection, in which each validated stripe is bounded by a green box in the BEV frame.

Crosswalk detection in the rectified BEV frame

Figure 19. Crosswalk detection in the rectified BEV frame. Each validated stripe of the repeating ladder pattern is bounded by a green box; the isolated marking at the bottom of the frame is correctly excluded by the group-consistency test, preventing a single stripe or floor reflection from triggering a false stop.

Behavioral Signaling. Upon a validated detection, the crosswalk node updates a boolean status flag in shared memory and immediately notifies the control node, which allows the vehicle to execute a controlled deceleration before the marked region rather than reacting only once the stripes are directly beneath the chassis. Because the crosswalk flag is transmitted over the same zero-copy shared-memory channel described in § 4.9, this safety-critical signal incurs no serialization latency and remains synchronized with the lane and sign outputs on a per-frame basis.

High-Throughput IPC Architecture

A critical bottleneck in complex ROS 1 environments is the latency and CPU overhead caused by TCP/IP serialization of large image payloads. To resolve this, we bypassed the standard sensor_msgs/Image pipelines. Both the Lane Perception and Sign Perception nodes access raw 480×848×3480 \times848 \times3 camera tensors directly via POSIX Shared Memory (multiprocessing.shared_memory). This zero-copy Inter-Process Communication (IPC) strategy reduces frame distribution latency to sub-millisecond levels, ensuring the entire perception stack operates synchronously at 30 FPS.

Experimental Results and Validation

Training Convergence Analysis

The model’s learning trajectory validates our two-stage training strategy.

Rapid Adaptation (Epochs 0-8): Sharp initial decline in loss using pre-trained weights.

Optimal Performance Window (Epochs 8-21): Loss minimized to its lowest sustainable point. Our custom dataset prevented overfitting.

Stability: Post-epoch 21, the manual learning rate scheduler prevented “exploding gradient” issues.

Figure 20 shows the training loss over epochs.

Training loss over epochs

Figure 20. Training loss over epochs. The period highlighted between Epochs 8 and 21 demonstrates the model achieving maximum accuracy without succumbing to overfitting constraints.

Qualitative Domain Adaptation

Figure 21a shows the model inference on the public TuSimple dataset. Figure 21b demonstrates the model operating in our custom lab environment. Despite differences in lighting and texture, the network successfully generalized its lane instance extraction capabilities. The most demanding test of this transfer is not the dataset the network was pre-trained on, but the raw competition track under laboratory lighting, where reflective flooring, hard shadows, and the vehicle’s own chassis occlusion are all present in a single frame.

End-to-End Lab Inference and Instance Separation

The clearest evidence that the trained network operates correctly on the physical platform is its in-lab output on live frames. Figure 22 overlays the network’s key point predictions—after the greedy embedding-affinity clustering of Algorithm 1—directly onto a raw track frame captured in the lab. The two lane boundaries are cleanly resolved into distinct instances, rendered here in blue (left boundary) and green (right boundary). Critically, the discriminative embedding loss (Eq. (42)) has done exactly what it was trained to do: even though the two point sets converge toward a common vanishing region near the crosswalk, the network assigns them to separate instances rather than merging them, and it correctly suppresses spurious detections on the reflective foreground and the static chassis in the lower frame. Read together with the domain-adaptation panels of Figure 21, this confirms that both the feature extractor and the instance-separation head transferred successfully from the public pre-training data to the competition environment.

TuSimple Dataset

(a) TuSimple Dataset

Custom Lab Track

(b) Custom Lab Track

Figure 21. Domain Adaptation Results. (a) Successful instance segmentation on the TuSimple pre-training dataset. (b) Successful transfer to the custom competition environment, handling distinct texture and lighting changes dynamically without ROI cropping.

PINet output in the lab environment

Figure 22. PINet output in the lab environment. Clustered key points are overlaid on a raw competition-track frame, with the two lane boundaries separated into distinct instances (blue: left, green: right) by the embedding-guided clustering stage. Foreground reflections and the chassis occlusion are correctly rejected.

This image-space clustered output is precisely the input consumed by the post-processing pipeline of § 4.6: the same blue/green instances, once passed through Inverse Perspective Mapping and adaptive-order polynomial fitting, yield the rectified Bird’s Eye View trajectory of Figure 17. The two figures therefore represent consecutive stages of a single frame’s journey from raw pixels to controller-ready state variables.

Control and Localization

Accurate localization is essential for enabling the autonomous vehicle to determine its position, orientation, and motion relative to the environment. In this system, vehicle dynamics are modeled using the Ackermann steering mechanism and the standard Bicycle Kinematic Model [25], which provides a mathematically tractable representation of motion for ground vehicles with small steering angles. This model is used both for localization estimates and as a foundation for motion planning and feedback-based correction.

Vehicle Kinematic Modeling

The bicycle model treats the vehicle as a single-track system with a front and rear wheel aligned along a common centerline. The steering input is applied at the front wheel, and the resulting motion is governed by geometric constraints imposed by the fixed wheelbase.

Figure 23 illustrates the kinematic abstraction of the vehicle used in this project.

Bicycle model representation of the vehicle kinematics

Figure 23. Bicycle model representation of the vehicle kinematics.

Following the Ackermann steering relationship, the steering geometry for the equivalent bicycle model may be expressed as:

cot⁡(δ)=cot⁡(δin)+cot⁡(δout)2(50)\cot(\delta) = \frac{\cot(\delta_{\mathrm{in}}) + \cot(\delta_{\mathrm{out}})}{2} \tag*{(50)}

Since R=L/tan⁡δR = L/\tan\delta is the turning radius of the rear-axle centre, this is the mean of the two wheel radii measured on the rear-axle line, and it is exact when the wheel axes meet at a common point on that line.

Given the front steering angle δ\delta and linear velocity VV, the kinematic state of the vehicle is represented by three parameters: xx (longitudinal position), yy (lateral position), and θ\theta (vehicle heading).

Depending on the reference point used (front axle or rear axle), the equations of motion are expressed as follows:

Front-Axle Model

x˙front=Vcos⁡(θ−δ)(51)\dot{x}_{\mathrm{front}} = V \cos(\theta- \delta) \tag*{(51)}
y˙front=Vsin⁡(θ−δ)(52)\dot{y}_{\mathrm{front}} = V \sin(\theta- \delta) \tag*{(52)}
θ˙=VLsin⁡(δ)(53)\dot{\theta} = \frac{V}{L}\sin(\delta) \tag*{(53)}

Rear-Axle Model

x˙rear=Vcos⁡(θ)(54)\dot{x}_{\mathrm{rear}} = V \cos(\theta) \tag*{(54)}
y˙rear=Vsin⁡(θ)(55)\dot{y}_{\mathrm{rear}} = V \sin(\theta) \tag*{(55)}
θ˙=VLtan⁡(δ)(56)\dot{\theta} = \frac{V}{L}\tan(\delta) \tag*{(56)}

Here, LL denotes the vehicle wheelbase, VV is the linear velocity of the equivalent rear wheel (at the rear-axle center), V′V' is the linear velocity of the equivalent front wheel (at the front-axle centre), θ\theta is the vehicle orientation, and δ\delta is the front steering angle. These expressions provide a motion-level representation for predicting the vehicle’s pose based on its current control inputs.

Steering-Limited Path Design

Due to the mechanical constraints of the Ackermann steering layout, the vehicle cannot arbitrarily translate between arbitrary pose configurations with a single steering command. Instead, its feasible motion is limited by the wheelbase LL and maximum steering angle δmax⁡\delta_{\max}. As a result, transitioning between two configurations typically requires a sequence of bounded curvature maneuvers [26].

Figure 24 illustrates an example of repositioning the vehicle using up to three maximum-steering arcs, a common strategy when a single steering motion is insufficient.

Vehicle relocation between two poses using up to three steering segments with maximum steering angle

Figure 24. Vehicle relocation between two poses using up to three steering segments with maximum steering angle.

The minimum turning radius imposed by the vehicle geometry is:

Rmin⁡=Ltan⁡(δmax⁡)(57)R_{\min} = \frac{L}{\tan(\delta_{\max})} \tag*{(57)}

To represent motion between an initial pose (xi,yi,θi)(x_i, y_i, \theta_i) and target pose (xj,yj,θj)(x_j, y_j, \theta_j), the path may be decomposed into three sequential curvature segments of angular change Δφ1\Delta\varphi_1, Δφ2\Delta\varphi_2, and Δφ3\Delta\varphi_3, where:

θi−θj=Δφ1+Δφ2+Δφ3(58)\theta_i - \theta_j = \Delta\varphi_1 + \Delta\varphi_2 + \Delta\varphi_3 \tag*{(58)}

Let the cumulative displacement vector be:

r⃗=[ΔxΔy]=Rmin⁡(M1[sin⁡(Δφ1)cos⁡(Δφ1)−1]−M2[sin⁡(Δφ2)cos⁡(Δφ2)−1]+M3[sin⁡(Δφ3)cos⁡(Δφ3)−1])(59)\vec{r} = \begin{bmatrix} \Delta x \\ \Delta y \end{bmatrix} = R_{\min}\left( M_1 \begin{bmatrix} \sin(\Delta\varphi_1) \\ \cos(\Delta\varphi_1)-1 \end{bmatrix} - M_2 \begin{bmatrix} \sin(\Delta\varphi_2) \\ \cos(\Delta\varphi_2)-1 \end{bmatrix} + M_3 \begin{bmatrix} \sin(\Delta\varphi_3) \\ \cos(\Delta\varphi_3)-1 \end{bmatrix} \right) \tag*{(59)}

where M1M_1, M2M_2, and M3M_3 are rotation matrices corresponding to the vehicle orientation at the start of each segment:

M1=[cos⁡(θi)−sin⁡(θi)sin⁡(θi)cos⁡(θi)](60)\mathbf{M}_1 = \begin{bmatrix} \cos(\theta_i) & -\sin(\theta_i) \\ \sin(\theta_i) & \cos(\theta_i) \end{bmatrix} \tag*{(60)}
M2=[cos⁡(θi−Δφ1)−sin⁡(θi−Δφ1)sin⁡(θi−Δφ1)cos⁡(θi−Δφ1)](61)\mathbf{M}_2 = \begin{bmatrix} \cos(\theta_i-\Delta\varphi_1) & -\sin(\theta_i-\Delta\varphi_1) \\ \sin(\theta_i-\Delta\varphi_1) & \cos(\theta_i-\Delta\varphi_1) \end{bmatrix} \tag*{(61)}
M3=[cos⁡(θi−Δφ1−Δφ2)−sin⁡(θi−Δφ1−Δφ2)sin⁡(θi−Δφ1−Δφ2)cos⁡(θi−Δφ1−Δφ2)](62)\mathbf{M}_3 = \begin{bmatrix} \cos(\theta_i-\Delta\varphi_1-\Delta\varphi_2) & -\sin(\theta_i-\Delta\varphi_1-\Delta\varphi_2) \\ \sin(\theta_i-\Delta\varphi_1-\Delta\varphi_2) & \cos(\theta_i-\Delta\varphi_1-\Delta\varphi_2) \end{bmatrix} \tag*{(62)}

Figure 25 provides a geometric representation of the three-segment motion formulation.

Geometric and mathematical representation of three-segment steering motion

Figure 25. Geometric and mathematical representation of three-segment steering motion.

This three-segment maneuver is specifically required for repositioning the vehicle into a region on its right side that cannot be reached through a single ordinary steering command, due to the geometric constraints imposed by the vehicle’s minimum turning radius. Repositioning into the mirror-symmetric region on the left side does not require re-deriving the maneuver: it suffices to mirror the target displacement about the vehicle’s longitudinal axis, solve Eq. (59) for that mirrored displacement to obtain Δφ1\Delta\varphi_1, Δφ2\Delta\varphi_2, Δφ3\Delta\varphi_3, and then negate all three resulting angular changes (i.e., use −Δφ1-\Delta\varphi_1, −Δφ2-\Delta\varphi_2, −Δφ3-\Delta\varphi_3) to obtain the angular changes that actually produce the desired displacement into the left-side region. See Figure 26.

Example three-segment steering maneuvers

Figure 26. Example three-segment steering maneuvers (tangent circular arcs) for different Δφ1\Delta\varphi_1, Δφ2\Delta\varphi_2, Δφ3\Delta\varphi_3 combinations, illustrating repositioning into regions unreachable by a single steering command.

A special case frequently encountered in structured environments (e.g., four-way intersections) is the 90° right turn with constant curvature. In this maneuver:

∣Δφ1∣=∣Δφ3∣(63)\lvert\Delta\varphi_1\rvert= \lvert\Delta\varphi_3\rvert \tag*{(63)}

This symmetry simplifies analytic evaluation and enables computation of the turn using closed-form expressions for geometry and displacement. In practical use, this model is executed with steering saturated at δmax⁡\delta_{\max} and verified using gyroscope feedback.

Asymmetric steering limits. Eq. (59) uses the same minimum radius Rmin⁡R_{\min} for all three arcs. This is appropriate when the vehicle moves according to the bicycle model with Ackermann-type steering and the maximum steering angle is the same in both directions. If the maximum wheel angle differs between turning to the right (positive limit) and turning to the left (negative limit), the minimum radius Rmin⁡=L/tan⁡∣δmax⁡∣R_{\min} = L/\tan\lvert\delta_{\max}\rvert takes a different value for each direction, Rmin⁡,wposR_{\min,w}^{\mathrm{pos}} and Rmin⁡,wnegR_{\min,w}^{\mathrm{neg}}, where w=r,lw = r,l denotes the reference wheel: the right wheel for repositioning into a region on the right side of the vehicle, and the left wheel for repositioning into a region on the left side of the vehicle. Two cases are distinguished. Case 1: The difference is not negligible. Each arc uses the radius of its own steering direction:

[ΔxΔy]=Rmin⁡,rposM1[sin⁡(Δφ1)cos⁡(Δφ1)−1]−Rmin⁡,rnegM2[sin⁡(Δφ2)cos⁡(Δφ2)−1]+Rmin⁡,rposM3[sin⁡(Δφ3)cos⁡(Δφ3)−1](64)\begin{bmatrix} \Delta x \\ \Delta y \end{bmatrix} = R_{\min,r}^{\mathrm{pos}} M_1 \begin{bmatrix} \sin(\Delta\varphi_1) \\ \cos(\Delta\varphi_1)-1 \end{bmatrix} - R_{\min,r}^{\mathrm{neg}} M_2 \begin{bmatrix} \sin(\Delta\varphi_2) \\ \cos(\Delta\varphi_2)-1 \end{bmatrix} + R_{\min,r}^{\mathrm{pos}} M_3 \begin{bmatrix} \sin(\Delta\varphi_3) \\ \cos(\Delta\varphi_3)-1 \tag*{(64)} \end{bmatrix}

For a region on the left side of the vehicle, the corresponding relation follows the same mirroring procedure described earlier: mirror the target displacement about the vehicle’s longitudinal axis, solve as in Eq. (64), then negate the resulting Δφ1\Delta\varphi_1, Δφ2\Delta\varphi_2, Δφ3\Delta\varphi_3, only now using the left-side radii Rmin⁡,lnegR_{\min,l}^{\mathrm{neg}}, Rmin⁡,lposR_{\min,l}^{\mathrm{pos}}, Rmin⁡,lnegR_{\min,l}^{\mathrm{neg}} in place of Rmin⁡,rposR_{\min,r}^{\mathrm{pos}}, Rmin⁡,rnegR_{\min,r}^{\mathrm{neg}}, Rmin⁡,rposR_{\min,r}^{\mathrm{pos}}.

Case 2: The difference is small enough that the two radii can be averaged.

Rˉmin⁡,r=Rmin⁡,rpos+Rmin⁡,rneg2,Rˉmin⁡,l=Rmin⁡,lpos+Rmin⁡,lneg2(65)\bar{R}_{\min,r} = \frac{R_{\min,r}^{\mathrm{pos}} + R_{\min,r}^{\mathrm{neg}}}{2}, \qquad\bar{R}_{\min,l} = \frac{R_{\min,l}^{\mathrm{pos}} + R_{\min,l}^{\mathrm{neg}}}{2} \tag*{(65)}

For a region on the right side of the vehicle, Eq. (59) is retained with Rmin⁡R_{\min} replaced by Rˉmin⁡,r\bar{R}_{\min,r}. For a region on the left side of the vehicle, following the same mirroring procedure described earlier, Eq. (59) is retained with Rmin⁡R_{\min} replaced by Rˉmin⁡,l\bar{R}_{\min,l}.

Navigation and Steering Control Algorithms

To maintain stable, lane-centered navigation under varying road topologies and speeds, multiple feedback controllers [27] are implemented in the vehicle’s decision and actuation loop. Each controller is selected for its specific performance characteristics and switching between them is performed according to the vehicle state and operational mode.

Figure 27 illustrates the two primary geometric feedback quantities used in lane-keeping control: the Cross-Track Error (CE), the lateral deviation of the vehicle center from the lane reference centerline, and the Heading Error (HE), the angular misalignment between the vehicle’s orientation and the tangent direction of the lane.

Geometric interpretation of cross-track and heading errors used in lane-keeping control

Figure 27. Geometric interpretation of cross-track and heading errors used in lane-keeping control.

PID Controller. A PID controller [28] is used primarily as a backup steering mechanism and for regulating the forward velocity. The steering command is computed as:

Steer=kp⋅CE+kd⋅HE+ki∫CE dt(66)\mathrm{Steer} = k_{\mathrm{p}} \cdot\mathrm{CE} + k_{\mathrm{d}} \cdot\mathrm{HE} + k_{\mathrm{i}} \int\mathrm{CE}\,\mathrm{d}t \tag*{(66)}

In final deployment, PID also regulates speed: when heavy curvature is detected, the proportional component automatically reduces forward velocity, improving cornering stability. A higher-priority obstacle-response P-regulator supersedes normal lane PID when vision or onboard sensors detect objects obstructing the path.

Stanley Controller. The Stanley controller [29, 30] is the primary steering law used for continuous lane following due to its kinematic and speed-adaptive formulation. The steering angle is computed as:

Steer=khe⋅HE+tan⁡−1(kce⋅CEvf+ks)(67)\mathrm{Steer} = k_{\mathrm{he}} \cdot\mathrm{HE} + \tan^{-1}\left(\frac{k_{\mathrm{ce}} \cdot\mathrm{CE}}{v_{\mathrm{f}} + k_{\mathrm{s}}}\right) \tag*{(67)}

where vfv_{\mathrm{f}} is the forward velocity and ksk_{\mathrm{s}} prevents singular behavior near zero speed.

This model-based formulation provides smoother transitions during acceleration and deceleration, reducing oscillation at higher speeds. Its direct grounding in the vehicle geometry makes it superior to a pure PID controller for continuous closed-loop lane tracking.

Pure Pursuit Controller. The Pure Pursuit controller [31, 32] computes the steering angle based on a geometric look-ahead point projected onto the lane reference, determined from the rear-axle position (Figure 28). The steering angle is calculated as

Pure Pursuit geometry illustrating the look-ahead point, look-ahead distance, and heading error angle

Figure 28. Pure Pursuit geometry illustrating the look-ahead point, look-ahead distance, and heading error angle.

Steer=tan⁡−1(2sin⁡α⋅Lkpp⋅vf)(68)\mathrm{Steer} = \tan^{-1}\left(\frac{2\sin\alpha\cdot L}{k_{\mathrm{pp}} \cdot v_{\mathrm{f}}}\right) \tag*{(68)}

where LL is the vehicle wheelbase, ld=kpp⋅vfl_{\mathrm{d}} = k_{\mathrm{pp}} \cdot v_{\mathrm{f}} is the look-ahead distance, kppk_{\mathrm{pp}} is the look-ahead gain, and vfv_{\mathrm{f}} is the forward velocity. The angle α\alpha is defined as the angle between the vehicle’s longitudinal heading direction and the line connecting the rear-axle position to the look-ahead point on the lane reference.

This method is particularly effective for tracking curved roads and complex trajectories, as it determines the steering command from the geometric relationship between the vehicle and the look-ahead point. The speed-dependent look-ahead distance allows the controller to adapt its tracking behavior to the vehicle’s forward speed.

Gain Calibration. Controller parameters for PID, Stanley, and Pure Pursuit were tuned empirically through iterative testing. Real-time visualization of control variables was used while adjusting responsiveness, lateral settling time, overshoot, and high-speed stability. The final gains delivered fast, non-oscillatory velocity control, minimal steady-state lane deviation at moderate/high speeds, and smooth cornering without abrupt steering.

Decision Making, Parking, and Intersection Handling

The decision layer of the vehicle converts high-level perception outputs into discrete driving actions. All logic is structured as a Finite State Machine (FSM) that evaluates lane geometry, traffic signs, crosswalk signals, and feedback from onboard sensors, then transitions between states such as lane following, parking, and intersection traversal.

Parking Behavior

During normal navigation, the vehicle continues lane following until a parking sign is detected by the vision subsystem. Once detected, the FSM transitions into the parking sequence shown in Figure 29. The vehicle searches for a valid free space using onboard sensors and begins the maneuver as soon as a suitable location is detected.

Finite State Machine for autonomous parking behavior

Figure 29. Finite State Machine for autonomous parking behavior.

The parking process consists of two main phases.

Open-Loop Geometric Maneuver: An open-loop geometric maneuver was developed to achieve the required vehicle alignment after turning. The maneuver consists of three sequential phases: maximum steering for trajectory insertion, straight displacement for position adjustment, and counter-steering for final heading correction. The motion parameters were calculated using the kinematic bicycle model and further refined through empirical calibration to compensate for practical effects such as tire slip, actuator delay, and mechanical backlash, resulting in repeatable and accurate maneuver execution.

Feedback-Based Correction: Once the initial maneuver completes, sensor data (vision and IMU) are used to refine the final position. The negative feedback loop corrects residual lateral and angular error to ensure precise alignment within the parking space.

Figure 30 illustrates example end states for parallel and perpendicular (cross) parking.

Example parking results: left – parallel parking; right – perpendicular (cross) parking.

Figure 30. Example parking results: left – parallel parking; right – perpendicular (cross) parking.

Sign-Approach Distance Estimation on Curved Paths

Correctly estimating the remaining distance to a sign matters beyond the intersection maneuver of § 5.4.3: the same estimate also drives the initial approach of the parking maneuver (§ 5.4.1). Once the park sign is detected, the vehicle must advance by the corresponding encoder distance so that its side-facing distance sensor reaches approximately the correct position; specifically, the free-space search algorithm for parking is initiated about 10 cm past the point where the park sign was first sighted. In this case, as in the intersection approach, the straight-line distance reported by the vision pipeline is not, in general, the distance the vehicle will actually travel: whenever the road curves during the approach, the vehicle follows a curved path rather than a straight line toward the sign, which the procedure below accounts for.

As long as the vision pipeline reports a valid sign distance, the vehicle drives directly on that live reading. From the moment this data is lost — or is deliberately no longer used — the required travel distance is instead estimated from three quantities available onboard: the yaw rate θ˙\dot{\theta} obtained from the compass (CMPS), the vehicle speed VV, and the last valid sign distance DD reported by vision before tracking was lost. Three cases are distinguished, summarized in Table 8 and detailed below.

CaseConditionAction
1∣θ˙∣<θ˙min⁡|\dot{\theta}| < \dot{\theta}_{\min}Path assumed locally straight: advance the last known sign distance DD directly, commanded by the encoder.
2∣θ˙∣>θ˙min⁡|\dot{\theta}| > \dot{\theta}_{\min} and D≥RD \ge RCurve detected, but the arcsine relation of Case 3 is outside its valid domain (D/R≥1D/R \ge1); fall back to the same straight-line action as Case 1.
3∣θ˙∣≥θ˙min⁡|\dot{\theta}| \ge\dot{\theta}_{\min} and D≤RD \le RCurve detected and the sign lies within the estimated turn radius: use the arc-length correction of Eqs. (70)–(71) instead of the raw distance DD.

Table 8. Sign-approach distance estimation: case selection based on yaw rate and the vision-reported distance.

Case 1 — straight path. If the magnitude of the yaw rate falls below a minimum threshold θ˙min⁡\dot{\theta}_{\min}, the road is assumed to be locally straight for the remainder of the approach. The vehicle then simply continues toward the sign using the last known distance DD, commanding the encoder to cover exactly that distance.

Cases 2–3 — curved path. Otherwise the vehicle is in a curve, and the local radius of curvature is estimated from the current motion state as

R=Vθ˙(69)R = \frac{V}{\dot{\theta}} \tag*{(69)}

If D>RD > R (Case 2), the sign lies beyond the estimated turn radius; the correction below is outside its valid range, so the vehicle falls back to the same straight-line action as Case 1, using DD directly.

Case 3 — within-radius correction. If D≤RD \le R, the sign lies within the estimated turn radius, and the raw distance DD increasingly underestimates the true travel distance as the path curves more sharply. Assuming the curvature does not change appreciably before the sign is reached (Figure 31), the subtended arc angle is

Geometry relating the estimated turn radius R, the vision-reported forward distance D, and the subtended angle θ

Figure 31. Geometry relating the estimated turn radius RR, the vision-reported forward distance DD, and the subtended angle θ\theta.

θ=sin⁡−1(D⋅θ˙V)=sin⁡−1(DR)(70)\theta= \sin^{-1}\left(\frac{D \cdot\dot{\theta}}{V}\right) = \sin^{-1}\left(\frac{D}{R}\right) \tag*{(70)}

and the encoder is commanded to travel the corresponding arc length rather than the chord distance DD:

dencoder=R⋅θ(71)d_{\mathrm{encoder}} = R \cdot\theta \tag*{(71)}

which gives a more accurate stopping distance than driving the raw vision distance DD whenever the path curvature is significant relative to the remaining distance to the sign.

Intersection Control and Turning Logic (Version 4)

When approaching an intersection, the correct maneuver is determined based on the recognized sign. The three supported actions (turn right, turn left, and go straight) are shown in Figure 32.

Available intersection actions: right turn, left turn, forward crossing.

Figure 32. Available intersection actions: right turn, left turn, forward crossing.

Once sign data is first acquired, the vehicle enters the “Intersection” state, and the maneuver proceeds through three stages: (1) approach and crosswalk-data acquisition, (2) pose correction relative to the crosswalk, and (3) turn execution. Each stage is described below and illustrated in Figure 33 and Figure 35.

Stage 1 sign-distance approach and crosswalk-data acquisition flowchart

Figure 33. Stage 1 — sign-distance approach and crosswalk-data acquisition at the crosswalk inner edge, with a single probing retry.

Stage 2 — iterative crosswalk pose correction, and Stage 3 — turn execution: From Stage 1: valid crosswalk data, vehicle at inner edge; Measure crosswalk-approach angle (vision, or CMPS-model estimate fused with the latest vision reading); Angle within allowed tolerance? Yes: Enter Stage 3: advance straight (encoder) until aligned behind crosswalk. No: Corrective feedback: shift vehicle left, away from right-hand edge; re-measure. Turn direction? Right: Right turn: solve three-steering geometry (Sec. 5.2) for CMPS₁,₂,₃; Apply CMPS₁, CMPS₂, CMPS₃ sequentially. Left: Left turn: rotate CCW by fixed CMPS corresponding to 90° ± δ. Intersection maneuver complete.

Figure 35. Stage 2 — iterative crosswalk pose correction, and Stage 3 — turn execution (unchanged).

Stage 1 — Approach and crosswalk-data acquisition (Figure 33). Because the probability of losing the sign from the camera’s field of view increases as the vehicle gets closer to it, the approach to the sign is not driven by live vision data alone: at each control step the vehicle checks whether current sign-distance vision data is valid, advancing on that live reading if so, and otherwise falling back to the curvature-based estimate of § 5.4.2. As the vehicle nears the hood-to-sign offset (≈10\approx10 cm), it begins polling for crosswalk data as well.

When the vehicle subsequently reaches ≈10\approx10 cm from the crosswalk’s inner edge, it checks whether crosswalk vision data is currently valid. If so, it advances the remaining distance to the inner edge on that data directly. If not — whether because crosswalk data had been in use and was suddenly lost, or because no crosswalk data had been received at all since the 10 cm point — the vehicle instead completes the approach by dead reckoning: in the first case, from the encoder progress accumulated since the 10 cm point; in the second, from the full curvature-based estimate of § 5.4.2.

Once at the crosswalk inner edge, the vehicle queries crosswalk data once more. If it is valid, the vehicle proceeds directly to Stage 2. If not, a single probing feedback maneuver is executed and the query is repeated exactly once; if crosswalk data is still unavailable after this one retry, the vehicle is considered to be in an unsafe state, since the direction and magnitude of any approach skew remain unknown.

Stage 2 — Pose correction relative to the crosswalk (Figure 35). With valid crosswalk data available, the crosswalk-approach angle is measured. If it exceeds the allowed tolerance, a corrective feedback maneuver shifts the vehicle a modest amount to the left, away from the right-hand corner of the intersection, and crosswalk data is queried again to re-measure the angle; this correction loop repeats until the angle falls within tolerance (see Figure 34). Rather than requiring a fresh valid vision reading at every iteration, the angle change produced by each feedback step can instead be estimated from the commanded CMPS rotation and fused with the most recent vision reading, reducing dependence on repeated crosswalk detections.

Feedback maneuver design for crosswalk alignment and lateral correction

Figure 34. Feedback maneuver design for crosswalk alignment and lateral correction. The illustrated maneuver positions the vehicle behind the crosswalk while shifting it slightly to the left. Alternative feedback maneuvers can achieve the same positioning and lateral shift without necessarily aligning the vehicle perpendicular to the crosswalk.

Stage 3 — Turn execution (Figure 35). Once the crosswalk-approach angle is within tolerance, the vehicle first advances straight, by encoder count, until it is aligned just behind the crosswalk. The subsequent action depends on the turn direction: for a right turn, the measured crosswalk-approach angle is substituted into the three-steering-wheel geometry of § 5.2 to solve for the three required encoder (CMPS) values, which are then applied to the vehicle in sequence; for a left turn, the vehicle instead performs a single rotation of fixed magnitude, counter-clockwise, corresponding to an encoder travel of 90∘±δ90^\circ\pm\delta, where δ\delta accounts for the observed approach angle.

Evolution of the Intersection-Handling Strategy

The intersection-handling logic was developed iteratively over four versions, moving from a fixed, open-loop maneuver to a closed-loop, kinematically grounded approach. Table 9 summarizes the progression; the following subsections describe each version in more detail.

VersionCore ApproachKey StrengthsKey Limitations
Ver. 1Open-loop, hardcoded actuation triggered at a fixed 10 cm distance from the sign (e.g., advance 30 cm, then turn until a preset heading change is sensed by the compass).Minimal development time.Only IMU feedback; almost entirely dependent on a fixed approach geometry.
Ver. 2Added intersection-approach angle. A PID loop first corrects heading error, then the same hardcoded turning actions as Ver. 1 are executed.Introduces road-relative feedback. Simpler algorithm; fewer tuneable parameters.High perception noise from a low-quality, poorly mounted camera; the vision data was only reliable in a narrow crosswalk viewing position, so the feedback loop had to re-sample it 2–3 times. Failed when the vehicle approached skewed toward the right edge of the road. Could not handle intersections on curved roads. Two-lane validation was added but was not accurate enough to enable feedback after the crosswalk or inside the intersection.
Ver. 3Generalized further: road curvature (κ\kappa) was introduced to estimate the distance the vehicle must travel through a bend to reach the sign, with turn direction (CW/CCW) taken from perception.Broader applicability across road geometries.More parameters to tune and longer development time than earlier versions. Increased generality reduced reliability: at plain, straight intersections the algorithm could still unnecessarily factor in crosswalk angle and curvature, causing failures over part of the intersection design space.
Ver. 4Adopted the vehicle’s bicycle-model kinematics to reach the target lane more reliably and directly, motivated by a reduced maximum steering angle that had been increasing the distance travelled in the opposing lane. Repositioning between an initial and target pose is solved as a three-segment steering maneuver (§ 5.2), with the required encoder (CMPS) travel computed for each segment. The same model-based CMPS estimation is used to judge how far the vehicle must travel through a curve to reach a set distance from the sign, replacing noisy curvature-based perception. A dedicated feedback behavior biases the vehicle slightly left to counteract drift toward the right edge; once the vehicle reaches the crosswalk at a shallow, safe angle with no risk of edge proximity, the three-segment maneuver is executed using the measured crosswalk-approach angle.More reliable: grounded in the actual vehicle kinematic model rather than hardcoded actions. More accurate sign approach in sharp intersection turns, by relying on the vehicle model and CMPS changes instead of noisy curvature perception.Highly sensitive to the mechanical design, steering calibration (§ 5.5) and measurement accuracy (§ 3–§ 5); imprecise geometry directly degrades the open-loop maneuver. Tends to look mechanically stiff, and a reverse segment is less visually natural — a potential drawback for real-world user acceptance.

Table 9. Summary of the four versions of the intersection-handling algorithm.

Version 1 — Open-Loop Baseline. The first version implemented the simplest possible behavior: once the vehicle was within 10 cm of the sign, a hardcoded sequence of actions executed the turn open-loop, with no feedback — for example, advancing 30 cm and then turning until the onboard compass (CMPS) sensed a preset change in heading, at which point the maneuver was considered complete. Its only advantage was a very short development time; it could not compensate for any deviation from the assumed approach geometry.

Version 2 — Heading-Error Feedback. The second version added the intersection-approach angle to the pipeline and began handling the heading error caused by a skewed arrival at the intersection. A PID controller first corrected this heading error, after which the vehicle executed the same type of hardcoded turning actions as version 1. This version incorporated road-relative feedback for the first time and kept the algorithm simple, with few design parameters. However, the camera in use was of low quality and poorly positioned on the chassis, so the processed vision data carried substantial error; because valid-looking data was only obtained from one

specific viewing position relative to the crosswalk, the motion-feedback loop had to re-acquire vision data two to three times per approach. The version performed poorly whenever the vehicle’s pose relative to the road was already skewed toward the right edge, and it could not handle intersections located on curved roads. Two-lane detection was added to verify that the target lane had been reached, but its accuracy was insufficient, so feedback after the crosswalk and inside the intersection itself remained unavailable.

Version 3 — Curvature Generalization. [1] The third version generalized the approach further by introducing an initial implementation of road curvature (κ\kappa) to estimate the distance the vehicle needed to travel through a bend to reach the sign, with the turn direction (clockwise or counter-clockwise) taken from the perception pipeline. This broadened the range of road geometries the algorithm could address, but at the cost of more tunable parameters and a longer development time than the previous versions. More importantly, the added generality reduced reliability: because the logic could still weight crosswalk angle and curvature even at plain, straight intersections, it occasionally failed on a subset of intersection types that the earlier, simpler logic had handled correctly.

Version 4 — Bicycle-Model-Based Maneuvering. The fourth version targeted more reliable motion: reaching the target lane faster and minimizing time spent in the opposing lane. A reduction in the vehicle’s maximum steering angle had made this worse, forcing a long excursion in the wrong lane before the vehicle could correct itself. To address this, the version adopted the vehicle’s motion geometry — the bicycle model (§ 5.1) — based on the observation that the vehicle’s motion lies on a circular arc. This raised the geometric question of how to move between two poses with defined initial and target headings when a single steering action cannot satisfy the constraint; the resulting solution was the three-segment steering maneuver described in § 5.2, in which the required encoder (CMPS) travel for each of the three segments is computed and applied to the vehicle. The same model-based CMPS estimation was reused to judge how far the vehicle should travel through a curve to reach a specified distance from the sign, replacing the noisy curvature-based perception of version 3. A dedicated feedback behavior was added to counter drift toward the right edge of the road: it biases the vehicle slightly to the left and is shaped so that its behavior does not itself tend toward the right edge. Once the vehicle reaches the crosswalk at a shallow, safe angle with no risk of edge proximity, the three-segment maneuver is executed using the measured crosswalk-approach angle.

Grounding the maneuver in the bicycle model made the behavior more reliable overall, since it replaced hardcoded actions with the actual vehicle kinematic model. It also improved accuracy when approaching the sign through sharp intersection turns, since the vehicle no longer depended on curvature data derived from noisy road-curve perception, relying instead on the vehicle model and CMPS changes. The main drawback is that the three-segment maneuver is strongly dependent on the vehicle’s mechanical design and on measurement accuracy (§ 2–§ 5): imprecise geometry directly reduces maneuver accuracy. In addition, the maneuver tends to look mechanically stiff, and when a reverse segment is required it can appear visually unnatural — a consideration for acceptance in real-world use.

Experimental Results and Validation

Steering Calibration

To calibrate the steering mechanism fabricated by the mechanical engineering team and to relate the commanded input uu to the wheel angles, an empirical data collection procedure was conducted in coordination with the embedded control subsystem.

At every setpoint, the combined (equivalent single-track) steering angle δcomb\delta_{\mathrm{comb}} reported below is computed directly from Eq. (50), i.e. as cot⁡−1(cot⁡δleft+cot⁡δright2)\cot^{-1}\left(\frac{\cot\delta_{\mathrm{left}}+\cot\delta_{\mathrm{right}}}{2}\right) applied to each individual trial, rather than as the simple arithmetic mean of the two measured wheel angles, so that it is consistent with the bicycle model of § 5.1.

The embedded controller maps control commands u∈{0,1,…,255}u \in\{0,1,\ldots,255\} to the steering actuator (servo motor). The servo hardware is configured to operate across a nominal angular range of −40∘-40^\circ to +40∘+40^\circ with an estimated positioning variance of σservo2=1.0∘\sigma_{\mathrm{servo}}^2 = 1.0^\circ. A total of 13 discrete control input setpoints were evaluated, spanning from 255 down to 0. At each setpoint, 3 independent measurements were collected for both the left wheel angle (δleft\delta_{\mathrm{left}}) and the right wheel angle (δright\delta_{\mathrm{right}}).

At extreme steering inputs, such as u=0u = 0 and u=255u = 255, the two front wheels turn by different angles (Figure 36), so the steering is not parallel. The equivalent angle δcomb\delta_{\mathrm{comb}} of Eq. (50) is defined for any pair of wheel angles; it therefore calibrates the input of the bicycle model but does not by itself verify the Ackermann condition.

Comparison of Left & Right Steering Raw Data and Combined System Response, showing theta_left_mean, theta_right_mean, and theta_comb_mean

Figure 36. Front-left and front-right wheel steering angles, alongside the equivalent steering angle derived from the Ackermann relationship.

Across the 13 tested setpoints, a one-sample t-test with a significance level of α=0.05\alpha= 0.05 showed that the combined steering angle (δcomb\delta_{\mathrm{comb}}), computed from the per-trial wheel angles via the cot-average relation of Eq. (50) rather than a simple arithmetic average, does not have a statistically significant deviation from the desired servo setpoint in 92.3% (12 of 13 setpoints) of the operating range, with P-values greater than 0.05 at every setpoint except for u=25u = 25. This indicates a high level of agreement between the commanded and measured steering angles. Table 10 and Figure 37 show this result.

Control Input uuCommanded Angle (°)Combined Mean (°)Combined StdDev (°)Combined Var (°)Mean Diff. (°)T-StatisticP-ValueSignificant (α=0.05\alpha= 0.05)
0−40−40.1982.2044.857−0.198−0.1560.8904No
25−32−35.9881.0681.140−3.988−6.4690.0231Yes
50−24−24.9171.0131.027−0.917−1.5670.2577No
75−16−18.2491.3741.887−2.249−2.8350.1051No
90−11−12.3211.8883.565−1.321−1.2110.3494No
115−3−3.5522.8548.146−0.552−0.3350.7696No
1280000001.0000No
15076.3822.1974.825−0.618−0.4880.6741No
1751415.2172.7917.7921.2170.7550.5290No
1901918.9911.5012.253−0.009−0.0100.9931No
2152727.8251.5282.3350.8250.9350.4484No
2403533.4801.8153.293−1.520−1.4510.2839No
2554036.5152.1584.657−3.485−2.7970.1076No

Table 10. Commanded steering angle versus the combined measured steering angle δcomb\delta_{\mathrm{comb}}, computed from the per-trial wheel angles via the cot-average relation of Eq. (50). Statistics are over 3 trials per setpoint; the one-sample tt-test uses α=0.05\alpha= 0.05.

Commanded vs. Measured Steering Angle (combined angle)

Figure 37. Commanded (input) steering angle versus the combined measured steering angle (mean_steer), computed solely from the cot-average of Eq. (50) — the individual per-wheel means are not used in this comparison. The dashed line marks perfect agreement (measured = commanded); error bars show ±1 StdDev across the 3 trials.

Additionally, the alignment of the steering system around the neutral position was checked. The left wheel crosses 0∘0^\circ at u≈129u \approx129 and the right wheel at u≈133u \approx133; at u=128u = 128 the right wheel reads −1.5∘-1.5^\circ and the left wheel −0.2∘-0.2^\circ. Larger angular offsets between the calibrated and the actual wheel angles appear at large ∣δ∣|\delta| as residuals outside the ±2 cm band in Figure 40 and as the systematic trends of Figure 41 (fitted offsets in Table 12). For closed-loop path tracking and high-level trajectory planning the relationship between the 8-bit integer control input uu and the equivalent bicycle steering angle δ\delta is modeled by Eq. (72). This fit is obtained by a direct linear regression of uu against the combined (equivalent single-track) steering angle of Eq. (50), evaluated from the per-trial measurements at each setpoint (as shown in Figure 38) — not by averaging the two individual wheel calibrations below, since a simple arithmetic average of δleft\delta_{\mathrm{left}} and δright\delta_{\mathrm{right}} is not equivalent to Eq. (50). The separate per-wheel calibrations δright(u)\delta_{\mathrm{right}}(u) and δleft(u)\delta_{\mathrm{left}}(u) are also present in Eq. (72), but are not used to derive δ(u)\delta(u).

Control Input vs. Steering Angle Characteristics (Input 0–255)

Figure 38. Linear regression model for control input uu vs. steering angle δ\delta.

StatisticRight wheelLeft wheelPooled
Sample size nn131326
Mean residual eˉ\bar{e} (cm)−0.05-0.05−0.09-0.09−0.07-0.07
Std. deviation ss (cm)2.181.131.70
Std. error of mean (cm)0.600.310.33
95% CI of mean (cm)[−1.36,1.27][-1.36, 1.27][−0.77,0.59][-0.77, 0.59][−0.75,0.62][-0.75, 0.62]
tt-test vs. zero, pp-value0.9410.7760.839
90% CI of mean (cm)[−1.12,1.03][-1.12, 1.03][−0.65,0.47][-0.65, 0.47][−0.64,0.50][-0.64, 0.50]
Equivalence to ±2\pm2 cm (TOST [36] pp)0.004<0.001<0.001<0.001<0.001
Bootstrap [34] 95% CI of mean (cm)[−1.23,1.04][-1.23, 1.04][−0.69,0.50][-0.69, 0.50][−0.73,0.55][-0.73, 0.55]
RMSE (cm)2.091.091.67
MAE (cm)1.510.661.09
Max ∣e∣|e| (cm)5.272.705.27
Limits of agreement [35] (cm)[−4.31,4.22][-4.31, 4.22][−2.30,2.12][-2.30, 2.12][−3.40,3.26][-3.40, 3.26]
Points within ±2\pm2 cm [95% CI, %]10/13 = 77% [46, 95]11/13 = 85% [55, 98]21/26 = 81% [61, 93]
Slope of ee vs. δ\delta (cm/deg)+0.069±0.012+0.069 \pm0.012−0.032±0.010-0.032 \pm0.010—
Correlation rr (pp-value)0.86 (<0.001<0.001)−0.69-0.69 (0.009)—
Shapiro–Wilk [33] pp (normality of ee)0.4400.0200.020
∣z∣≤2|z| \le2 (uncertainty-weighted)12/1313/1325/26
χ2/n\chi^2/n (pp-value)0.77 (0.689)0.22 (0.999)0.49 (0.985)
Best-fit angular offset φ\varphi (deg)−5.4±1.1-5.4 \pm1.1+2.9±0.9+2.9 \pm0.9—
RMSE after offset (cm)1.210.80—

Table 12. Summary statistics of the residuals (all lengths in cm unless noted).

Model prediction and measured lengths for right and left wheels

Figure 40. Model prediction L/cos⁡δL/\cos\delta (line) and measured lengths with ±2\pm2 cm error bars (points).

Residuals versus wheel steering angle for the right and left front wheels

Figure 41. Residual e=dmodel−dmease = d_{\mathrm{model}} - d_{\mathrm{meas}} versus wheel steering angle δ\delta for the right (left panel) and left (right panel) front wheels. The shaded band marks the ±2\pm2 cm measurement tolerance and the dashed line is the linear trend of the residuals (right wheel: +0.069+0.069 cm/deg, r=0.86r = 0.86; left wheel: −0.032-0.032 cm/deg, r=−0.69r = -0.69).

δ(u)=0.3133u−40.923∘,δright(u)=0.3326u−44.089∘δleft(u)=0.2977u−38.295∘(72)\begin{aligned} \delta(u) &= 0.3133u - 40.923^\circ, \\ \delta_{\mathrm{right}}(u) &= 0.3326u - 44.089^\circ\\ \delta_{\mathrm{left}}(u) &= 0.2977u - 38.295^\circ \tag*{(72)} \end{aligned}

Experimental Validation of the Bicycle-Model Geometry

Reliable maneuvering in confined spaces (parking and intersection alignment, § 5.4.3) relies on the vehicle’s kinematic geometry. Our planner assumes the bicycle model of § 5.1, in which the motion is fully determined by the wheelbase LL and the front steering angle δ\delta (for example Rmin⁡=L/tan⁡δmax⁡R_{\min} = L/\tan\delta_{\max}, Eq. (57)). Before building maneuver planning on this assumption, we tested whether the relation actually holds on our physical vehicle, how large the deviations are, and whether they are random or systematic. In short: how does the real vehicle behave compared with the model? The dimensions of the vehicle used in the tests are listed in Table 1. The wheelbase L=25.0±1.0L = 25.0 \pm1.0 cm is the only parameter entering the model prediction.

The steering command was swept from 0 to 255 at 13 setpoints. The steering angle of each front wheel follows the linear calibration of the previous section (§ 5.5.1). At every setpoint and for each front wheel separately, the rolling direction of the wheel was extended on the ground until it met the line through the two rear wheels (the rear-axle line), as illustrated in Figure 39. The length dd of this segment, from the wheel to the intersection point, was measured with an estimated uncertainty of ±2\pm2 cm. One measurement was taken per wheel and setpoint (n=13n = 13 per wheel).

Measured quantity: the segment $d$ from the front wheel, along its rolling direction, to the rear-axle line.

Figure 39. Measured quantity: the segment dd from the front wheel, along its rolling direction, to the rear-axle line.

In the bicycle model the wheelbase LL, the rear-axle line and the wheel’s heading form a right triangle, so the predicted length is as Eq. (73), which equals L=25.5±1.0L = 25.5 \pm1.0 cm at δ=0\delta= 0 and grows as the steering angle increases. The measured value was compared with this prediction at the calibrated angle of each wheel. Table 11 lists the model prediction, the measured length and the residual at every setpoint, and Figure 40 compares them graphically.

Right wheelLeft wheel
uuδ\delta (°)Model (cm)Meas. (cm)ee (cm)δ\delta (°)Model (cm)Meas. (cm)ee (cm)
0−44.09-44.0935.5036.7−1.20-1.20−38.30-38.3032.4930.0+2.49+2.49
25−35.77-35.7731.4336.7−5.27-5.27−30.85-30.8529.7029.5+0.20+0.20
50−27.46-27.4628.7429.3−0.56-0.56−23.41-23.4127.7927.5+0.29+0.29
75−19.14-19.1426.9928.7−1.71-1.71−15.97-15.9726.5226.3+0.22+0.22
90−14.15-14.1526.3027.2−0.90-0.90−11.50-11.5026.0225.8+0.22+0.22
115−5.84-5.8425.6326.1−0.47-0.47−4.06-4.0625.5625.6−0.04-0.04
128−1.52-1.5225.5125.5+0.01+0.01−0.19-0.1925.5025.4+0.10+0.10
1505.8025.6325.5+0.13+0.136.3625.6625.8−0.14-0.14
17514.1226.2925.7+0.59+0.5913.8026.2626.6−0.34-0.34
19019.1126.9926.2+0.79+0.7918.2726.8527.5−0.65-0.65
21527.4228.7326.8+1.93+1.9325.7128.3031.0−2.70-2.70
24035.7331.4128.0+3.41+3.4133.1530.4631.5−1.04-1.04
25540.7233.6531.0+2.65+2.6537.6232.1932.0+0.19+0.19

Table 11. Model prediction, measured length and residual e=dmodel−dmease = d_{\mathrm{model}} - d_{\mathrm{meas}} per steering command uu. Values with ∣e∣>2\lvert e\rvert> 2 cm are in bold.

dmodel(δ)=Lsec⁡(δ)(73)d_{\mathrm{model}}(\delta) = L \sec(\delta) \tag*{(73)}

The results show that the mean model residual was only −0.05-0.05 cm for the right wheel and −0.09-0.09 cm for the left wheel, with no statistically significant mean bias (p=0.941p = 0.941 and p=0.776p = 0.776, respectively). The RMSE was 2.09 cm for the right wheel and 1.09 cm for the left wheel, while 21 of the 26 measurements (81%) were within the ±2\pm2 cm measurement tolerance (Table 12).

Although the average error was small, the residuals showed a systematic dependence on steering angle, as shown in Figure 41. The right-wheel error increased with steering angle (r=0.86r = 0.86), while the left-wheel error showed an opposite trend (r=−0.69r = -0.69); the fitted slopes and correlations are listed in Table 12. This indicates that the main deviation is not purely random and is likely related to a small offset between the calibrated and actual wheel angles. Consistently, fitting a single angular offset φ\varphi per wheel reduces the RMSE from 2.09 to 1.21 cm (right wheel) and from 1.09 to 0.80 cm (left wheel), with best-fit offsets of −5.4∘-5.4^\circ and +2.9∘+2.9^\circ, respectively (Table 12).

Overall, the measured wheel directions agree with the bicycle-model geometry within the measurement tolerance for most setpoints, with a systematic residual consistent with small angular calibration offsets. The bicycle model is therefore used as an approximate model. This experiment checks each wheel separately; whether the vehicle as a whole follows circular arcs is examined in the next subsection through the Ackermann condition.

Implication for control. Since R=L/tan⁡δR = L/\tan\delta, the sensitivity

dRdδ=Lsin⁡2(δ)(74)\frac{dR}{d\delta} = \frac{L}{\sin^{2}(\delta)} \tag*{(74)}

is about 0.9–1.2 cm per degree over the calibrated extremes of Table 14, i.e. by roughly 3.5% of RR. This holds in both steering directions: at the negative limit (δ=−44.1∘\delta= -44.1^\circ, right wheel) Rmin⁡=26.3R_{\min} = 26.3 cm, while at the positive limit (δ=+40.7∘\delta= +40.7^\circ) Rmin⁡=29.6R_{\min} = 29.6 cm. An angular offset of 3–5°, as suggested by Figure 41, therefore changes Rmin⁡R_{\min} by roughly 3–6 cm.

WheelExtreme angle δ\delta (deg)SetpointRmin⁡R_{\min} (cm)
Right−44.09-44.09u=0u = 026.3±1.426.3 \pm1.4
Right+40.72+40.72u=255u = 25529.6±1.629.6 \pm1.6
Left−38.30-38.30u=0u = 032.3±1.732.3 \pm1.7
Left+37.62+37.62u=255u = 25533.1±1.833.1 \pm1.8

Table 14. Estimated minimum turning radius for each front wheel at both extreme calibrated steering angles (δmin⁡\delta_{\min} at u=0u = 0 and δmax⁡\delta_{\max} at u=255u = 255) over the tested command range (u∈[0,255]u \in[0, 255]).

Experimental Validation of the Ackermann Model

Although the vehicle was designed and controlled under the assumption of an Ackermann steering model, experimental testing revealed significant deviations between the predicted and observed vehicle motion. Therefore, the validity of the Ackermann model for the present vehicle configuration was investigated experimentally.

The calibration in this section validates the steering mechanism through the equivalent single-track angle of Eq. (50). This quantity can be computed for any pair of wheel angles, including parallel (non-Ackermann) steering, so agreement between δcomb\delta_{\mathrm{comb}} and the commanded angle shows that the equivalent angle is calibrated. It does not show that the two wheels obey the Ackermann condition. Likewise, the observation that the inner and outer wheels turn by different angles shows only that the wheels are not parallel; it does not show that the difference is the correct one. The geometric Ackermann condition relates the two angles to the vehicle dimensions:

∣cot⁡∣δo∣−cot⁡∣δi∣∣=wL(75)\left|\cot|\delta_o| - \cot|\delta_i|\right| = \frac{w}{L} \tag*{(75)}

With the front track width w=18±0.5w = 18 \pm0.5 cm and the wheelbase L=25.5±1.0L = 25.5 \pm1.0 cm, the required value is w/L=0.706±0.034w/L = 0.706 \pm0.034.

The calibrated mean wheel angles of the 13 setpoints (u=0,…,255u = 0,\ldots,255) were used, with an angular uncertainty of about 1∘1^\circ per wheel (see the calibration above). Each setpoint is plotted against the servo commanded angle, which spans −40∘-40^\circ to +40∘+40^\circ (u=0u = 0 corresponds to −40∘-40^\circ, u=128u = 128 corresponds to 0∘0^\circ and u=255u = 255 corresponds to +40∘+40^\circ. The quantity plotted is the absolute value ∣cot⁡∣δL∣−cot⁡∣δR∣∣\lvert\cot\lvert\delta_L\rvert-\cot\lvert\delta_R\rvert\rvert (Eq. 75) of the difference between the cotangents of the left and right wheel angles, which equals ∣cot⁡∣δo∣−cot⁡∣δi∣∣\lvert\cot\lvert\delta_o\rvert-\cot\lvert\delta_i\rvert\rvert. Ideal Ackermann steering gives the constant value w/L=0.706w/L=0.706 on both sides of neutral and parallel steering gives 0. The absolute value does not show which wheel turns more; this sign is kept in Table 13, where a negative value means that the outer wheel turns more than the inner wheel. The inner wheel was taken as the right wheel for positive commands and the left wheel for negative commands, consistent with the smaller right-wheel radius at u=255u=255 in Table 14. For each setpoint the measured value was compared with w/Lw/L, and the outer-wheel angle required by the Ackermann condition was computed from the measured inner-wheel angle. The Ackermann ratio is the measured value divided by w/Lw/L: 100% is ideal Ackermann and 0% is parallel steering. Setpoints with a wheel angle below 15∘15^\circ (servo commands from about −11∘-11^\circ to +14∘+14^\circ) are excluded from the chart and the table, because the difference of two cotangents is dominated by noise near straight-ahead. Table 13 and Figure 42 summarise the comparison. Figure 42 is the main result: it shows the measured ∣cot⁡∣δL∣−cot⁡∣δR∣∣\lvert\cot\lvert\delta_L\rvert-\cot\lvert\delta_R\rvert\rvert against the servo command together with the ideal Ackermann line (0.706) and the parallel-steering line (0).

uuδL\delta_L (°)δR\delta_R (°)cot⁡δo−cot⁡δi\cot\delta_o-\cot\delta_iRequired δo\delta_o (°)Measured δo\delta_o (°)Ackermann (%)
0−38.3−44.1−0.234 ± 0.0626.944.1−33
25−30.9−35.8−0.286 ± 0.0822.835.8−41
50−23.4−27.5−0.385 ± 0.1418.327.5−55
75−16.0−19.1−0.614 ± 0.2813.419.1−87
190+18.3+19.1+0.142 ± 0.2415.618.3+20
215+25.7+27.4+0.149 ± 0.1220.825.7+21
240+33.2+35.7+0.141 ± 0.0825.533.2+20
255+37.6+40.7+0.136 ± 0.0628.237.6+19

Table 13. Measured versus required Ackermann geometry. A negative sign means the outer wheel turns more than the inner wheel.

Absolute cotangent difference of left/right wheel angles vs servo angle

Figure 42. Measured ∣cot⁡∣δL∣−cot⁡∣δR∣∣\lvert\cot\lvert\delta_L\rvert-\cot\lvert\delta_R\rvert\rvert (blue, with ±1∘\pm1^\circ uncertainty bars) against the servo commanded angle from −40∘-40^\circ to +40∘+40^\circ. The green line is the ideal Ackermann value w/L=0.706w/L=0.706 and the dashed red line is parallel steering (0). The measured points lie below the ideal line at every command. On the negative side the outer wheel turns more than the inner wheel (negative sign in Table 13), which the absolute value does not show.

At the steering limits the right wheel turns about 8% more than the left wheel at +40∘+40^\circ (40.7∘40.7^\circ against 37.6∘37.6^\circ) and about 15% more at −40∘-40^\circ (44.1∘44.1^\circ against 38.3∘38.3^\circ), and the difference δR−δL\delta_R - \delta_L changes linearly with the servo command and crosses zero near +12∘+12^\circ (u≈166u \approx166), not at 0∘0^\circ (Figure 43). This pattern is typical of two wheels driven with unequal effective gain plus a small neutral offset (the right wheel reads −1.5∘-1.5^\circ at u=128u = 128). It is not the pattern produced by a steering trapezoid. The equivalent-angle result of Eq. (50) is therefore still valid for the bicycle model, but the physical mechanism does not implement Ackermann geometry. Consequence. The two front wheels do not roll about a common centre on the rear-axle line, so the outer wheel is partly dragged sideways, most strongly near the steering limits. This is expected to cause tyre slip, a turning radius different from L/tan⁡δL/\tan\delta, and different behaviour in left and right turns. These effects were not measured here. Because errors of this kind accumulate in open-loop maneuvers, the feedback correction of § 5.4.3 remains necessary.

Difference between right and left wheel angles vs servo angle

Figure 43. Difference between the right and left wheel angles, δR−δL\delta_R - \delta_L, against the servo commanded angle (points) with a linear fit. The difference changes linearly with the command (R2=0.9997R^2 = 0.9997) and crosses zero near +12∘+12^\circ (u≈166u \approx166), not at 0∘0^\circ.

Turning-Radius Estimate

For the maneuver planning of § 5.2 we convert the validated wheel geometry into a turning radius using the standard bicycle-model relation

Rwheel(δ)=Ltan⁡δ,(76)R_{\mathrm{wheel}}(\delta) = \frac{L}{\tan\delta}, \tag*{(76)}

applied directly to each front wheel’s own calibrated angle δ\delta, rather than to the single equivalent bicycle angle of Eq. (50). This gives the physical radius traced by that wheel, which is what determines the space the vehicle occupies while turning.

Why the minimum matters. The space required for a turn is set by the smallest achievable radius, which occurs at the largest steering angle the linkage can reach. Because each front wheel has its own calibrated angle, and the two ends of the steering range do not give the same angle, we evaluate RR at both extreme calibrated angles of each wheel in Table 11: the minimum angle (δmin⁡\delta_{\min}, at u=0u = 0) and the maximum angle (δmax⁡\delta_{\max}, at u=255u = 255), consistent with Eq. (57). The wheelbase tolerance (σL=0.5\sigma_L = 0.5 cm) and the angular calibration uncertainty (≈1∘\approx1^\circ, § 5.5.1) are propagated through Eq. (76) to obtain σR\sigma_R. The result is reported per wheel and per steering end, giving four radii, because the two wheels and the two steering directions are not interchangeable for this purpose. The results are listed in Table 14 and plotted in Figure 44.

Turning radius vs. wheel steering angle

Figure 44. Turning radius R=L/tan⁡δR = L/\tan\delta over the wheel steering angle ∣δ∣=28|\delta| = 28–48∘48^\circ, with the propagated uncertainty band (Rupper/RlowerR_{\mathrm{upper}}/R_{\mathrm{lower}}) and the four extreme operating points (u=0u = 0 and u=255u = 255 for each wheel, plotted at ∣δ∣|\delta|).

Future Work

While the current sensor fusion and adaptive Kalman filter architecture is designed to isolate and reject transient magnetic anomalies, several limitations remain regarding long-term calibration stability and dynamic environmental changes. Future development of the heading estimation and magnetometer calibration pipeline will focus on the following key areas:

Dynamic Motor-Current Compensation. The current magnetometer calibration (Stage 4) is entirely static; it estimates the hard- and soft-iron distortions caused by the physical chassis but does not account for the dynamic magnetic field generated by the vehicle’s own DC motor. In future iterations, we plan to model this motor-induced field. By correlating the commanded motor PWM signal—or data from a dedicated current sensor—with the induced magnetic deviation, this dynamic hard-iron offset can be actively subtracted from the raw AK8963 readings before they enter the calibration buffer.

Autonomous Calibration Recovery. As demonstrated in the magnetic disturbance tests, a persistent, lasting anomaly (such as a strong magnet attached to the chassis or transitioning into a structurally different indoor environment) pollutes the active sliding window. Currently, this forces the Kalman filter into an extended dead-reckoning state and necessitates a manual reset and a full 360∘360^\circ rotation to re-establish the baseline. Future work will investigate dual-buffer or multiple-hypothesis calibration schemes. By maintaining a shadow buffer of the disturbed field, the system could autonomously detect when a permanent shift has occurred and seamlessly transition to the new calibration ellipsoid without requiring human intervention or halting the vehicle.

Passive 3D Ellipsoid Population for Ground Vehicles. Because a planar ground vehicle does not naturally pitch or roll, the zz-axis of the magnetometer experiences almost zero variance, forcing the system to rely on a constrained 2D calibration mode to avoid singular matrix inversions. This 2D mode is vulnerable to tilt errors if the vehicle encounters uneven terrain. A future improvement will involve harvesting the minor pitch and roll variations caused by longitudinal acceleration, hard braking, and suspension dynamics. By selectively filtering and storing these rare out-of-plane samples over extended driving periods, the algorithm could slowly accumulate enough zz-axis variance to passively solve the full 3D ellipsoid without requiring the operator to manually tumble the vehicle in 3D space.

Full Localization. The set of sensors, actuators, and perception pipeline are theoretically enough to evaluate a robot full state: [x,y,ψ]T[x, y, \psi]^T. This will be developed in the future.

Additionally in the control section, because the Ackermann condition is not satisfied, the bicycle model describes the real vehicle less accurately than assumed: the path actually followed for a given steering command differs from the one predicted by R=L/tan⁡δR = L/\tan\delta, and the difference is not the same for left and right turns. Any algorithm whose result depends on a model-based prediction of the vehicle motion therefore inherits this error. For the current mechanism we recommend relying on feedback wherever possible and keeping model-based algorithms to a minimum.

If the vehicle is to keep this steering structure, a more accurate motion model should be identified from data instead of derived from geometry. We therefore propose to record the motion of the vehicle (displacement and rotation) in many experiments with different steering commands, speeds and turn directions, and to train a neural network on this supervised dataset to predict the motion from the vehicle state and command. Such a model would include the left–right asymmetry and wheel slip that the bicycle model does not contain, and could replace the bicycle model in the planner. This was not performed in this work.

Acknowledgments

The authors wish to note that the submission of this manuscript was delayed due to prolonged disruptions to research activities caused by civil unrest and subsequent military conflict in Iran. These circumstances significantly restricted access to laboratory facilities and delayed manuscript preparation. The work presented here corresponds to the robotic system as completed in early 2025.

Additionally, the authors used AI language models to assist with English-language editing, LaTeX formatting, and manuscript preparation, owing to limited fluency in English. All technical content, experiments, data, and conclusions are the authors’ own, and the authors have reviewed the text and take full responsibility for its contents.

Author Contributions

Name(s)Role and Contribution Summary
Marya Sabouri DodaranDesign of motion-geometry-based maneuvering algorithms for sign and signal response; experimental calibration and validation of the vehicle kinematic model; contributions to lane-keeping and cruise control
Ali Taheri GarakaniImplementation of AI-based lane detection model, and model data post-processing
Mandana Shabanzadeh Nasrollah-AbadiElectronics, hardware design and integration
Yousef AshrafiROS architecture and system communication
Mohammad Mahdi AzimiEmbedded firmware design, sensor data handling, heading estimation algorithm, actuator interfacing and control, and inter-processor communication system design
Erfan Nekouei GhahfarokhiDesign and training of AI-based lane detection model
Amirhossein Kheiri HolighiSign and traffic light detection (vision algorithms)
Seyyed Sobhan Hosseini HajibakandehImplementation of control algorithms to maintain lane position and prevent vehicle drift
Matin JamshidiMechanical chassis, car body and drivetrain system design
Amirhossein Gholizadeh BehbahaniImplementation of an adaptive cruise control system for vehicle speed regulation under various conditions and collision prevention
Mohammad Hossein MoaiyeriTeam supervision

Table 15.

References

  1. [1]A. Kheiri Holighi, S. S. Hosseini Hajibekandeh, A. Gholizadeh Behbahani, K. Khatibi, S. Najafi Shabestari, G. Ghoreshi, A. Dadnavi, S. Sadeghi, S. Karimi Maksous, M. Jamshidi, M. Shabanzadeh Nasrolah Abadi, and M. H. Moayeri, “Technical report of 1:10 scale autonomous vehicle robot,” arXiv preprint arXiv:2410.11746, 2024.arxiv.org/abs/2410.11746
  2. [2]Espressif Systems, ESP32 Technical Reference Manual, 2023.
  3. [3]NVIDIA, Jetson Orin NX Developer Kit User Guide.
  4. [4]M. Quigley, K. Conley, B. Gerkey, J. Faust, T. Foote, J. Leibs, R. Wheeler, and A. Y. Ng, “ROS: an open-source Robot Operating System,” in ICRA Workshop on Open Source Software, vol. 3, no. 3.2, 2009.
  5. [5]R. Barry, Mastering the FreeRTOS Real Time Kernel: A Hands-On Tutorial Guide. Real Time Engineers Ltd., 2016.
  6. [6]STMicroelectronics, VL53L1X: Time-of-Flight Long Distance Ranging Sensor with Advanced Ranging Technology, Datasheet.
  7. [7]InvenSense, MPU-9250 Product Specification, Rev. 1.1, 2016.
  8. [8]Devantech Ltd., CMPS14 Tilt Compensated Compass Module, Technical Documentation.
  9. [9]STMicroelectronics, L298: Dual Full-Bridge Driver, Datasheet.
  10. [10]NXP Semiconductors, UM10204: I²C-bus Specification and User Manual, Rev. 7.0, 2021.
  11. [11]R. E. Kalman, “A new approach to linear filtering and prediction problems,” Journal of Basic Engineering, vol. 82, no. 1, pp. 35–45, 1960.DOI
  12. [12]G. Welch and G. Bishop, “An introduction to the Kalman filter,” Dept. Comput. Sci., Univ. North Carolina at Chapel Hill, Tech. Rep. TR 95-041, 1995.
  13. [13]D. Simon, Optimal State Estimation: Kalman, H∞, and Nonlinear Approaches. Hoboken, NJ, USA: Wiley, 2006.
  14. [14]J. F. Vasconcelos, G. Elkaim, C. Silvestre, P. Oliveira, and B. Cardeira, “Geometric approach to strapdown magnetometer calibration in sensor frame,” IEEE Trans. Aerosp. Electron. Syst., vol. 47, no. 2, pp. 1293–1306, 2011.DOI
  15. [15]V. Renaudin, M. H. Afzal, and G. Lachapelle, “Complete triaxis magnetometer calibration in the magnetic domain,” J. Sensors, vol. 2010, Art. no. 967245, 2010.DOI
  16. [16]A. Fitzgibbon, M. Pilu, and R. B. Fisher, “Direct least square fitting of ellipses,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 21, no. 5, pp. 476–480, 1999.DOI
  17. [17]G. H. Golub and C. F. Van Loan, Matrix Computations, 4th ed. Baltimore, MD, USA: Johns Hopkins Univ. Press, 2013.
  18. [18]Asahi Kasei Microdevices, AK8963: 3-Axis Electronic Compass, Datasheet MS1356-E-02, Oct. 2013.
  19. [19]L. Keselman, J. I. Woodfill, A. Grunnet-Jepsen, and A. Bhowmik, “Intel RealSense stereoscopic depth cameras,” in Proc. IEEE Conf. Computer Vision and Pattern Recognition Workshops (CVPRW), 2017, pp. 1267–1276.arxiv.org/abs/1705.05548
  20. [20]Y. Ko, Y. Lee, S. Azam, F. Munir, M. Jeon, and W. Pedrycz, “Key points estimation and point instance segmentation approach for lane detection,” IEEE Trans. Intell. Transp. Syst., vol. 23, no. 7, pp. 8949–8958, 2022.arxiv.org/abs/2002.06604
  21. [21]F. Yu and V. Koltun, “Multi-scale context aggregation by dilated convolutions,” in Proc. Int. Conf. Learning Representations (ICLR), 2016.arxiv.org/abs/1511.07122
  22. [22]B. De Brabandere, D. Neven, and L. Van Gool, “Semantic instance segmentation with a discriminative loss function,” arXiv preprint arXiv:1708.02551, 2017.arxiv.org/abs/1708.02551
  23. [23]TuSimple, “TuSimple lane detection benchmark,” 2017. [Online]. Available: https://github.com/TuSimple/tusimple-benchmark
  24. [24]R. Hartley and A. Zisserman, Multiple View Geometry in Computer Vision, 2nd ed. Cambridge, U.K.: Cambridge Univ. Press, 2004.DOI
  25. [25]R. Rajamani, Vehicle Dynamics and Control, 2nd ed. New York, NY, USA: Springer, 2012.
  26. [26]L. E. Dubins, “On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents,” American Journal of Mathematics, vol. 79, no. 3, pp. 497–516, 1957.DOI
  27. [27]B. Paden, M. Čáp, S. Z. Yong, D. Yershov, and E. Frazzoli, “A survey of motion planning and control techniques for self-driving urban vehicles,” IEEE Trans. Intell. Veh., vol. 1, no. 1, pp. 33–55, 2016.arxiv.org/abs/1604.07446
  28. [28]K. J. Åström and T. Hägglund, Advanced PID Control. Research Triangle Park, NC, USA: ISA, 2006.
  29. [29]S. Thrun et al., “Stanley: The robot that won the DARPA Grand Challenge,” Journal of Field Robotics, vol. 23, no. 9, pp. 661–692, 2006.
  30. [30]G. M. Hoffmann, C. J. Tomlin, M. Montemerlo, and S. Thrun, “Autonomous automobile trajectory tracking for off-road driving: Controller design, experimental validation and racing,” in Proc. American Control Conf. (ACC), 2007, pp. 2296–2301.DOI
  31. [31]R. C. Coulter, “Implementation of the pure pursuit path tracking algorithm,” Robotics Institute, Carnegie Mellon Univ., Pittsburgh, PA, USA, Tech. Rep. CMU-RI-TR-92-01, 1992.
  32. [32]J. M. Snider, “Automatic steering methods for autonomous automobile path tracking,” Robotics Institute, Carnegie Mellon Univ., Pittsburgh, PA, USA, Tech. Rep. CMU-RI-TR-09-08, 2009.
  33. [33]S. S. Shapiro and M. B. Wilk, “An analysis of variance test for normality (complete samples),” Biometrika, vol. 52, no. 3/4, pp. 591–611, 1965.DOI
  34. [34]B. Efron, “Bootstrap methods: Another look at the jackknife,” The Annals of Statistics, vol. 7, no. 1, pp. 1–26, 1979.
  35. [35]J. M. Bland and D. G. Altman, “Statistical methods for assessing agreement between two methods of clinical measurement,” The Lancet, vol. 327, no. 8476, pp. 307–310, 1986.pubmed.ncbi.nlm.nih.gov/2868172
  36. [36]D. Lakens, “Equivalence tests: A practical primer for t tests, correlations, and meta-analyses,” Social Psychological and Personality Science, vol. 8, no. 4, pp. 355–362, 2017.

Paper details

vlnc:W_dj2fvtzq
Version
v1 · latest
Published
Fresh — added to the Hub within the last 30 days
Added
License
CC BY 4.0
Usage

views · 0

downloads · 0

A view counts once per browser per day, a download each time the PDF is requested.

  • Computer Science

AI language models

View statement
Looking for related papers…
Contents