mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-13 10:37:54 -05:00
add math descriptions
This commit is contained in:
Atsushi
@@ -2,45 +2,112 @@
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"# Quintic polynomials planner"
|
||||
],
|
||||
"metadata": {
|
||||
"collapsed": false
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## Quintic polynomials for one dimensional robot motion\n",
|
||||
"\n",
|
||||
"We assume a dimensional robot motion $x(t)$ at time $t$ is formulated as a quintic polynomials based on time as follows:\n",
|
||||
"We assume a one-dimensional robot motion $x(t)$ at time $t$ is formulated as a quintic polynomials based on time as follows:\n",
|
||||
"\n",
|
||||
"$x(t) = a_0+a_1t+a_2t^2+a_3t^3+a_4t^4$\n",
|
||||
"$x(t) = a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5$ --(1)\n",
|
||||
"\n",
|
||||
"a_0, a_1. a_2, a_3 are parameters of the quintic polynomial.\n",
|
||||
"$a_0, a_1. a_2, a_3, a_4, a_5$ are parameters of the quintic polynomial.\n",
|
||||
"\n",
|
||||
"It is assumed that terminal states (start and end) are known as boundary conditions.\n",
|
||||
"\n",
|
||||
"Start position, velocity, and acceleration are $x_s, v_s, a_s$ respectively.\n",
|
||||
"\n",
|
||||
"End position, velocity, and acceleration are $x_e, v_e, a_e$ respectively.\n",
|
||||
"\n",
|
||||
"So, when time is 0.\n",
|
||||
"\n",
|
||||
"$x(0) = a_0 = x_s$ -- (2)\n",
|
||||
"\n",
|
||||
"Then, differentiating the equation (1) with t, \n",
|
||||
"\n",
|
||||
"$x'(t) = a_1+2a_2t+3a_3t^2+4a_4t^3+5a_5t^4$ -- (3)\n",
|
||||
"\n",
|
||||
"So, when time is 0,\n",
|
||||
"\n",
|
||||
"$x(0) = a_0 = x_s$\n",
|
||||
"$x'(0) = a_1 = v_s$ -- (4)\n",
|
||||
"\n",
|
||||
"$x_s$ is a start x position.\n",
|
||||
"Then, differentiating the equation (3) with t again, \n",
|
||||
"\n",
|
||||
"Then, differentiating this equation with t, \n",
|
||||
"\n",
|
||||
"$x'(t) = a_1+2a_2t+3a_3t^2+4a_4t^3$\n",
|
||||
"$x''(t) = 2a_2+6a_3t+12a_4t^2$ -- (5)\n",
|
||||
"\n",
|
||||
"So, when time is 0,\n",
|
||||
"\n",
|
||||
"$x'(0) = a_1 = v_s$\n",
|
||||
"$x''(0) = 2a_2 = a_s$ -- (6)\n",
|
||||
"\n",
|
||||
"$v_s$ is a initial speed for x axis.\n",
|
||||
"so, we can calculate $a_0$, $a_1$, $a_2$ with eq. (2), (4), (6) and boundary conditions.\n",
|
||||
"\n",
|
||||
"=== TBD ==== \n"
|
||||
],
|
||||
"metadata": {
|
||||
"collapsed": false
|
||||
}
|
||||
"$a_3, a_4, a_5$ are still unknown in eq(1).\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"We assume that the end time for a maneuver is $T$, we can get these equations from eq (1), (3), (5):\n",
|
||||
"\n",
|
||||
"$x(T)=a_0+a_1T+a_2T^2+a_3T^3+a_4T^4+a_5T^5=x_e$ -- (7)\n",
|
||||
"\n",
|
||||
"$x'(T)=a_1+2a_2T+3a_3T^2+4a_4T^3+5a_5T^4=v_e$ -- (8)\n",
|
||||
"\n",
|
||||
"$x''(T)=2a_2+6a_3T+12a_4T^2+20a_5T^3=a_e$ -- (9)\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"From eq (7), (8), (9), we can calculate $a_3, a_4, a_5$ to solve the linear equations.\n",
|
||||
"\n",
|
||||
"$Ax=b$\n",
|
||||
"\n",
|
||||
"$\\begin{bmatrix} T^3 & T^4 & T^5 \\\\ 3T^2 & 4T^3 & 5T^4 \\\\ 6T & 12T^2 & 20T^3 \\end{bmatrix}\n",
|
||||
"\\begin{bmatrix} a_3\\\\ a_4\\\\ a_5\\end{bmatrix}=\\begin{bmatrix} x_e-x_s-v_sT-0.5a_sT^2\\\\ v_e-v_s-a_sT\\\\ a_e-a_s\\end{bmatrix}$\n",
|
||||
"\n",
|
||||
"We can get all unknown parameters now"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## Quintic polynomials for two dimensional robot motion (x-y)\n",
|
||||
"\n",
|
||||
"If you use two quintic polynomials along x axis and y axis, you can plan for two dimensional robot motion in x-y plane.\n",
|
||||
"\n",
|
||||
"$x(t) = a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5$ --(10)\n",
|
||||
"\n",
|
||||
"$y(t) = b_0+b_1t+b_2t^2+b_3t^3+b_4t^4+b_5t^5$ --(11)\n",
|
||||
"\n",
|
||||
"It is assumed that terminal states (start and end) are known as boundary conditions.\n",
|
||||
"\n",
|
||||
"Start position, orientation, velocity, and acceleration are $x_s, y_s, \\theta_s, v_s, a_s$ respectively.\n",
|
||||
"\n",
|
||||
"End position, orientation, velocity, and acceleration are $x_e, y_e. \\theta_e, v_e, a_e$ respectively.\n",
|
||||
"\n",
|
||||
"Each velocity and acceleration boundary condition can be calculated with each orientation.\n",
|
||||
"\n",
|
||||
"$v_{xs}=v_scos(\\theta_s), v_{ys}=v_ssin(\\theta_s)$\n",
|
||||
"\n",
|
||||
"$v_{xe}=v_ecos(\\theta_e), v_{ye}=v_esin(\\theta_e)$\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": []
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
@@ -52,25 +119,25 @@
|
||||
"language_info": {
|
||||
"codemirror_mode": {
|
||||
"name": "ipython",
|
||||
"version": 2
|
||||
"version": 3
|
||||
},
|
||||
"file_extension": ".py",
|
||||
"mimetype": "text/x-python",
|
||||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython2",
|
||||
"version": "2.7.6"
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.7.5"
|
||||
},
|
||||
"pycharm": {
|
||||
"stem_cell": {
|
||||
"cell_type": "raw",
|
||||
"source": [],
|
||||
"metadata": {
|
||||
"collapsed": false
|
||||
}
|
||||
},
|
||||
"source": []
|
||||
}
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
"nbformat_minor": 0
|
||||
}
|
||||
"nbformat_minor": 1
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user