This repository contains the project report on trajectory optimization for a 2-degree-of-freedom (DoF) robot using Nonlinear Model Predictive Control (NMPC). The report, authored by Shubhankar Kulkarni and dated March 10, 2024, explores optimal control techniques to guide the robot from an initial position to a target setpoint while considering dynamics, constraints, and disturbances. The PDF report is available here.
The report addresses point-to-point trajectory generation for a 2 DoF robot in an ideal environment, focusing on finding the optimal path while respecting joint constraints, velocities, and input torques. It derives the robot's dynamics using Euler-Lagrange equations and formulates the problem as an Optimal Control Problem (OCP). Various integration methods, cost functions, and control strategies are implemented and analyzed, including handling free-end-time scenarios and disturbances for robustness. Results include simulations showing joint angles, velocities, controls, and trajectories in both joint and Cartesian spaces.
This work is particularly useful for students and researchers in robotics, control systems, and optimization, demonstrating practical applications of NMPC in nonlinear systems.
The report is structured into sections that build from foundational concepts to advanced implementations. Here's a breakdown with brief details:
Introduces model predictive control (MPC) and its applications in robotics, such as trajectory optimization and setpoint stabilization. It highlights the importance of factors like joint constraints, velocities, and environmental limits in computing optimal paths for robots.
Defines the 2 DoF robot's dynamics without contact forces, using the Euler-Lagrange form: ( B(q)\ddot{q} + C(q, \dot{q})\dot{q} + g(q) = u ). Details the inertial matrix ( B(q) ), Coriolis matrix ( C(q, \dot{q}) ), and gravity vector ( g(q) ), along with initial/final conditions (e.g., ( q_0 = (-5, -4)^T ) rad, ( q_T = (\pi/2, 0)^T ) rad) and constraints on torques (( u \in [-1000, 1000] ) Nm) and velocities (( \dot{q} \in [-3\pi/2, 3\pi/2] ) rad/s).
Explains derivation of the dynamics from kinetic (( T )) and potential (( V )) energies via the Lagrangian ( L = T - V ). Computes energies for each link and solves the Euler-Lagrange equations to obtain the nonlinear system model, introducing cosine and sine terms for nonlinearity.
Reformulates the dynamics into state-space form ( \dot{x} = f(x, u) ), where states are joint angles ( q ) and velocities ( \dot{q} ). Inverts the inertial matrix to solve for accelerations and expands into a full matrix equation including system dynamics ( A ), input ( B ), and gravity terms.
Discusses joint space (q1-q2) vs. Cartesian space (x-y), with forward kinematics: ( x_e = l_1 \cos q_1 + l_2 \cos(q_1 + q_2) ), ( y_e = l_1 \sin q_1 + l_2 \sin(q_1 + q_2) ). Explains advantages of joint space for control, lower dimensionality, and avoiding singularities.
Formulates the OCP with a Linear Quadratic Regulator (LQR) cost: ( J = \int_0^3 (x^{T} Q x^ + u^{T} R u^)/2 , dt ), minimizing errors from setpoint. An alternative minimizes torques only: ( J = \int_0^3 u^2 / 2 , dt ). Time horizon is fixed at 3 seconds.
Scales matrices ( Q ), ( R ), and terminal ( P ) to handle torque magnitudes (e.g., ( R = 10^{-6} I )). Presents the continuous-time system and uses discrete methods for integration due to programming constraints.
Describes the 4th-order Runge-Kutta integrator for solving ODEs, with steps for computing intermediate derivatives. Shows results for varying discretization steps ( N ) (10 to 200), demonstrating smoother trajectories as ( N ) increases, stabilizing around ( N = 100 ).
Introduces the 2nd-order Heun's method as an improved Euler variant, computing average slopes. Compares outputs to Runge-Kutta, noting similarities for this problem but lower computation cost, though less accurate for complex systems.
Minimizes ( \int_0^3 (1/2) u(t)^2 , dt ) subject to dynamics, emphasizing stability by penalizing large controls to avoid oscillations and ensure smooth operation.
Introduces variable time ( \tau = t / t_1 ) to optimize duration, reformulating dynamics with ( t_1 ) as a decision variable. Solves minimum-time problems (constrained to 0-3 seconds) and compromises via ( \min 10^{12} \alpha T + (1-\alpha) \int_0^T |u|^2 , dt ), with ( \alpha ) balancing time and controls. Minimum time found: 2.13 seconds; balanced at 7.8 seconds for ( \alpha = 0.9 ).
Explains MPC as repeated OCP solving in a receding horizon. Details closed-loop feedback (resolving with updated states) vs. open-loop. Uses direct multiple shooting for discretization and Runge-Kutta for accuracy. Analyzes disturbances in dynamics (( u + \zeta )), data, external forces, and model parameters, showing instability with noise (e.g., Gaussian variance 0.1 leads to infeasibility).
Discusses feasibility (limited by unbounded joints) and asymptotic stability using Lyapunov function ( V(x) = 10000 (x(t_k + T) - x_f)^2 ). Proves conditions for endpoints and robustness to noise.
Summarizes approaches to setpoint stabilization, minimum time (2.13 seconds), and suggestions for improvements like bounding joints and using KKT conditions for optimality.