Context
This project supports the physical validation stage of an ongoing research effort in safe autonomous control. The objective is to transfer the control framework from simulation-based development toward deployment on the Toyota HSR, providing the motion-planning and closed-loop control infrastructure needed to evaluate navigation under real sensing, actuation, and environmental uncertainty. This project will be continually updated as the researcher's paper approaches submission and I recieve more tasks. The current progress is stated below.
Current Progress
I've developed and integrated a nonlinear MPC controller for the HSR omnidirectional base using ROS 2, CasADi, and IPOPT. The controller performs receding-horizon trajectory optimization with velocity constraints, tracking and smoothness objectives, and time-varying hyperplane constraints for obstacle avoidance. I implemented PRM* and RRT* global planners that automatically generate collision-free guide paths for the MPC, replacing manually specified waypoints and allowing the system to adapt its route to the obstacle configuration; the choice depends on if the obstacles are dynamic (use RRT*) or static (use PRM*). I also refactored the optimization into a fixed-size, parameterized CasADi function with trajectory warm-starting, reducing mean solve time by 75.4%, from 153.08 ms to 37.61 ms, while maintaining sufficient computational margin for the controller’s 200 ms update period. I also implemented a particle-filter-based multi-object tracker between perception and the planning/control stack. Each obstacle is represented by a 4D position-and-velocity state, ([p_x,p_y,v_x,v_y]), estimated using 500 particles with a constant-velocity motion model, Gaussian measurement weighting, and effective-sample-size-triggered systematic resampling. I added gated nearest-neighbor data association and track-management logic so obstacles maintain persistent tracks through noisy measurements and temporary missed detections. The tracker publishes filtered obstacle estimates directly into the existing planner/MPC interface; in standalone testing, it reduced position RMSE from 7.65 cm for raw detections to 5.46 cm while continuing to estimate obstacle state during dropped measurements.
Videos sped up for easier viewing
In Progress
There is quite a bit of work for the near future, but it can be summarized in a couple entries.
1) Validation the MPC and global planners
2) Integration with a vision system developed by another member of the lab
3) Deployment on a physical Toyota HSR platform