Multistage approach for trajectory optimization for a wheeled inverted pendulum passing under an obstacle

A robotic system constructed as a wheeled inverted pendulum (WIP) serves as an impressive demonstrator, since this kind of system is inherently nonlinear, unstable, and nonminimum phase. These properties may pose several difficulties, when it comes to control and trajectory planning. This paper shows a method for deriving a highly dynamic trajectory compliant with the system dynamics by means of solving an optimal control problem (OCP) using multiple shooting. The assumed task includes that the WIP should pass a height-restricting barrier. This can be achieved by leaning back or forth, in order to reduce the overall height of the WIP. The constraints inherent to the definition of this trajectory are nonconvex due to the shape of the robot. The constraint functions have a local minimum in an infeasible region. This can cause problems when the initial guess is within this infeasible region. To overcome this, a multistage approach is proposed for this special OCP to evade the infeasible local minimum. After solving four stages of subsequent optimization problems, the optimal trajectory is obtained and can be used as feedforward for the real system.