Project
Introduction
This project implements a complete kinematic modeling and control system for 4-wheel mecanum robots, enabling true omnidirectional movement through coordinated wheel velocity control. The system features bidirectional kinematic transformations using Eigen-based matrix operations, with forward kinematics computed via Moore-Penrose pseudo-inverse using Singular Value Decomposition (SVD). The implementation demonstrates precise figure-8 trajectory execution through a 9-waypoint path sequence, showcasing the robot's ability to seamlessly combine translational and rotational motions. By leveraging ROS2's multi-threaded executor architecture, the system achieves real-time 20Hz control loops with sub-millisecond matrix computations. The mecanum wheel configuration with 45° roller angles enables independent control of all three degrees of freedom (X, Y, θ) in the planar space, allowing movements impossible for conventional wheeled robots.
Objectives
-
To derive and implement inverse kinematics transforming robot twist (vx, vy, ωz) to individual wheel velocities using Jacobian matrix
-
To develop forward kinematics using SVD-based pseudo-inverse for robust wheel-to-twist transformation
-
To create complex figure-8 trajectory controller with smooth waypoint transitions and coordinated motion
-
To implement multi-threaded ROS2 architecture enabling parallel odometry processing and control execution
-
To validate omnidirectional capabilities through forward, lateral, diagonal, and rotational motion primitives
-
To achieve real-time performance with Eigen-optimized matrix operations for 20Hz control frequency
Tools and Technologies
-
Framework: ROS2 Humble with multi-threaded executor
-
Programming Language: C++ 17
-
Linear Algebra: Eigen3 library for matrix operations
-
Matrix Computations: SVD decomposition, Moore-Penrose pseudo-inverse
-
Kinematic Modeling: 4x3 Jacobian matrix, 3x4 pseudo-inverse
-
Threading: Callback groups for parallel execution
-
Quaternion Math: TF2 for orientation tracking from odometry
-
Communication: Float32MultiArray for wheel velocities, Twist for robot commands
-
Build System: Colcon with CMake
-
Package Structure: Modular design with separate kinematics and trajectory packages
-
Version Control: Git
Source Code
-
GitHub Repository: Mecanum Kinematics Control
-
Documentation: README with mathematical derivations
Video Result
-
Figure-8 Trajectory Demo: Complex path execution showing coordinated omnidirectional motion
-
Omnidirectional Capabilities: Motion primitives demonstration including lateral and diagonal movements
-
Performance Metrics: 20Hz control rate, <1ms matrix operations, ±5cm position repeatability
Process and Development
The project is structured into five critical components: mecanum wheel kinematic modeling, SVD-based forward kinematics implementation, figure-8 trajectory generation, multi-threaded ROS2 architecture, and real-time control validation.
Task 1: Inverse Kinematics Matrix Derivation
Jacobian Matrix Construction: Developed 4x3 transformation matrix H mapping robot twist [ωz, vx, vy]ᵀ to wheel velocities [ω₁, ω₂, ω₃, ω₄]ᵀ based on mecanum geometry with wheelbase l=0.085m, track width w=0.135m, wheel radius r=0.05m.
Kinematic Constraints: Incorporated 45° roller angle constraints into matrix formulation, with diagonal terms (-l-w)/r and (l+w)/r capturing coupling between rotation and translation.
Matrix Implementation: Created Eigen-based matrix multiplication wheel_velocity = H * twist with fixed-size matrices for compile-time optimization and vectorized operations.
Task 2: Forward Kinematics via SVD Pseudo-Inverse
Moore-Penrose Computation: Implemented pseudo-inverse H⁺ = (HᵀH)⁻¹Hᵀ using Eigen's JacobiSVD with thin U/V decomposition for numerical stability.
Singular Value Handling: Applied SVD solver with identity matrix target to compute least-squares solution for overdetermined system (4 equations, 3 unknowns).
Twist Recovery: Developed transformation twist = H⁺ * wheel_velocities recovering robot motion from measured wheel speeds, essential for odometry validation.
Task 3: Figure-8 Trajectory Controller
Waypoint Sequence Design: Created 9-point trajectory with strategic heading changes: W1(0°, forward-left), W4(90°, pure rotation), W5(180°, reverse motion), W9(90°, return) forming figure-8 pattern.
Velocity Frame Transformation: Implemented body-to-world frame conversion using rotation matrix R(yaw) transforming waypoint commands based on current robot orientation from odometry.
Timing Control: Configured 120 iterations per waypoint at 50ms intervals (6 seconds per segment) with 250ms pause between waypoints for smooth transitions.
Task 4: Multi-Threaded Architecture
Callback Group Separation: Implemented MutuallyExclusive callback groups for odometry subscription and control timer preventing race conditions in shared state access.
Parallel Execution: Utilized MultiThreadedExecutor enabling simultaneous odometry processing at 100Hz while maintaining 20Hz control loop without blocking.
Thread-Safe Design: Protected yaw angle updates from quaternion conversion using separate callback context, ensuring consistent orientation data during twist calculations.
Task 5: Omnidirectional Motion Validation
Motion Primitives: Tested six fundamental movements: forward/backward (all wheels same), lateral (alternating pairs), diagonal (combined velocities), rotation (diagonal opposition).
Velocity Scaling: Applied safety factor of 3.0 limiting maximum wheel speed to 1.0 rad/s from theoretical 3.0 rad/s preventing motor saturation.
Open-Loop Performance: Achieved ±5cm repeatability for figure-8 completion despite open-loop control, validating kinematic model accuracy and wheel synchronization.
Results
The system successfully demonstrates true omnidirectional motion with seamless transitions between translational and rotational movements. Figure-8 trajectory execution completes in 60 seconds with consistent path following across multiple runs. Matrix operations complete in under 1 millisecond enabling 20Hz control without computational delays. The SVD-based pseudo-inverse provides numerically stable forward kinematics even with measurement noise. Coordinated wheel control maintains synchronization with maximum deviation of 0.1 rad/s between wheels. The multi-threaded architecture achieves parallel processing with zero message drops or timing violations. Visual demonstrations confirm lateral movement capability impossible with conventional differential drive robots.
Key Insights
-
SVD Robustness: Singular Value Decomposition handles near-singular matrices better than direct pseudo-inverse computation, critical for low-speed operations.
-
Frame Transformation Importance: Body-to-world velocity transformation using real-time yaw feedback essential for trajectory tracking in global coordinates.
-
Mecanum Geometry: 45° roller angle provides optimal force distribution for omnidirectional motion but reduces maximum forward speed by factor of √2.
-
Thread Safety: Callback group separation prevents data races while enabling true parallel execution of perception and control loops.
-
Open-Loop Limitations: ±5cm position drift accumulates over time highlighting need for closed-loop control with position feedback for precision tasks.
Future Work
-
Closed-Loop Control: Implement PID or MPC controller using odometry feedback for position error correction and drift compensation
-
Dynamic Trajectory Generation: Add Bezier curve or spline-based path planning for smooth arbitrary trajectories beyond predefined waypoints
-
Slip Compensation: Develop adaptive algorithms detecting and correcting wheel slip especially during high-acceleration maneuvers
-
Visual Odometry Integration: Combine wheel odometry with camera-based localization for improved position estimation
-
Obstacle Avoidance: Integrate LiDAR or depth camera for reactive navigation while maintaining omnidirectional capabilities
-
Multi-Robot Coordination: Extend to swarm control with formation maintenance using distributed consensus algorithms