Smoothed and regularized fischer-burmeister solver for embedded real-time constrained optimal control problems in autonomous systems

A method for controlling an autonomous vehicle having a controller, the system comprising:

• in each sampling period, the controller
• inputting sensor values and estimators that are calculated based on the sensor values and dynamic models and record the sensor values and the estimators in a memory of the controller,
• incorporating the sensor values and the estimators into conditions for optimality associated with a constrained optimal control problem,
• mapping the conditions for optimality to a non-smooth system using Fischer-Burmeister function,
• performing Newton method iterations on a sequence of smoothed systems approximating the non-smooth system in order to converge on a solution, and
• issuing commands that control actuators of the autonomous vehicle.

A controller for controlling an autonomous vehicle,

• the controller configured to:
• in each sampling period,
• input sensor values and estimators that are calculated based on the sensor values and dynamic models and record the sensor values and the estimators in a memory of the controller,
• incorporate the sensor values and the estimators into conditions for optimality associated with a constrained optimal control problem,
• map the conditions for optimality to a non-smooth system using Fischer-Burmeister function,
• smooth the non-smooth system and apply Newton method iterations to the smoothed system in order to converge on a solution, and
• issue commands that control operation of actuators in the autonomous vehicle.