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

MT3006 - Robótica 2

¿Qué estamos buscando?

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

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

estimación de pose

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

predicción + correcció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

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

Predicción de pose empleando dead-reckoning

Dead-reckoning

emplea la combinación de una posición (o pose) previamente determinada junto a un estimado de la velocidad (o aceleración) del robot para estimar la posición (o pose) actual del mismo

\(\equiv\) integración de recorrido

Dead-reckoning

consideraremos dos esquemas para efectuarlo, uno empleando odometría y otro típico en fusión de sensores inerciales

\(\equiv\) integración de recorrido

Odometría para robots con ruedas

Odometría para robots con ruedas

encoder rotacional magnético

D_L=r\theta_L=2\pi r \dfrac{\Delta\mathrm{tick}_L}{N}
D_R=r\theta_R=2\pi r \dfrac{\Delta\mathrm{tick}_R}{N}
D_c=\dfrac{D_R+D_L}{2}\triangleq \delta_\rho
\delta_\theta=\dfrac{D_R-D_L}{\ell}

\(r: \quad \) radio de la rueda

\(\ell: \quad\) distancia entre ruedas

\(\Delta\mathrm{tick}_R: \quad\) conteo diferencial de "muescas"

\(N: \quad \) número de "muescas" por revolución (debe tomar en consideración la caja reductora)

\(\delta_\rho: \quad\) distancia lineal recorrida

\(\delta_\theta: \quad\) distancia angular recorrida

{^N}\boldsymbol{\xi}=\begin{bmatrix} x \\ y \\ \theta \end{bmatrix} \triangleq \begin{bmatrix} \xi_1 \\ \xi_2 \\ \xi_3 \end{bmatrix}
{^N}\boldsymbol{\xi}_{k+1}={^N}\boldsymbol{\xi}_k+\begin{bmatrix} \delta_\rho\cos(\xi_{3,k}) \\ \delta_\rho\sin(\xi_{3,k}) \\ \delta_\theta \end{bmatrix}
{^N}\boldsymbol{\xi}=\begin{bmatrix} x \\ y \\ \theta \end{bmatrix} \triangleq \begin{bmatrix} \xi_1 \\ \xi_2 \\ \xi_3 \end{bmatrix}
{^N}\boldsymbol{\xi}_{k+1}={^N}\boldsymbol{\xi}_k+\begin{bmatrix} \delta_\rho\cos(\xi_{3,k}) \\ \delta_\rho\sin(\xi_{3,k}) \\ \delta_\theta \end{bmatrix}

¿Problema?

NO estamos tomando en consideración el posible drift en los encoders

puede reconstruirse la distancia lineal desde las revoluciones de la rueda

las irregularidades hacen que la rueda presente más revoluciones que las que debería

puede reconstruirse la distancia lineal desde las revoluciones de la rueda

las irregularidades hacen que la rueda presente más revoluciones que las que debería

¿Solución?

Modelar estas irregularidades como ruido (de proceso)

{^N}\boldsymbol{\xi}_{k+1}=\begin{bmatrix} \xi_{1,k}+(\delta_\rho+w_{\rho,k})\cos(\xi_{3,k}) \\ \xi_{2,k}+(\delta_\rho+w_{\rho,k})\sin(\xi_{3,k}) \\ \xi_{3,k}+\delta_\theta+w_{\theta,k} \end{bmatrix}
w_{\rho,k} \sim \mathcal{N}(0, \sigma_{w_\rho}^2) \qquad w_{\theta,k} \sim \mathcal{N}(0, \sigma_{w_\theta}^2)
{^N}\boldsymbol{\xi}_{k+1}=\begin{bmatrix} \xi_{1,k}+(\delta_\rho+w_{\rho,k})\cos(\xi_{3,k}) \\ \xi_{2,k}+(\delta_\rho+w_{\rho,k})\sin(\xi_{3,k}) \\ \xi_{3,k}+\delta_\theta+w_{\theta,k} \end{bmatrix}
w_{\rho,k} \sim \mathcal{N}(0, \sigma_{w_\rho}^2) \qquad w_{\theta,k} \sim \mathcal{N}(0, \sigma_{w_\theta}^2)
\mathbf{x}_{k+1}
\mathcal{F}\left(\mathbf{x}_k, \mathbf{u}_k, \mathbf{w}_k\right)
\begin{bmatrix} x_1 \\ x_2 \\ x_3 \end{bmatrix}=\begin{bmatrix} \xi_1 \\ \xi_2 \\ \xi_3 \end{bmatrix}
\begin{bmatrix} u_1 \\ u_2 \end{bmatrix}=\begin{bmatrix} \delta_\rho \\ \delta_\theta \end{bmatrix}
\begin{bmatrix} w_1 \\ w_2 \end{bmatrix}=\begin{bmatrix} w_\rho \\ w_\theta \end{bmatrix}

