The pursuit of developing a humanoid robot with human-like locomotion and adaptive cognitive capabilities remains a paramount focus in robotics. Among the various challenges, enabling a humanoid robot to reliably track and follow a visual path in real-time is fundamental for autonomous navigation in structured environments. While significant research has been dedicated to visual servoing and path planning, many existing solutions for humanoid robot trajectory tracking, especially under monocular vision constraints, still grapple with issues of stability, resource efficiency, and adaptive decision-making under perceptual uncertainty.
Traditional frame-by-frame image processing methods for humanoid robot navigation often discard previous computational states upon receiving new visual input. This approach, while reactive, leads to inefficient use of computational resources and can result in oscillatory or jittery motion, as every frame—potentially containing noise or momentary aberrations—directly dictates a new action command. This is particularly wasteful for a humanoid robot, where gait generation and stabilization already demand significant processing power. The resulting instability manifests as unnecessary swaying during straight-line tracking, consuming extra energy and reducing overall system robustness.
To address these limitations, this work presents a novel trajectory tracking methodology designed explicitly for a monocular vision-based humanoid robot platform. Our core innovation lies in shifting from an instantaneous, frame-dependent control paradigm to a temporal-fusion approach. We treat a sequence of image frames over a defined operational cycle as a cohesive unit for decision-making. By applying a dynamically weighted fusion algorithm to the visual data and motion commands derived across this cycle, our system for the humanoid robot can filter out spurious noise, reconcile conflicting data, and generate smoother, more reliable locomotion commands. This method significantly enhances the precision and energy efficiency of the humanoid robot‘s trajectory tracking.
System Architecture and Hardware Design
The proposed system is built around a 19-degree-of-freedom (DOF) bipedal humanoid robot platform. The core processing is performed by an OpenMV H7 module, which is centered on a powerful STM32H750VBT6 ARM Cortex-M7 microcontroller. This module runs a customized firmware based on the MicroPython/OpenMV framework, providing a high-level, scriptable environment for efficient computer vision algorithms. The vision sensor is an OV7725 VGA-resolution (640×480) CMOS camera, strategically mounted on the robot’s torso to provide a forward-looking view of the walking path. The image is processed at a lower resolution of 160×120 pixels to balance processing speed and adequate feature detail for trajectory recognition.
The hardware architecture is summarized in the block diagram below, illustrating the data flow from image acquisition to actuator control:
| Module | Component | Primary Function |
|---|---|---|
| Perception Unit | OV7725 Camera | Captures real-time RGB image data of the environment. |
| Processing Core | STM32H750VBT6 (OpenMV H7) | Executes image processing, trajectory algorithm, and system control logic. |
| Actuation System | 19x Servos + Controller Board | Converts digital motion commands into precise joint movements for the humanoid robot walk. |
| Interface & Debug | UART, LCD, LEDs | Facilitates communication between modules and provides visual feedback for debugging. |
The OpenMV module communicates motion vectors (like turn angle and forward step commands) to a dedicated servo controller board via a UART serial protocol. This board then drives the network of servos in the humanoid robot‘s legs and hips to execute the planned walking gait. This decoupled architecture allows the vision algorithm to run independently on a capable processor while leaving the time-critical servo pulse generation to a dedicated controller.
Visual Processing Pipeline for Trajectory Extraction
The trajectory tracking algorithm begins with raw image acquisition. The OV7725 sensor outputs data in RGB565 format. To correct for inherent lens distortion (barrel or pincushion effects) which could misrepresent the path geometry, a software-based calibration and rectification process is first applied using pre-calculated correction matrices. This ensures that the subsequent processing operates on geometrically accurate data, which is crucial for a humanoid robot to make correct spatial judgments.
The core of visual feature extraction is isolating the guiding line (typically a colored tape or painted line) from the background. We employ the LAB color space for its superior perceptual uniformity compared to RGB. A thresholding operation is performed in the LAB domain to create a binary image:
Let $I_{L}, I_{A}, I_{B}$ represent the L, A, and B channels of the input image. A pixel at coordinate $(u,v)$ is classified as belonging to the trajectory if its color values fall within predefined thresholds:
$$
\text{BinaryMask}(u,v) = \begin{cases}
1 & \text{if } L_{min} \leq I_{L}(u,v) \leq L_{max} \land A_{min} \leq I_{A}(u,v) \leq A_{max} \land B_{min} \leq I_{B}(u,v) \leq B_{max} \\
0 & \text{otherwise}
\end{cases}
$$
This operation converts the image into a binary representation where the trajectory appears white (value 1) against a black background (value 0). Morphological operations like erosion and dilation can be optionally applied to reduce noise and connect broken segments of the line.
To efficiently analyze the trajectory’s position and orientation, the binary image is divided into Regions of Interest (ROIs). We use a grid-based partition scheme as shown below:
| Region Name | Horizontal Bounds | Vertical Bounds | Purpose |
|---|---|---|---|
| A (Far Top) | Full Width | Top 24 pixels | Predict distant path curvature. |
| B (Near Top) | Full Width | Next 24 pixels | Guide immediate turning decisions. |
| C (Central) | Full Width | Central 24 pixels | Primary region for current lateral error calculation. |
| D (Near Bottom) | Full Width | Next 24 pixels | Provide stabilizing feedback near the robot. |
| E (Far Bottom) | Full Width | Bottom 24 pixels | Assess trajectory at the robot’s feet. |
| F (Left Edge) | Leftmost 32 pixels | Full Height | Detect left-hand intersections or sharp turns. |
| G (Right Edge) | Rightmost 32 pixels | Full Height | Detect right-hand intersections or sharp turns. |
Within each primary tracking ROI (A through E), the algorithm performs a blob detection to find the centroid of the white pixels (the line). The horizontal coordinate $x_i$ of the centroid in region $i$ is extracted, with the image width center as the origin (x=0). If no blob is found in a region, a default coordinate (e.g., the center of that region or a value indicating “no data”) is assigned. This per-region data forms a discrete representation of the line’s lateral position at different look-ahead distances for the humanoid robot.
The Weighted Temporal Fusion Algorithm
The instantaneous trajectory position $X_{inst}$ is calculated by fusing the horizontal centroid data from regions A to E. We assign higher weights to regions that are more reliable for determining the current walking direction (typically the central and near regions). Let $x_a, x_b, x_c, x_d, x_e$ be the normalized horizontal coordinates (scaled to [-1, 1]) from regions A to E, respectively. The fused lateral error is computed as:
$$
X_{inst} = w_a \cdot x_a + w_b \cdot x_b + w_c \cdot x_c + w_d \cdot x_d + w_e \cdot x_e
$$
where the weight vector $\mathbf{W} = [w_a, w_b, w_c, w_d, w_e]$ is empirically chosen, for example, $\mathbf{W} = [1, 2, 3, 2, 2]$. This $X_{inst}$ is directly proportional to the required steering correction for the humanoid robot; a positive value indicates a needed turn to the right, and negative to the left.
The key innovation is the treatment of this stream of $X_{inst}$ values and their derived motion commands over time. A humanoid robot‘ gait cycle takes a finite amount of time $T_{step}$. Issuing a new command based solely on the latest $X_{inst}$ before the previous step is complete leads to the instability described earlier. Therefore, we define a Decision Fusion Cycle (DFC) with a duration $T_{DFC} \approx 1.5 \times T_{step}$. During this cycle, the system collects a sequence of $N$ candidate motion commands $C = \{C_1, C_2, …, C_N\}$, each derived from an $X_{inst}$ at a different point in time.
Instead of using only the last command $C_N$, we evaluate the consensus within the cycle. For the current candidate command $C_{current}$, we compare it with all previous commands $C_k$ in the ongoing DFC buffer. A similarity score $S_k$ is assigned for each comparison: $S_k = +1$ if $C_{current} \approx C_k$, and $S_k = -1$ if they differ significantly. Crucially, not all past commands are equally trusted. Commands generated more recently are typically more relevant than older ones. Thus, we apply a temporal weighting function $f(t)$ to each similarity score, where $t$ is the age of the historical command $C_k$ relative to the current moment. A linear or exponential decay function can be used. The aggregate validation score $V$ for $C_{current}$ is:
$$
V = \sum_{k=1}^{N-1} f(t_k) \cdot S_k
$$
A threshold $\theta$ is then applied to make a final decision:
$$
\text{Final Action} = \begin{cases}
\text{Execute } C_{current} & \text{if } V \geq \theta \\
\text{Discard } C_{current}; \text{ Repeat previous command} & \text{if } V < \theta
\end{cases}
$$
This mechanism acts as a low-pass filter and a consistency checker. Erroneous commands caused by transient visual noise (e.g., glare, shadows) that disagree with the recent trend will yield a low $V$ score and be discarded, preventing the humanoid robot from making a jerky, corrective movement based on bad data. Only commands consistent with the recent pattern of motion are executed, leading to remarkably smoother trajectory tracking.
Trajectory Typing and Motion Command Generation
The lateral error $X_{inst}$ is sufficient for following straight or gently curved lines. However, for robust navigation, the humanoid robot must recognize specific path topologies like sharp turns (90-degree corners) or intersections (T-junctions, crossings). This is the role of the edge ROIs (F and G).
By monitoring the presence, vertical position, and continuity of blobs in the left (F) and right (G) edge columns, the system can classify the upcoming path segment. The logic is encapsulated in the following decision table:
| Condition (F: Left, G: Right) | Inferred Path Type | Primary Action for Humanoid Robot |
|---|---|---|
| Blob in C, No blob in F & G | Straight or Wide Curve | Walk forward, steer based on $X_{inst}$. |
| Strong blob in G, weak/no blob in C | Sharp Right Turn Ahead | Initiate pre-programmed right turn gait sequence. |
| Strong blob in F, weak/no blob in C | Sharp Left Turn Ahead | Initiate pre-programmed left turn gait sequence. |
| Blobs in F, C, and G (vertical alignment varies) | Intersection | Stop and execute predefined decision logic (e.g., turn as per task). |
This high-level typing, combined with the continuous $X_{inst}$ for fine control, allows the humanoid robot to handle a variety of structured paths. The motion command $C$ sent to the servo controller is therefore a tuple: Command = (Base Gait Type, Steering Parameter). For example, (‘Walk_Forward’, -0.15) or (‘Execute_Right_Turn’, 0).
Experimental Validation and Results
The system was validated using a custom test track featuring straight lines, S-curves, 90-degree turns, and T-junctions. The performance of the humanoid robot was evaluated against two key metrics: 1) Tracking Accuracy measured as the average deviation from the path centerline, and 2) Gait Stability measured by the variance in the step cycle period and torso orientation.
Comparative tests were conducted between the proposed Weighted Temporal Fusion (WTF) method and a conventional Immediate-Frame-Reaction (IFR) method. The results are summarized below:
| Performance Metric | Immediate-Frame-Reaction (IFR) Method | Proposed Weighted Temporal Fusion (WTF) Method |
|---|---|---|
| Avg. Lateral Error (mm) | 25.4 ± 12.1 | 14.7 ± 6.3 |
| Torso Roll/Pitch Variance (°) | High (Frequent jitter) | Low (Smooth motion) |
| Successful Turn Completion (%) | 75% | 95% |
| Computational Load (Avg. CPU Use) | Consistently High (~85%) | Lower & Smoother (~70%) |
The experimental data clearly demonstrates the advantages of our approach. The WTF method significantly reduced the average tracking error by approximately 42%, indicating superior precision in the humanoid robot‘s path-following. More importantly, the drastic reduction in torso orientation variance confirms a much smoother gait, as the humanoid robot was not constantly reacting to minor visual fluctuations. The higher success rate in completing sharp turns stems from the reliable detection of turn signatures in the edge ROIs and the stable execution of the corresponding gait sequence without being interrupted by spurious commands. Furthermore, by discarding inconsistent commands, the algorithm avoided unnecessary computations for planning aborted movements, leading to a more efficient use of the humanoid robot‘s main processor.
Conclusion
This paper presented a comprehensive design for a monocular vision-based trajectory tracking system for a humanoid robot. By leveraging the OpenMV ecosystem on a capable STM32H7 platform, we implemented a sophisticated processing pipeline that goes beyond simple per-frame reaction. The introduction of a weighted temporal fusion algorithm marks a significant step forward. It allows the humanoid robot to integrate information over time, effectively distinguishing between true trajectory features and visual noise, thereby generating more stable and reliable locomotion commands. The combination of zonal image analysis for continuous error correction and edge-based detection for path topology recognition creates a robust and versatile navigation system. Experimental results validate that this method not only improves tracking accuracy and gait smoothness for the humanoid robot but also does so with greater computational efficiency. This work provides a practical and effective reference solution for enhancing the autonomous navigation capabilities of resource-constrained, vision-driven humanoid robot platforms in structured environments.