传感器融合:基于ESKF的IMU+GPS数据融合

[TOC]

Overview

System State Vector

the nominal-state $\mathbf{x}$ and the error-state $\delta \mathbf{x}$

\[\mathbf{x} = \begin{bmatrix} \mathbf{p} \\ \mathbf{v} \\ \mathbf{q} \\ \mathbf{b}_a \\ \mathbf{b}_g \end{bmatrix} \in \mathbb{R}^{16 \times 1} \quad \delta \mathbf{x} = \begin{bmatrix} \delta \mathbf{p} \\ \delta \mathbf{v} \\ \delta \boldsymbol{\theta} \\ \delta \mathbf{b}_a \\ \delta \mathbf{b}_g \end{bmatrix} \in \mathbb{R}^{15 \times 1}\]

the true-state

\[\mathbf{x}_{t} = \mathbf{x} \oplus \delta \mathbf{x}\]

ENU Coordinate

  • using ENU coordinate
  • in ENU coordinate, $\mathbf{g} = \begin{bmatrix} 0 & 0 & -9.81007 \end{bmatrix}^T$
  • extrinsic between IMU and GPS: ${}^{I} \mathbf{p}_{G p s}$

State Prediction (IMU-driven system kinematics in discrete time)

The nominal state kinematics

\[\begin{array}{l} \mathbf{p} \leftarrow \mathbf{p}+\mathbf{v} \Delta t+\frac{1}{2}\left(\mathbf{R}\left(\mathbf{a}_{m}-\mathbf{a}_{b}\right)+\mathbf{g}\right) \Delta t^{2} \\ \mathbf{v} \leftarrow \mathbf{v}+\left(\mathbf{R}\left(\mathbf{a}_{m}-\mathbf{a}_{b}\right)+\mathbf{g}\right) \Delta t \\ \mathbf{q} \leftarrow \mathbf{q} \otimes \mathbf{q}\left\{\left(\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}\right) \Delta t\right\} \\ \mathbf{a}_{b} \leftarrow \mathbf{a}_{b} \\ \boldsymbol{\omega}_{b} \leftarrow \boldsymbol{\omega}_{b} \end{array}\]

The error-state kinematics

\[\begin{aligned} \delta \mathbf{p} & \leftarrow \delta \mathbf{p}+\delta \mathbf{v} \Delta t \\ \delta \mathbf{v} & \leftarrow \delta \mathbf{v}+\left(-\mathbf{R}\left[\mathbf{a}_{m}-\mathbf{a}_{b}\right]_{\times} \delta \boldsymbol{\theta}-\mathbf{R} \delta \mathbf{a}_{b}+\delta \mathbf{g}\right) \Delta t+\mathbf{v}_{\mathbf{i}} \\ \delta \boldsymbol{\theta} & \leftarrow \mathbf{R}^{\top}\left\{\left(\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}\right) \Delta t\right\} \delta \boldsymbol{\theta}-\delta \boldsymbol{\omega}_{b} \Delta t+\boldsymbol{\theta}_{\mathbf{i}} \\ \delta \mathbf{a}_{b} & \leftarrow \delta \mathbf{a}_{b}+\mathbf{a}_{\mathbf{i}} \\ \delta \boldsymbol{\omega}_{b} & \leftarrow \delta \boldsymbol{\omega}_{b}+\boldsymbol{\omega}_{\mathbf{i}} \\ \delta \mathbf{g} & \leftarrow \delta \mathbf{g} \end{aligned}\]

where

\[\begin{array}{ll} \mathbf{V}_{\mathbf{i}}=\sigma_{\tilde{\mathbf{a}}_{n}}^{2} \Delta t^{2} \mathbf{I} & {\left[m^{2} / s^{2}\right]} \\ \Theta_{\mathbf{i}}=\sigma_{\tilde{\boldsymbol{\omega}}_{n}}^{2} \Delta t^{2} \mathbf{I} & {\left[r a d^{2}\right]} \\ \mathbf{A}_{\mathbf{i}}=\sigma_{\mathbf{a}_{w}}^{2} \Delta t \mathbf{I} & {\left[m^{2} / s^{4}\right]} \\ \boldsymbol{\Omega}_{\mathbf{i}}=\sigma_{\boldsymbol{\omega}_{w}}^{2} \Delta t \mathbf{I} & {\left[r a d^{2} / s^{2}\right]} \end{array}\]

