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

時間 2019-11-10

標籤 slam 語音機器人 diy 系列感知大腦校準數據融合 imu 慣性傳感器简体版

原文原文鏈接

摘要

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

1.ydlidar-x4激光雷達python

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

3.輪式里程計與運動控制算法

4.音響麥克風與攝像頭django

5.機器人大腦嵌入式主板性能對比ubuntu

6.作一個能走路和對話的機器人centos

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偏差模型：ide

偏差主要來自三部分：噪聲（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控制過程能夠實現對陀螺儀測量角速度的補償，利用補償後的角速度從新估計出來的姿態，就是濾波後的姿態了。

（圖18）MPU9250模塊九軸數據融合後的效果

MPU9250模塊九軸數據融合後的效果如圖18，圖中左側顯示的分別是加速度、角速度、磁力計的實時數據，圖中右側顯示的是融合後的姿態實時數據，能夠看出融合後的姿態仍是很是平穩的。