The field of legged robotics presents a spectrum of morphologies, among which the biped humanoid robot holds a distinct position. Its structure, mirroring the human form, offers potential advantages in terms of lower joint complexity and energy consumption compared to multi-legged counterparts when navigating environments designed for humans. However, this structural similarity brings forth significant research challenges. One of the most persistent and complex problems is achieving robust indoor localization and navigation. The fundamental task for a humanoid robot is not merely to move from point A to point B, but to do so while dynamically perceiving its environment, accurately determining its own position and orientation, and making real-time adjustments to its gait and path when encountering obstacles. This integration of perception, localization, and motion planning in an unstable, bipedal platform makes the problem particularly difficult.
Current methodologies for robot indoor navigation can be broadly categorized. Inertial navigation systems (INS) rely on accelerometers and gyroscopes to track movement from a known starting point. While suitable for short-term tracking, these systems suffer from cumulative drift errors, where small measurement inaccuracies integrate over time, leading to significantly degraded positioning accuracy. On the other end, constrained navigation methods, such as following magnetic tapes, visual markers, or pre-installed beacons, can offer high precision. However, they require significant modification of the environment, limiting their application to controlled, structured settings and reducing general autonomy. The quest, therefore, is for a system that provides sustainable accuracy without heavy environmental pre-configuration.
This work presents the design and implementation of an indoor positioning and autonomous motion planning control system for a biped humanoid robot. The core innovation lies in the fusion of two technological approaches: ZigBee-based wireless positioning and multi-sensor data fusion for environmental perception and state estimation. We establish spatial and kinematic models of the robot’s motion process to derive real-time position and attitude information, which in turn feeds into an optimized motion planning algorithm. The system utilizes a Raspberry Pi 4B single-board computer coupled with a 32-channel serial bus servo controller as the core computational and actuation platform. This combination effectively manages the high-level decision-making for navigation and the low-level, precise control of the numerous joints in the humanoid robot, enabling sophisticated functionalities like targeted navigation and obstacle avoidance.
ZigBee-Based Indoor Positioning Algorithm
In wireless sensor networks (WSNs), positioning techniques are often distance-dependent. They are classified into range-based and range-free mechanisms. Range-free mechanisms, while simpler, typically require a very high density of reference nodes to achieve acceptable accuracy, leading to increased cost and complexity. Range-based mechanisms, which calculate position based on measured distances, offer more direct accuracy. Common methods include Time of Arrival (ToA), Time Difference of Arrival (TDoA), Angle of Arrival (AoA), and Received Signal Strength Indicator (RSSI). Among these, RSSI-based ranging is particularly well-suited for ZigBee networks due to its inherent advantages: it requires no additional hardware on standard radio chips, is computationally fast, and is relatively easy to implement. This makes it an ideal candidate for a cost-effective localization system on a humanoid robot.
The principle of RSSI-based ranging involves measuring the attenuation of a radio signal as it propagates from a transmitter to a receiver. The path loss model describes this attenuation. A common log-distance path loss model is given by:
$$ P_d = P_{d0} – 10 \mu \log_{10}\left(\frac{d}{d_0}\right) + X_\sigma $$
Where:
$P_d$ is the received signal strength (in dBm) at distance $d$.
$P_{d0}$ is the received signal strength at a reference distance $d_0$ (typically 1 meter).
$\mu$ is the path loss exponent, which varies with the environment (e.g., 2 for free space, higher for cluttered indoor areas).
$X_\sigma$ is a zero-mean Gaussian random variable representing shadowing effects.
From this model, the estimated distance $\hat{d}$ can be derived:
$$ \hat{d} = d_0 \cdot 10^{\frac{P_{d0} – P_d}{10 \mu}} $$
In practice, $A = P_{d0} + X_\sigma$ is often treated as a calibrated constant for a specific environment and hardware. Thus, the distance calculation simplifies to:
$$ \hat{d} = 10^{\frac{A – P_d}{10 \mu}} $$
With distance estimates to several known anchor nodes (reference points), the robot’s position (the “blind” node) can be calculated. Trilateration is a common method, but its accuracy is highly sensitive to errors in individual distance estimates, which are inevitable with RSSI due to multipath fading and interference. To improve robustness, we employ a Triangular Centroid Algorithm. Instead of relying on the intersection of three circles (which may not meet at a single point due to errors), this algorithm calculates a centroid based on the region of overlap.
The algorithm procedure is as follows:
1. From all available anchor nodes, select three that provide the most geometrically diverse and strong RSSI readings.
2. For anchors A, B, and C at known positions $(x_a, y_a)$, $(x_b, y_b)$, $(x_c, y_c)$ with estimated distances $d_a$, $d_b$, $d_c$, consider them as circles.
3. Find the intersection points between pairs of these circles. Due to measurement noise, three circles yield up to three pairwise intersection points, labeled M, N, and P. The coordinates of point M, the intersection between the circles defined by anchors B and C, can be found by solving:
$$ (x_m – x_b)^2 + (y_m – y_b)^2 = d_b^2 $$
$$ (x_m – x_c)^2 + (y_m – y_c)^2 = d_c^2 $$
(Subject to also being approximately within $d_a$ of anchor A). Points N and P are calculated similarly for other circle pairs.
4. The estimated position of the humanoid robot $(x_r, y_r)$ is then the centroid of the triangle formed by these intersection points:
$$ x_r = \frac{x_m + x_n + x_p}{3}, \quad y_r = \frac{y_m + y_n + y_p}{3} $$
This method effectively averages out some of the ranging errors, providing a more stable and accurate location estimate than plain trilateration, which is crucial for the precise foot placement and balance management of a humanoid robot.
For hardware implementation, the CC2530 system-on-chip was selected. It integrates an 8051 microcontroller core with a robust RF transceiver compliant with the IEEE 802.15.4/ZigBee standard, facilitating the creation of a low-power, mesh-capable network for anchor and robot node communication.
| Technology | Principle | Accuracy | Cost | Infrastructure Need | Suitability for Humanoid Robot |
|---|---|---|---|---|---|
| Inertial (IMU) | Dead reckoning | Degrades over time | Low-Medium | None | Poor (drift unacceptable for long tasks) |
| UWB | Time-of-flight | Very High (10-30 cm) | High | High (special anchors) | Good but expensive |
| Wi-Fi Fingerprinting | RSSI pattern matching | Medium (2-5 m) | Low | Medium (survey needed) | Moderate (requires pre-mapping) |
| ZigBee (RSSI+Triangulation) | Signal strength ranging | Medium (1-3 m) | Low | Low (few anchors) | Very Good (balance of cost/accuracy/autonomy) |
Motion Planning Algorithm for Humanoid Navigation
Autonomous navigation for a mobile robot, especially a humanoid robot with complex dynamics, involves finding a collision-free path from a start to a goal position. In unknown or partially known environments, the robot must rely on its onboard sensors to discover obstacles and replan accordingly. Classic reactive algorithms like the Bug algorithms provide simple yet effective strategies. The basic Bug algorithm has the robot follow the straight-line vector from start to goal. Upon hitting an obstacle, it circumnavigates the obstacle’s boundary until it can resume following the original line-to-goal. While computationally lightweight, standard Bug algorithms can generate highly inefficient paths in cluttered environments, with repetitive loops and extensive boundary tracing.
Our approach is an optimized and improved strategy derived from the Bug algorithm concept, specifically tailored for the sensor suite and capabilities of a biped humanoid robot. The key enhancement is the integration of real-time attitude and detailed proximity data, allowing for more intelligent decisions during obstacle avoidance, rather than simple blind wall-following.
The algorithm workflow is as follows:
1. Initialization: The robot receives a goal coordinate $(x_g, y_g)$ from the control system. It obtains its current position $(x_r, y_r)$ from the ZigBee localization module and its heading angle $\theta$ from the inertial measurement unit (IMU).
2. Compute Goal Direction: Calculate the desired heading $\theta_{goal}$ towards the goal:
$$ \theta_{goal} = \arctan2(y_g – y_r, x_g – x_r) $$
3. Navigate: The robot attempts to walk in the direction of $\theta_{goal}$. The high-level controller sends adjusted joint angle trajectories to the servo controller to achieve this heading.
4. Obstacle Detection: An array of forward and side-facing laser distance sensors continuously monitors the environment. An obstacle is detected if any sensor reading $d_{sensor}$ falls below a safety threshold $d_{safe}$.
5. Decision & Avoidance:
a. If no obstacle is detected, continue Step 3.
b. If an obstacle is detected ahead, the robot stops. Instead of immediately initiating a full boundary follow, the algorithm first checks for a viable detour. Using side sensor data, it assesses if sufficient clearance exists to the left or right.
c. It then commands the humanoid robot to execute a sequence of in-place turns and short forward steps to maneuver around the obstacle’s edge, constantly checking the clearance and recalculating if the goal direction is now unobstructed. This process uses the IMU’s $\theta$ to ensure controlled turning.
6. Resume Navigation: Once the line-of-sight to the goal is re-established (checked by the absence of obstacles in the $\theta_{goal}$ direction over a certain distance), the robot reorients itself and resumes direct navigation toward the goal.
7. Termination: The process repeats until the robot’s position is within a defined tolerance $\epsilon$ of the goal position: $\sqrt{(x_g-x_r)^2 + (y_g-y_r)^2} < \epsilon$.
This method reduces the “wall-hugging” behavior of the classic Bug algorithm, as the humanoid robot can use its sensor-derived “understanding” of the obstacle’s extent to choose a more efficient avoidance maneuver, significantly improving path optimality in multi-obstacle environments.
System Design and Sensor Fusion Architecture
The overall system is architected as two interdependent modules communicating via the central Raspberry Pi 4B controller. The Positioning Module handles communication with the ZigBee network, RSSI processing, and execution of the triangular centroid algorithm. The Motion Planning & Control Module is responsible for reading all onboard sensors, fusing this data, running the navigation algorithm, and generating servo control commands. The system’s operational flow is cyclic: sense → localize → plan → act.
A critical aspect of robust perception for a humanoid robot is multi-sensor data fusion. Relying on a single sensor type is prone to failure and provides incomplete state information. Our system employs a heterogeneous sensor suite:
Laser Distance Sensors (VL53L0X-based): Multiple laser ranging modules provide high-precision, short-to-medium range distance measurements. They are immune to ambient light interference and offer fast response times, ideal for detecting imminent obstacles and drop-offs. Several sensors are mounted at strategic points on the robot’s torso and legs, facing forward and sideways to create a rudimentary proximity “bubble.”
Inertial Measurement Unit (IMU – JY901): This sensor is pivotal for state estimation. It provides raw data from a 3-axis accelerometer, 3-axis gyroscope, and a 3-axis magnetometer. Through sensor fusion algorithms (e.g., a complementary filter or Kalman filter running on the Raspberry Pi), these raw data are processed to yield a stable and accurate estimate of the robot’s attitude—specifically its roll ($\phi$), pitch ($\theta$), and yaw ($\psi$) angles. This attitude information is essential for maintaining balance during walking and for executing precise turns during navigation.
The fusion logic can be conceptually represented. The IMU provides the robot’s orientation in the world frame. The laser sensors provide distances to obstacles in the robot’s body frame. Using the attitude from the IMU, these obstacle vectors can be transformed into the world frame, building a localized, oriented map of immediate surroundings. Simultaneously, the ZigBee module provides an absolute world-frame position $(x_r, y_r)$. All this information—world position, world orientation, and a local obstacle map—converges at the motion planner to decide the next action.
A significant hardware integration challenge was the limited number of I²C ports on the Raspberry Pi. Both the laser sensors and the IMU communicate via the I²C protocol. To solve this, an I²C multiplexer/expander board was incorporated. This device allows a single I²C master (the Raspberry Pi) to communicate with multiple I²C device buses, enabling the connection of all necessary sensors without conflict.
| Sensor Type | Specific Model/Data | Primary Function | Update Rate | Data Fusion Role |
|---|---|---|---|---|
| Wireless Transceiver | CC2530 (ZigBee) | RSSI measurement for ranging | ~10 Hz | Provides global (x,y) position estimate. |
| Time-of-Flight Laser | VL53L0X (Multiple) | Distance to objects (2cm – 2m) | > 50 Hz | Provides local obstacle map in robot frame. |
| Inertial Measurement Unit | JY901 (Accel, Gyro, Mag) | Attitude (Roll, Pitch, Yaw) | > 100 Hz | Provides body orientation; transforms local obstacle data to world frame; critical for balance. |
Experimental Analysis and System Performance
The integrated system was implemented on a custom-built biped humanoid robot platform. Experiments were conducted in a controlled indoor laboratory space measuring approximately 8m x 6m. Four ZigBee anchor nodes were placed at known coordinates in the corners of the area. The humanoid robot, equipped with the sensor suite and control computer, was tasked with navigating between predefined waypoints while static and dynamic obstacles were introduced.
Localization Performance: The ZigBee-based triangular centroid algorithm demonstrated consistent performance. The average static positioning error was approximately 1.2 meters, with a standard deviation of 0.4 meters. While not centimeter-accurate like UWB systems, this level of precision was sufficient for room-scale navigation and goal-oriented behavior for the humanoid robot. The error is attributed primarily to the inherent volatility of RSSI signals in indoor environments due to multipath effects and dynamic interference. The system successfully tracked the robot’s movement across the room, updating its position at a rate compatible with the robot’s walking speed.
Navigation and Obstacle Avoidance: The motion planning system proved effective. The humanoid robot was able to:
1. Receive a goal coordinate and calculate the initial direction.
2. Walk towards the goal using a stable, pre-programmed gait pattern.
3. Reliably detect obstacles (boxes, chairs) placed in its path using the front laser sensors.
4. Halt its forward motion, execute the improved avoidance routine—often a turn and a side step—and successfully navigate around the obstacle.
5. Re-acquire the goal direction and proceed to the final target.
The use of the IMU for controlled in-place turns was crucial. It allowed the robot to adjust its heading by a precise angle (e.g., 90 degrees) rather than turning for an arbitrary time, making the avoidance behavior more predictable and efficient. The fusion of side sensor data during avoidance prevented the robot from getting too close to the obstacle or other walls.
| Component | Model/Specification | Role in System |
|---|---|---|
| Main Controller | Raspberry Pi 4B (4GB RAM) | Runs high-level algorithms: sensor fusion, localization, motion planning, network communication. |
| Servo Controller | 32-Channel Serial Bus Controller | Interfaces with Raspberry Pi via UART; provides precise PWM control for all robot joint servos. |
| Positioning Node | CC2530 ZigBee Module | On-robot module for RSSI measurement and communication with anchor network. |
| Ranging Sensor | WL_LaserDis (VL53L0X) | Multiple units for obstacle detection and proximity sensing. |
| Attitude Sensor | JY901 9-Axis IMU | Provides orientation and rotational data for balance and navigation. |
| Communication Bridge | I²C Expander Board | Enables connection of multiple I²C sensors to the single Raspberry Pi I²C bus. |
Conclusion and Future Work
This work successfully designed and implemented a comprehensive indoor positioning and motion planning system for a biped humanoid robot. The system synergistically combines ZigBee wireless technology for coarse-grained absolute localization with a suite of onboard laser and inertial sensors for fine-grained environmental perception and state estimation. By employing an optimized triangular centroid algorithm for positioning and an improved, sensor-informed Bug-like algorithm for navigation, the humanoid robot achieves autonomous goal-directed movement with basic obstacle avoidance capabilities. The architecture based on the Raspberry Pi and a dedicated servo controller provides a flexible and powerful platform for implementing complex behaviors on a humanoid robot.
Future work will focus on several key enhancements to advance the system’s capabilities and robustness:
1. Advanced Sensor Fusion: Implementing a more sophisticated fusion filter, such as an Extended Kalman Filter (EKF) or a Particle Filter, to tightly integrate the ZigBee position updates, IMU dead reckoning, and laser scan matching. This would create a more accurate, smooth, and reliable state estimate, reducing the latency and error in the robot’s perceived location and orientation.
2. Dynamic Gait Adjustment: Integrating the navigation planner with a dynamic gait engine. Currently, the robot uses a fixed walking gait. Future versions could adjust step length, frequency, and foot placement in real-time based on the planned path and detected terrain, making the humanoid robot‘s movement more adaptive and efficient.
3. Multi-Robot Coordination: Extending the ZigBee network protocol to support multiple humanoid robot agents. This would involve developing communication protocols for sharing location data and simple intentions, enabling the study of coordinated tasks, swarm behaviors, or collaborative exploration in indoor environments.
4. Platform Generalization: Porting the core software architecture to other robotic platforms, such as wheeled, tracked, or multi-legged robots, to test its versatility and identify platform-specific adaptations required for the perception and planning modules.
In conclusion, the presented system establishes a functional foundation for autonomous indoor operation of humanoid robots. By addressing the fundamental challenges of localization and reactive planning with a pragmatic fusion of accessible technologies, it provides a viable pathway toward more intelligent and capable humanoid agents capable of operating in human-centric spaces.