The error-state Jacobian and perturbation matrices

The error-state system is now

\[\delta \mathbf{x} \leftarrow f\left(\mathbf{x}, \delta \mathbf{x}, \mathbf{u}_{m}, \mathbf{i}\right)=\mathbf{F}_{\mathbf{x}}\left(\mathbf{x}, \mathbf{u}_{m}\right) \cdot \delta \mathbf{x}+\mathbf{F}_{\mathbf{i}} \cdot \mathbf{i}\]

The ESKF prediction equations are written:

\[\begin{array}{l} \hat{\delta \mathbf{x}} \leftarrow \mathbf{F}_{\mathbf{x}}\left(\mathbf{x}, \mathbf{u}_{m}\right) \cdot \hat{\delta \mathbf{x}} \\ \mathbf{P} \leftarrow \mathbf{F}_{\mathbf{x}} \mathbf{P} \mathbf{F}_{\mathbf{x}}^{\top}+\mathbf{F}_{\mathbf{i}} \mathbf{Q}_{\mathbf{i}} \mathbf{F}_{\mathbf{i}}^{\top} \end{array}\] \[\delta \mathbf{x} \sim \mathcal{N}\{\hat{\delta} \mathbf{x}, \mathbf{P}\}\]

where

\[\mathbf{F}_{\mathbf{x}} = \left.\frac{\partial f}{\partial \delta \mathbf{x}}\right|_{\mathbf{x}, \mathbf{u}_{m}} = \left[\begin{array}{ccccc} \mathbf{I} & \mathbf{I} \Delta t & 0 & 0 & 0 \\ 0 & \mathbf{I} & -\mathbf{R}\left[\mathbf{a}_{m}-\mathbf{a}_{b}\right]_{\times} \Delta t & -\mathbf{R} \Delta t & 0 \\ 0 & 0 & \mathbf{R}^{\top}\left\{\left(\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}\right) \Delta t\right\} & 0 & \mathbf{I} \Delta t \\ 0 & 0 & 0 & \mathbf{I} & 0 \\ 0 & 0 & 0 & 0 & \mathbf{I} \end{array}\right]\] \[\mathbf{F}_{\mathbf{i}}= \left.\frac{\partial f}{\partial \mathbf{i}}\right|_{\mathbf{x}, \mathbf{u}_{m}}= \left[\begin{array}{llll} 0 & 0 & 0 & 0 \\ \mathbf{I} & 0 & 0 & 0 \\ 0 & \mathbf{I} & 0 & 0 \\ 0 & 0 & \mathbf{I} & 0 \\ 0 & 0 & 0 & \mathbf{I} \end{array}\right]\] \[\mathbf{Q}_{\mathbf{i}}= \left[\begin{array}{cccc} \mathbf{V}_{\mathbf{i}} & 0 & 0 & 0 \\ 0 & \boldsymbol{\Theta}_{\mathbf{i}} & 0 & 0 \\ 0 & 0 & \mathbf{A}_{\mathbf{i}} & 0 \\ 0 & 0 & 0 & \Omega_{\mathbf{i}} \end{array}\right]\]

ESKF Measurement Update (Fusing IMU with GPS data)

GPS measurement

\[\mathbf{y}=h\left(\mathbf{x}_{t}\right)+v , \quad v \sim \mathcal{N}\{0, \mathbf{V}\}\]

convert the GPS raw data which is in WGS84 coordinate to ENU by GeographicLib

\[h\left(\hat{\mathbf{x}}_{t}\right) = {}^{G} \mathbf{p}_{G p s} = {}^{G} \mathbf{p}_{I} + {}_{I}^{G} \mathbf{R} \cdot {}^{I} \mathbf{p}_{G p s}\]

the filter correction equations

