導(dǎo)引與控制是AGV小車的關(guān)鍵部分。AGV小車依靠導(dǎo)引可以沿一定路線自動(dòng)行駛。根據(jù)導(dǎo)航方式不同，可以分為直接坐標(biāo)導(dǎo)引、電磁導(dǎo)引、磁帶導(dǎo)引、光學(xué)導(dǎo)引、激光導(dǎo)引、慣性導(dǎo)航、圖象識(shí)別導(dǎo)引、GPS（全球定位系統(tǒng)）導(dǎo)航等，通過導(dǎo)航方式AGV確定自身位置，按照預(yù)先規(guī)定的路徑行走。

其中慣性導(dǎo)航是在AGV小車上安裝陀螺儀，在行駛區(qū)域的地面上安裝定位塊，AGV可通過對(duì)陀螺儀偏差信號(hào)的計(jì)算及地面定位塊信號(hào)的采集來確定自身的位置和方向，從而實(shí)現(xiàn)導(dǎo)引。此項(xiàng)技術(shù)在軍方運(yùn)用較早，其主要優(yōu)點(diǎn)是技術(shù)先進(jìn)，定位準(zhǔn)確性高，靈活性強(qiáng)，便于組合和兼容，適用領(lǐng)域廣，已被國(guó)外的許多AGV生產(chǎn)廠家采用。其缺點(diǎn)是制造成本較高，導(dǎo)引的精度和可靠性與陀螺儀的制造精度及使用壽命密切相關(guān)。

而目前比較研究比較多且較先進(jìn)的激光導(dǎo)引是采用SLAM機(jī)器人定位技術(shù)（實(shí)時(shí)定位與地圖生成）實(shí)現(xiàn)AGV的路徑地圖。簡(jiǎn)單來講當(dāng)AGV機(jī)器人在一個(gè)陌生環(huán)境走一圈即自動(dòng)掃描并生成該場(chǎng)景電子地圖，該電子地圖是AGV的激光器每秒幾萬個(gè)坐標(biāo)點(diǎn)拼接生成，然后在已知地圖任意創(chuàng)建工作站點(diǎn)即屬性，AGV在行駛運(yùn)動(dòng)過程中實(shí)時(shí)與已知的基準(zhǔn)地圖坐標(biāo)進(jìn)行匹配，從而實(shí)現(xiàn)AGV高精度地位。部分廠家的激光AGV通過多種傳感器融合優(yōu)化算法其重復(fù)運(yùn)行精度達(dá)到5毫米，并且環(huán)境動(dòng)態(tài)移動(dòng)的人和物對(duì)其沒有任何影響。

激光牽引AGV.jpg

激光導(dǎo)引AGV

1.0 IMU指的是慣性測(cè)量單元。

IMU大多用在需要進(jìn)行運(yùn)動(dòng)控制的設(shè)備，如汽車和機(jī)器人上。也被用在需要用姿態(tài)進(jìn)行精密位移推算的場(chǎng)合，如潛艇、飛機(jī)、導(dǎo)彈和航天器的慣性導(dǎo)航設(shè)備等。

慣性測(cè)量單元是測(cè)量物體三軸姿態(tài)角(或角速率)以及加速度的裝置。一般的，一個(gè)IMU包含了三個(gè)單軸的加速度計(jì)和三個(gè)單軸的陀螺，加速度計(jì)檢測(cè)物體在載體坐標(biāo)系統(tǒng)獨(dú)立三軸的加速度信號(hào)，而陀螺檢測(cè)載體相對(duì)于導(dǎo)航坐標(biāo)系的角速度信號(hào)，測(cè)量物體在三維空間中的角速度和加速度，并以此解算出物體的姿態(tài)。

1.1 IMU的優(yōu)點(diǎn)

IMU 有輸出頻率高、能輸出6DoF自由度測(cè)量信息等優(yōu)點(diǎn) 。視覺 SLAM 存在輸出頻率低、旋轉(zhuǎn)運(yùn)動(dòng)時(shí)、或運(yùn)動(dòng)速率加快時(shí)定位易失敗等問題，

1.2 IMU的不足

IMU的數(shù)據(jù)必須經(jīng)過標(biāo)定，至少要將零偏等數(shù)據(jù)計(jì)算出來；

IMU的零偏包括固定零偏（受到應(yīng)力和其他因素的影響）和啟動(dòng)零偏（每次啟動(dòng)不一樣，啟動(dòng)之后就固定不變）；

標(biāo)定一般標(biāo)定的是固定零偏和啟動(dòng)零偏，班寧傳感出品的IMU在算法上進(jìn)行了優(yōu)化，目前已經(jīng)做到了很高的精度，完全滿足了AGV的應(yīng)用場(chǎng)景。同時(shí)班寧的IMU在-40℃-85℃范圍內(nèi)進(jìn)行校準(zhǔn)，滿足全溫區(qū)的使用。

1.3 用IMU定位難點(diǎn)分析

IMU定位原理

對(duì)于acc+gyr方式：acc+gyr得到比較準(zhǔn)確姿態(tài)，去除acc重力分量影響；二次積分可以得到位置；

對(duì)于acc+gyr+mag方式：需要acc+gyr+mag方式得到比較準(zhǔn)確姿態(tài)，其他處理方式一樣；

難點(diǎn)分析

IMU定位不準(zhǔn)，主要由于3點(diǎn)造成：

a. 姿態(tài)不準(zhǔn)導(dǎo)致重力影響去除不了，誤差不斷累積；

