MT3006 - Robótica 2

Localización empleando odometría, sensores inerciales y sistemas de posicionamiento II

Completando el problema

plantear el problema de localización bajo la perspectiva del filtro de Kalman

predicción + corrección

propiocepción

exterocepción

estimación de pose

plantear el problema de localización bajo la perspectiva del filtro de Kalman

predicción + corrección

propiocepción

exterocepción

estimación de pose

Corrección de pose empleando mediciones exteroceptivas

GNSS en ambientes abiertos

modelo de medición para un receptor GPS (GNSS)

\mathbf{y}_k=\mathbf{C}\mathbf{x}_k+\mathbf{v}_k=\mathbf{I}{^I}\mathbf{p}_k+\mathbf{v}_k
\mathbf{y}_k \equiv {^N}\mathbf{p}_k+\mathbf{v}_k
\mathbf{Q}_v= \begin{bmatrix} \sigma_x^2 & 0 & 0 \\ 0 & \sigma_y^2 & 0 \\ 0 & 0 & \sigma_z^2 \end{bmatrix}

modelo de medición para un receptor GPS (GNSS)

\mathbf{y}_k=\mathbf{C}\mathbf{x}_k+\mathbf{v}_k=\mathbf{I}{^I}\mathbf{p}_k+\mathbf{v}_k
\sim \mathcal{N}\left(\mathbf{0},\mathbf{Q}_v\right)
\mathbf{y}_k \equiv {^N}\mathbf{p}_k+\mathbf{v}_k
\mathbf{Q}_v= \begin{bmatrix} \sigma_x^2 & 0 & 0 \\ 0 & \sigma_y^2 & 0 \\ 0 & 0 & \sigma_z^2 \end{bmatrix}

modelo de medición para un receptor GPS (GNSS)

por lo general "grandes", pero el sensor presenta bias "perfecto"

\mathbf{y}_k=\mathbf{C}\mathbf{x}_k+\mathbf{v}_k=\mathbf{I}{^I}\mathbf{p}_k+\mathbf{v}_k
\sim \mathcal{N}\left(\mathbf{0},\mathbf{Q}_v\right)
\mathbf{y}_k \equiv {^N}\mathbf{p}_k+\mathbf{v}_k

Ejemplo: módulo FGPMMOPA6B

Ejemplo: módulo FGPMMOPA6B

¿GPS en espacios cerrados?

sistemas de captura de movimiento

óptico (ej. OptiTrack)

UWB (ej. DWM1000)

¿GPS en espacios cerrados?

mismo modelo de medición que para el GPS

óptico (ej. OptiTrack)

UWB (ej. DWM1000)

con varianzas mucho menores y también con capacidad de medir orientación completa (i.e. el mejor sensor posible*)

con varianzas menores al GPS pero mayores a sistemas ópticos y más accesibles*

Orientación empleando IMUs

se hizo la predicción de orientación con los giroscopios

\begin{bmatrix} {^S}\dot{\boldsymbol{\theta}}_{NS} \\ {^S}\dot{\mathbf{b}}_g \end{bmatrix}= \begin{bmatrix} \mathbf{0} & -\mathbf{I} \\ \mathbf{0} & \mathbf{0} \end{bmatrix} \begin{bmatrix} {^S}\boldsymbol{\theta}_{NS} \\ {^S}\mathbf{b}_{bg} \end{bmatrix} + \begin{bmatrix} \mathbf{I} \\ \mathbf{0} \end{bmatrix} \boldsymbol{\omega}_g + \begin{bmatrix} -{^S}\mathbf{n}_g \\ {^S}\mathbf{b}_{bg} \end{bmatrix}
\begin{bmatrix} \dot{\mathbf{q}} \\ {^S}\dot{\mathbf{b}}_g \end{bmatrix}= \begin{bmatrix} \dfrac{1}{2}\boldsymbol{\Omega}\left( \boldsymbol{\omega}_g-{^S}\mathbf{b}_g-{^S}\mathbf{n}_g \right)\mathbf{q} \\ {^S}\mathbf{b}_{bg} \end{bmatrix}

Orientación empleando IMUs

se hizo la predicción de orientación con los giroscopios

\begin{bmatrix} {^S}\dot{\boldsymbol{\theta}}_{NS} \\ {^S}\dot{\mathbf{b}}_g \end{bmatrix}= \begin{bmatrix} \mathbf{0} & -\mathbf{I} \\ \mathbf{0} & \mathbf{0} \end{bmatrix} \begin{bmatrix} {^S}\boldsymbol{\theta}_{NS} \\ {^S}\mathbf{b}_{bg} \end{bmatrix} + \begin{bmatrix} \mathbf{I} \\ \mathbf{0} \end{bmatrix} \boldsymbol{\omega}_g + \begin{bmatrix} -{^S}\mathbf{n}_g \\ {^S}\mathbf{b}_{bg} \end{bmatrix}
\begin{bmatrix} \dot{\mathbf{q}} \\ {^S}\dot{\mathbf{b}}_g \end{bmatrix}= \begin{bmatrix} \dfrac{1}{2}\boldsymbol{\Omega}\left( \boldsymbol{\omega}_g-{^S}\mathbf{b}_g-{^S}\mathbf{n}_g \right)\mathbf{q} \\ {^S}\mathbf{b}_{bg} \end{bmatrix}

