github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 04:28:04 -05:00
Code Issues Packages Projects Releases Wiki Activity

update docs

Browse Source
This commit is contained in:
Atsushi Sakai
2019-01-13 10:19:01 +09:00
parent b4434e4737
commit e3c1a69f2e
25 changed files with 917 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
AerialNavigation/rocket_powered_landing/figure.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 672 KiB

37
AerialNavigation/rocket_powered_landing/rocket_powered_landing.ipynb
View File

File diff suppressed because one or more lines are too long

2
ArmNavigation/two_joint_arm_to_point_control/Planar_Two_Link_IK.ipynb
View File

@@ -418,7 +418,7 @@
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.6.6"
"version": "3.6.7"
}
},
"nbformat": 4,

34
PathTracking/cgmres_nmpc/cgmres_nmpc.ipynb
View File

@@ -148,20 +148,28 @@
"\n",
"Partial differential equations of the Hamiltonian are:\n",
"\n",
"$\\frac{\\partial H}{\\partial \\bf{x}}=\\\\ \n",
"[\\frac{\\partial H}{\\partial x}= 0,\\\\\n",
"\\frac{\\partial H}{\\partial y}= 0,\\\\\n",
"\\frac{\\partial H}{\\partial \\theta}= -\\lambda_1vsin\\theta+\\lambda_2vcos\\theta,\\\\\n",
"\\frac{\\partial H}{\\partial v}=-\\lambda_1cos\\theta+\\lambda_2sin\\theta+\\lambda_3\\frac{1}{WB}sin(u_{\\delta})]\\\\\n",
"$\n",
"$\\begin{equation*}\n",
"\\frac{\\partial H}{\\partial \\bf{x}}=\\\\ \n",
"\\begin{bmatrix}\n",
"\\frac{\\partial H}{\\partial x}= 0&\\\\\n",
"\\frac{\\partial H}{\\partial y}= 0&\\\\\n",
"\\frac{\\partial H}{\\partial \\theta}= -\\lambda_1vsin\\theta+\\lambda_2vcos\\theta&\\\\\n",
"\\frac{\\partial H}{\\partial v}=-\\lambda_1cos\\theta+\\lambda_2sin\\theta+\\lambda_3\\frac{1}{WB}sin(u_{\\delta})&\\\\\n",
"\\end{bmatrix}\n",
"\\\\\n",
"\\end{equation*}$\n",
"\n",
"\n",
"$\\frac{\\partial H}{\\partial \\bf{u}}=\\\\ \n",
"[\\frac{\\partial H}{\\partial u_a}= u_a+\\lambda_4+2\\rho_1u_a,\\\\\n",
"\\frac{\\partial H}{\\partial u_\\delta}= u_\\delta+\\lambda_3\\frac{v}{WB}cos(u_{\\delta})+2\\rho_2u_\\delta\\\\\n",
"\\frac{\\partial H}{\\partial d_a}= -\\phi_a+2\\rho_1d_a,\\\\\n",
"\\frac{\\partial H}{\\partial d_\\delta}=-\\phi_\\delta+2\\rho_2d_\\delta]\\\\\n",
"$"
"$\\begin{equation*}\n",
"\\frac{\\partial H}{\\partial \\bf{u}}=\\\\ \n",
"\\begin{bmatrix}\n",
"\\frac{\\partial H}{\\partial u_a}= u_a+\\lambda_4+2\\rho_1u_a&\\\\\n",
"\\frac{\\partial H}{\\partial u_\\delta}= u_\\delta+\\lambda_3\\frac{v}{WB}cos(u_{\\delta})+2\\rho_2u_\\delta&\\\\\n",
"\\frac{\\partial H}{\\partial d_a}= -\\phi_a+2\\rho_1d_a&\\\\\n",
"\\frac{\\partial H}{\\partial d_\\delta}=-\\phi_\\delta+2\\rho_2d_\\delta&\\\\\n",
"\\end{bmatrix}\n",
"\\\\\n",
"\\end{equation*}$"
]
},
{
@@ -192,7 +200,7 @@
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.6.6"
"version": "3.6.7"
}
},
"nbformat": 4,

2
PathTracking/model_predictive_speed_and_steer_control/Model_predictive_speed_and_steering_control.ipynb
View File

@@ -325,7 +325,7 @@
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.6.6"
"version": "3.6.7"
}
},
"nbformat": 4,

19
SLAM/FastSLAM1/FastSLAM1.ipynb
View File

@@ -37,6 +37,19 @@
"cell_type": "markdown",
"metadata": {},
"source": [
"### Simulation\n",
"\n",
"This is a feature based SLAM example using FastSLAM 1.0.\n",
"\n",
"The blue line is ground truth, the black line is dead reckoning, the red\n",
"line is the estimated trajectory with FastSLAM.\n",
"\n",
"The red points are particles of FastSLAM.\n",
"\n",
"Black points are landmarks, blue crosses are estimated landmark\n",
"positions by FastSLAM.\n",
"\n",
"\n",
"![gif](https://github.com/AtsushiSakai/PythonRobotics/raw/master/SLAM/FastSLAM1/animation.gif)"
]
},
@@ -487,7 +500,9 @@
"\n",
"The figure shows 100 particles distributed uniformly between [-0.5, 0.5] with the weights of each particle distributed according to a Gaussian funciton. \n",
"\n",
"The resampling picks $i \\in 1,...,N$ particles with probability to pick particle with index $i \\propto \\omega_i $, where $\\omega_i$ is the weight of that particle\n",
"The resampling picks \n",
"\n",
"$i \\in 1,...,N$ particles with probability to pick particle with index $i ∝ \\omega_i$, where $\\omega_i$ is the weight of that particle\n",
"\n",
"\n",
"To get the intuition of the resampling step we will look at a set of particles which are initialized with a given x location and weight. After the resampling the particles are more concetrated in the location where they had the highest weights. This is also indicated by the indices \n"
@@ -653,7 +668,7 @@
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.6.5"
"version": "3.6.7"
}
},
"nbformat": 4,