(se requiere un EKF)

Otros tipos de odometría

visual (cámaras)

láser (LiDAR)

Dead-reckoning con sensores inerciales

velocidad angular

fuerza externa específica

aceleración

posición

orientación

rotar

\displaystyle \int\int
\displaystyle \int

remover gravedad

Dead-reckoning con sensores inerciales

velocidad angular

fuerza externa específica

aceleración

posición

orientación

rotar

\displaystyle \int\int
\displaystyle \int

remover gravedad

predicción

(con propiocepción)

Dead-reckoning con sensores inerciales

velocidad angular

fuerza externa específica

aceleración

posición

orientación

rotar

\displaystyle \int\int
\displaystyle \int

remover gravedad

próxima clase...

corrección

(con exterocepción)

Estimando posición con acelerómetros

la IMU más simple

= 3 acelerómetros

+ 3 giroscopios

{^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{n}_a \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_a)

Estimando posición con acelerómetros

modelo considerado para un acelerómetro de tres ejes

{^S}\dot{\mathbf{b}}_a={^S}\mathbf{b}_{ba} \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_a)

ruido de medición

bias o deriva

{^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{n}_a \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_a)

Estimando posición con acelerómetros

modelo considerado para un acelerómetro de tres ejes

{^S}\dot{\mathbf{b}}_a={^S}\mathbf{b}_{ba} \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_a)

ruido de medición

bias o deriva

OJO

Estimando posición con acelerómetros

{^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{n}_a \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_a)

modelo considerado para un acelerómetro de tres ejes

{^S}\dot{\mathbf{b}}_a={^S}\mathbf{b}_{ba} \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_a)

ruido de medición

bias o deriva

OJO

claramente esta aceleración NO puede sólo integrarse

 

\(\Rightarrow\) se emplea un truco para poder encontrar un "modelo de proceso" que genere las mediciones

\mathbf{p}(t)\triangleq \displaystyle\int_{t_0}^{t}\int_{t_0}^{t} \left(\mathbf{a}(\tau) d\tau\right) d\tau
\begin{bmatrix} {^N}\dot{\mathbf{p}} \\ {^N} \ddot{\mathbf{p}} \end{bmatrix}= \begin{bmatrix} \mathbf{0} & \mathbf{I} \\ \mathbf{0} & \mathbf{0} \end{bmatrix}\begin{bmatrix} {^N}\mathbf{p} \\ {^N} \dot{\mathbf{p}} \end{bmatrix} + \begin{bmatrix} \mathbf{0} \\ \mathbf{I} \end{bmatrix} {^N}\mathbf{a}
{^N}\mathbf{a}={^N}\mathbf{R}_S({^S}\mathbf{a}-{^S}\mathbf{b}_a-{^S}\mathbf{n}_a)+{^N}\mathbf{g}
\mathbf{p}(t)\triangleq \displaystyle\int_{t_0}^{t}\int_{t_0}^{t} \left(\mathbf{a}(\tau) d\tau\right) d\tau
\begin{bmatrix} {^N}\dot{\mathbf{p}} \\ {^N} \ddot{\mathbf{p}} \end{bmatrix}= \begin{bmatrix} \mathbf{0} & \mathbf{I} \\ \mathbf{0} & \mathbf{0} \end{bmatrix}\begin{bmatrix} {^N}\mathbf{p} \\ {^N} \dot{\mathbf{p}} \end{bmatrix} + \begin{bmatrix} \mathbf{0} \\ \mathbf{I} \end{bmatrix} {^N}\mathbf{a}

mediciones como entrada

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

necesita la orientación

necesita el bias \(\to\) puede estimarse