\[\begin{array}{l} \mathbf{K}=\mathbf{P} \mathbf{H}^{\top}\left(\mathbf{H} \mathbf{P} \mathbf{H}^{\top}+\mathbf{V}\right)^{-1} \\ \hat{\delta} \mathbf{x} \leftarrow \mathbf{K}\left(\mathbf{y}-h\left(\hat{\mathbf{x}}_{t}\right)\right) \\ \mathbf{P} \leftarrow(\mathbf{I}-\mathbf{K} \mathbf{H}) \mathbf{P} \end{array}\]

where

\[\mathbf{H} \left.\triangleq \frac{\partial h}{\partial \delta \mathbf{x}}\right|_{\mathbf{x}} = \left[\begin{array}{lll} \mathbf{I} & \mathbf{0} & -{}_{I}^{G} \mathbf{R} \lfloor {}^{I} \mathbf{p}_{G p s} \rfloor _{\times} & \mathbf{0} & \mathbf{0} \end{array}\right]\]

the best true-state estimate

\[\hat{\mathbf{x}}_{t}=\mathbf{x} \oplus \hat{\delta} \mathbf{x}\]

Results

path on ROS rviz and plot the result path(fusion_gps.csv & fusion_state.csv) on google map:

Reference

  • Quaternion kinematics for the error-state Kalman filter
Read More

图像分析之ORB特征

[TOC]

Overview

  • Oriented FAST + Rotated BRIEF

Feature/Corner Detector

  • FAST方法 – 定位特征点坐标
  • 矩方法 – 特征点方向
  • 非极大值抑制 – 特征点响应值(分数)
  • 图像金字塔 – 特征点具有 尺度不变性

Descriptor

  • BRIEF – 特征点描述子
  • 特征点方向 – 描述子具有 旋转不变性

oFAST: FAST Keypoint Orientation

Multiscale Image Pyramid

  • level: 8
  • scale: 1.2
  • down sample: bilinear interpolation
  • produce FAST features (filtered by Harris ???) at each level in the pyramid

FAST (Features from Accelerated and Segments Test)

  • FAST-9: ≥ 9 contiguous pixels brighter than p+threshold

Procedure:

  1. Select a pixel p whose intensity is $I_p$ and Select appropriate threshold value $t$
  2. the pixel p is a corner if there exists a set of $n$ contiguous pixels in the circle (of 16 pixels) which are all brighter than $I_p+t$, or all darker than $I_p−t$
    • Rapid rejection by testing 1, 9, 5 then 13: First 1 and 9 are tested if they are too brighter or darker. If so, then checks 5 and 13
    • If p is a corner, then at least three of these must all be brighter than $I_p+t$ or darker than $I_p−t$

NMS (Non-Maximal Suppression)

非极大值抑制主要是为了避免图像上得到的“角点”过于密集,主要过程是,每个特征点会计算得到相应的响应得分,然后以当前像素点p为中心,取其邻域(如3x3 的邻域),判断当前像素p的响应值是否为该邻域内最大的,如果是,则保留,否则,则抑制。

Uniform Distribution

  DistributeOctTree()

ORB-SLAM中使用四叉树来快速筛选特征点,筛选的目的是非极大值抑制,取局部特征点邻域中FAST角点相应值最大的点,而如何搜索到这些扎堆的的点,则采用的是四叉树的分快思想,递归找到成群的点,并从中找到相应值最大的点。

