iKap: Kinematics-aware Planning with Imperative Learning

TL;DR

iKap Tackles the challenge of vision-to-planning under realistic robot kinematics by formulating end-to-end learning as a self-supervised bi-level optimization (BLO) problem that couples a perception-planning network with a differentiable MPC layer enforcing $x_{t+1}=F(x_t,u_t)$ and trajectory feasibility. The system comprises a depth-based perception encoder, a planning network that outputs keypoint references, and a differentiable MPC using $ ext{SE}(2)$-based Dubins-car kinematics to produce executable trajectories, with gradients flowing through the BLO via implicit differentiation. The upper-level cost $oldsymbol{U}=oldsymbol{A}oldsymbol{C}^ ext{F}+oldsymbol{B}oldsymbol{C}^ ext{E}+oldsymbol{G}oldsymbol{C}^ ext{T}$ guides learning to minimize collisions, environmental proximity, and trajectory tracking error. Results in simulation and on a real Unitree GO2 show higher success rates and lower tracking errors than the state-of-the-art baselines, with real-world feasibility demonstrated despite noisy depth data, underscoring the method’s practical impact and adaptability to different controllers.

Abstract

Trajectory planning in robotics aims to generate collision-free pose sequences that can be reliably executed. Recently, vision-to-planning systems have gained increasing attention for their efficiency and ability to interpret and adapt to surrounding environments. However, traditional modular systems suffer from increased latency and error propagation, while purely data-driven approaches often overlook the robot's kinematic constraints. This oversight leads to discrepancies between planned trajectories and those that are executable. To address these challenges, we propose iKap, a novel vision-to-planning system that integrates the robot's kinematic model directly into the learning pipeline. iKap employs a self-supervised learning approach and incorporates the state transition model within a differentiable bi-level optimization framework. This integration ensures the network learns collision-free waypoints while satisfying kinematic constraints, enabling gradient back-propagation for end-to-end training. Our experimental results demonstrate that iKap achieves higher success rates and reduced latency compared to the state-of-the-art methods. Besides the complete system, iKap offers a visual-to-planning network that seamlessly works with various controllers, providing a robust solution for robots navigating complex environments.

Figures (6)

• Figure 1: The kinematics-aware planner, iKap, incorporates kinematic state transition functions directly into the learning pipeline. This integration allows the network to learn feasible waypoint distributions and produce more executable trajectories. (a) The robot, constrained by a minimum turning radius, must reach the red star while avoiding obstacles. (b) A real-world depth image alongside the predicted trajectory. (c) A geometry-based planner may generate the blue trajectory, which does not satisfy the kinematic constraints, making it difficult to execute.
• Figure 2: The training pipeline for the planner is based on BLO. In this framework, the networks, the optimized object, generate waypoints based on the given depth perception and goals. Then, the low-level trajectory optimization module tracks a kinematics-feasible trajectory. The embedded kinematics and gradients from the lower-level module are then utilized to supervise and train the networks.
• Figure 3: The real-world experiment pipeline. The experiments are conducted on the Unitree Go2 robot dog. We use AirVO to estimate the robot's odometry, command the goal waypoint to the robot, and input this information into iKap along with depth images captured by the RealSense D435i camera.
• Figure 4: Qualitative performance. (a) iPlanner without controller: The planner generates sharp-angled trajectories that are difficult to execute. (b) iPlanner with PID controller: Without considering kinematics, iPlanner accumulates significant tracking errors, making control challenging. Near the goal, these errors cause iPlanner to get stuck in a looping behavior. (c, d) U-turn tests: iPlanner (c) generates backward trajectories, which are difficult for the MPC controller to execute. In contrast, iKap+MPC in (d) plans feasible trajectories that the controller can follow. (e, f) Trajectory Tracking: Using PID (e) and MPC (f) to track the waypoints generated by iKap. The robot can navigate robustly.
• Figure 5: The trajectory tracking error against the minimum turning radius by PID and MPC controller in Garage simulation environment