Abstract: This paper presents a fast numerical algorithm for velocity optimization based on Pontryagin's minimum principle (PMP). Considering the difficulties associated with the application of the PMP when state constraints exist, ...