es una cantidad inercial, por lo que pueden obtenerse mediciones exteroceptivas

{^S}\mathbf{a}={^S}\mathbf{R}_N\left({^N}\mathbf{a}-{^N}\mathbf{g}\right)+{^S}\mathbf{b}_a+{^S}\mathbf{n}_a

si se asume que la gravedad predomina sobre la aceleración lineal* del cuerpo (sensor)

{^S}\mathbf{a}={^S}\mathbf{R}_N\left({^N}\mathbf{a}-{^N}\mathbf{g}\right)+{^S}\mathbf{b}_a+{^S}\mathbf{n}_a
{^S}\mathbf{a}=-{^S}\mathbf{R}_N {^N}\mathbf{g}+{^S}\mathbf{b}_a+{^S}\mathbf{e}_a

si se asume que la gravedad predomina sobre la aceleración lineal* del cuerpo (sensor)

{^S}\mathbf{a}={^S}\mathbf{R}_N\left({^N}\mathbf{a}-{^N}\mathbf{g}\right)+{^S}\mathbf{b}_a+{^S}\mathbf{n}_a

el nuevo "ruido" debe considerar también la incertidumbre del modelo

{^S}\mathbf{a}=-{^S}\mathbf{R}_N {^N}\mathbf{g}+{^S}\mathbf{b}_a+{^S}\mathbf{e}_a
{^S}\mathbf{a}=-{^S}\mathbf{R}_N {^N}\mathbf{g}+{^S}\mathbf{b}_a+{^S}\mathbf{e}_a
{^S}\mathbf{R}_N^\top={^N}\mathbf{R}_S=\mathbf{R}_z(\psi)\mathbf{R}_y(\theta)\mathbf{R}_x(\phi)

IMPORTANTE: este "sensor" tiene una limitante

{^N}\mathbf{g}=\begin{bmatrix} 0 \\ 0 \\ g \end{bmatrix}
{^S}\mathbf{a}=-\begin{bmatrix} -g\sin(\theta) \\ g\cos(\theta)\sin(\phi) \\ g\cos(\phi)\cos(\theta) \end{bmatrix}+{^S}\mathbf{b}_a+{^S}\mathbf{e}_a
{^S}\mathbf{a}=-{^S}\mathbf{R}_N {^N}\mathbf{g}+{^S}\mathbf{b}_a+{^S}\mathbf{e}_a
{^S}\mathbf{R}_N^\top={^N}\mathbf{R}_S=\mathbf{R}_z(\psi)\mathbf{R}_y(\theta)\mathbf{R}_x(\phi)

IMPORTANTE: este "sensor" tiene una limitante

{^N}\mathbf{g}=\begin{bmatrix} 0 \\ 0 \\ g \end{bmatrix}
{^S}\mathbf{a}=-\begin{bmatrix} -g\sin(\theta) \\ g\cos(\theta)\sin(\phi) \\ g\cos(\phi)\cos(\theta) \end{bmatrix}+{^S}\mathbf{b}_a+{^S}\mathbf{e}_a

no aparece el yaw \(\psi\)

{^S}\mathbf{a}=-{^S}\mathbf{R}_N {^N}\mathbf{g}+{^S}\mathbf{b}_a+{^S}\mathbf{e}_a
{^S}\mathbf{R}_N^\top={^N}\mathbf{R}_S=\mathbf{R}_z(\psi)\mathbf{R}_y(\theta)\mathbf{R}_x(\phi)

IMPORTANTE: este "sensor" tiene una limitante

{^N}\mathbf{g}=\begin{bmatrix} 0 \\ 0 \\ g \end{bmatrix}
{^S}\mathbf{a}=-\begin{bmatrix} -g\sin(\theta) \\ g\cos(\theta)\sin(\phi) \\ g\cos(\phi)\cos(\theta) \end{bmatrix}+{^S}\mathbf{b}_a+{^S}\mathbf{e}_a

no aparece el yaw \(\psi\)

por ende, para recuperar la orientación (cuaternión) completa se requiere del apoyo de algún sensor adicional (típicamente un magnetómetro)