b. 器件本身漂移不能去除，如acc從靜止到運(yùn)動(dòng)再到靜止，acc積分得到的速度應(yīng)該為0，而實(shí)際不為0；

c. 通過筆者的測(cè)試，班寧的BNS系列IMU，在本身漂移方面有著優(yōu)異的表現(xiàn)；

現(xiàn)對(duì)上述3點(diǎn)進(jìn)行說明：

重力影響：可以采用一定的方法(如MahonyAHRS、MadgwickAHRS、Kalman，具體可參見xio)：對(duì)于acc+gyr方式，acc只能在靜止時(shí)得到姿態(tài)，運(yùn)動(dòng)時(shí)不能，因而在運(yùn)動(dòng)情況下姿態(tài)不準(zhǔn)確，重力影響去除不了；對(duì)于acc+gyr+mag，可以用mag得到姿態(tài)，提高姿態(tài)準(zhǔn)確性，當(dāng)有磁場(chǎng)干擾時(shí)，該方式效果也存在問題；

本身漂移：對(duì)于此問題，一般的處理方法為用高通濾波去除，但高通濾波后得到的只是相對(duì)運(yùn)動(dòng)關(guān)系，不能得到準(zhǔn)確的位置信息；因而在實(shí)際應(yīng)用中可以根據(jù)實(shí)際情況設(shè)計(jì)相應(yīng)的處理方法，如步態(tài)定位中，可以采用運(yùn)動(dòng)靜止判斷，如下圖所示為步態(tài)數(shù)據(jù)運(yùn)動(dòng)靜止判斷矯正后的效果：

靜止與運(yùn)動(dòng).png

所以在應(yīng)用中，IMU定位存在不可避免的漂移，需針對(duì)具體應(yīng)用設(shè)計(jì)相應(yīng)的處理算法。

由于大多數(shù)AGV在使用IMU的過程中，都是希望直接得到相對(duì)航向角Yaw,俯仰角Pitch,橫滾角Roll，還有加速度的信息，班寧的IMU可以高精度的輸出這些信息。

1.4 IMU實(shí)際數(shù)據(jù)

以班寧的BNS系列IMU為例，提供6DoF自由度的姿態(tài)信息。

IMU采用符合CAN輸出特性，CAN總線數(shù)據(jù)速率250KHz，信號(hào)更新頻率200Hz。同時(shí)支持485和SPI輸出。消息格式報(bào)告數(shù)據(jù)，以16進(jìn)制輸出結(jié)果表示。

圖片.png

以ID為F02D的加速度三軸方向的數(shù)據(jù)值及算法如下：

圖片.png

IMU在靜止?fàn)顟B(tài)下的部分連續(xù)數(shù)據(jù)記錄如下：

圖片.png

可以明顯的看到低位值BL的數(shù)據(jù)存在變動(dòng)，這就是即使在靜止?fàn)顟B(tài)下也會(huì)有的零偏。這就需要進(jìn)行標(biāo)定固定零偏以及通過算法估計(jì)啟動(dòng)零偏的殘差值和溫漂。

未標(biāo)定的SLAM建圖效果對(duì)比如下：

圖片.png

無IMU地圖有IMU地圖（未標(biāo)定）

SLAM地圖的好壞有很多因素的綜合疊加影響，譬如建圖的順序，先閉環(huán)后細(xì)節(jié)，激光的距離，移動(dòng)物體的影響等，所以筆者認(rèn)為結(jié)合激光和IMU各自的優(yōu)點(diǎn)，取長(zhǎng)補(bǔ)短融合傳感建圖，并在AGV行進(jìn)中定位，利用IMU的特點(diǎn)幫助在高速運(yùn)動(dòng)和旋轉(zhuǎn)中與地圖匹配定位，并且能夠進(jìn)行多自由度的分析，及姿態(tài)的調(diào)整和極限位置保護(hù)。

1.5 視覺傳感器和IMU傳感器融合

同SLAM發(fā)展過程類似，視覺融合IMU問題也可以分成基于濾波和基于優(yōu)化兩大類。

同時(shí)按照是否把圖像特征信息加入狀態(tài)向量來進(jìn)行分類，可以分為松耦合和緊耦合兩大類。

松耦合將視覺傳感器和 IMU 作為兩個(gè)單獨(dú)的模塊，兩個(gè)模塊均可以計(jì)算得到位姿信息，然后一般通過 EKF 進(jìn)行融合。

圖片.png

緊耦合則是指將視覺和 IMU 得到的中間數(shù)據(jù)通過一個(gè)優(yōu)化濾波器進(jìn)行處理，緊耦合需要把圖像特征加入到特征向量中，最終得到位姿信息的過程。由于這個(gè)原因，系統(tǒng)狀態(tài)向量最終的維度也會(huì)非常高，同時(shí)計(jì)算量也很大。

圖片.png

總體來看，松耦合算法相對(duì)簡(jiǎn)單，且擴(kuò)展性很強(qiáng)，可以加入多種傳感器信息進(jìn)行融合，但融合得到的位姿估計(jì)精度相對(duì)較差；緊耦合算法實(shí)現(xiàn)起來相對(duì)復(fù)雜，可擴(kuò)展性差，但融合得到的位姿估計(jì)精度較高。

總之，IMU和激光傳感器的融合算法是核心，初期建圖的策略及行進(jìn)中的算法誤差補(bǔ)償也是至關(guān)重要的。

附一下筆者實(shí)測(cè)班寧的IMU部分?jǐn)?shù)據(jù)：