【自己動(dòng)手做一臺(tái)SLAM導(dǎo)航機(jī)器人】第三章：感知與大腦
本專欄目錄
第三章：感知與大腦
第四章：差分底盤設(shè)計(jì)
第五章：樹莓派3開發(fā)環(huán)境搭建
第六章：SLAM建圖與自主避障導(dǎo)航
第七章：語音交互與自然語言處理
附錄A：用于ROS機(jī)器人交互的Android手機(jī)APP開發(fā)
附錄B：用于ROS機(jī)器人管理調(diào)度的后臺(tái)服務(wù)器搭建
附錄C：如何選擇ROS機(jī)器人平臺(tái)進(jìn)行SLAM導(dǎo)航入門
視頻教程
https://www.bilibili.com/video/BV1jS4y1a7Lz
在我的想象中機(jī)器人首先應(yīng)該能自由的走來走去，然后應(yīng)該能流利的與主人對(duì)話。朝著這個(gè)理想，我準(zhǔn)備設(shè)計(jì)一個(gè)能自由行走，并且可以與人語音對(duì)話的機(jī)器人。實(shí)現(xiàn)的關(guān)鍵是讓機(jī)器人能通過傳感器感知周圍環(huán)境，并通過機(jī)器人大腦處理并輸出反饋和執(zhí)行動(dòng)作。本章節(jié)涉及到的傳感器有激光雷達(dá)、IMU、輪式里程計(jì)、麥克風(fēng)、音響、攝像頭，和用于處理信息的嵌入式主板。關(guān)于傳感器的ROS驅(qū)動(dòng)程序開發(fā)和在機(jī)器人上的使用在后面的章節(jié)會(huì)展開，本章重點(diǎn)對(duì)機(jī)器人傳感器和嵌入式主板進(jìn)行講解，主要內(nèi)容：
1.ydlidar-x4激光雷達(dá)
2.帶自校準(zhǔn)九軸數(shù)據(jù)融合IMU慣性傳感器
3.輪式里程計(jì)與運(yùn)動(dòng)控制
4.音響麥克風(fēng)與攝像頭
5.機(jī)器人大腦嵌入式主板性能對(duì)比
6.做一個(gè)能走路和對(duì)話的機(jī)器人
下面這本書是本篇文章的參考文獻(xiàn)，大家有需要可以入手一本：
1.ydlidar-x4激光雷達(dá)
在移動(dòng)機(jī)器人中，獲取機(jī)器人周圍障礙物和環(huán)境的輪廓形狀是非常重要的。使用激光雷達(dá)正是為了實(shí)現(xiàn)這個(gè)目的。利用掃描得到的障礙物信息，機(jī)器人就可以利用SLAM建立地圖、并進(jìn)行避障和自主導(dǎo)航?？紤]到成本，推薦大家選用低成本的2D激光雷達(dá)，這里推薦的ydlidar-x4激光雷達(dá)正是一款極低成本的2D激光雷達(dá)，作為學(xué)習(xí)性能足夠用了。
1.1.硬件概述
雷達(dá)主體由激光測距模組和電機(jī)構(gòu)成，雷達(dá)主體需要連接轉(zhuǎn)接板，用于實(shí)現(xiàn)外部供電和UART轉(zhuǎn)USB，如圖1。
測距頻率是指每秒鐘測距的次數(shù)（即激光測距模組的采樣率），測距頻率默認(rèn)為5KHz；掃描頻率是指電機(jī)每秒鐘轉(zhuǎn)過的圈數(shù)，掃描頻率默認(rèn)為8Hz；測距范圍是指落在此距離范圍的障礙物才能被測量，測距范圍為0.12~10m；掃描角度為0~360度，也就是能夠360度全方位掃描；測距分辨率就是測距精度；角度分辨率就是兩個(gè)相鄰掃描點(diǎn)之間的夾角。ydlidar-x4激光雷達(dá)性能參數(shù)，如圖2。
關(guān)于激光雷達(dá)的性能參數(shù)對(duì)SLAM建圖與避障導(dǎo)航的影響，這里做一個(gè)簡短的分析。
掃描頻率：
掃描頻率越高，電機(jī)轉(zhuǎn)動(dòng)一圈的時(shí)間約短，掃描獲取1幀雷達(dá)數(shù)據(jù)的時(shí)間越短，這樣可以使SLAM建圖和避障導(dǎo)航實(shí)時(shí)性更好。簡單點(diǎn)說就是機(jī)器人運(yùn)動(dòng)速度很快時(shí)，掃描到的點(diǎn)云數(shù)據(jù)連續(xù)性也比較好，這有利于SLAM算法建立穩(wěn)定的地圖，有利于避障導(dǎo)航算法及時(shí)發(fā)現(xiàn)并避開障礙物。
測距范圍：
小于測距范圍最小值的區(qū)域就是雷達(dá)的測量盲區(qū)，處于盲區(qū)中的障礙物無法被探測，所以，盲區(qū)當(dāng)然是越小越好以保證機(jī)器人不發(fā)生碰撞；大于測距范圍最大值的區(qū)域就是雷達(dá)超量程的區(qū)域，在超量程的區(qū)域的障礙物無法被探測或者可以探測但誤差很大，所以，在比較開闊的環(huán)境下應(yīng)該采用遠(yuǎn)距離量程的雷達(dá)。
掃描角度：
大部分雷達(dá)都是0~360度全方位掃描的，所以就沒什么太大的區(qū)別了，其實(shí)就是機(jī)器人不用轉(zhuǎn)動(dòng)身體的情況下就能一次性掃描出四周的障礙物信息。
測距分辨率：
測距分辨率也就是測距精度，測距精度越高當(dāng)然有利于SLAM建圖和避障導(dǎo)航，但是測距精度越越高的雷達(dá)成本當(dāng)然也越高，現(xiàn)在國產(chǎn)低成本的雷達(dá)普遍為厘米級(jí)（cm）的精度，差一點(diǎn)的雷達(dá)5cm左右的精度，稍微好一點(diǎn)的雷達(dá)2cm左右的精度，如果要達(dá)到毫米級(jí)（mm）的精度成本就非常高了。
角度分辨率：
角度分辨率決定了兩個(gè)相鄰點(diǎn)云之間的夾角，由于雷達(dá)是通過旋轉(zhuǎn)進(jìn)行掃描的，隨著距離增加點(diǎn)云會(huì)越來越稀疏。如果角度分辨率比較低，在掃描遠(yuǎn)距離物理時(shí)只能得到非常稀疏的幾個(gè)點(diǎn)云，這樣的點(diǎn)云基本上沒有什么用處了。
角度分辨率 = 360 /（測距頻率 / 掃描頻率） ?
從上面的角度分辨率計(jì)算公式來看，一般測距頻率為常數(shù)值（由激光模組特性決定），那么通過降低掃描頻率可以提升角度分辨率，但同時(shí)掃描頻率降低會(huì)影響雷達(dá)的實(shí)時(shí)性，所以這是一個(gè)權(quán)衡的過程，根據(jù)實(shí)際情況做選擇。
1.2.工作原理
激光雷達(dá)之所以流行，得益于它能夠精確的測距。主流的激光雷達(dá)基于兩種原理：一種是三角測距法，另一種是飛行時(shí)間（TOF）測距法。其實(shí)很好理解，就是利用了最基本的數(shù)學(xué)與物理知識(shí)。
三角測距原理如圖3，激光器發(fā)射一束激光，被物體A反射后，照射到圖像傳感器的A’，這樣就形成了一個(gè)三角形，通過解算可以求出物體A到激光器的距離。激光束被不同距離的物體反射后，形成不同的三角形。我們不難發(fā)現(xiàn)隨物體距離不斷變遠(yuǎn)，反射激光在圖像傳感器上的位置變化會(huì)越來越小，也就是越來越難以分辨。這正是三角測距的一大缺點(diǎn)，物體距離越遠(yuǎn)，測距誤差越大。
飛行時(shí)間（TOF）測距原理如圖4，激光器發(fā)出激光時(shí)，計(jì)時(shí)器開始計(jì)時(shí)，接收器接收到反射回來的激光時(shí)，計(jì)時(shí)器停止計(jì)時(shí)，得到激光傳播的時(shí)間后，通過光速一定這個(gè)條件，很容易計(jì)算出激光器到障礙物的距離。由于光速傳播太快了，要獲取精確的傳播時(shí)間太難了。所以這種激光雷達(dá)自然而然成本也會(huì)高很多，但是測距精度很高。
1.3.在機(jī)器人中使用ydlidar-x4激光雷達(dá)
ydlidar-x4激光雷達(dá)通過串口與機(jī)器人相連接，機(jī)器人中通過運(yùn)行雷達(dá)ROS驅(qū)動(dòng)，來實(shí)現(xiàn)讀取串口的雷達(dá)數(shù)據(jù)和將雷達(dá)數(shù)據(jù)發(fā)布到/scan這個(gè)主題，這樣機(jī)器人上的其它節(jié)點(diǎn)就可以通過訂閱/scan主題來獲取激光雷達(dá)數(shù)據(jù)了。關(guān)于雷達(dá)ROS驅(qū)動(dòng)在機(jī)器人上的具體使用，將在后面的章節(jié)中具體講解。
2.帶自校準(zhǔn)九軸數(shù)據(jù)融合IMU慣性傳感器
IMU是慣性測量單元的簡稱，用于測量物體的三軸姿態(tài)角（roll、pitch、yaw）、三軸加速度（acc_x、acc_y、acc_z）、三軸角速度（w_x、w_y、w_z）等。IMU慣性測量單元在制造過程中，由于物理因素，導(dǎo)致IMU慣性測量單元實(shí)際的坐標(biāo)軸與理想的坐標(biāo)軸之間會(huì)有一定的偏差，同時(shí)三軸加速度、三軸角速度、三軸磁力計(jì)的原始值會(huì)與真實(shí)值有一個(gè)固定的偏差等。這里提到的自校準(zhǔn)就是要通過給的補(bǔ)償值來減小或消除坐標(biāo)軸的偏差及原始值的固定偏差，也就是所謂的IMU內(nèi)部標(biāo)定。如果將IMU安裝到機(jī)器人或攝像頭上后，需要知道IMU與機(jī)器人或攝像頭的相對(duì)位置，這個(gè)時(shí)候進(jìn)行的標(biāo)定就是所謂的IMU外部標(biāo)定。特此說明，這里提到的自校準(zhǔn)指IMU內(nèi)部標(biāo)定。這里提到的九軸數(shù)據(jù)融合，是指通過三軸加速度、三軸角速度數(shù)據(jù)融合得到更加精準(zhǔn)的三軸加速度、三軸角速度，同時(shí)通過三軸加速度、三軸角速度、三軸磁力計(jì)數(shù)據(jù)融合得到測量物體的三軸姿態(tài)角。選用一款帶自校準(zhǔn)九軸數(shù)據(jù)融合的IMU，能很好的提升機(jī)器人的SLAM建圖與導(dǎo)航性能，同時(shí)降低機(jī)器人上軟件的開發(fā)難度。出于這一點(diǎn)，我們選用了一款基于MPU9250的.帶自校準(zhǔn)九軸數(shù)據(jù)融合IMU，如圖6。
2.1.自校準(zhǔn)
IMU誤差模型：
誤差主要來自三部分：噪聲（bias and noise）、尺度誤差（scale error）、軸偏差（axis misalignment）。通過對(duì)這些誤差的補(bǔ)償來實(shí)現(xiàn)IMU測量數(shù)據(jù)的校準(zhǔn)，校準(zhǔn)的數(shù)學(xué)表達(dá)如圖7。
噪聲部分考慮零偏Bias和高斯白噪聲noise。零偏Bias也叫隨機(jī)游走，一般是由傳感器內(nèi)部構(gòu)造、溫度變換多方面綜合影響的結(jié)果；高斯白噪聲noise，一般是由于AD轉(zhuǎn)換引起的一種外部噪聲。
尺度誤差部分，來自于AD轉(zhuǎn)換中量化過程，比如采樣電壓值1V對(duì)應(yīng)acc(x)軸的1.4g，同樣采樣電壓值1V對(duì)應(yīng)acc(y)軸的1.6g，也就是說不通的軸上AD轉(zhuǎn)換量化是不同的。
軸偏差部分，三軸加速度、三軸角速度、三軸磁力計(jì)的坐標(biāo)軸嚴(yán)格上都不是正交坐標(biāo)系。但是最后我們期望的使用值默認(rèn)是在正交坐標(biāo)系下測量的，所以就需要將在非正交坐標(biāo)系測量的原始值變換到正交坐標(biāo)系中。
加速度校準(zhǔn)：
校準(zhǔn)過程中需要判斷傳感器是否處于靜止?fàn)顟B(tài)，其實(shí)很簡單，在時(shí)間t內(nèi)（t一般取50s），分別計(jì)算acc(x)、acc(y)、acc(z)三軸數(shù)據(jù)的方差var[acc(x)]、var[acc(y)]、var[acc(z)]，如果var[acc(x)]+var[acc(y)]+var[acc(z)]小于閾值H（H為經(jīng)驗(yàn)值，需要通過實(shí)驗(yàn)法確定），就判斷傳感器靜止。
在靜止?fàn)顟B(tài)下，加速度計(jì)測量值的二范數(shù)等于當(dāng)?shù)刂亓铀俣萭。在這一約束條件下，利用最小二乘法進(jìn)行優(yōu)化問題求解，由于加速度采樣值取得是一個(gè)小窗口采樣區(qū)間的平均值，所以可以忽略白噪聲Na的影響，便可以求解出待標(biāo)定參數(shù)Ta、Sa、Ba。在靜止?fàn)顟B(tài)取一串加速度采樣值共M個(gè)，構(gòu)建代價(jià)函數(shù)如圖8，對(duì)代價(jià)函數(shù)進(jìn)行最優(yōu)化求解即可，可以選用ceres或g2o等優(yōu)化工具來完成具體的優(yōu)化計(jì)算過程。
角速度校準(zhǔn)：
角速度校準(zhǔn)分為兩部分：用Allan方差校準(zhǔn)零偏Bias、用最優(yōu)化方法求解尺度誤差Sw和軸偏差Tw。加速度校準(zhǔn)中需要用到加速度校準(zhǔn)信息，所以加速度校準(zhǔn)好壞影響整個(gè)IMU校準(zhǔn)。
同加速度校準(zhǔn)一樣，也需要在靜止?fàn)顟B(tài)下采集角速度，簡便的做法是在采集加速度的同時(shí)也采集角速度。利用角速度采樣值計(jì)算Allan方差，Allan方差計(jì)算比較繁瑣就不展開，請(qǐng)直接參考wiki百科：https://en.wikipedia.org/wiki/Allan_variance。
Allan方差中共有5個(gè)噪聲參數(shù)：量化噪聲Q、角度隨機(jī)游走N、零偏B、角速度隨機(jī)游走K、角速度斜坡R。通過圖9的步驟便可以求出角速度的零偏Bw，由于角速度采樣時(shí)是一個(gè)小窗口采樣區(qū)間的平均值，所以可以忽略白噪聲Nw的影響。
角速度的零偏Bw校準(zhǔn)完后，將零偏值Bw帶入，接著校準(zhǔn)尺度誤差Sw和軸偏差Tw。挑選加速度校準(zhǔn)過程中兩靜止?fàn)顟B(tài)夾雜的動(dòng)態(tài)片段，這樣經(jīng)過校準(zhǔn)后的靜態(tài)值的平均值可作為acc_calib向量的起始值acc_calib(k-1)和旋轉(zhuǎn)完成后的結(jié)束值acc_calib(k)。
通過圖10的步驟便可以求出角速度的尺度誤差Sw和軸偏差Tw。
磁力計(jì)校準(zhǔn)：
與加速度校準(zhǔn)比較類似，磁力計(jì)測量值的二范數(shù)等于當(dāng)?shù)卮艌鰪?qiáng)度m，不同的是磁力的測量不需要在靜止?fàn)顟B(tài)。在這一約束條件下，利用最小二乘法進(jìn)行優(yōu)化問題求解，由于磁力采樣值取得是一個(gè)小窗口采樣區(qū)間的平均值，所以可以忽略白噪聲Nm的影響，便可以求解出待標(biāo)定參數(shù)Tm、Sm、Bm。構(gòu)建代價(jià)函數(shù)如圖11，對(duì)代價(jià)函數(shù)進(jìn)行最優(yōu)化求解即可。
由于需要用加速度/磁力計(jì)進(jìn)行融合，需要將磁力計(jì)測量值變換到加速度坐標(biāo)系。變換矩陣Tm2a求解比較簡單，通過多組關(guān)聯(lián)的加速度acc_calib和磁力計(jì)mag_calib，通過最小二次法優(yōu)化求解，可以求出變換矩陣Tm2a。這里基于一個(gè)假設(shè)，磁力向量與加速度向量成固定夾角，也就是兩向量的點(diǎn)乘為一個(gè)常數(shù)C，利用這個(gè)約束可以構(gòu)建代價(jià)函數(shù)如圖12，對(duì)代價(jià)函數(shù)進(jìn)行最優(yōu)化求解即可。
IMU校準(zhǔn)軟件流程圖：
IMU手動(dòng)校準(zhǔn)與自校準(zhǔn)對(duì)比：
通常的校準(zhǔn)過程是，將IMU接入PC端，手動(dòng)將IMU置不同的狀態(tài)，PC端通過采集這些數(shù)據(jù)，完成校準(zhǔn)。另一種校準(zhǔn)過程是，將校準(zhǔn)算法內(nèi)置在IMU模塊上的MCU中，在IMU工作的過程中實(shí)時(shí)采集數(shù)據(jù)，并自動(dòng)完成校準(zhǔn)，不需要人為的干預(yù)。本文介紹的MPU9250模塊就屬于自校準(zhǔn)這一方式。手動(dòng)校準(zhǔn)與自校準(zhǔn)對(duì)比如圖14。
2.2.九軸數(shù)據(jù)融合
IMU的校準(zhǔn)完成后，就可以利用校準(zhǔn)后的九軸數(shù)據(jù)進(jìn)行數(shù)據(jù)融合，得到IMU的空間姿態(tài)了。
MPU9250模塊包括三軸加速度計(jì)、三軸陀螺儀和三軸磁力計(jì)。因?yàn)榧铀俣?磁力計(jì)具有高頻噪聲，也就是說它們的瞬時(shí)值不夠精確，解算出來的姿態(tài)會(huì)震蕩，但長期來看姿態(tài)方向是對(duì)的。而陀螺儀具有低頻噪聲，即每個(gè)時(shí)刻的得到的角速度是比較精確的，使用積分就能得到旋轉(zhuǎn)角度（姿態(tài)），但是積分會(huì)累積誤差，因此積分到后面姿態(tài)就不對(duì)了，也就是漂移現(xiàn)象。 加速度/磁力計(jì)和陀螺儀在頻域上的特性互補(bǔ)，可以融合這三種傳感器的數(shù)據(jù)，提高精度和系統(tǒng)的動(dòng)態(tài)特性。
由于歐拉角在求解姿態(tài)時(shí)存在萬向節(jié)鎖，不能用于全姿態(tài)解算；故采用四元數(shù)Quaternion進(jìn)行姿態(tài)解算。常用的九軸數(shù)據(jù)融合算法包括：高低通互補(bǔ)濾波、擴(kuò)展卡爾曼濾波EKF、Mahony濾波。在實(shí)際情況中根據(jù)不同的用途進(jìn)行選擇，這里對(duì)這三種濾波方法進(jìn)行原理性的介紹。
高低通互補(bǔ)濾波：
高低通互補(bǔ)濾波的過程如圖15,通過加權(quán)系數(shù)融合陀螺儀估計(jì)姿態(tài)和加速度/磁力計(jì)融合姿態(tài)，達(dá)到對(duì)陀螺儀低通濾波、對(duì)加速度/磁力計(jì)高通濾波的效果。
擴(kuò)展卡爾曼濾波EKF：
和高低通互補(bǔ)濾波過程類似，也是分為估計(jì)、校正。
EKF濾波的過程如圖16，想要了解EKF濾波的詳細(xì)推導(dǎo)請(qǐng)直接參考wiki百科：
https://en.wikipedia.org/wiki/Extended_Kalman_filter。估計(jì)過程中，利用陀螺儀積分推算出估計(jì)姿態(tài)；在校正過程中，計(jì)算估計(jì)姿態(tài)下的加速度/磁力計(jì)，再用在實(shí)際姿態(tài)下測量的加速度/磁力計(jì)按照EKF校正方程進(jìn)行校正。
Mahony濾波：
同樣，Mahony濾波也分為估計(jì)、校正。主要使用了PI控制思想。
Mahony濾波的過程如圖17。在估計(jì)過程中，同樣利用陀螺儀積分推算出估計(jì)姿態(tài)；在校正過程中，計(jì)算估計(jì)姿態(tài)下的加速度/磁力計(jì)，測量在實(shí)際姿態(tài)下的加速度/磁力計(jì)，這兩個(gè)向量叉乘便可以得到姿態(tài)誤差e，姿態(tài)誤差e通過PI控制過程可以實(shí)現(xiàn)對(duì)陀螺儀測量角速度的補(bǔ)償，利用補(bǔ)償后的角速度重新估計(jì)出來的姿態(tài)，就是濾波后的姿態(tài)了。
MPU9250模塊九軸數(shù)據(jù)融合后的效果如圖18，圖中左側(cè)顯示的分別是加速度、角速度、磁力計(jì)的實(shí)時(shí)數(shù)據(jù)，圖中右側(cè)顯示的是融合后的姿態(tài)實(shí)時(shí)數(shù)據(jù)，可以看出融合后的姿態(tài)還是非常平穩(wěn)的。
2.3.在機(jī)器人中使用IMU
MPU9250-IMU模塊通過串口與機(jī)器人相連接，機(jī)器人中通過運(yùn)行IMU的ROS驅(qū)動(dòng)，來實(shí)現(xiàn)讀取串口的IMU數(shù)據(jù)和將IMU數(shù)據(jù)發(fā)布到/imu這個(gè)主題，這樣機(jī)器人上的其它節(jié)點(diǎn)就可以通過訂閱/imu主題來獲取IMU數(shù)據(jù)了。關(guān)于IMU的ROS驅(qū)動(dòng)在機(jī)器人上的具體使用，將在后面的章節(jié)中具體講解。
3.輪式里程計(jì)與運(yùn)動(dòng)控制
底盤提供輪式里程計(jì)與運(yùn)動(dòng)控制，是機(jī)器人SLAM建圖與避障導(dǎo)航的基礎(chǔ)。所以對(duì)底盤進(jìn)行了解，和熟悉輪式里程計(jì)與運(yùn)動(dòng)控制的底層原理是很有益處的。這里以我們的miiboo機(jī)器人的底盤為例，對(duì)底盤上的輪式里程計(jì)和運(yùn)動(dòng)控制的原理進(jìn)行分析。
3.1.硬件概述
底盤主要由電機(jī)控制板和帶編碼器的減速電機(jī)構(gòu)成，如圖20。電機(jī)控制板通過串口與機(jī)器人的大腦（如樹莓派3）相連接，通過接收大腦下發(fā)的控制指令，利用PID算法對(duì)電機(jī)進(jìn)行控制；同時(shí)，采集電機(jī)上的編碼器數(shù)據(jù)發(fā)送給大腦，利用航跡推演算法得到底盤的里程計(jì)信息。
3.2.輪式里程計(jì)與運(yùn)動(dòng)控制
如圖21，為輪式里程計(jì)與運(yùn)動(dòng)控制的系統(tǒng)框圖。首先是機(jī)器人大腦發(fā)送控制命令，其實(shí)就是期望左、右電機(jī)達(dá)到的目標(biāo)轉(zhuǎn)速，我們都知道在一個(gè)控制系統(tǒng)中，被控對(duì)象很難完全按照期望目標(biāo)來運(yùn)行，這就需要引入反饋對(duì)被控對(duì)象進(jìn)行實(shí)時(shí)的閉環(huán)控制，讓被控對(duì)象盡量逼近期望目標(biāo)，電機(jī)控制板主要就是用來實(shí)現(xiàn)這個(gè)過程。同時(shí)，電機(jī)控制板還負(fù)責(zé)對(duì)電機(jī)編碼信號(hào)進(jìn)行采樣，將單位采樣時(shí)間（一般為10ms）內(nèi)的編碼脈沖累計(jì)值作為里程數(shù)據(jù)發(fā)送給機(jī)器人大腦，機(jī)器人大腦利用航跡推演算法求解出里程計(jì)信息。
通信協(xié)議：
電機(jī)控制板與機(jī)器人大腦之間采用串口通信。電機(jī)左、右輪期望轉(zhuǎn)速被封裝到串口的字符串中，作為控制命令發(fā)送給電機(jī)控制板；單位時(shí)間（一般10ms）內(nèi)采樣到的電機(jī)編碼脈沖累計(jì)值（等效為實(shí)際電機(jī)速度）作為里程數(shù)據(jù)，以同樣的方式被封裝到串口的字符串中發(fā)送給機(jī)器人大腦?？梢钥闯?，控制命令與里程數(shù)據(jù)遵循一樣的封裝協(xié)議，協(xié)議具體形式如圖22。
電機(jī)控制：
電機(jī)控制最常用的就是PID控制算法，如圖23為PID算法流程。以電機(jī)轉(zhuǎn)速控制為例，r(t)就是給定的目標(biāo)轉(zhuǎn)速，c(t)就是電機(jī)實(shí)際運(yùn)行時(shí)的轉(zhuǎn)速，通過閉環(huán)反饋可以求得r(t)與c(t)的偏差值e(t)，PID控制算法中的比例（P）、積分（I）、微分（D）調(diào)節(jié)器利用e(t)生成新的控制量u(t)，u(t)通過執(zhí)行機(jī)構(gòu)（電機(jī)驅(qū)動(dòng)器）作用于被控對(duì)象（電機(jī)），電機(jī)的實(shí)際運(yùn)行速度c(t)通過閉環(huán)反饋，進(jìn)入下一次PID調(diào)節(jié)。就這樣，不斷的通過閉環(huán)反饋調(diào)節(jié)，使電機(jī)實(shí)際運(yùn)行速度c(t)最終逼近給定的目標(biāo)速度r(t)。
在連續(xù)和離散時(shí)間域上PID會(huì)有不同的表現(xiàn)形式，在連續(xù)時(shí)間域上積分、微分調(diào)節(jié)通過積分計(jì)算、微分計(jì)算實(shí)現(xiàn)，而在離散時(shí)間域上積分、微分調(diào)節(jié)通過累加和、差分計(jì)算實(shí)現(xiàn)。由于電機(jī)控制需要在程序上進(jìn)行實(shí)現(xiàn)，所以需要采用離散域的PID。按照PID算法生成的調(diào)節(jié)量的形式，又可以分為位置式PID和增量式PID；位置式PID生成的u(t)為直接的控制量，增量式PID生成的Δu(t)是控制量的修正量，需要疊加上一次的u(t-1)才能作為控制量。離散形式的位置式與增量式PID數(shù)學(xué)表達(dá)如圖24。
可以看出，利用位置PID的數(shù)學(xué)表達(dá)式經(jīng)過簡單的變形就能得到增量PID的數(shù)學(xué)表達(dá)式。增量算法不需要做累加，控制量增量的確定僅與最近幾次誤差采樣值有關(guān)，計(jì)算誤差或計(jì)算精度問題，對(duì)控制量的計(jì)算影響較小。而位置算法要用到過去的誤差的累加值，容易產(chǎn)生大的累加誤差。增量式算法得出的是控制量的增量，例如閥門控制中、只輸出閥門開度的變化部分，誤動(dòng)作影響小，必要時(shí)通過邏輯判斷限制或禁止本次輸出，不會(huì)嚴(yán)重影響系統(tǒng)的工作。而位置算法的輸出是控制量的全量輸出，誤動(dòng)作影響大。增量型PID的程序?qū)崿F(xiàn)，如如圖25。關(guān)于PID參數(shù)的整定，將放在后面的文章進(jìn)行詳細(xì)講解。
差動(dòng)兩輪底盤輪式里程計(jì)：
輪式里程計(jì)是機(jī)器人底盤的重要組成部分，采用航跡推演算法對(duì)機(jī)器人的位姿進(jìn)行估計(jì)，并對(duì)機(jī)器人當(dāng)前的速度、旋轉(zhuǎn)速度、左右輪速度進(jìn)行轉(zhuǎn)換。無論是機(jī)器人的定位導(dǎo)航還是普通的運(yùn)動(dòng)控制，都需要輪式里程計(jì)。
如圖26，為通過航跡推演計(jì)算里程計(jì)的過程。隨時(shí)間推移機(jī)器人底盤的實(shí)時(shí)位姿p1、p2、p3、...、pn連接起來就形成了機(jī)器人的航跡，考慮很短的時(shí)間內(nèi)兩相鄰機(jī)器人位姿p1和p2，在已知機(jī)器人位姿p1和機(jī)器人當(dāng)前左右輪速度vl、vr的條件下，利用微積分的思想可以推算出機(jī)器人在下一個(gè)時(shí)刻的位姿p2，通過這樣不斷的推演，就可以計(jì)算出機(jī)器人當(dāng)前的位姿以及速度、角速度等信息，這就是所謂的航跡推演。關(guān)于航跡推演的具體數(shù)學(xué)推導(dǎo)和程序?qū)崿F(xiàn)，將在后面的文章中進(jìn)行展開講解。
底盤電機(jī)控制板軟件框架：
如圖27，為stm32電機(jī)控制板軟件架構(gòu)。底盤中的電機(jī)控制與里程數(shù)據(jù)采集的程序在stm32單片機(jī)上實(shí)現(xiàn)，TIM1定時(shí)器產(chǎn)生周期性的循環(huán)，循環(huán)中進(jìn)行電機(jī)編碼器數(shù)據(jù)采集、PID計(jì)算、電機(jī)速度pwm控制，剩下的就是usart1串口與usart2串口跟機(jī)器人大腦之間的通信了，底盤debug接口是用于stm32程序開發(fā)階段使用的，所以在機(jī)器人正常運(yùn)行的過程中只需要使用底盤控制接口。關(guān)于stm32部分的代碼和對(duì)應(yīng)機(jī)器人大腦中ROS驅(qū)動(dòng)代碼將在后面介紹。
3.3.在機(jī)器人中使用底盤
通過前面的講解，我們已經(jīng)對(duì)機(jī)器人底盤的用途及工作原理有了一定的了解，并且知道了電機(jī)控制和里程計(jì)的工作過程。這時(shí)候肯定很想知道如何在機(jī)器人中把底盤使用起來呢？其實(shí)很簡單，和激光雷達(dá)、IMU這些傳感器一樣，底盤也可以當(dāng)做一個(gè)傳感器來使用，只不過不同之處是這個(gè)傳感器與機(jī)器人大腦是雙向交互的，機(jī)器人大腦向底盤發(fā)送控制命令，底盤反饋里程數(shù)據(jù)給機(jī)器人大腦。但是，不論交互的細(xì)節(jié)如何，只需要裝上底盤的ROS驅(qū)動(dòng)包，上層算法只需要發(fā)布和訂閱相應(yīng)的主題就能達(dá)到使用底盤的目的。
底盤通過串口與機(jī)器人相連接，機(jī)器人中通過運(yùn)行底盤控制ROS驅(qū)動(dòng)，來實(shí)現(xiàn)讀取串口的速度反饋，利用航跡推演算法計(jì)算得到里程計(jì)并發(fā)布到/odom這個(gè)主題；底盤控制ROS驅(qū)動(dòng)訂閱/cmd_vel主題的運(yùn)動(dòng)控制數(shù)據(jù)，并轉(zhuǎn)換為速度控制指令通過串口發(fā)送給底盤。這樣機(jī)器人上的其它節(jié)點(diǎn)就可以通過發(fā)布/cmd_vel主題來對(duì)底盤進(jìn)行控制，通過訂閱/odom主題獲取底盤的里程計(jì)。關(guān)于底盤控制ROS驅(qū)動(dòng)、底盤里程計(jì)標(biāo)定、底盤的debug，將在后面的章節(jié)中具體講解。
4.音響麥克風(fēng)與攝像頭
要讓機(jī)器人能跟人進(jìn)行對(duì)話，需要麥克風(fēng)和音響。其實(shí)麥克風(fēng)就相當(dāng)于機(jī)器人的耳朵，音響就相當(dāng)于機(jī)器人的嘴巴。攝像頭作為機(jī)器人的眼睛，攝像頭可以用來幫助機(jī)器人定位或認(rèn)識(shí)環(huán)境。
4.1.音響
如圖29，為音響的基本配件。這里需要說明一下，由于后面使用樹莓派3作為機(jī)器人的大腦，不過樹莓派3的自帶聲卡不好用，驅(qū)動(dòng)容易崩潰，所以這里推薦使用免驅(qū)的USB聲卡。其實(shí)，聲卡就是DA轉(zhuǎn)換器，就是將數(shù)字音頻信號(hào)轉(zhuǎn)換為模擬電壓信號(hào)；音響就是將聲卡輸出的模擬電壓進(jìn)行放大并通過喇叭播放出來。
4.2.麥克風(fēng)與攝像頭
如圖30，攝像頭上直接集成了麥克風(fēng)。這里選用的是四麥陣列指向性麥克風(fēng)，可以對(duì)特定方向上的聲音拾取，并過濾其他方向上的雜音。這里的攝像頭是640x480像素60fps。
4.3.在機(jī)器人中使用音響麥克風(fēng)與攝像頭
我們只需要在機(jī)器人上安裝對(duì)應(yīng)的ROS驅(qū)動(dòng)，就可以通過發(fā)布和訂閱相應(yīng)的主題來實(shí)現(xiàn)對(duì)傳感器的訪問了。如圖31，iat語音識(shí)別節(jié)點(diǎn)用于驅(qū)動(dòng)麥克風(fēng)，并將麥克風(fēng)采集的語音轉(zhuǎn)換為文字；問答（QA）和自然語言處理（NLP）節(jié)點(diǎn)處理iat語音識(shí)別節(jié)點(diǎn)發(fā)布的文字，并將處理結(jié)果發(fā)布給tts語音合成節(jié)點(diǎn)；tts語音合成節(jié)點(diǎn)訂閱問答（QA）和自然語言處理（NLP）節(jié)點(diǎn)發(fā)布的文字，并驅(qū)動(dòng)聲卡將文字轉(zhuǎn)換為語音；攝像頭ROS驅(qū)動(dòng)將攝像頭數(shù)據(jù)直接發(fā)布到相應(yīng)的話題。這樣機(jī)器人上的其他節(jié)點(diǎn)都可以通過訂閱與發(fā)布相應(yīng)的節(jié)點(diǎn)來訪問麥克風(fēng)、聲卡和攝像頭。關(guān)于圖像、語音、文字等處理的具體應(yīng)用將在后面詳細(xì)展開。
5.機(jī)器人大腦嵌入式主板性能對(duì)比
從事SLAM與機(jī)器人導(dǎo)航也有幾年時(shí)間了，期間用過不少的嵌入式主板做開發(fā)。機(jī)器人是軟硬件結(jié)合的一個(gè)實(shí)體，這里就對(duì)機(jī)器人的大腦（嵌入式主板）進(jìn)行一些討論。結(jié)合我用過的一些嵌入式開發(fā)板，展開對(duì)比分析，具體型號(hào)如圖32。
5.1.樹莓派3
樹莓派一直很火，現(xiàn)在已經(jīng)推出第三代了。這里放一張樹莓派0、樹莓派2、樹莓派3的全家福吧，如圖33。
接下來看看具體的性能參數(shù)，如圖34。如果想了解更多關(guān)于樹莓派的資料，可以閱讀樹莓派的wiki教程https://en.wikipedia.org/wiki/Raspberry_Pi。
樹莓派3，售價(jià)200RMB左右，CPU是1.2Ghz ARM-Cortex-A53，內(nèi)存1GB，板載wifi模塊，還有一個(gè)多媒體顯示GPU（不過感覺沒什么用）。CPU和內(nèi)存配置算的的上是同等價(jià)位嵌入式主板的戰(zhàn)斗機(jī)了，板載wifi這個(gè)也很實(shí)用。雖然樹莓派支持安裝很多種linux系統(tǒng)，由于我這里要跑ROS機(jī)器人系統(tǒng)，所以我選擇了安裝ubuntu-mate-16.04, ubuntu-mate-16.04 LTS實(shí)際上是ubuntu-16.04 LTS的一部分，為桌面、Raspberry Pi 2和3單片機(jī)準(zhǔn)備的，這也是Ubuntu MATE的首個(gè)LTS長期支持版，亮點(diǎn)在于包含MATE 1.12.1桌面環(huán)境，針對(duì)平板支持多點(diǎn)觸控和“自然滾動(dòng)”，對(duì)多屏幕設(shè)置提供更好的支持，更好的會(huì)話管理，擴(kuò)展的systemd支持，改進(jìn)過的Power小程序——可顯示產(chǎn)品型號(hào)和提供商信息等內(nèi)容。
安裝好ubuntu-mate-16.04操作系統(tǒng)后，上電可以看到圖35所示的系統(tǒng)界面，就可以安裝kinetic版本的ROS了，然后就可以驗(yàn)證SLAM算法了。Gmapping激光SLAM建圖和ros-navigation自動(dòng)導(dǎo)航跑的都很順暢；google-cartographer進(jìn)行建圖和重定位也沒什么問題；跑ORB-SLAM2的Mono模式就不太行了，幀率5幀以內(nèi)。
5.2.Firefly-RK3399
螢火蟲開發(fā)板還是很強(qiáng)大的板子分RK3288和RK3399兩個(gè)版本，先看一下官方的宣傳廣告。
如圖36所示，RK3399雙核CortexA72+四核CortexA53的CPU和2GB/4GB可選配的內(nèi)存，一看這些配置就知道很強(qiáng)大，的確跑各種視覺算法很不錯(cuò)，不過1000RMB的售價(jià)感覺有點(diǎn)小貴。
5.3.Nvidia-jetson-TK1
Tegra K1是Nvidia推出的一款A(yù)I級(jí)別的嵌入式主板，ARM-Cortex-A15的CPU，192個(gè)CUDA核心的kepler架構(gòu)的GPU，2GB內(nèi)存，如圖37。有人用Tegra K1做了一個(gè)計(jì)算集群，感覺還挺有意思的，如圖38。
5.4.Nvidia-jetson-TX2
Jetson TX2的是可以作為核武器的處理器的（@~@），性能是十分強(qiáng)大的。簡單的智能小車或者機(jī)器人不推薦使用TX2，性價(jià)比比較低。利用TX2做處理器，控制移動(dòng)平臺(tái)（高精度的小車底盤）做SLAM我覺得是一個(gè)相當(dāng)有意思的項(xiàng)目，TX2的處理能力非常適合實(shí)現(xiàn)機(jī)器視覺。
圖39中左邊是官方的開發(fā)板和擴(kuò)展板，不過由于官方擴(kuò)展板體積太大了用在很多地方不方便，于是網(wǎng)上推出了一款小巧的擴(kuò)展板Connect Tech Inc很不錯(cuò)。
這個(gè)是性能表，看上去也是叼叼的，不過售價(jià)不便宜接近5000RMB。。。
5.5.Intel-NUC
前面介紹的都是ARM架構(gòu)的主板，現(xiàn)在介紹一款X86架構(gòu)的主板NUC。
之前用過Intel-NUC7-i7,19V供電65W功耗感覺不適合嵌入式級(jí)別的應(yīng)用場合，而且3000RMB的售價(jià)也沒法和Nvidia-jetson-TX2比較性價(jià)比，所以不推薦在機(jī)器人上使用。
5.6.Intel-Edison
其實(shí)這是intel一個(gè)失敗的嘗試，主打物聯(lián)網(wǎng)應(yīng)用，CPU采用intel的Atom處理器，最大的亮點(diǎn)是可以在主板上直接擴(kuò)展Ardunio單片機(jī)開發(fā)板。如圖42。
其實(shí)個(gè)人不推薦用Edison來開發(fā)SLAM算法，不過也有公司做這方面的嘗試，比如上海思嵐科技的SLAMWARE-CORE就是Edison的模仿者。
5.7.Google-Tango-phone
其實(shí)Tango-phone是一個(gè)完整的AR方案，手機(jī)內(nèi)集成了深度相機(jī)和VO視覺里程計(jì)。
看網(wǎng)上的演示視頻也是十分的炫酷，不過最終這個(gè)項(xiàng)目還是沒有在google中火起來，可能還是存在不少問題的吧。
5.8.總結(jié)
最后，總結(jié)一些各個(gè)開發(fā)板的性能對(duì)比，見圖45。玩機(jī)器人和SLAM的朋友們，如果是中低端需求推薦樹莓派3，高端需求推薦jetson-TX2。
6.做一個(gè)能走路和對(duì)話的機(jī)器人
在我的想象中機(jī)器人首先應(yīng)該能自由的走來走去，然后應(yīng)該能流利的與主人對(duì)話。朝著這個(gè)理想，我準(zhǔn)備設(shè)計(jì)一個(gè)能自由行走，并且可以與人語音對(duì)話的機(jī)器人。為了更形象的表達(dá)我的想法，我的小學(xué)五年級(jí)的繪畫水平也是豁出去了，如果有大神路過，懇請(qǐng)多多包涵，畫好后的樣子大概就是這樣啦，如圖46。
有了想法，接下來就要開始親自動(dòng)手DIY了。體驗(yàn)動(dòng)手樂趣的同時(shí)，以玩耍的形式融入當(dāng)下前沿的SLAM、自動(dòng)導(dǎo)航、圖像識(shí)別、語音識(shí)別、自然語言處理等技術(shù)，提高自己的同時(shí)找到屬于兒時(shí)的那一份快了。
說干就干，首先需要準(zhǔn)備好激光雷達(dá)、IMU、底盤、音響、麥克風(fēng)、攝像頭這些傳感器，然后還要準(zhǔn)備一塊樹莓派3開發(fā)板作為機(jī)器人的大腦。其實(shí)這些知識(shí)已經(jīng)通過前面的文章打好了基礎(chǔ)，所以就好辦多了，準(zhǔn)備好這些東西就好了，如圖47。
準(zhǔn)備好傳感器和樹莓派3后，就可以進(jìn)行組裝了。經(jīng)過緊張有趣的組裝后，一臺(tái)有趣的機(jī)器人就被DIY出來了，這時(shí)候終于可以看到傳說中的機(jī)器人的真容了，如圖48，雖然機(jī)器人的真實(shí)樣子沒有想象中的那么有藝術(shù)感，但科技感十足有木有@^@
麻雀雖小五臟俱全，現(xiàn)在就來對(duì)這個(gè)會(huì)說話會(huì)走路的機(jī)器人做一做剖析吧。機(jī)器人的骨架是由亞克力板和銅柱組合而成；兩個(gè)帶編碼器的減速電機(jī)和一個(gè)萬向輪作為運(yùn)動(dòng)執(zhí)行機(jī)構(gòu)；可充電鋰電池給整個(gè)機(jī)器人供電；stm32電機(jī)控制板用于控制電機(jī)運(yùn)動(dòng)并提供里程數(shù)據(jù)，是底盤的核心部件；激光雷達(dá)提供環(huán)境障礙信息，用于SLAM建圖和避障導(dǎo)航；IMU用于在里程計(jì)數(shù)據(jù)融合、SLAM建圖、導(dǎo)航中提供慣導(dǎo)數(shù)據(jù)；免驅(qū)USB聲卡、音響、四麥陣列指向性麥克風(fēng)作為語音交互過程中的輸入輸出部件；攝像頭用于物體識(shí)別、物體跟蹤、環(huán)境監(jiān)控、視覺輔助定位；樹莓派3是整個(gè)機(jī)器人的中央處理單元，各個(gè)傳感器的ROS驅(qū)動(dòng)、SLAM算法、導(dǎo)航算法、語音交互、自然語言處理算法、圖像處理算法都將運(yùn)行在上面。為了更好的理解機(jī)器人的工作原理，這里給出機(jī)器人的硬件框架，如圖49。
機(jī)器人的硬件搭建完畢后，就要賦予機(jī)器人靈魂了。這里說的靈魂就是我們的軟件及算法，包括各個(gè)傳感器的ROS驅(qū)動(dòng)、輪式里程計(jì)與IMU融合、激光SLAM建圖、自主導(dǎo)航避障、語音識(shí)別、語音合成、自然語言處理、物體識(shí)別、物體跟蹤、遠(yuǎn)程視頻監(jiān)控、視覺輔助定位、機(jī)器人與工作PC、Android手機(jī)之間的通信等。為了更好的理解機(jī)器人的工作原理，這里給出機(jī)器人的軟件框架，如圖50。
做了如此長的鋪墊，大家肯定已經(jīng)等不及要親自動(dòng)手去開發(fā)機(jī)器人上的軟件代碼了。別急，接下來的章節(jié)將跟你娓娓道來，讓你體驗(yàn)動(dòng)手寫程序真正的快樂$^$
后記
為了防止后續(xù)大家找不到本篇文章，我同步制作了一份文章的pdf和本專欄涉及的例程代碼放在github和gitee方便大家下載，如果下面給出的github下載鏈接打不開，可以嘗試gitee下載鏈接：
github下載鏈接：https://github.com/xiihoo/DIY_A_SLAM_Navigation_Robot
gitee下載鏈接：https://gitee.com/xiihoo-robot/DIY_A_SLAM_Navigation_Robot
技術(shù)交流
QQ技術(shù)交流群：117698356
參考文獻(xiàn)
[1] 張虎，機(jī)器人SLAM導(dǎo)航核心技術(shù)與實(shí)戰(zhàn)[M]. 機(jī)械工業(yè)出版社，2022.