\begin{bmatrix} {^N}\dot{\mathbf{p}} \\ {^N} \ddot{\mathbf{p}} \\ {^S}\dot{\mathbf{b}}_a \end{bmatrix}= \begin{bmatrix} \mathbf{0} & \mathbf{I} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & -{^N}\mathbf{R}_S \\ \mathbf{0} & \mathbf{0} & \mathbf{0} \end{bmatrix} \begin{bmatrix} {^N}\mathbf{p} \\ {^N}\dot{\mathbf{p}} \\ {^S}\mathbf{b}_{a} \end{bmatrix} + \begin{bmatrix} \mathbf{0} & \mathbf{0} \\ {^N}\mathbf{R}_S & \mathbf{I} \\ \mathbf{0} & \mathbf{0} \end{bmatrix} \begin{bmatrix} {^S}\mathbf{a} \\ {^N}\mathbf{g} \end{bmatrix} + \begin{bmatrix} \mathbf{0} & \mathbf{0} \\ -{^N}\mathbf{R}_S & \mathbf{0} \\ \mathbf{0} & \mathbf{I} \end{bmatrix} \begin{bmatrix} {^S}\mathbf{n}_a \\ {^S}\mathbf{b}_{ba} \end{bmatrix}
\begin{bmatrix} {^N}\dot{\mathbf{p}} \\ {^N} \ddot{\mathbf{p}} \\ {^S}\dot{\mathbf{b}}_a \end{bmatrix}= \begin{bmatrix} \mathbf{0} & \mathbf{I} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & -{^N}\mathbf{R}_S \\ \mathbf{0} & \mathbf{0} & \mathbf{0} \end{bmatrix} \begin{bmatrix} {^N}\mathbf{p} \\ {^N}\dot{\mathbf{p}} \\ {^S}\mathbf{b}_{a} \end{bmatrix} + \begin{bmatrix} \mathbf{0} & \mathbf{0} \\ {^N}\mathbf{R}_S & \mathbf{I} \\ \mathbf{0} & \mathbf{0} \end{bmatrix} \begin{bmatrix} {^S}\mathbf{a} \\ {^N}\mathbf{g} \end{bmatrix} + \begin{bmatrix} \mathbf{0} & \mathbf{0} \\ -{^N}\mathbf{R}_S & \mathbf{0} \\ \mathbf{0} & \mathbf{I} \end{bmatrix} \begin{bmatrix} {^S}\mathbf{n}_a \\ {^S}\mathbf{b}_{ba} \end{bmatrix}
\mathbf{x}
\mathbf{u}
\mathbf{w}
\begin{bmatrix} {^N}\dot{\mathbf{p}} \\ {^N} \ddot{\mathbf{p}} \\ {^S}\dot{\mathbf{b}}_a \end{bmatrix}= \begin{bmatrix} \mathbf{0} & \mathbf{I} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & -{^N}\mathbf{R}_S \\ \mathbf{0} & \mathbf{0} & \mathbf{0} \end{bmatrix} \begin{bmatrix} {^N}\mathbf{p} \\ {^N}\dot{\mathbf{p}} \\ {^S}\mathbf{b}_{a} \end{bmatrix} + \begin{bmatrix} \mathbf{0} & \mathbf{0} \\ {^N}\mathbf{R}_S & \mathbf{I} \\ \mathbf{0} & \mathbf{0} \end{bmatrix} \begin{bmatrix} {^S}\mathbf{a} \\ {^N}\mathbf{g} \end{bmatrix} + \begin{bmatrix} \mathbf{0} & \mathbf{0} \\ -{^N}\mathbf{R}_S & \mathbf{0} \\ \mathbf{0} & \mathbf{I} \end{bmatrix} \begin{bmatrix} {^S}\mathbf{n}_a \\ {^S}\mathbf{b}_{ba} \end{bmatrix}

si se considera por aparte (estados | procesos distintos) la estimación de orientación, esto (discretizado) corresponde a las matrices \(\mathbf{A}[k], \mathbf{B}[k]\) y \(\mathbf{F}[k]\) de un sistema LTV, y se emplea el filtro de Kalman para la fusión de sensores

\mathbf{x}
\mathbf{u}
\mathbf{w}
\begin{bmatrix} {^N}\dot{\mathbf{p}} \\ {^N} \ddot{\mathbf{p}} \\ {^S}\dot{\mathbf{b}}_a \end{bmatrix}= \begin{bmatrix} \mathbf{0} & \mathbf{I} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & -{^N}\mathbf{R}_S \\ \mathbf{0} & \mathbf{0} & \mathbf{0} \end{bmatrix} \begin{bmatrix} {^N}\mathbf{p} \\ {^N}\dot{\mathbf{p}} \\ {^S}\mathbf{b}_{a} \end{bmatrix} + \begin{bmatrix} \mathbf{0} & \mathbf{0} \\ {^N}\mathbf{R}_S & \mathbf{I} \\ \mathbf{0} & \mathbf{0} \end{bmatrix} \begin{bmatrix} {^S}\mathbf{a} \\ {^N}\mathbf{g} \end{bmatrix} + \begin{bmatrix} \mathbf{0} & \mathbf{0} \\ -{^N}\mathbf{R}_S & \mathbf{0} \\ \mathbf{0} & \mathbf{I} \end{bmatrix} \begin{bmatrix} {^S}\mathbf{n}_a \\ {^S}\mathbf{b}_{ba} \end{bmatrix}

