S62_driving_actions.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "view-in-github",
    "tags": [
     "no-tex"
    ]
   },
   "source": [
    "<a href=\"https://colab.research.google.com/github/gtbook/robotics/blob/main/S62_driving_actions.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {
    "id": "nAvx4-UCNzt2"
   },
   "source": [
    "# Kinematics for Driving\n",
    "\n",
    "> Cars are more complex than differential drive robots: they cannot turn in place, and they typically have front-wheel steering.\n",
    "\n",
    "<img src=\"Figures6/S62-Autonomous_Vehicle_with_LIDAR_and_cameras-06.jpg\" alt=\"Splash image with steampunk autonomous car\" width=\"40%\" align=center style=\"vertical-align:middle;margin:10px 0px\">"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this section we introduce a kinematic model for cars.\n",
    "Unlike the differential drive robots of the previous chapter, cars have four wheels, two of which are used for steering,\n",
    "and two of which are used to induce linear motion (either the two rear or two front wheels, except in the case of four-wheel drive vehicles).\n",
    "Therefore, we might choose to model the car as having two inputs: a steering angle and an acceleration.\n",
    "While this might be a more accurate physical model, we will use a slightly simplified model, in which the\n",
    "forward speed and turning rate are the inputs.\n",
    "Unless the car is traveling at high speed, or experiencing large accelerations, this model is\n",
    "accurate enough for most purposes."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Car kinematics\n",
    "\n",
    "We will assume that our car has front-wheel steering, and that forward velocity is achieved by actuation\n",
    "of the rear wheels.\n",
    "We assign a body-attached frame to the car with origin at the midpoint of the rear axle and $x$-axis pointing forward,\n",
    "as shown in the figure below.\n",
    "If the tires roll without slipping, the instantaneous velocity of the car is always in the direction of\n",
    "the body-attached $x$-axis.\n",
    "We denote the forward speed by $v_\\mathrm{car}$ (note that $v_\\mathrm{car}$ denotes the *scalar* speed of the car,\n",
    "and not the car velocity, which is a *vector* quantity)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "<figure id=\"fig:TransformComposition2\">\n",
    "<img src=\"https://github.com/gtbook/robotics/blob/main/Figures6/car1.png?raw=1\" style=\"width:8cm\" alt=\"\">\n",
    "<figcaption>Coordinate frame for a car-like vehicle. </figcaption>\n",
    "</figure>\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "At first glance, it may appear that the two front wheels have the same orientation w.r.t. the car frame.\n",
    "This is not the case, however.\n",
    "As the origin of the car frame traces out a curve in the plane, in order for all wheels to roll without\n",
    "slipping, their instantaneous velocities must be tangent to a set of circles that share a common origin,\n",
    "called the center of the turning circle.\n",
    "For the rear wheels, because they share a common axle and maintain a fixed orientation w.r.t. the car frame,\n",
    "the axle coincides with the radius of concentric circles,\n",
    "and each of the rear wheels has instantaneous velocity that is perpendicular to this radius.\n",
    "For the front wheels, the inner wheel follows a circle with smaller radius than the outside wheel."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<figure id=\"fig:AckermannSteering1\">\n",
    "<img src=\"https://github.com/gtbook/robotics/blob/main/Figures6/ackermann1.PNG?raw=1\"  style=\"width:8cm\" alt=\"\">\n",
    "<figcaption>Ackermann steering.</figcaption>\n",
    "</figure>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This kind of steering for the front wheels is called **Ackermann steering**, as illustrated in the figure above.\n",
    "The physical mechanism required to implement Ackermann steering is slightly complex,\n",
    "but happily we can model the system by using a single *virtual wheel* placed\n",
    "at the midpoint between the two front wheels, rolling in a direction perpendicular to the line from\n",
    "the center of the turning circle to this midpoint.\n",
    "We denote by $\\phi$ the angle from the car $x$-axis to the forward direction of this virtual wheel,\n",
    "as shown in the figure below."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<figure id=\"fig:AckermannSteering2\">\n",
    "<img src=\"https://github.com/gtbook/robotics/blob/main/Figures6/car2.png?raw=1\" style=\"width:8cm\" alt=\"\">\n",
    "<figcaption>Virtual wheel used to model Ackermann steering.</figcaption>\n",
    "</figure>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The configuration of the car can be represented as $q = (x,y,\\theta, \\phi)$, in which $x,y$ denotes the position\n",
    "of the origin of the car frame, $\\theta$ denotes the orientation of car frame (the angle from the world $x$-axis\n",
    "to the car frame $x$-axis), and $\\phi$ denotes the steering angle.\n",
    "The inputs to the car are\n",
    "* $u_1= v_\\mathrm{car}$, the linear speed of the car, and\n",
    "* $u_2 = \\dot{\\phi}$, the turning rate of the virtual front wheel."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "<figure id=\"fig:CarConfigurationParams\">\n",
    "<img src=\"https://github.com/gtbook/robotics/blob/main/Figures6/car3.png?raw=1\" style=\"width:12cm\" alt=\"\">\n",
    "<figcaption>Configuration parameters for the car.</figcaption>\n",
    "</figure>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Differential Kinematics\n",
    "\n",
    "The differential kinematics for the car are specified as an equation,\n",
    "$\\dot{q} = f(q, u_1, u_2)$.\n",
    "The first two components, $\\dot{x}$ and $\\dot{y}$ are straightforward functions\n",
    "of  $v_\\mathrm{car}$ and $\\theta$\n",
    "\n",
    "$$\n",
    "\\begin{aligned}\n",
    "\\dot{x} &=  v_\\mathrm{car} \\cos \\theta = u_1 \\cos \\theta \\\\\n",
    "\\dot{y} &=  v_\\mathrm{car} \\sin \\theta = u_1 \\sin \\theta \n",
    "\\end{aligned}\n",
    "$$\n",
    "\n",
    "The steering angle is also straightforward:\n",
    "\n",
    "$$\n",
    "\\dot{\\phi} = u_2 $$\n",
    "\n",
    "Note that using this model, we cannot directly control the car's orientation $\\theta$.\n",
    "We cannot even directly choose the steering angle $\\phi$.\n",
    "We have direct control only over $\\dot{\\phi}$."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The equation for $\\dot{\\theta}$ is slightly more complex.\n",
    "From the figure below, we observe the following\n",
    "\n",
    "$$\n",
    "\\begin{aligned}\n",
    "v_\\mathrm{wheel} \\cos \\phi &=  v_\\mathrm{car}  \\\\\n",
    "v_\\mathrm{wheel} \\sin \\phi &=  v_\\mathrm{tangent}  \\\\\n",
    "\\end{aligned}\n",
    "$$\n",
    "\n",
    "Combining these to eliminate $v_\\mathrm{wheel}$ we obtain\n",
    "\n",
    "$$\n",
    "\\frac{v_\\mathrm{car}}{\\cos \\phi} \\sin \\phi = v_\\mathrm{car} \\tan \\phi = v_\\mathrm{tangent}\n",
    "$$\n",
    "\n",
    "Now, using $L \\dot{\\theta} = v_\\mathrm{tangent}$ (where $L$ is the baseline distance between front and rear axles),\n",
    "we obtain\n",
    "\n",
    "$$\n",
    "\\dot{\\theta} = v_\\mathrm{car} \\frac{1}{L} \\tan \\phi = u_1 \\frac{1}{L} \\tan \\phi\n",
    "$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Collecting these terms together, we can write the differential kinematics\n",
    "in matrix form as:\n",
    "\n",
    "$$\n",
    "\\begin{bmatrix}\n",
    "\\dot{x}\\\\\n",
    "\\dot{y}\\\\\n",
    "\\dot{\\theta}\\\\\n",
    "\\dot{\\phi}\n",
    "\\end{bmatrix}\n",
    "=\n",
    "\\begin{bmatrix}\n",
    "\\cos \\theta\\\\\n",
    "\\sin \\theta \\\\\n",
    "\\frac{1}{L} \\tan \\phi\\\\\n",
    "0\n",
    "\\end{bmatrix}\n",
    "u_1\n",
    "+\n",
    "\\begin{bmatrix}\n",
    "0\\\\\n",
    "0\\\\\n",
    "0\\\\\n",
    "1\n",
    "\\end{bmatrix}\n",
    "u_2\n",
    "$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "<figure id=\"fig:CarRelevantVelocities\">\n",
    "<img src=\"https://github.com/gtbook/robotics/blob/main/Figures6/car4.png?raw=1\" style=\"width:12cm\" alt=\"\">\n",
    "<figcaption>Relevant velocities for computing the differential kinamatics of the car.</figcaption>\n",
    "</figure>"
   ]
  }
 ],
 "metadata": {
  "colab": {
   "collapsed_sections": [],
   "include_colab_link": true,
   "name": "S62_driving_actions.ipynb",
   "provenance": []
  },
  "interpreter": {
   "hash": "c6e4e9f98eb68ad3b7c296f83d20e6de614cb42e90992a65aa266555a3137d0d"
  },
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.18"
  },
  "latex_metadata": {
   "affiliation": "Georgia Institute of Technology",
   "author": "Frank Dellaert and Seth Hutchinson",
   "title": "Introduction to Robotics"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}