Orientation by Intensity Centroid (IC)

  • Rosin defines the moments of a patch as:

    \[m_{p q}=\sum_{x, y} x^{p} y^{q} I(x, y)\]

    the first order moment of a patch $I$ (patch size = 31)

    \[\begin{aligned} m_{10} &= \sum_{x=-15}^{15} \sum_{y=-15}^{15} x I(x, y) = \sum_{y=0}^{15} {\color{blue} \sum_{x=-15}^{15} x \left[ I(x,y) - I(x, -y) \right] } \\ m_{01} &= \sum_{x=-15}^{15} \sum_{y=-15}^{15} y I(x, y) = \sum_{y=1}^{15} {\color{blue} \sum_{x=-15}^{15} y \left[ I(x,y) - I(x, -y) \right] } \end{aligned}\]
  • the intensity centroid

    \[C=\left(\frac{m_{10}}{m_{00}}, \frac{m_{01}}{m_{00}}\right)\]
  • the orientation (the vector from the corner’s center to the centroid)

    \[\theta=\operatorname{atan} 2\left(m_{01}, m_{10}\right)\]
    const int PATCH_SIZE = 31;
    const int HALF_PATCH_SIZE = 15;
    
    int m01 = 0;
    int m10 = 0;
    for(int y=-half_patch_size; y<half_patch_size; ++y){
        for(int x=-half_patch_size; x<half_patch_size; ++x){
            m01 += y * image.at<uchar>(kp.pt.y+y, kp.pt.x+x);
            m10 += x * image.at<uchar>(kp.pt.y+y, kp.pt.x+x);
        }
    }
    kp.angle = std::atan2(m01, m10)/CV_PI*180.0;
    

Gaussian Filter

  • ref: 图像分析之高斯滤波

  • start by smoothing image using a Gaussian kernel at each level in the pyramid in order to prevent the descriptor from being sensitive to high-frequency noise

  • Gaussian Kernel size: 7x7, Sigma: 2

  • 利用高斯滤波的可分离性,实现水平和垂直分开滤波,降低计算量,并进行定点化

rBRIEF: Rotation-Aware Brief

