摘要 在我的想象中機器人首先應該能自由的走來走去，然後應該能流利的與主人對話。朝著這個理想，我準備設計一個能自由行走，並且可以與人語音對話的機器人。實現的關鍵是讓機器人能通過感測器感知周圍環境，並通過機器人大腦處理並輸出反饋和執行動作。本章節涉及到的感測器有激光雷達、IMU、輪式里程計、麥克風、音響 ...
摘要
在我的想象中機器人首先應該能自由的走來走去，然後應該能流利的與主人對話。朝著這個理想，我準備設計一個能自由行走，並且可以與人語音對話的機器人。實現的關鍵是讓機器人能通過感測器感知周圍環境，並通過機器人大腦處理並輸出反饋和執行動作。本章節涉及到的感測器有激光雷達、IMU、輪式里程計、麥克風、音響、攝像頭，和用於處理信息的嵌入式主板。關於感測器的ROS驅動程式開發和在機器人上的使用在後面的章節會展開，本章節重點對機器人感測器和嵌入式主板進行講解，主要內容：
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控制過程可以實現對陀螺儀測量角速度的補償，利用補償後的角速度重新估計出來的姿態，就是濾波後的姿態了。
（圖18）MPU9250模塊九軸數據融合後的效果
MPU9250模塊九軸數據融合後的效果如圖18，圖中左側顯示的分別是加速度、角速度、磁力計的實時數據，圖中右側顯示的是融合後的姿態實時數據，可以看出融合後的姿態還是非常平穩的。
2.3.在機器人中使用IMU
（圖19）在機器人中使用MPU9250-IMU模塊
MPU9250-IMU模塊通過串口與機器人相連接，機器人中通過運行IMU的ROS驅動，來實現讀取串口的IMU數據和將IMU數據發佈到/imu這個主題，這樣機器人上的其它節點就可以通過訂閱/imu主題來獲取IMU數據了。關於IMU的ROS驅動在機器人上的具體使用，將在後面的章節中具體講解。
後記
如果大家對博文的相關類容感興趣，或有什麼技術疑問，歡迎加入下麵的《SLAM+語音機器人DIY》QQ技術交流群，一起討論學習^_^
