SLAM+語音機器人DIY系列：（三）感知與大腦——2.帶自校准九軸數據融合IMU慣性傳感器

本文轉載自查看原文 2019-02-19 02:43 4738 感知與大腦

摘要

在我的想象中機器人首先應該能自由的走來走去，然后應該能流利的與主人對話。朝着這個理想，我准備設計一個能自由行走，並且可以與人語音對話的機器人。實現的關鍵是讓機器人能通過傳感器感知周圍環境，並通過機器人大腦處理並輸出反饋和執行動作。本章節涉及到的傳感器有激光雷達、IMU、輪式里程計、麥克風、音響、攝像頭，和用於處理信息的嵌入式主板。關於傳感器的ROS驅動程序開發和在機器人上的使用在后面的章節會展開，本章節重點對機器人傳感器和嵌入式主板進行講解，主要內容：

1.ydlidar-x4激光雷達

2.帶自校准九軸數據融合IMU慣性傳感器

IMU是慣性測量單元的簡稱，用於測量物體的三軸姿態角（roll、pitch、yaw）、三軸加速度（acc_x、acc_y、acc_z）、三軸角速度（w_x、w_y、w_z）等。IMU慣性測量單元在制造過程中，由於物理因素，導致IMU慣性測量單元實際的坐標軸與理想的坐標軸之間會有一定的偏差，同時三軸加速度、三軸角速度、三軸磁力計的原始值會與真實值有一個固定的偏差等。這里提到的自校准就是要通過給的補償值來減小或消除坐標軸的偏差及原始值的固定偏差，也就是所謂的IMU內部標定。如果將IMU安裝到機器人或攝像頭上后，需要知道IMU與機器人或攝像頭的相對位置，這個時候進行的標定就是所謂的IMU外部標定。特此說明，這里提到的自校准指IMU內部標定。這里提到的九軸數據融合，是指通過三軸加速度、三軸角速度數據融合得到更加精准的三軸加速度、三軸角速度，同時通過三軸加速度、三軸角速度、三軸磁力計數據融合得到測量物體的三軸姿態角。選用一款帶自校准九軸數據融合的IMU，能很好的提升機器人的SLAM建圖與導航性能，同時降低機器人上軟件的開發難度。出於這一點，我們選用了一款基於MPU9250的.帶自校准九軸數據融合IMU，如圖6。

（圖6）帶自校准九軸數據融合IMU慣性傳感器

2.1.自校准

IMU誤差模型：

誤差主要來自三部分：噪聲（bias and noise）、尺度誤差（scale error）、軸偏差（axis misalignment）。通過對這些誤差的補償來實現IMU測量數據的校准，校准的數學表達如圖7。

（圖7）校准的數學表達

噪聲部分考慮零偏Bias和高斯白噪聲noise。零偏Bias也叫隨機游走，一般是由傳感器內部構造、溫度變換多方面綜合影響的結果；高斯白噪聲noise，一般是由於AD轉換引起的一種外部噪聲。

尺度誤差部分，來自於AD轉換中量化過程，比如采樣電壓值1V對應acc(x)軸的1.4g，同樣采樣電壓值1V對應acc(y)軸的1.6g，也就是說不通的軸上AD轉換量化是不同的。

軸偏差部分，三軸加速度、三軸角速度、三軸磁力計的坐標軸嚴格上都不是正交坐標系。但是最后我們期望的使用值默認是在正交坐標系下測量的，所以就需要將在非正交坐標系測量的原始值變換到正交坐標系中。

加速度校准：

校准過程中需要判斷傳感器是否處於靜止狀態，其實很簡單，在時間t內（t一般取50s），分別計算acc(x)、acc(y)、acc(z)三軸數據的方差var[acc(x)]、var[acc(y)]、var[acc(z)]，如果var[acc(x)]+var[acc(y)]+var[acc(z)]小於閾值H（H為經驗值，需要通過實驗法確定），就判斷傳感器靜止。

在靜止狀態下，加速度計測量值的二范數等於當地重力加速度g。在這一約束條件下，利用最小二乘法進行優化問題求解，由於加速度采樣值取得是一個小窗口采樣區間的平均值，所以可以忽略白噪聲Na的影響，便可以求解出待標定參數Ta、Sa、Ba。在靜止狀態取一串加速度采樣值共M個，構建代價函數如圖8，對代價函數進行最優化求解即可，可以選用ceres或g2o等優化工具來完成具體的優化計算過程。

（圖8）代價函數

角速度校准：

角速度校准分為兩部分：用Allan方差校准零偏Bias、用最優化方法求解尺度誤差Sw和軸偏差Tw。加速度校准中需要用到加速度校准信息，所以加速度校准好壞影響整個IMU校准。

同加速度校准一樣，也需要在靜止狀態下采集角速度，簡便的做法是在采集加速度的同時也采集角速度。利用角速度采樣值計算Allan方差，Allan方差計算比較繁瑣就不展開，請直接參考wiki百科：https://en.wikipedia.org/wiki/Allan_variance。

