mirror of
https://gitlab.ewi.tudelft.nl/ee2l1/2025-2026/A.K.03.git
synced 2025-12-12 16:00:56 +01:00
208 lines
9.9 KiB
Plaintext
208 lines
9.9 KiB
Plaintext
{
|
||
"cells": [
|
||
{
|
||
"cell_type": "markdown",
|
||
"metadata": {},
|
||
"source": [
|
||
"# Controller\n",
|
||
"**⚠️ Optional Advanced method**\n",
|
||
"The contents of this module cover advanced topics that not all students are expected to understand."
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "markdown",
|
||
"metadata": {},
|
||
"source": [
|
||
"### The complex problem \n",
|
||
"\n",
|
||
"**Find a control law** $$ \\mathbf{u}(t) = \\begin{bmatrix} v(t) \\\\ \\phi(t) \\end{bmatrix} $$ which are the numbers that are sent to KITT as car velocity and steering that moves the car from the initial state $$ \\mathbf{x}(0) = \\mathbf{x}_0 $$ to the goal state $$ \\mathbf{x}(T) = \\mathbf{x}_g $$ in finite time T , while satisfying the following conditions:\n",
|
||
"\n",
|
||
"\n",
|
||
" "
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "markdown",
|
||
"metadata": {},
|
||
"source": [
|
||
"\n",
|
||
"\n",
|
||
"- **Kinematic Equations**: The car's motion is governed by equations that relate its position, orientation, velocity, and steering angle.\n",
|
||
"- **Non-Holonomic Constraint**: The car cannot move sideways; it can only move forward or backward along its current orientation.\n",
|
||
"- **Control Input Limits**:\n",
|
||
" - **Steering Angle Limit**: The steering angle cannot exceed a maximum absolute value.\n",
|
||
" - **Velocity Limit**: The car's speed must be within specified minimum and maximum values."
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "markdown",
|
||
"metadata": {},
|
||
"source": [
|
||
"> **Simplifying the problem**\n",
|
||
">\n",
|
||
"> So as you see the control problem is quite complex. To make controlling the car from point A to point B manageable, we simplify the complex two-dimensional problem into a one-dimensional one. The idea is to do this by:\n",
|
||
">\n",
|
||
"> - **Planning a Feasible Path**: Create a path that the car can physically follow, considering its inability to move sideways and its steering limits.\n",
|
||
"> - **Projecting Movement onto the Path**: Instead of controlling the car's position in the entire plane, we focus on its position along this path.\n",
|
||
"> - **One-Dimensional Control**: Adjust the car's speed to control its progress along the path, effectively turning a complex navigation task into controlling how fast it moves forward.\n",
|
||
">\n",
|
||
"> By simplifying the problem in this way, we can more easily manage the car's movement while still respecting all its physical constraints."
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "markdown",
|
||
"metadata": {},
|
||
"source": [
|
||
"\n",
|
||
"\n",
|
||
"\n",
|
||
"### Partitioning by projection\n",
|
||
"\n",
|
||
"If you could project our two-dimensional space onto\n",
|
||
"a one-dimensional space S, then you could use KITT’s position in S for one-dimensional control. The figure below depicts this idea. The line represents the trajectory that you would want to follow.\n",
|
||
"\n",
|
||
"Let KITT's trajectory be given by, \n",
|
||
"\n",
|
||
"$$ \\mathbf{x}(t) = \\left[ x(t), y(t) \\right]^T $$\n",
|
||
"\n",
|
||
"You can define the projection of \\((x, y)\\) onto \\(S\\) by\n",
|
||
"$$\n",
|
||
"z = z(x, y)\n",
|
||
"$$\n",
|
||
"\n",
|
||
"where $z \\in S$. The projected position $z$ can be regarded as the distance you travel on $ S $. KITT's speed should then be continuously controlled so that $z$ approaches the desired distance without overshooting. Thus, using $z$ as a measure for distance allows for one-dimensional control in a plane.\n",
|
||
"\n",
|
||
"This approach requires knowing KITT's trajectory: you need an *estimate* of KITT's future movement. This is given by the planned trajectory. You can now state your wanted projection: The projected position $z$ is given by the arc length of the planned trajectory from KITT's current position to its target position. This allows for velocity control along any trajectory in a plane.\n",
|
||
"\n",
|
||
" <img src=\"trajectory.jpg\" alt=\"KITTtraject-figure\" width=\"250px\">\n",
|
||
"\n",
|
||
"*KITT's trajectory*\n",
|
||
"<!--\n",
|
||
"```{figure} trajectory.jpg\n",
|
||
"---\n",
|
||
"height/width: 150px\n",
|
||
"name: KITTtraject-figure\n",
|
||
"---\n",
|
||
"KITT's trajectory\n",
|
||
"```\n",
|
||
"-->\n"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "markdown",
|
||
"metadata": {
|
||
"vscode": {
|
||
"languageId": "plaintext"
|
||
}
|
||
},
|
||
"source": [
|
||
"### Steering model\n",
|
||
"\n",
|
||
"If we steer a car by making the front wheels make an angle to the baseline, then the car will follow a circle. The angle $\\phi$ of the wheels will determine the radius $R$ of the circle (if the angle is zero, then the radius will become infinity: driving straight is a limit case). The dynamic model for driving on a line remains valid, except that it now describes the distance we drive on the circle.\n",
|
||
"\n",
|
||
"Thus, the hard part at this point is to derive a relation between the angle $\\phi$ and the radius $R$. Consider KITT with distance L between both wheel axes. The rear axis of the car is located at [0; 0]\n",
|
||
"and the front axis at [L; 0] in a way that they are parallel to each other. The front wheels are turned an angle $\\phi$ relative to the positive x-axis. If the car drives at a speed $v$, then after a very small time $∆t$, the rear axis\n",
|
||
"is located at:\n",
|
||
"$$\n",
|
||
"\\begin{bmatrix}v\\, \\cos(\\phi)\\Delta t\\\\0 \\end{bmatrix}\n",
|
||
"$$\n",
|
||
"and the front axis at:\n",
|
||
"$$\n",
|
||
"\\begin{bmatrix}L + v\\, \\cos(\\phi)\\Delta t\\\\ v\\, \\sin(\\phi)\\Delta t \\end{bmatrix}\n",
|
||
"$$\n",
|
||
"The situation is sketched below.\n",
|
||
"\n",
|
||
"\n",
|
||
"<img src=\"steering1.png\" alt=\"KITTsteering-figure\" width=\"250px\">\n",
|
||
"\n",
|
||
"\n",
|
||
"*Visualization of KITT steering*\n",
|
||
"\n",
|
||
"The car was first parallel to the positive x-axis. Therefore, if $\\theta$ denotes the angle of the car relative to the\n",
|
||
"positive x-axis,\n",
|
||
"$$\n",
|
||
"θ(t) = 0\n",
|
||
"$$\n",
|
||
"and\n",
|
||
"$$\n",
|
||
"θ(t + ∆t) = \\arctan( \\frac{v\\, \\sin (φ)∆t} {L} )\n",
|
||
"$$\n",
|
||
".\n",
|
||
"We can now evaluate the rate at which KITT turns by\n",
|
||
"$$\n",
|
||
"\\begin{aligned}\n",
|
||
"\\frac{d}{dt}\\theta(t) &= \\lim_{\\Delta t \\to 0} \\frac{\\theta(t + \\Delta t) - \\theta(t)}{\\Delta t}\\\\\n",
|
||
"&= \\lim_{\\Delta t \\to 0} \\frac{\\arctan[v\\, \\sin(\\phi) \\Delta t / L]}{\\Delta t}\\\\\n",
|
||
"&= \\lim_{\\Delta t \\to 0} (1 + \\frac{v^2\\, \\sin(\\phi) \\Delta t^2]}{L^2})^{-1}\\; \\frac{v\\, \\sin(\\phi)}{L} \\\\\n",
|
||
"&= \\frac{v\\, \\sin(\\phi)}{L}\n",
|
||
"\\end{aligned}\n",
|
||
"$$\n",
|
||
"We have found a relationship between the angle of KITT’s wheels and the rate at which KITT turns."
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "markdown",
|
||
"metadata": {},
|
||
"source": [
|
||
"### Following your goal\n",
|
||
"\n",
|
||
"When calculating the accelleration of KITT, you assumed that you were able to let KITT follow any predefined trajectory. You will now design a controller which keeps KITT on track.\n",
|
||
"\n",
|
||
"Intuitively, you will think of a solution where you know your current position/orientation, and you always steer towards the target. Once you are oriented towards the target, your \"angle error\" is zero, and you just have to drive straight. In all other cases, the angle error determines how much you need to steer. It is easy to see that this approach might work, but also might become unstable once you are very close to the target.\n",
|
||
"\n",
|
||
"Suppose KITT is driving on its trajectory $S$, where its orientation with respect to the $x$-axis is given by $\\theta$. Let the angle tangent to its trajectory $S$ be given by $\\theta_t$. Ideally, this angle should be equal to KITT's orientation. If KITT's orientation deviates from this angle, KITT should turn its wheels to steer back. This motivates a feedback law given by\n",
|
||
"\n",
|
||
"$$\n",
|
||
"\\phi = -k(\\theta - \\theta_t)\n",
|
||
"$$\n",
|
||
"\n",
|
||
"for a positive $k$. Substituting in the rate at which KITT turns yields the autonomous non-linear system\n",
|
||
"\n",
|
||
"$$\n",
|
||
"L \\,\\frac{d}{dt}\\theta \\;+\\; v\\, \\sin(k(\\theta-\\theta_t)) = 0\\,.\n",
|
||
"$$\n",
|
||
"\n",
|
||
"You should choose $k$ such that this system is asymptotically stable. To investigate the stability, you introduce a potential function (error function) given by\n",
|
||
"\n",
|
||
"$$\n",
|
||
"T(\\theta) = \\frac{1}{2}(\\theta-\\theta_t)^2 \\,.\n",
|
||
"$$\n",
|
||
"\n",
|
||
"A first observation is that $T(\\theta)=0$ if and only if $\\theta=\\theta_t$, which is exactly what you want. Second, notice that $T(\\theta) \\ge 0$. you can conclude that $T$'s minimum corresponds to our equilibrium point. Notice that\n",
|
||
"\n",
|
||
"$$\n",
|
||
"\\frac{d}{dt} T(\\theta) = -(\\theta-\\theta_t)\\frac{v\\, \\sin{(k(\\theta-\\theta_t))}}{L}.\n",
|
||
"$$\n",
|
||
"\n",
|
||
"The figure below depicts both $T(\\theta)$ and its time-derivative.\n",
|
||
"\n",
|
||
"\n",
|
||
"<img src=\"potentialfunction.jpg\" alt=\"KITTpotential-figure\" width=\"250px\">\n",
|
||
"\n",
|
||
"*Graph of potential function and it's derivative*\n",
|
||
"\n",
|
||
"Consider KITT's orientation at any instant. If $ |\\theta| < \\theta_m $, then by the figure above the potential function will have a negative derivative. But then it will decrease over time and will keep decreasing until $\\theta = \\theta_t$. So in conclusion, if $ |\\theta| < \\theta_m $, then $\\theta$ will converge to its equilibrium point. By inspection of Equation (\\ref{eq:lyapunov}) you can now state that Equation (\\ref{eq:angle-diffeq}) is locally asymptotically stable for any\n",
|
||
"\n",
|
||
"$$\n",
|
||
"-\\frac{\\pi}{k} < \\theta - \\theta_t < \\frac{\\pi}{k}.\n",
|
||
"$$\n",
|
||
"\n",
|
||
"This treatment is also called the investigation of *Lyapunov stability*, where $T$ is called the *Lyapunov function*. It is an extensive topic in the control literature."
|
||
]
|
||
}
|
||
],
|
||
"metadata": {
|
||
"kernelspec": {
|
||
"display_name": "Python 3",
|
||
"language": "python",
|
||
"name": "python3"
|
||
},
|
||
"language_info": {
|
||
"name": "python",
|
||
"version": "3.12.6"
|
||
}
|
||
},
|
||
"nbformat": 4,
|
||
"nbformat_minor": 2
|
||
}
|