{ "cells": [ { "cell_type": "markdown", "id": "f8bfc15c", "metadata": {}, "source": [ "# Linear Quadratic Regulator Example\n", "\n", "Richard M. Murray, 25 Jan 2022\n", "\n", "This notebook contains an example of LQR control applied to the PVTOL system." ] }, { "cell_type": "code", "execution_count": null, "id": "c120d65c", "metadata": {}, "outputs": [], "source": [ "import numpy as np\n", "import matplotlib.pyplot as plt\n", "import control as ct" ] }, { "cell_type": "markdown", "id": "77e2ed47", "metadata": {}, "source": [ "## System description\n", "\n", "We use the PVTOL dynamics from the textbook, which are contained in the file pvtol. The vehicle model is both an I/O system model and a flat system model (for the case when the viscous damping coefficient $c$ is zero)." ] }, { "cell_type": "code", "execution_count": null, "id": "7adc6cf1", "metadata": {}, "outputs": [], "source": [ "from IPython.display import YouTubeVideo\n", "display(YouTubeVideo('ZFb5kFpgCm4', width=640, height=480))\n", "\n", "from pvtol import pvtol, plot_results\n", "print(pvtol)" ] }, { "cell_type": "code", "execution_count": null, "id": "ea50d7cd", "metadata": {}, "outputs": [], "source": [ "# Find the equilibrium point corresponding to hover\n", "xeq, ueq = ct.find_eqpt(pvtol, np.zeros(6), np.zeros(2), np.zeros(6), iy=[1, 2])\n", "# print(ct.StateSpace.__str__(pvtol.linearize(xeq, ueq)))\n", "ueq" ] }, { "cell_type": "markdown", "id": "7cb8840b", "metadata": {}, "source": [ "## Linear Quadratic Regulator Design\n", "\n", "Linearizing the system around an equilibrium point, we can apply LQR to obtain a stabilizing compensator." ] }, { "cell_type": "code", "execution_count": null, "id": "b3cc8857", "metadata": {}, "outputs": [], "source": [ "# Get the linearized dynamics\n", "sys = pvtol.linearize(xeq, ueq)" ] }, { "cell_type": "code", "execution_count": null, "id": "5cfa1ba7", "metadata": {}, "outputs": [], "source": [ "# Start with a diagonal weighting\n", "Qx1 = np.diag([1, 1, 1, 1, 1, 1])\n", "Qu1a = np.diag([1, 1])\n", "K, X, E = ct.lqr(sys, Qx1, Qu1a)" ] }, { "cell_type": "markdown", "id": "863d07de", "metadata": {}, "source": [ "To create a controller for the system, we need to create an I/O system that takes in the desired trajectory $(x_\\text{d}, u_\\text{d})$ and the current state $x$ and generates the control law\n", "\n", "$$\n", "u = u_\\text{d} - K (x - x_\\text{d})\n", "$$\n", "\n", "The function create_statefbk_iosystem() from the ctrlutil package does this. (Take a look inside [ctrlutil.py](/edit/python/ctrlutil.py) to see what is going on.)" ] }, { "cell_type": "code", "execution_count": null, "id": "c19bfdc5", "metadata": {}, "outputs": [], "source": [ "import ctrlutil as ct_\n", "print(ct_.create_statefbk_iosystem.__doc__)" ] }, { "cell_type": "code", "execution_count": null, "id": "5db704e6", "metadata": {}, "outputs": [], "source": [ "control, pvtol_closed = ct_.create_statefbk_iosystem(pvtol, K)\n", "print(control, \"\\n\")\n", "print(pvtol_closed)" ] }, { "cell_type": "markdown", "id": "bedcb0c0", "metadata": {}, "source": [ "## Closed loop system simulation\n", "\n", "We now generate a trajectory for the system and track that trajectory.\n", "\n", "For this simple example, we take the system input to be a \"step\" input that moves the system 1 meter to the right. More complex trajectories (eg, using the results from HW #3) could also be used." ] }, { "cell_type": "code", "execution_count": null, "id": "a497aa2c", "metadata": {}, "outputs": [], "source": [ "# Generate a step response by setting xd, ud\n", "Tf = 15\n", "T = np.linspace(0, Tf, 100)\n", "xd = np.outer(np.array([1, 0, 0, 0, 0, 0]), np.ones_like(T))\n", "ud = np.outer(ueq, np.ones_like(T))\n", "ref = np.vstack([xd, ud])\n", "\n", "response = ct.input_output_response(pvtol_closed, T, ref, xeq)\n", "plot_results(response.time, response.states, response.outputs[6:])" ] }, { "cell_type": "markdown", "id": "f014e660", "metadata": {}, "source": [ "The limitations of the linear controlller can be seen if we take a larger step, say 10 meters." ] }, { "cell_type": "code", "execution_count": null, "id": "a141f100", "metadata": {}, "outputs": [], "source": [ "xd = np.outer(np.array([10, 0, 0, 0, 0, 0]), np.ones_like(T))\n", "ref = np.vstack([xd, ud])\n", "response = ct.input_output_response(pvtol_closed, T, ref, xeq)\n", "plot_results(response.time, response.states, response.outputs[6:])" ] }, { "cell_type": "markdown", "id": "8adb6ff4", "metadata": {}, "source": [ "A better trajectory can be obtained by using a (nonfeasible) trajectory that goes from 0 to 10 in 10 seconds." ] }, { "cell_type": "code", "execution_count": null, "id": "a075a0a7", "metadata": {}, "outputs": [], "source": [ "xf = np.array([10, 0, 0, 0, 0, 0])\n", "xd = np.array([xf/10 * t if t < 10 else xf for t in T]).T\n", "ref = np.vstack([xd, ud])\n", "response = ct.input_output_response(pvtol_closed, T, ref, xeq)\n", "plot_results(response.time, response.states, response.outputs[6:])" ] }, { "cell_type": "code", "execution_count": null, "id": "ae2f5fb2", "metadata": {}, "outputs": [], "source": [] } ], "metadata": { "language_info": { "name": "python" } }, "nbformat": 4, "nbformat_minor": 5 }