Ziqi Lu and Tao Pang
FrontEnd
A 2D robot with two translational DOFs: \(X_B^W = [x, y]\)
Noisy measurements of landmarks’ distance \(d\) and bearing \(b\).
BackEnd
Landmark-based SLAM solved with Levenberg-Marquardt.
Active SLAM with horizon \(L\)
Computes robot control \[u^*_{k:k+L-1} = \text{argmin} J_k(u_{k:k+L-1}),\]
where \(J_k(u_{k:k+L-1})\) accounts for
robot pose uncertainty (sum of covariance trace),
Distance to current goal \(\|X^W_{B_{k + L}} - X^W_{B_{\text{goal}}}\|^2\) + ...
: robot goals.
: landmark ground truths.
: landmark beliefs.
: current robot position (ground truth).
: robot sensing range.
Landmark measurements.
:Robot pose beliefs. \(k\): current time step.
:landmark beliefs.
Additional details:
: information matrix of current robot pose and landmark beliefs.
Control effort
Pose uncertainty
Distance to goal
Using existing libraries are nice, but...
We need \(u^*_{k:k+L-1} = \text{argmin} J_k(u_{k:k+L-1})\), where
: robot goals.
: landmark ground truths.
: landmark beliefs.
: current robot position (ground truth).
: robot sensing range.
: robot covariance ellipse.
: robot goals.
: landmark ground truths.
: landmark beliefs.
: current robot position (ground truth).
: robot sensing range.
: robot covariance ellipse.
: robot goals.
: landmark ground truths.
: landmark beliefs.
: current robot position (ground truth).
: robot sensing range.
: robot trajectory belief.
: robot trajectory ground truth.
Small \(\alpha\): reaching goal.
Large \(\alpha\): Reducing uncertainty.
When \(\alpha\) is small, the robot mostly follows the goals.
Small \(\alpha\): reaching goal.
Large \(\alpha\): reducing uncertainty.
time
As alpha grows, the robot is driven away from goals and towards landmarks.
Small \(\alpha\): reaching goal.
Large \(\alpha\): reducing uncertainty.
time
A loop closure is achieved, reducing the variance of the entire trajectory.
The robot reaches its goal with much smaller variance.
Small \(\alpha\): reaching goal.
Large \(\alpha\): reducing uncertainty.
Without active SLAM.
Control effort
Pose uncertainty
Distance to goal
Our objective function \(J_k(u_{k:k+L-1})\) shares the same form as \(f(x)\).
Numerical differentiation
import numpy as np
def calc_derivative_numerical(x, f):
# derivative of f
df = np.zeros_like(x)
n = len(x)
h = 1e-3
for i in range(n):
x_plus = x.copy()
x_minus = x.copy()
x_plus[i] += h
x_minus[i] -= h
df[i] = (f(x_plus) - f(x_minus)) / 2 / h
return df
import numpy as np
from pydrake.math import AutoDiffXd as AD
def calc_derivative_forward(x, f):
n = len(x)
# initialize array of x with scalar type AD.
x_ad = np.zeros(n, dtype=object)
for i in range(n):
dx_i = np.zeros(n)
dx_i[i] = 1.
x_ad[i] = AD(x[i], dx_i)
# evaluate f with x_ad.
y_ad = f(x_ad)
return y_ad.derivatives()
Reverse-mode AutoDiff
import torch
def calc_derivative_reverse(x, f):
# initialize array of x as tensor.
x_torch = torch.tensor(x, requires_grad=True)
# evaluate f with x_torch.
y_torch = f(x_torch)
# backprop.
y_torch.backward()
return x_torch.grad
Forward-mode AutoDiff
x0 = AD(3, [1, 0])
x1 = AD(4, [0, 1])
y = x0 + x1 # AD(7, [1, 1])
value \(\in \mathbb{R}\)
partial derivatives\(\in \mathbb{R}^n\)
Computes \(f(\cdot)\) \(2n\) times.
Computes \(f(\cdot)\) \(n + 1\) times.
Computes \(f(\cdot)\) \(2\) times.
\(u_{k:k+L-1} \in \mathbb{R}^{20}\) (\(L=10\)), \(k=1\), 9 landmarks.
Control effort
Pose uncertainty
Distance to goal
Control effort
Pose uncertainty
Distance to goal
Most of the time is spent on computing \(f(\cdot)\), as expected.
Control effort
Pose uncertainty
Distance to goal
Control effort
Pose uncertainty
Distance to goal
Evaluating \(J(u)\) and computing \(\nabla_u J(u)\) takes the same amount of time, as expected.
\(u_{k:k+L-1} \in \mathbb{R}^{20}\) (\(L=10\)), \(k=9\), 28 landmarks.
\(u_{k:k+L-1} \in \mathbb{R}^{20}\) (\(L=10\)), \(k=46\), 15 landmarks.
Making it faster:
FrontEnd
A 2D robot with two translational DOFs: \(X_B^W = [x, y]\)
Noisy measurements of landmarks’ distance \(d\) and bearing \(b\).
BackEnd
Active SLAM with horizon \(L\)
Computes robot control \[u^*_{k:k+L-1} = \text{argmin} J_k(u_{k:k+L-1}),\]
where \(J_k(u_{k:k+L-1})\) accounts for
robot pose uncertainty,
Distance to current goal.
Landmark measurements.
:Robot pose beliefs.
:landmark beliefs.
Additional details:
:landmark beliefs.
: information matrix of current beliefs.
:landmark beliefs.
: robot goals.
: landmark ground truths.
: landmark beliefs.
: current robot position (ground truth).
: robot sensing range.
: robot covariance ellipse.
: robot trajectory belief.
: robot trajectory ground truth.
As alpha grows, the robot is driven away from goals and towards landmarks.
Small \(\alpha\): reaching goal.
Large \(\alpha\): reducing uncertainty.
time
A loop closure is achieved, reducing the variance of the entire trajectory.
\(\alpha = 0\)
\(\alpha=1\).