{^S}\mathbf{a}=-{^S}\mathbf{R}_N {^N}\mathbf{g}+{^S}\mathbf{b}_a+{^S}\mathbf{e}_a
{^S}\mathbf{R}_N^\top={^N}\mathbf{R}_S=\mathbf{R}_z(\psi)\mathbf{R}_y(\theta)\mathbf{R}_x(\phi)

IMPORTANTE: este "sensor" tiene una limitante

{^N}\mathbf{g}=\begin{bmatrix} 0 \\ 0 \\ g \end{bmatrix}
{^S}\mathbf{a}=-\begin{bmatrix} -g\sin(\theta) \\ g\cos(\theta)\sin(\phi) \\ g\cos(\phi)\cos(\theta) \end{bmatrix}+{^S}\mathbf{b}_a+{^S}\mathbf{e}_a

no aparece el yaw \(\psi\)

por ende, para recuperar la orientación (cuaternión) completa se requiere del apoyo de algún sensor adicional (típicamente un magnetómetro)

*puede obtenerse, sin embargo, un modelo para el acelerómetro que sólo considere los ángulos que puede estimar

\phi=\arctan\left(\dfrac{{^S}a_y}{{^S}a_z}\right)
\theta=\arctan\left(\dfrac{-{^S}a_x}{\sqrt{{^S}a_y^2+{^S}a_z^2}}\right)
\mathbf{y}_k= \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \end{bmatrix} \mathbf{x}_k + \begin{bmatrix} v_{\phi, k} \\ v_{\theta, k} \end{bmatrix}
v_{\phi,k} \sim \mathcal{N}(0,\sigma^2_\phi)
v_{\theta,k} \sim \mathcal{N}\left(0,\sigma^2_\theta\right)

estas son nuevas varianzas que consideran la incertidumbre de los ángulos como tal

campo magnético de la Tierra

vector de campo magnético en el marco de navegación

Magnetómetros para medir \(\psi\)

si se asume que el sensor no se mueve sobre distancias significativas en comparación al tamaño de la Tierra, el campo magnético local puede asumirse constante

{^N}\mathbf{m}=\begin{bmatrix} \cos\delta \\ 0 \\ \sin\delta \end{bmatrix}=\begin{bmatrix} m_N \\ 0 \\ m_D \end{bmatrix}

si se asume que el sensor no se mueve sobre distancias significativas en comparación al tamaño de la Tierra, el campo magnético local puede asumirse constante

{^N}\mathbf{m}=\begin{bmatrix} \cos\delta \\ 0 \\ \sin\delta \end{bmatrix}=\begin{bmatrix} m_N \\ 0 \\ m_D \end{bmatrix}

inclinación magnética o dip (taza entre las componentes del campo), depende de la posición en la Tierra

{^S}\mathbf{m}={^S}\mathbf{R}_N {^N}\mathbf{m}+{^S}\mathbf{e}_m
\boldsymbol{\mathcal{H}}(\phi_k,\theta_k,\psi_k)

e igualmente el ruido de nuevo toma en consideración la incertidumbre del modelo

{^S}\mathbf{m}={^S}\mathbf{R}_N {^N}\mathbf{m}+{^S}\mathbf{e}_m
\boldsymbol{\mathcal{H}}(\phi_k,\theta_k,\psi_k)

e igualmente el ruido de nuevo toma en consideración la incertidumbre del modelo

este modelo el yaw se considera con respecto al Norte magnético, de necesitar el Norte verdadero debe considerarse también la declinación magnética

\psi_\mathrm{mag}=\arctan\left(\dfrac{{^S}m_y}{{^S}m_x}\right)
\psi_\mathrm{real}=\psi_\mathrm{mag}-\Delta\left({^I}\mathbf{p}\right)

ángulo de declinación magnética (depende de la posición \({^I}\mathbf{p}\) del sensor en el marco de la Tierra)

y_k=\psi_k+v_{\psi,k}
v_{\psi,k} \sim \mathcal{N}\left(0,\sigma^2_\psi\right)

Un último detalle implícito

\{I\}
\{N\}
x \ [\mathrm{North}]
y \ [\mathrm{East}]
z \ [\mathrm{Down}]

Poniéndolo todo junto

IMU + GPS:

Poniéndolo todo junto

IMU + GPS:

Odometría + GPS + brújula:

>> mt3006_clase13_ekfloc.m

Un preámbulo a mapeo:

corrección de pose empleando un mapa conocido

mapa con landmarks conocidos

sensor de posición \(\kappa_i\) + sensor de bearing \(\beta_i\)

"falda" de sensado

landmarks numerados

Referencias

  1. MT3006 - Localización y mapeo en robótica móvil.pdf
  2. Kok et. al, Using Inertial Sensors for Position and Orientation Estimation.
  3. https://ahrs.readthedocs.io/en/latest/filters.html.