Brief of BRIEF (Binary robust independent elementary feature)

  • In brief, each keypoint is described by a feature vector which is 128–512 bits string.

  • vector dim = 256 bits (32 bytes)

  • each vector $\longleftrightarrow$ each keypoint

  • for each bit, select a pair of points in a patch $I$ which centered a corner $\mathbf{p}$ and compare their intensity

    \[\mathbf{S}= \left(\begin{array}{l} \mathbf{p}_{1}, \ldots, \mathbf{p}_{n} \\ \mathbf{q}_{1}, \dots, \mathbf{q}_{n} \end{array}\right) \in \mathbb{R}^{(2 \times 2) \times 256}\] \[\tau(\mathbf{I} ; \mathbf{p}_i, \mathbf{q}_i):= \left\{\begin{array}{ll} 1 & : \mathbf{I}(\mathbf{p}_i) < \mathbf{I}(\mathbf{q}_i) \\ 0 & : \mathbf{I}(\mathbf{p}_i) \geq \mathbf{I}(\mathbf{q}_i) \end{array}\right.\]
  • the descriptor (each bit $\longleftrightarrow$ each pair of points $(\mathbf{p}_i, \mathbf{q}_i)$):

    \[f(n) = \sum_{i=1}^n 2^{i-1} \tau(\mathbf{I} ; \mathbf{p}_i, \mathbf{q}_i), \quad (n = 256)\]

steered BRIEF

  • 为了具有旋转不变性,引入该算法,但方差很小、相关性高

    \[\mathbf{R}_{\theta} = \begin{bmatrix} \cos{\theta} & -\sin{\theta} \\ \sin{\theta} & \cos{\theta} \end{bmatrix}\] \[\mathbf{S}_{\theta}=\mathbf{R}_{\theta} \mathbf{S}= \left(\begin{array}{l} \mathbf{p}_{1}', \ldots, \mathbf{p}_{n}' \\ \mathbf{q}_{1}', \dots, \mathbf{q}_{n}' \end{array}\right)\]

rBRIEF

  • rBRIEF shows significant improvement in the variance and correlation over steered BRIEF

  • 为了把steered BRIEF方差增大,相关性降低
  • 基于统计规律,利用了贪心算法进行筛选

  • construct a lookup table of precomputed BRIEF patterns
    // construct a lookup table of precomputed BRIEF patterns
    
    // 训练好的31*31邻域256对像素点坐标
    static int ORB_pattern[256*4] = {
          8, -3,  9,  5 /*mean (0), correlation (0)*/,
          4,  2,  7,-12 /*mean (1.12461e-05), correlation (0.0437584)*/,
        -11,  9, -8,  2 /*mean (3.37382e-05), correlation (0.0617409)*/,
          7,-12, 12,-13 /*mean (5.62303e-05), correlation (0.0636977)*/
          //                       .
          //                       .
          //                       .
    }
    
  • steered BRIEF

  • for each bit of the descriptors

    \[\mathbf{p}_i' = \mathbf{R}_{\theta} (\mathbf{p}_i-\mathbf{p}) + \mathbf{p} \\ \mathbf{q}_i' = \mathbf{R}_{\theta} (\mathbf{q}_i-\mathbf{p}) + \mathbf{p}\] \[\tau(\mathbf{I} ; \mathbf{p}_i', \mathbf{q}_i'):= \left\{\begin{array}{ll} 1 & : \mathbf{I}(\mathbf{p}_i') < \mathbf{I}(\mathbf{q}_i') \\ 0 & : \mathbf{I}(\mathbf{p}_i') \geq \mathbf{I}(\mathbf{q}_i') \end{array}\right.\]
    double degRad = kp.angle/180.0*CV_PI;
    double dcos = std::cos(degRad);
    double dsin = std::sin(degRad);
    
    vector<bool> d(256, false);
    for (int i = 0; i < 256; i++) {
        d[i] = 0;
    
        int p_offset_x = ORB_pattern[4*i+0];
        int p_offset_y = ORB_pattern[4*i+1];
        int q_offset_x = ORB_pattern[4*i+2];
        int q_offset_y = ORB_pattern[4*i+3];
    
        double pu = kp.pt.x + (dcos*p_offset_x - dsin*p_offset_y);
        double pv = kp.pt.y + (dsin*p_offset_x + dcos*p_offset_y);
        double qu = kp.pt.x + (dcos*q_offset_x - dsin*q_offset_y);
        double qv = kp.pt.y + (dsin*q_offset_x + dcos*q_offset_y);
    
        int pI = image.at<uchar>((int)pv, (int)pu);
        int qI = image.at<uchar>((int)qv, (int)qu);
    
        d[i] = (pI>=qI) ? 0 : 1;
    }
    
Read More

图像分析之高斯滤波

[TOC]

高斯函数

一维高斯函数

\[f(x) = A \cdot e^{-\frac{(x-\mu)^2}{2{\sigma}^2}}\]

多维高斯函数

\[f_{X}\left(x_{1}, x_{2}, \cdots, x_{k}\right) = A \cdot \exp \left(-\frac{1}{2}(X-\mu)^{T} \Sigma^{-1}(X-\mu)\right)\]

一维正态分布 高斯概率密度函数

\[f(x) = \frac{1}{\sqrt{2\pi} \sigma} e^{-\frac{(x-\mu)^2}{2{\sigma}^2}}\]

多元正态分布 高斯概率密度函数

\[f_{X}\left(x_{1}, x_{2}, \cdots, x_{k}\right) = \frac{1}{\sqrt{(2 \pi)^{k}|\Sigma|}} \cdot \exp \left(-\frac{1}{2}(X-\mu)^{T} \Sigma^{-1}(X-\mu)\right)\]

图像高斯滤波(模糊、平滑)

图像大多数噪声均属于高斯噪声,因此高斯滤波器应用也较广泛。高斯滤波是一种线性平滑滤波,适用于消除高斯噪声,广泛应用于图像去噪。

在图像处理中,高斯滤波一般有两种实现方式,一是 用离散化窗口滑窗卷积,另一种通过 傅里叶变换。最常见的就是第一种滑窗实现,只有当离散化的窗口非常大,用滑窗计算量非常大(即使用可分离滤波器的实现)的情况下,可能会考虑基于傅里叶变化的实现方法。

二维高斯核模板或卷积核(假设x与y方向方差一致):

\[f(x,y) = \frac{1}{2\pi{\sigma}^2} \cdot \exp \left( -\frac{(x-\frac{m}{2})^2 + (y-\frac{n}{2})^2}{2{\sigma}^2} \right) \quad s.t. \quad x \in [0, m), \quad y \in [0, n)\]
double *generate_gaussian_template(unsigned int m, double sigma = 0.84089642) {
    unsigned int n = m;
    double *gaussian_template = new double[m * n];
    double sum = 0.0;
    for (int y = 0; y < m; y++) {
        for (int x = 0; x < n; x++) {
            double r2 = std::pow((double) x - m / 2, 2) + std::pow((double) y - n / 2, 2);
            double sigma2Inv = 1.f / (2 * std::pow(sigma, 2));
            double exp = std::exp(-r2 * sigma2Inv);
            sum += *(gaussian_template + y * m + x) = sigma2Inv / M_PI * exp;
        }
    }
    double sumInv = 0.0;
    if (sum > 1e-6)
        sumInv = 1.0 / sum;
    for (int y = 0; y < m; y++)
        for (int x = 0; x < n; x++)
            *(gaussian_template + y * m + x) *= sumInv;
    return gaussian_template;
}

二维高斯核模板或卷积核(以模板中心为原点):

\[G(u, v) = \frac{1}{2 \pi \sigma^{2}} e^{-\frac{u^{2}+v^{2}}{2 \sigma^{2}}}, \quad s.t. \quad u \in [-w, w], \quad v \in [-w, w]\]

归一化:

\[G_n(u, v) = \frac{1}{s} \cdot e^{-\frac{u^{2}+v^{2}}{2 \sigma^{2}}}, \quad s = \sum_{u=-w}^{w} \sum_{v=-w}^{w} e^{-\frac{u^{2}+v^{2}}{2 \sigma^{2}}}\]

图像高斯滤波:

\[\quad I^{\prime}(i,j) = \sum_{u=-w}^{w} \sum_{v=-w}^{w} I(i+u, j+v) G_n(u,v)\]

高斯滤波的可分离性

\[s = \sum_{u=-w}^{w} \sum_{v=-w}^{w} \cdot g(u) \cdot g(v) = \left(\sum_{u=-w}^{w} \cdot g(u) \right) \cdot \left(\sum_{v=-w}^{w} \cdot g(v) \right) = s^\prime \cdot s^\prime\] \[G_n(u, v) = \frac{1}{s} \cdot e^{-\frac{u^{2}}{2 \sigma^{2}}} \cdot e^{-\frac{v^{2}}{2 \sigma^{2}}} = \frac{1}{s} \cdot g(u) \cdot g(v) = \frac{g(u)}{s^\prime} \cdot \frac{g(v)}{s^\prime}\]

核矩阵:

\[\begin{aligned} \mathbf{G}_{(2w+1)\times(2w+1)} &= \frac{1}{s}\left[\begin{array}{ccccc} g(-w) g(-w) & \dots & g(-w) g(0) & \dots & g(-w) g(w) \\ \vdots & & \vdots & & \vdots \\ g(0) g(-w) & \dots & g(0) g(0) & \dots & g(0) g(w) \\ \vdots & & \vdots & & \vdots \\ g(w) g(-w) & \dots & g(w) g(0) & \dots & g(w) g(w) \end{array}\right] \\ &= \frac{1}{s^\prime} \left[\begin{array}{c} g(-w) \\ \vdots \\ g(0) \\ \vdots \\ g(w) \end{array}\right] \cdot \frac{1}{s^\prime} \left[\begin{array}{ccccc} g(-w) & \ldots & g(0) & \ldots & g(w) \end{array}\right] \end{aligned}\]

图像高斯滤波:

\[\begin{aligned} I^{\prime}(i, j) &=\sum_{u=-w}^{w} \sum_{v=-w}^{w} I(i+u, j+v) G_n(u, v) \\ &=\sum_{u=-w}^{w} \sum_{v=-w}^{w} I(i+u, j+v) \frac{1}{s} g(u) g(v) \\ &=\sum_{u=-w}^{w} \sum_{v=-w}^{w} I(i+u, j+v) \frac{1}{s^{\prime}} g(u) \frac{1}{s^{\prime}} g(v) \\ &=\sum_{u=-w}^{w} \left[ {\color{blue} \sum_{v=-w}^{w} I(i+u, j+v) \frac{g(v)}{s^{\prime}} } \right] \frac{g(u)}{s^{\prime}} \\ &=\sum_{u=-w}^{w} {\color{blue} S(i+u) } \frac{g(u)}{s^{\prime}} \end{aligned}\]
Read More

MPU6050 + Arduino + Processing

[TOC]

Overview

Arduino UNO & MPU6050 GY521

Hardware components

Arduino Apps

  1. Download jrowberg/i2cdevlib/Arduino, copy I2Cdev and MPU6050 to Arduino libraries
  2. Open Arduino IDE, File->Examples->MPU6050->MPU6050_MPU6
  3. #define OUTPUT_READABLE_YAWPITCHROLL -> //#define OUTPUT_READABLE_YAWPITCHROLL //#define OUTPUT_TEAPOT -> #define OUTPUT_TEAPOT
  4. compile and upload

Processing Apps for 3D Simulation

  1. install Processing IDE
  2. install ToxicLib into the Processing’s libraries folder (modes -> java -> libraries), unzip toxiclibs-complete-0020.zip in ToxicLibs and place aLL the contents there
  3. running the teapot demo from the MPU6050_DMP6 example from Jeff Rowberg’s MPU6050 libary

Read More

IMU概述

[TOC]

Overview

陀螺仪知道“我们转了个身”,加速计知道“我们又向前走了几米”,而磁力计则知道“我们是向西方向”的。

9轴传感器:3轴加速度计、3轴陀螺仪、3轴磁力计

Accelerometer

加速计(Accelerometer、G-Sensor)也叫重力感应器,检测设备受到的加速度的大小和方向。

加速度计有两种:一种是角加速度计,是由陀螺仪(角速度传感器)改进的;另一种就是线加速度计。

加速度传感器利用重力加速度,根据测某方向重力加速度分量与重力加速度的关系,可以用于检测设备的倾斜角度,但是它会受到运动加速度的影响,使倾角测量不够准确,所以通常需利用陀螺仪和磁传感器补偿。

加速度传感器可以检测交流信号以及物体的振动,人在走动的时候会产生一定规律性的振动,而加速度传感器可以检测振动的过零点,从而计算出人所走的步或跑步所走的步数,从而计算出人所移动的位移。并且利用一定的公式可以计算出卡路里的消耗。

  • 加速度计的 低频特性好,可以测量低速的静态加速度;
  • 加速度计在较长时间的测量值(测量飞行器的角度)是正确的,然而在较短时间内由于信号噪声以及运动方向的加速度存在,会有很大的误差。具体表现为加速度静止不动时值很稳定,但是移动起来数据波动很大。

Gyroscope

陀螺仪(Gyroscope、GYRO-Sensor)也叫地感器,它的轴由于陀螺效应始终与初始方向平行,这样就可以通过与初始方向的偏差计算出实际方向。

三轴陀螺仪的工作原理是通过测量三维坐标系内陀螺转子的垂直轴与设备之间的夹角,并计算角速度,通过夹角和角速度来判别物体在三维空间的运动状态。三轴陀螺仪可以同时测定上、下、左、右、前、后等6个方向(合成方向同样可分解为三轴坐标),最终可判断出设备的移动轨迹和加速度。

  • 陀螺仪的特性就是 高频特性好,可以测量高速的旋转运动;缺点是存在 零点漂移,容易受温度/加速度等的影响。

  • 陀螺仪测得的是角速度,在较短时间内则比较准确,而较长时间则会有 积分漂移 产生误差。

Magnetometer

磁力计(Magnetic、M-Sensor)也叫地磁、磁感器,可用于测试磁场强度和方向,定位设备的方位,磁力计的原理跟指南针原理类似,可以测量出当前设备与东南西北四个方向上的夹角。

即使使用了加速度计和陀螺仪,也只可以用于测得飞机的俯仰和横滚角度;对于偏航角度,由于偏航角和重力方向正交,无法用加速度计测量得到,而只用陀螺仪测的角度会存在积分漂移的问题。

IMU Errors

Read More

^