C++ implementation of Passivity Based Nonlinear Model Predictive Control (NMPC) designed for controlling a robotic manipulator with two joints. The implementation includes an objective function that minimizes a combination of kinetic energy and error to a desired state, and a passivity constraint to ensure stable control behavior. This is inpired by MATLAB example [3].
Passivity Based Nonlinear Model Predictive Control
Passivity Based Nonlinear Model Predictive is an MPC control scheme where a passivity-based constraint is used to obtain a nonlinear model predictive control scheme with guaranteed closed loop stability for any, possible arbitrarily small, prediction horizon.
The dynamics of robotic manipulator can be expressed as:
Here, , and are vectors that represent the joint angles, velocities, and accelerations. The control input vector is the torque
- is the manipulator inertia matrix.
- is the Coriolis matrix.
- is the gravity vector.
The control objective is to select torque such that the joint angles track a desired reference . To enforce closed-loop stability, the controller includes a passivity constraint [2].
Passivity Constraint
To define the passivity constraint, first we define the tracking error vector as the difference between the joint angles and the desired reference angles.
To achieve good tracking performance, we define the storage function as
,
where . Taking the derivative of we obtain the relationship , where Therefore, the system is passive from to .
To enforce closed-loop stability, we define the passivity constraint as follows [2].
See src/manipulator.cpp
for code a example and results below.
Results for 2 link manipulator
As depicted in the figure above, the cost function is strictly decreasing, a property inforced via the passivity constraint.
References
[1] Hatanaka, Takeshi, Nikhil Chopra, Masayuki Fujita, and Mark W. Spong. Passivity-Based Control and Estimation in Networked Robotics. Communications and Control Engineering. Cham: Springer International Publishing, 2015. https://doi.org/10.1007/978-3-319-15171-7.
[2] Raff, Tobias, Christian Ebenbauer, and Frank Allgöwer. “Nonlinear Model Predictive Control: A Passivity-Based Approach.” In Assessment and Future Directions of Nonlinear Model Predictive Control, edited by Rolf Findeisen, Frank Allgöwer, and Lorenz T. Biegler, 358:151–62. Berlin, Heidelberg: Springer Berlin Heidelberg, 2007. https://doi.org/10.1007/978-3-540-72699-9_12.