si se considera en conjunto la estimación de posición y orientación, entonces esto (discretizado) es no lineal y deberá emplearse un EKF con los jacobianos de la dinámica del proceso

\mathbf{x}
\mathbf{u}
\mathbf{w}
\boldsymbol{\omega}_g=\begin{bmatrix}\omega_x \\ \omega_y \\ \omega_z \end{bmatrix}= {^S}\boldsymbol{\omega}_{NS} + {^S}\mathbf{b}_g + {^S}\mathbf{n}_g
{^S}\mathbf{n}_g \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_g)

Estimando orientación con giroscopios

modelo considerado para un giroscopio de tres ejes

{^S}\dot{\mathbf{b}}_g={^S}\mathbf{b}_{bg} \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_g)

ruido de medición

bias o deriva

\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}_{g} \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}

+ discretización...

{^S}\dot{\boldsymbol{\theta}}_{NS} \triangleq {^S}\boldsymbol{\omega}_{NS}=\boldsymbol{\omega}_g-{^S}\mathbf{b}_g-{^S}\mathbf{n}_g
\mathbf{w}

*

\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}_{g} \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}

+ discretización...

{^S}\dot{\boldsymbol{\theta}}_{NS} \triangleq {^S}\boldsymbol{\omega}_{NS}=\boldsymbol{\omega}_g-{^S}\mathbf{b}_g-{^S}\mathbf{n}_g
\mathbf{w}

cuidado, no es el mejor modelo* porque asume ángulos de roll, pitch y yaw

una mejor opción es emplear cuaterniones unitarios (con rotación referenciada con respecto al marco del sensor)

\dot{\boldsymbol{\mathcal{Q}}}=\dfrac{1}{2} \boldsymbol{\mathcal{Q}} * \left\{0, {^S}\boldsymbol{\omega}_{NS} \right\} \sim \dot{\mathbf{q}}=\dfrac{1}{2}\boldsymbol{\Omega}\left({^S}\boldsymbol{\omega}_{NS}\right)\mathbf{q}
\boldsymbol{\Omega}\left(\boldsymbol{\omega}\right)= \begin{bmatrix} \mathbf{0} & -\boldsymbol{\omega}^\top \\ \boldsymbol{\omega} & -\boldsymbol{\mathcal{S}}\left(\boldsymbol{\omega}\right) \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}
\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}
\mathbf{x}=\begin{bmatrix} \mathbf{q} \\ {^S}\mathbf{b}_g \end{bmatrix}
\mathbf{u}=\boldsymbol{\omega}_g
\mathbf{w}=\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}
\mathbf{x}=\begin{bmatrix} \mathbf{q} \\ {^S}\mathbf{b}_g \end{bmatrix}
\mathbf{u}=\boldsymbol{\omega}_g
\mathbf{w}=\begin{bmatrix} {^S}\mathbf{n}_g \\ {^S}\mathbf{b}_{bg} \end{bmatrix}

+ discretización y recordando re-normalizar los cuaterniones para que sean unitarios

\mathbf{q}=\dfrac{\tilde{\mathbf{q}}}{\|\mathbf{q}\|}
\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}
\mathbf{x}=\begin{bmatrix} \mathbf{q} \\ {^S}\mathbf{b}_g \end{bmatrix}
\mathbf{u}=\boldsymbol{\omega}_g
\mathbf{w}=\begin{bmatrix} {^S}\mathbf{n}_g \\ {^S}\mathbf{b}_{bg} \end{bmatrix}

+ discretización y recordando re-normalizar los cuaterniones para que sean unitarios

\mathbf{q}=\dfrac{\tilde{\mathbf{q}}}{\|\mathbf{q}\|}

esto funciona sin problemas, pero la tendencia actual es emplear un algoritmo de optimización llamado filtro de Madgwick para estimar la orientación

https://ahrs.readthedocs.io/en/latest/filters/madgwick.html

¿Por qué no podemos quedarnos sólo con estas mediciones?

se necesita hacer corrección empleando sensores exteroceptivos

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.