Allan方差中共有5個噪聲參數：量化噪聲Q、角度隨機游走N、零偏B、角速度隨機游走K、角速度斜坡R。通過圖9的步驟便可以求出角速度的零偏Bw，由於角速度采樣時是一個小窗口采樣區間的平均值，所以可以忽略白噪聲Nw的影響。

（圖9）用Allan方差校准零偏Bias

角速度的零偏Bw校准完后，將零偏值Bw帶入，接着校准尺度誤差Sw和軸偏差Tw。挑選加速度校准過程中兩靜止狀態夾雜的動態片段，這樣經過校准后的靜態值的平均值可作為acc_calib向量的起始值acc_calib(k-1)和旋轉完成后的結束值acc_calib(k)。

通過圖10的步驟便可以求出角速度的尺度誤差Sw和軸偏差Tw。

（圖10）求角速度的尺度誤差Sw和軸偏差Tw

磁力計校准：

與加速度校准比較類似，磁力計測量值的二范數等於當地磁場強度m，不同的是磁力的測量不需要在靜止狀態。在這一約束條件下，利用最小二乘法進行優化問題求解，由於磁力采樣值取得是一個小窗口采樣區間的平均值，所以可以忽略白噪聲Nm的影響，便可以求解出待標定參數Tm、Sm、Bm。構建代價函數如圖11，對代價函數進行最優化求解即可。

（圖11）代價函數

由於需要用加速度/磁力計進行融合，需要將磁力計測量值變換到加速度坐標系。變換矩陣Tm2a求解比較簡單，通過多組關聯的加速度acc_calib和磁力計mag_calib，通過最小二次法優化求解，可以求出變換矩陣Tm2a。這里基於一個假設，磁力向量與加速度向量成固定夾角，也就是兩向量的點乘為一個常數C，利用這個約束可以構建代價函數如圖12，對代價函數進行最優化求解即可。

（圖12）代價函數

IMU校准軟件流程圖：

（圖13）代價函數

IMU手動校准與自校准對比：

通常的校准過程是，將IMU接入PC端，手動將IMU置不同的狀態，PC端通過采集這些數據，完成校准。另一種校准過程是，將校准算法內置在IMU模塊上的MCU中，在IMU工作的過程中實時采集數據，並自動完成校准，不需要人為的干預。本文介紹的MPU9250模塊就屬於自校准這一方式。手動校准與自校准對比如圖14。

（圖14）手動校准與自校准對比

2.2.九軸數據融合

IMU的校准完成后，就可以利用校准后的九軸數據進行數據融合，得到IMU的空間姿態了。

MPU9250模塊包括三軸加速度計、三軸陀螺儀和三軸磁力計。因為加速度/磁力計具有高頻噪聲，也就是說它們的瞬時值不夠精確，解算出來的姿態會震盪，但長期來看姿態方向是對的。而陀螺儀具有低頻噪聲，即每個時刻的得到的角速度是比較精確的，使用積分就能得到旋轉角度（姿態），但是積分會累積誤差，因此積分到后面姿態就不對了，也就是漂移現象。加速度/磁力計和陀螺儀在頻域上的特性互補，可以融合這三種傳感器的數據，提高精度和系統的動態特性。

由於歐拉角在求解姿態時存在萬向節鎖，不能用於全姿態解算；故采用四元數Quaternion進行姿態解算。常用的九軸數據融合算法包括：高低通互補濾波、擴展卡爾曼濾波EKF、Mahony濾波。在實際情況中根據不同的用途進行選擇，這里對這三種濾波方法進行原理性的介紹。

高低通互補濾波：

（圖15）高低通互補濾波的過程

高低通互補濾波的過程如圖15,通過加權系數融合陀螺儀估計姿態和加速度/磁力計融合姿態，達到對陀螺儀低通濾波、對加速度/磁力計高通濾波的效果。

擴展卡爾曼濾波EKF：

和高低通互補濾波過程類似，也是分為估計、校正。

（圖16）EKF濾波的過程

EKF濾波的過程如圖16，想要了解EKF濾波的詳細推導請直接參考wiki百科：https://en.wikipedia.org/wiki/Extended_Kalman_filter。估計過程中，利用陀螺儀積分推算出估計姿態；在校正過程中，計算估計姿態下的加速度/磁力計，再用在實際姿態下測量的加速度/磁力計按照EKF校正方程進行校正。

Mahony濾波：

同樣，Mahony濾波也分為估計、校正。主要使用了PI控制思想。

（圖17）Mahony濾波的過程

Mahony濾波的過程如圖17。在估計過程中，同樣利用陀螺儀積分推算出估計姿態；在校正過程中，計算估計姿態下的加速度/磁力計，測量在實際姿態下的加速度/磁力計，這兩個向量叉乘便可以得到姿態誤差e，姿態誤差e通過PI控制過程可以實現對陀螺儀測量角速度的補償，利用補償后的角速度重新估計出來的姿態，就是濾波后的姿態了。