3
docs/jupyternotebook2rst.py
View File

@@ -82,7 +82,8 @@ def main():
# print(notebook_path_list)
for npath in notebook_path_list:
generate_rst(npath)
if "template" not in npath:
generate_rst(npath)
print("done!!")

552
docs/modules/FastSLAM1.rst Normal file
View File

@@ -0,0 +1,552 @@
FastSLAM1.0
-----------
.. code-block:: ipython3
from IPython.display import Image
Image(filename="animation.png",width=600)
.. image:: FastSLAM1_files/FastSLAM1_1_0.png
:width: 600px
Simulation
~~~~~~~~~~
This is a feature based SLAM example using FastSLAM 1.0.
The blue line is ground truth, the black line is dead reckoning, the red
line is the estimated trajectory with FastSLAM.
The red points are particles of FastSLAM.
Black points are landmarks, blue crosses are estimated landmark
positions by FastSLAM.
.. figure:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/SLAM/FastSLAM1/animation.gif
:alt: gif
gif
Introduction
~~~~~~~~~~~~
FastSLAM algorithm implementation is based on particle filters and
belongs to the family of probabilistic SLAM approaches. It is used with
feature-based maps (see gif above) or with occupancy grid maps.
As it is shown, the particle filter differs from EKF by representing the
robot’s estimation through a set of particles. Each single particle has
an independent belief, as it holds the pose :math:`(x, y, \theta)` and
an array of landmark locations
:math:`[(x_1, y_1), (x_2, y_2), ... (x_n, y_n)]` for n landmarks.
- The blue line is the true trajectory
- The red line is the estimated trajectory
- The red dots represent the distribution of particles
- The black line represent dead reckoning tracjectory
- The blue x is the observed and estimated landmarks
- The black x is the true landmark
I.e. Each particle maintains a deterministic pose and n-EKFs for each
landmark and update it with each measurement.
Algorithm walkthrough
~~~~~~~~~~~~~~~~~~~~~
The particles are initially drawn from a uniform distribution the
represent the initial uncertainty. At each time step we do:
- Predict the pose for each particle by using :math:`u` and the motion
model (the landmarks are not updated).
- Update the particles with observations :math:`z`, where the weights
are adjusted based on how likely the particle to have the correct
pose given the sensor measurement
- Resampling such that the particles with the largest weights survive
and the unlikely ones with the lowest weights die out.
1- Predict
~~~~~~~~~~
The following equations and code snippets we can see how the particles
distribution evolves in case we provide only the control :math:`(v,w)`,
which are the linear and angular velocity repsectively.
:math:`\begin{equation*} F= \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix} \end{equation*}`
:math:`\begin{equation*} B= \begin{bmatrix} \Delta t cos(\theta) & 0\\ \Delta t sin(\theta) & 0\\ 0 & \Delta t \end{bmatrix} \end{equation*}`
:math:`\begin{equation*} X = FX + BU \end{equation*}`
:math:`\begin{equation*} \begin{bmatrix} x_{t+1} \\ y_{t+1} \\ \theta_{t+1} \end{bmatrix}= \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} x_{t} \\ y_{t} \\ \theta_{t} \end{bmatrix}+ \begin{bmatrix} \Delta t cos(\theta) & 0\\ \Delta t sin(\theta) & 0\\ 0 & \Delta t \end{bmatrix} \begin{bmatrix} v_{t} + \sigma_v\\ w_{t} + \sigma_w\\ \end{bmatrix} \end{equation*}`
The following snippets playsback the recorded trajectory of each
particle.
To get the insight of the motion model change the value of :math:`R` and
re-run the cells again. As R is the parameters that indicates how much
we trust that the robot executed the motion commands.
It is interesting to notice also that only motion will increase the
uncertainty in the system as the particles start to spread out more. If
observations are included the uncertainty will decrease and particles
will converge to the correct estimate.
.. code-block:: ipython3
# CODE SNIPPET #
import numpy as np
import math
from copy import deepcopy
# Fast SLAM covariance
Q = np.diag([3.0, np.deg2rad(10.0)])**2
R = np.diag([1.0, np.deg2rad(20.0)])**2
# Simulation parameter
Qsim = np.diag([0.3, np.deg2rad(2.0)])**2
Rsim = np.diag([0.5, np.deg2rad(10.0)])**2
OFFSET_YAWRATE_NOISE = 0.01
DT = 0.1 # time tick [s]
SIM_TIME = 50.0 # simulation time [s]
MAX_RANGE = 20.0 # maximum observation range
M_DIST_TH = 2.0 # Threshold of Mahalanobis distance for data association.
STATE_SIZE = 3 # State size [x,y,yaw]
LM_SIZE = 2 # LM srate size [x,y]
N_PARTICLE = 100 # number of particle
NTH = N_PARTICLE / 1.5 # Number of particle for re-sampling
class Particle:
def __init__(self, N_LM):
self.w = 1.0 / N_PARTICLE
self.x = 0.0
self.y = 0.0
self.yaw = 0.0
# landmark x-y positions
self.lm = np.zeros((N_LM, LM_SIZE))
# landmark position covariance
self.lmP = np.zeros((N_LM * LM_SIZE, LM_SIZE))
def motion_model(x, u):
F = np.array([[1.0, 0, 0],
[0, 1.0, 0],
[0, 0, 1.0]])
B = np.array([[DT * math.cos(x[2, 0]), 0],
[DT * math.sin(x[2, 0]), 0],
[0.0, DT]])
x = F @ x + B @ u
x[2, 0] = pi_2_pi(x[2, 0])
return x
def predict_particles(particles, u):
for i in range(N_PARTICLE):
px = np.zeros((STATE_SIZE, 1))
px[0, 0] = particles[i].x
px[1, 0] = particles[i].y
px[2, 0] = particles[i].yaw
ud = u + (np.random.randn(1, 2) @ R).T # add noise
px = motion_model(px, ud)
particles[i].x = px[0, 0]
particles[i].y = px[1, 0]
particles[i].yaw = px[2, 0]
return particles
def pi_2_pi(angle):
return (angle + math.pi) % (2 * math.pi) - math.pi
# END OF SNIPPET
N_LM = 0
particles = [Particle(N_LM) for i in range(N_PARTICLE)]
time= 0.0
v = 1.0 # [m/s]
yawrate = 0.1 # [rad/s]
u = np.array([v, yawrate]).reshape(2, 1)
history = []
while SIM_TIME >= time:
time += DT
particles = predict_particles(particles, u)
history.append(deepcopy(particles))
.. code-block:: ipython3
# from IPython.html.widgets import *
from ipywidgets import *
import numpy as np
import matplotlib.pyplot as plt
%matplotlib inline
# playback the recorded motion of the particles
def plot_particles(t=0):
x = []
y = []
for i in range(len(history[t])):
x.append(history[t][i].x)
y.append(history[t][i].y)
plt.figtext(0.15,0.82,'t = ' + str(t))
plt.plot(x, y, '.r')
plt.axis([-20,20, -5,25])
interact(plot_particles, t=(0,len(history)-1,1));
.. parsed-literal::
interactive(children=(IntSlider(value=0, description='t', max=499), Output()), _dom_classes=('widget-interact'…
2- Update
~~~~~~~~~
For the update step it is useful to observe a single particle and the
effect of getting a new measurements on the weight of the particle.
As mentioned earlier, each particle maintains :math:`N` :math:`2x2` EKFs
to estimate the landmarks, which includes the EKF process described in
the EKF notebook. The difference is the change in the weight of the
particle according to how likely the measurement is.
The weight is updated according to the following equation:
:math:`\begin{equation*} w_i = |2\pi Q|^{\frac{-1}{2}} exp\{\frac{-1}{2}(z_t - \hat z_i)^T Q^{-1}(z_t-\hat z_i)\} \end{equation*}`
Where, :math:`w_i` is the computed weight, :math:`Q` is the measurement
covariance, :math:`z_t` is the actual measurment and :math:`\hat z_i` is
the predicted measurement of particle :math:`i`.
To experiment this, a single particle is initialized then passed an
initial measurement, which results in a relatively average weight.
However, setting the particle coordinate to a wrong value to simulate
wrong estimation will result in a very low weight. The lower the weight
the less likely that this particle will be drawn during resampling and
probably will die out.
.. code-block:: ipython3
# CODE SNIPPET #
def observation(xTrue, xd, u, RFID):
# calc true state
xTrue = motion_model(xTrue, u)
# add noise to range observation
z = np.zeros((3, 0))
for i in range(len(RFID[:, 0])):
dx = RFID[i, 0] - xTrue[0, 0]
dy = RFID[i, 1] - xTrue[1, 0]
d = math.sqrt(dx**2 + dy**2)
angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
if d <= MAX_RANGE:
dn = d + np.random.randn() * Qsim[0, 0] # add noise
anglen = angle + np.random.randn() * Qsim[1, 1] # add noise
zi = np.array([dn, pi_2_pi(anglen), i]).reshape(3, 1)
z = np.hstack((z, zi))
# add noise to input
ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1] + OFFSET_YAWRATE_NOISE
ud = np.array([ud1, ud2]).reshape(2, 1)
xd = motion_model(xd, ud)
return xTrue, z, xd, ud
def update_with_observation(particles, z):
for iz in range(len(z[0, :])):
lmid = int(z[2, iz])
for ip in range(N_PARTICLE):
# new landmark
if abs(particles[ip].lm[lmid, 0]) <= 0.01:
particles[ip] = add_new_lm(particles[ip], z[:, iz], Q)
# known landmark
else:
w = compute_weight(particles[ip], z[:, iz], Q)
particles[ip].w *= w
particles[ip] = update_landmark(particles[ip], z[:, iz], Q)
return particles
def compute_weight(particle, z, Q):
lm_id = int(z[2])
xf = np.array(particle.lm[lm_id, :]).reshape(2, 1)
Pf = np.array(particle.lmP[2 * lm_id:2 * lm_id + 2])
zp, Hv, Hf, Sf = compute_jacobians(particle, xf, Pf, Q)
dx = z[0:2].reshape(2, 1) - zp
dx[1, 0] = pi_2_pi(dx[1, 0])
try:
invS = np.linalg.inv(Sf)
except np.linalg.linalg.LinAlgError:
print("singuler")
return 1.0
num = math.exp(-0.5 * dx.T @ invS @ dx)
den = 2.0 * math.pi * math.sqrt(np.linalg.det(Sf))
w = num / den
return w
def compute_jacobians(particle, xf, Pf, Q):
dx = xf[0, 0] - particle.x
dy = xf[1, 0] - particle.y
d2 = dx**2 + dy**2
d = math.sqrt(d2)
zp = np.array(
[d, pi_2_pi(math.atan2(dy, dx) - particle.yaw)]).reshape(2, 1)
Hv = np.array([[-dx / d, -dy / d, 0.0],
[dy / d2, -dx / d2, -1.0]])
Hf = np.array([[dx / d, dy / d],
[-dy / d2, dx / d2]])
Sf = Hf @ Pf @ Hf.T + Q
return zp, Hv, Hf, Sf
def add_new_lm(particle, z, Q):
r = z[0]
b = z[1]
lm_id = int(z[2])
s = math.sin(pi_2_pi(particle.yaw + b))
c = math.cos(pi_2_pi(particle.yaw + b))
particle.lm[lm_id, 0] = particle.x + r * c
particle.lm[lm_id, 1] = particle.y + r * s
# covariance
Gz = np.array([[c, -r * s],
[s, r * c]])
particle.lmP[2 * lm_id:2 * lm_id + 2] = Gz @ Q @ Gz.T
return particle
def update_KF_with_cholesky(xf, Pf, v, Q, Hf):
PHt = Pf @ Hf.T
S = Hf @ PHt + Q
S = (S + S.T) * 0.5
SChol = np.linalg.cholesky(S).T
SCholInv = np.linalg.inv(SChol)
W1 = PHt @ SCholInv
W = W1 @ SCholInv.T
x = xf + W @ v
P = Pf - W1 @ W1.T
return x, P
def update_landmark(particle, z, Q):
lm_id = int(z[2])
xf = np.array(particle.lm[lm_id, :]).reshape(2, 1)
Pf = np.array(particle.lmP[2 * lm_id:2 * lm_id + 2, :])
zp, Hv, Hf, Sf = compute_jacobians(particle, xf, Pf, Q)
dz = z[0:2].reshape(2, 1) - zp
dz[1, 0] = pi_2_pi(dz[1, 0])
xf, Pf = update_KF_with_cholesky(xf, Pf, dz, Q, Hf)
particle.lm[lm_id, :] = xf.T
particle.lmP[2 * lm_id:2 * lm_id + 2, :] = Pf
return particle
# END OF CODE SNIPPET #
# Setting up the landmarks
RFID = np.array([[10.0, -2.0],
[15.0, 10.0]])
N_LM = RFID.shape[0]
# Initialize 1 particle
N_PARTICLE = 1
particles = [Particle(N_LM) for i in range(N_PARTICLE)]
xTrue = np.zeros((STATE_SIZE, 1))
xDR = np.zeros((STATE_SIZE, 1))
print("initial weight", particles[0].w)
xTrue, z, _, ud = observation(xTrue, xDR, u, RFID)
# Initialize landmarks
particles = update_with_observation(particles, z)
print("weight after landmark intialization", particles[0].w)
particles = update_with_observation(particles, z)
print("weight after update ", particles[0].w)
particles[0].x = -10
particles = update_with_observation(particles, z)
print("weight after wrong prediction", particles[0].w)
.. parsed-literal::
initial weight 1.0
weight after landmark intialization 1.0
weight after update 0.023098460073039763
weight after wrong prediction 7.951154575772496e-07
3- Resampling
~~~~~~~~~~~~~
In the reseampling steps a new set of particles are chosen from the old
set. This is done according to the weight of each particle.
The figure shows 100 particles distributed uniformly between [-0.5, 0.5]
with the weights of each particle distributed according to a Gaussian
funciton.
The resampling picks
:math:`i \in 1,...,N` particles with probability to pick particle with
index :math:`i ∝ \omega_i`, where :math:`\omega_i` is the weight of that
particle
To get the intuition of the resampling step we will look at a set of
particles which are initialized with a given x location and weight.
After the resampling the particles are more concetrated in the location
where they had the highest weights. This is also indicated by the
indices
.. code-block:: ipython3
# CODE SNIPPET #
def normalize_weight(particles):
sumw = sum([p.w for p in particles])
try:
for i in range(N_PARTICLE):
particles[i].w /= sumw
except ZeroDivisionError:
for i in range(N_PARTICLE):
particles[i].w = 1.0 / N_PARTICLE
return particles
return particles
def resampling(particles):
"""
low variance re-sampling
"""
particles = normalize_weight(particles)
pw = []
for i in range(N_PARTICLE):
pw.append(particles[i].w)
pw = np.array(pw)
Neff = 1.0 / (pw @ pw.T) # Effective particle number
# print(Neff)
if Neff < NTH: # resampling
wcum = np.cumsum(pw)
base = np.cumsum(pw * 0.0 + 1 / N_PARTICLE) - 1 / N_PARTICLE
resampleid = base + np.random.rand(base.shape[0]) / N_PARTICLE
inds = []
ind = 0
for ip in range(N_PARTICLE):
while ((ind < wcum.shape[0] - 1) and (resampleid[ip] > wcum[ind])):
ind += 1
inds.append(ind)
tparticles = particles[:]
for i in range(len(inds)):
particles[i].x = tparticles[inds[i]].x
particles[i].y = tparticles[inds[i]].y
particles[i].yaw = tparticles[inds[i]].yaw
particles[i].w = 1.0 / N_PARTICLE
return particles, inds
# END OF SNIPPET #
def gaussian(x, mu, sig):
return np.exp(-np.power(x - mu, 2.) / (2 * np.power(sig, 2.)))
N_PARTICLE = 100
particles = [Particle(N_LM) for i in range(N_PARTICLE)]
x_pos = []
w = []
for i in range(N_PARTICLE):
particles[i].x = np.linspace(-0.5,0.5,N_PARTICLE)[i]
x_pos.append(particles[i].x)
particles[i].w = gaussian(i, N_PARTICLE/2, N_PARTICLE/20)
w.append(particles[i].w)
# Normalize weights
sw = sum(w)
for i in range(N_PARTICLE):
w[i] /= sw
particles, new_indices = resampling(particles)
x_pos2 = []
for i in range(N_PARTICLE):
x_pos2.append(particles[i].x)
# Plot results
fig, ((ax1,ax2,ax3)) = plt.subplots(nrows=3, ncols=1)
fig.tight_layout()
ax1.plot(x_pos,np.ones((N_PARTICLE,1)), '.r', markersize=2)
ax1.set_title("Particles before resampling")
ax1.axis((-1, 1, 0, 2))
ax2.plot(w)
ax2.set_title("Weights distribution")
ax3.plot(x_pos2,np.ones((N_PARTICLE,1)), '.r')
ax3.set_title("Particles after resampling")
ax3.axis((-1, 1, 0, 2))
fig.subplots_adjust(hspace=0.8)
plt.show()
plt.figure()
plt.hist(new_indices)
plt.xlabel("Particles indices to be resampled")
plt.ylabel("# of time index is used")
plt.show()
.. image:: FastSLAM1_files/FastSLAM1_12_0.png
.. image:: FastSLAM1_files/FastSLAM1_12_1.png
References
~~~~~~~~~~
http://www.probabilistic-robotics.org/
http://ais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/slam10-fastslam.pdf

BIN
docs/modules/FastSLAM1_files/FastSLAM1_12_0.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 14 KiB

BIN
docs/modules/FastSLAM1_files/FastSLAM1_12_1.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 6.0 KiB

BIN
docs/modules/FastSLAM1_files/FastSLAM1_1_0.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 43 KiB

5
docs/modules/aerial_navigation.rst
View File

@@ -10,4 +10,9 @@ This is a 3d trajectory following simulation for a quadrotor.
|3|
rocket powered landing
-----------------------------
.. include:: rocket_powered_landing.rst
.. |3| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/AerialNavigation/drone_3d_trajectory_following/animation.gif

101
docs/modules/cgmres_nmpc.rst Normal file
View File

@@ -0,0 +1,101 @@
Nonlinear Model Predictive Control with C-GMRES
-----------------------------------------------
.. code-block:: ipython3
from IPython.display import Image
Image(filename="Figure_4.png",width=600)
.. image:: cgmres_nmpc_files/cgmres_nmpc_1_0.png
:width: 600px
.. code-block:: ipython3
Image(filename="Figure_1.png",width=600)
.. image:: cgmres_nmpc_files/cgmres_nmpc_2_0.png
:width: 600px
.. code-block:: ipython3
Image(filename="Figure_2.png",width=600)
.. image:: cgmres_nmpc_files/cgmres_nmpc_3_0.png
:width: 600px
.. code-block:: ipython3
Image(filename="Figure_3.png",width=600)
.. image:: cgmres_nmpc_files/cgmres_nmpc_4_0.png
:width: 600px
.. figure:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathTracking/cgmres_nmpc/animation.gif
:alt: gif
gif
Mathematical Formulation
~~~~~~~~~~~~~~~~~~~~~~~~
Motion model is
:math:`\dot{x}=vcos\theta`
:math:`\dot{y}=vsin\theta`
:math:`\dot{\theta}=\frac{v}{WB}sin(u_{\delta})` (tan is not good for
optimization)
:math:`\dot{v}=u_a`
Cost function is
:math:`J=\frac{1}{2}(u_a^2+u_{\delta}^2)-\phi_a d_a-\phi_\delta d_\delta`
Input constraints are
:math:`|u_a| \leq u_{a,max}`
:math:`|u_\delta| \leq u_{\delta,max}`
So, Hamiltonian　is
:math:`J=\frac{1}{2}(u_a^2+u_{\delta}^2)-\phi_a d_a-\phi_\delta d_\delta\\ +\lambda_1vcos\theta+\lambda_2vsin\theta+\lambda_3\frac{v}{WB}sin(u_{\delta})+\lambda_4u_a\\ +\rho_1(u_a^2+d_a^2+u_{a,max}^2)+\rho_2(u_\delta^2+d_\delta^2+u_{\delta,max}^2)`
Partial differential equations of the Hamiltonian are:
:math:`\begin{equation*} \frac{\partial H}{\partial \bf{x}}=\\ \begin{bmatrix} \frac{\partial H}{\partial x}= 0&\\ \frac{\partial H}{\partial y}= 0&\\ \frac{\partial H}{\partial \theta}= -\lambda_1vsin\theta+\lambda_2vcos\theta&\\ \frac{\partial H}{\partial v}=-\lambda_1cos\theta+\lambda_2sin\theta+\lambda_3\frac{1}{WB}sin(u_{\delta})&\\ \end{bmatrix} \\ \end{equation*}`
:math:`\begin{equation*} \frac{\partial H}{\partial \bf{u}}=\\ \begin{bmatrix} \frac{\partial H}{\partial u_a}= u_a+\lambda_4+2\rho_1u_a&\\ \frac{\partial H}{\partial u_\delta}= u_\delta+\lambda_3\frac{v}{WB}cos(u_{\delta})+2\rho_2u_\delta&\\ \frac{\partial H}{\partial d_a}= -\phi_a+2\rho_1d_a&\\ \frac{\partial H}{\partial d_\delta}=-\phi_\delta+2\rho_2d_\delta&\\ \end{bmatrix} \\ \end{equation*}`
Ref
~~~
- `Shunichi09/nonlinear_control: Implementing the nonlinear model
predictive control, sliding mode
control <https://github.com/Shunichi09/nonlinear_control>`__
- `非線形モデル予測制御におけるCGMRES法をpythonで実装する -
Qiita <https://qiita.com/MENDY/items/4108190a579395053924>`__

BIN
docs/modules/cgmres_nmpc_files/cgmres_nmpc_1_0.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 19 KiB

BIN
docs/modules/cgmres_nmpc_files/cgmres_nmpc_2_0.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 33 KiB

BIN
docs/modules/cgmres_nmpc_files/cgmres_nmpc_3_0.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 30 KiB

BIN
docs/modules/cgmres_nmpc_files/cgmres_nmpc_4_0.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 25 KiB

2
docs/modules/extended_kalman_filter_localization.rst
View File

@@ -102,7 +102,7 @@ Its Jacobian matrix is
:math:`\begin{equation*} J_F= \begin{bmatrix} \frac{dx}{dx}& \frac{dx}{dy} & \frac{dx}{d\phi} & \frac{dx}{dv}\\ \frac{dy}{dx}& \frac{dy}{dy} & \frac{dy}{d\phi} & \frac{dy}{dv}\\ \frac{d\phi}{dx}& \frac{d\phi}{dy} & \frac{d\phi}{d\phi} & \frac{d\phi}{dv}\\ \frac{dv}{dx}& \frac{dv}{dy} & \frac{dv}{d\phi} & \frac{dv}{dv}\\ \end{bmatrix} \end{equation*}`
:math:`\begin{equation*} 　= \begin{bmatrix} 1& 0 & -v cos(\phi)dt & sin(\phi)dt\\ 0 & 1 & v cos(\phi)dt & sin(\phi) dt\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ \end{bmatrix} \end{equation*}`
:math:`\begin{equation*} 　= \begin{bmatrix} 1& 0 & -v sin(\phi)dt & cos(\phi)dt\\ 0 & 1 & v cos(\phi)dt & sin(\phi) dt\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ \end{bmatrix} \end{equation*}`
Observation Model
~~~~~~~~~~~~~~~~~

2
docs/modules/path_planning.rst
View File

@@ -176,6 +176,8 @@ This is a path planning code with RRT\*
Black circles are obstacles, green line is a searched tree, red crosses
are start and goal positions.
.. include:: rrt_star.rst
Ref:
- `Incremental Sampling-based Algorithms for Optimal Motion

3
docs/modules/path_tracking.rst
View File

@@ -96,6 +96,9 @@ Ref:
- `notebook <https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.ipynb>`__
.. include:: cgmres_nmpc.rst
.. |2| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathTracking/move_to_pose/animation.gif
.. |3| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathTracking/pure_pursuit/animation.gif
.. |4| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathTracking/stanley_controller/animation.gif

146
docs/modules/rocket_powered_landing.rst Normal file
View File

@@ -0,0 +1,146 @@
Simulation
----------
.. code-block:: ipython3
from IPython.display import Image
Image(filename="figure.png",width=600)
.. image:: rocket_powered_landing_files/rocket_powered_landing_1_0.png
:width: 600px
.. figure:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/AerialNavigation/rocket_powered_landing/animation.gif
:alt: gif
gif
Equation generation
-------------------
.. code-block:: ipython3
import sympy as sp
import numpy as np
from IPython.display import display
sp.init_printing(use_latex='mathjax')
.. code-block:: ipython3
# parameters
# Angular moment of inertia
J_B = 1e-2 * np.diag([1., 1., 1.])
# Gravity
g_I = np.array((-1, 0., 0.))
# Fuel consumption
alpha_m = 0.01
# Vector from thrust point to CoM
r_T_B = np.array([-1e-2, 0., 0.])
def dir_cosine(q):
return np.matrix([
[1 - 2 * (q[2] ** 2 + q[3] ** 2), 2 * (q[1] * q[2] +
q[0] * q[3]), 2 * (q[1] * q[3] - q[0] * q[2])],
[2 * (q[1] * q[2] - q[0] * q[3]), 1 - 2 *
(q[1] ** 2 + q[3] ** 2), 2 * (q[2] * q[3] + q[0] * q[1])],
[2 * (q[1] * q[3] + q[0] * q[2]), 2 * (q[2] * q[3] -
q[0] * q[1]), 1 - 2 * (q[1] ** 2 + q[2] ** 2)]
])
def omega(w):
return np.matrix([
[0, -w[0], -w[1], -w[2]],
[w[0], 0, w[2], -w[1]],
[w[1], -w[2], 0, w[0]],
[w[2], w[1], -w[0], 0],
])
def skew(v):
return np.matrix([
[0, -v[2], v[1]],
[v[2], 0, -v[0]],
[-v[1], v[0], 0]
])
.. code-block:: ipython3
f = sp.zeros(14, 1)
x = sp.Matrix(sp.symbols(
'm rx ry rz vx vy vz q0 q1 q2 q3 wx wy wz', real=True))
u = sp.Matrix(sp.symbols('ux uy uz', real=True))
g_I = sp.Matrix(g_I)
r_T_B = sp.Matrix(r_T_B)
J_B = sp.Matrix(J_B)
C_B_I = dir_cosine(x[7:11, 0])
C_I_B = C_B_I.transpose()
f[0, 0] = - alpha_m * u.norm()
f[1:4, 0] = x[4:7, 0]
f[4:7, 0] = 1 / x[0, 0] * C_I_B * u + g_I
f[7:11, 0] = 1 / 2 * omega(x[11:14, 0]) * x[7: 11, 0]
f[11:14, 0] = J_B ** -1 * \
(skew(r_T_B) * u - skew(x[11:14, 0]) * J_B * x[11:14, 0])
.. code-block:: ipython3
display(sp.simplify(f)) # f
.. math::
\left[\begin{matrix}- 0.01 \sqrt{ux^{2} + uy^{2} + uz^{2}}\\vx\\vy\\vz\\\frac{- 1.0 m - ux \left(2 q_{2}^{2} + 2 q_{3}^{2} - 1\right) - 2 uy \left(q_{0} q_{3} - q_{1} q_{2}\right) + 2 uz \left(q_{0} q_{2} + q_{1} q_{3}\right)}{m}\\\frac{2 ux \left(q_{0} q_{3} + q_{1} q_{2}\right) - uy \left(2 q_{1}^{2} + 2 q_{3}^{2} - 1\right) - 2 uz \left(q_{0} q_{1} - q_{2} q_{3}\right)}{m}\\\frac{- 2 ux \left(q_{0} q_{2} - q_{1} q_{3}\right) + 2 uy \left(q_{0} q_{1} + q_{2} q_{3}\right) - uz \left(2 q_{1}^{2} + 2 q_{2}^{2} - 1\right)}{m}\\- 0.5 q_{1} wx - 0.5 q_{2} wy - 0.5 q_{3} wz\\0.5 q_{0} wx + 0.5 q_{2} wz - 0.5 q_{3} wy\\0.5 q_{0} wy - 0.5 q_{1} wz + 0.5 q_{3} wx\\0.5 q_{0} wz + 0.5 q_{1} wy - 0.5 q_{2} wx\\0\\1.0 uz\\- 1.0 uy\end{matrix}\right]
.. code-block:: ipython3
sp.simplify(f.jacobian(x)) # A
.. math::
\left[\begin{array}{cccccccccccccc}0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\frac{ux \left(2 q_{2}^{2} + 2 q_{3}^{2} - 1\right) + 2 uy \left(q_{0} q_{3} - q_{1} q_{2}\right) - 2 uz \left(q_{0} q_{2} + q_{1} q_{3}\right)}{m^{2}} & 0 & 0 & 0 & 0 & 0 & 0 & \frac{2 \left(q_{2} uz - q_{3} uy\right)}{m} & \frac{2 \left(q_{2} uy + q_{3} uz\right)}{m} & \frac{2 \left(q_{0} uz + q_{1} uy - 2 q_{2} ux\right)}{m} & \frac{2 \left(- q_{0} uy + q_{1} uz - 2 q_{3} ux\right)}{m} & 0 & 0 & 0\\\frac{- 2 ux \left(q_{0} q_{3} + q_{1} q_{2}\right) + uy \left(2 q_{1}^{2} + 2 q_{3}^{2} - 1\right) + 2 uz \left(q_{0} q_{1} - q_{2} q_{3}\right)}{m^{2}} & 0 & 0 & 0 & 0 & 0 & 0 & \frac{2 \left(- q_{1} uz + q_{3} ux\right)}{m} & \frac{2 \left(- q_{0} uz - 2 q_{1} uy + q_{2} ux\right)}{m} & \frac{2 \left(q_{1} ux + q_{3} uz\right)}{m} & \frac{2 \left(q_{0} ux + q_{2} uz - 2 q_{3} uy\right)}{m} & 0 & 0 & 0\\\frac{2 ux \left(q_{0} q_{2} - q_{1} q_{3}\right) - 2 uy \left(q_{0} q_{1} + q_{2} q_{3}\right) + uz \left(2 q_{1}^{2} + 2 q_{2}^{2} - 1\right)}{m^{2}} & 0 & 0 & 0 & 0 & 0 & 0 & \frac{2 \left(q_{1} uy - q_{2} ux\right)}{m} & \frac{2 \left(q_{0} uy - 2 q_{1} uz + q_{3} ux\right)}{m} & \frac{2 \left(- q_{0} ux - 2 q_{2} uz + q_{3} uy\right)}{m} & \frac{2 \left(q_{1} ux + q_{2} uy\right)}{m} & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & - 0.5 wx & - 0.5 wy & - 0.5 wz & - 0.5 q_{1} & - 0.5 q_{2} & - 0.5 q_{3}\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0.5 wx & 0 & 0.5 wz & - 0.5 wy & 0.5 q_{0} & - 0.5 q_{3} & 0.5 q_{2}\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0.5 wy & - 0.5 wz & 0 & 0.5 wx & 0.5 q_{3} & 0.5 q_{0} & - 0.5 q_{1}\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0.5 wz & 0.5 wy & - 0.5 wx & 0 & - 0.5 q_{2} & 0.5 q_{1} & 0.5 q_{0}\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\end{array}\right]
.. code-block:: ipython3
sp.simplify(f.jacobian(u)) # B
.. math::
\left[\begin{matrix}- \frac{0.01 ux}{\sqrt{ux^{2} + uy^{2} + uz^{2}}} & - \frac{0.01 uy}{\sqrt{ux^{2} + uy^{2} + uz^{2}}} & - \frac{0.01 uz}{\sqrt{ux^{2} + uy^{2} + uz^{2}}}\\0 & 0 & 0\\0 & 0 & 0\\0 & 0 & 0\\\frac{- 2 q_{2}^{2} - 2 q_{3}^{2} + 1}{m} & \frac{2 \left(- q_{0} q_{3} + q_{1} q_{2}\right)}{m} & \frac{2 \left(q_{0} q_{2} + q_{1} q_{3}\right)}{m}\\\frac{2 \left(q_{0} q_{3} + q_{1} q_{2}\right)}{m} & \frac{- 2 q_{1}^{2} - 2 q_{3}^{2} + 1}{m} & \frac{2 \left(- q_{0} q_{1} + q_{2} q_{3}\right)}{m}\\\frac{2 \left(- q_{0} q_{2} + q_{1} q_{3}\right)}{m} & \frac{2 \left(q_{0} q_{1} + q_{2} q_{3}\right)}{m} & \frac{- 2 q_{1}^{2} - 2 q_{2}^{2} + 1}{m}\\0 & 0 & 0\\0 & 0 & 0\\0 & 0 & 0\\0 & 0 & 0\\0 & 0 & 0\\0 & 0 & 1.0\\0 & -1.0 & 0\end{matrix}\right]
Ref
~~~
- Python implementation of ‘Successive Convexification for 6-DoF Mars
Rocket Powered Landing with Free-Final-Time’ paper by Michael Szmuk
and Behçet Açıkmeşe.
- inspired by EmbersArc/SuccessiveConvexificationFreeFinalTime:
Implementation of “Successive Convexification for 6-DoF Mars Rocket
Powered Landing with Free-Final-Time”
https://github.com/EmbersArc/SuccessiveConvexificationFreeFinalTime

BIN
docs/modules/rocket_powered_landing_files/rocket_powered_landing_1_0.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 672 KiB

35
docs/modules/rrt_star.rst Normal file
View File

@@ -0,0 +1,35 @@
Title
-----
.. code-block:: ipython3
from IPython.display import Image
Image(filename="Figure_1.png",width=600)
.. image:: rrt_star_files/rrt_star_1_0.png
:width: 600px
.. figure:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/Localization/extended_kalman_filter/animation.gif
:alt: gif
gif
Section1
~~~~~~~~
Section2
~~~~~~~~
:math:`\begin{equation*} F= \begin{bmatrix} 1 & 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 \\ \end{bmatrix} \end{equation*}`
Ref
~~~
- `Sampling-based Algorithms for Optimal Motion
Planning <https://arxiv.org/pdf/1105.1186.pdf>`__

BIN
docs/modules/rrt_star_files/rrt_star_1_0.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 117 KiB

13
docs/modules/slam.rst
View File

@@ -37,18 +37,7 @@ Ref:
- `PROBABILISTIC ROBOTICS`_
FastSLAM 1.0
------------
This is a feature based SLAM example using FastSLAM 1.0.
The blue line is ground truth, the black line is dead reckoning, the red
line is the estimated trajectory with FastSLAM.
The red points are particles of FastSLAM.
Black points are landmarks, blue crosses are estimated landmark
positions by FastSLAM.
.. include:: FastSLAM1.rst
|5|