胡天林，陳志偉，趙德博，卞嘉煒，歐陽童潔

（廈門大學(xué)a.航空航天學(xué)院；b.現(xiàn)代教育技術(shù)與實踐訓(xùn)練中心，福建廈門 361002）

0 引言

自定位是提高室內(nèi)輪式機器人自主能力的關(guān)鍵問題之一［1］。移動機器人在室內(nèi)的定位方式主要有視覺導(dǎo)航定位、光反射定位、超聲波反射測距定位和慣性系統(tǒng)定位等［2］。在復(fù)雜的室內(nèi)環(huán)境中，定位方式若完全依賴所處環(huán)境，一方面，用于傳感器的成本較高；另一方面，實現(xiàn)方法也較為復(fù)雜。

由于組成慣性定位系統(tǒng)的設(shè)備安裝在機器人本體內(nèi)，對外界環(huán)境的依賴度低，也不向外輻射能量，不易受到干擾，所以在室內(nèi)環(huán)境中慣性定位系統(tǒng)是常用的技術(shù)方案。其缺點是定位誤差會隨時間逐漸增大［3］，在實際的場景中常與其他方案相互補充。目前基于編碼器與慣性測量單元（Inertial Measurement Unit，IMU）的定位系統(tǒng)應(yīng)用廣泛。張馳洲［4］利用編碼器和陀螺儀等慣性定位傳感器，提出了一種生成實時地圖的掃地機器人全局路徑規(guī)劃方法；張斌等［5］基于里程計和光纖陀螺儀對無人工作面采煤機的自主定位進行了研究與實驗；張磊等［6］研究了MEMS 陀螺儀與光電編碼器組合的慣性定位，運用回歸決策函數(shù)補充微機械陀螺儀輸出誤差；龔趙慧等［7］設(shè)計了一種融合IMU的半直接單目視覺里程計，通過IMU信息彌補視覺里程計的缺陷，由此提高跟蹤精度與系統(tǒng)魯棒性。

基于正交編碼器與IMU的傳統(tǒng)定位方案，在模型上忽略了編碼器里程數(shù)據(jù)中的轉(zhuǎn)動分量，模型誤差較大。尤其是當機器人平動兼轉(zhuǎn)動時，速度越高，其誤差累積的速度更快。針對該問題，本文提出一種新的基于正交編碼器與IMU的機器人室內(nèi)定位系統(tǒng)。

1 機械方案

定位系統(tǒng)的機械結(jié)構(gòu)如圖1 所示，主要由從動輪（裝配有編碼器）、懸掛機構(gòu)和支撐結(jié)構(gòu)等組成。支撐結(jié)構(gòu)可靈活固定在機器人底盤上，通過懸掛連接從動輪。IMU和電路板安裝在支撐結(jié)構(gòu)的下方。

從動輪不提供動力，主要跟隨機器人移動。兩個從動輪正交擺放，且與編碼器碼盤同軸連接。移動時，編碼器用于記錄里程信息。為保證編碼器里程數(shù)據(jù)準確可靠，從動輪應(yīng)始終與地面緊密接觸，不能打滑或卡頓。全向輪可沿各個方向移動且不易堵轉(zhuǎn)，選用作為從動輪。由于定位系統(tǒng)在工作時需緊貼地面，系統(tǒng)必須具備有懸掛功能，懸掛機構(gòu)由滑軌和鉤簧組成。鉤簧在滑軌背面，連接滑軌頂部和支撐結(jié)構(gòu)。定位系統(tǒng)安裝在機器人底盤上，運行過程中鉤簧下拉，保持拉伸狀態(tài)，保證輪子始終貼地。

2 硬件方案

2.1 IMU

IMU用于測量物體多軸姿態(tài)角或加速度。陀螺儀和加速計是IMU的主要元件，通過測量物體運動的角速度、加速度等，從而獲得物體的俯仰角、偏航角和滾轉(zhuǎn)角，以判別物體的運動姿態(tài)及控制運動軌跡。IMU在航空航天領(lǐng)域應(yīng)用廣泛［8］。

系統(tǒng)采用的IMU 是九軸姿態(tài)角度傳感器JY901，該模塊集成了高精度的陀螺儀、加速計、地磁場傳感器，自帶有卡爾曼濾波算法，能夠快速解出模塊當前的實時運動姿態(tài)。其測量角速度可達2 000 °／s，加速度穩(wěn)定性為0.01g，角速度穩(wěn)定性為0.05°／s，數(shù)據(jù)輸出頻率為0.1～200 Hz。通信方式主要為串口通信，波特率一般設(shè)定為115.20 Kbit／s。

主要使用JY901 來測量機器人的偏航角。

2.2 編碼器

系統(tǒng)采用光電編碼器測量里程數(shù)據(jù)，其原理是通過光電轉(zhuǎn)換將輸出軸上的機械幾何位移量轉(zhuǎn)換成數(shù)字信號。光電編碼器由光碼盤和光電檢測裝置組成，光碼盤上等分地開通有若干個長方形孔，當從動輪旋轉(zhuǎn)時，檢測裝置檢測并輸出若干脈沖信號。為了判斷轉(zhuǎn)向，一般輸出兩組存在一定相位差的方波信號，分別稱為A相、B 相，如圖2 所示。在A 相上升沿或下降沿時刻，通過判斷B相的狀態(tài)來判斷從動輪是正轉(zhuǎn)還是反轉(zhuǎn)，并記錄其轉(zhuǎn)過的脈沖數(shù)［9］。系統(tǒng)使用的編碼器線數(shù)為1 024，在信號采集過程中采用4 倍頻的方法。

圖2 A、B相時序與正反轉(zhuǎn)的對應(yīng)關(guān)系

2.3 STM32F103 核心板

STM32F103 的電路板作為系統(tǒng)的核心，負責采集并融合IMU和編碼器的數(shù)據(jù)，通過算法解算出機器人當前坐標位置，最后將坐標封裝成數(shù)據(jù)包發(fā)送給機器人主控［10］。核心板需要與傳感器和機器人主控通信。IMU JY901 需要1 個串口通道，數(shù)據(jù)輸出接口需要1個串口通道，2 組編碼器則需要2 個定時器通道。該系統(tǒng)信號流向如圖3 所示，單片機STM32F103 接受IMU的偏航角和編碼器的脈沖值，數(shù)據(jù)融合后將坐標解算結(jié)果發(fā)給機器人主控。

圖3 定位系統(tǒng)信號流圖

3 軟件方案

3.1 定位模型構(gòu)建

簡化后定位系統(tǒng)模型如圖4 所示。黑線為兩臂，交點為定位中心，編碼器和從動輪簡化為紅色箭頭，其箭頭指向為編碼器正方向。兩個編碼器分別記為A、B編碼器。假設(shè)定位系統(tǒng)中心到兩輪的臂長分別為Ra、Rb。

圖4 定位系統(tǒng)的簡化模型

定位系統(tǒng)一般情況下的運動狀態(tài)如圖5 所示。G1為定位系統(tǒng)上一時刻的狀態(tài)；G2為此時的狀態(tài)。設(shè)定位系統(tǒng)在G1時刻的偏航角為β；編碼器的正方向沿順時針與世界坐標系X軸的夾角為編碼器的正方向角；運動軌跡方向沿順時針與世界坐標系X軸的夾角為運動軌跡的正方向角［11］。

圖5 定位系統(tǒng)狀態(tài)變化示意

G1狀態(tài)表示為

G1狀態(tài)時，A編碼器的坐標及其正方向角為

同理B編碼器的坐標及其正方向角為

假設(shè)由G1到G2時刻，定位系統(tǒng)偏航角轉(zhuǎn)動了Δθ，定位中心的坐標改變了ΔX、ΔY，可以得到G2時的狀態(tài)為

設(shè)A編碼器在X方向上的坐標增量記為ΔXA，在Y方向的坐標增量記為ΔYA。同理，B 編碼器在X方向上的坐標增量為ΔXB，在Y方向的坐標增量記為ΔYB。由式（2）、（3）、（5）、（6）可得：

3.2 傳統(tǒng)定位模型

傳統(tǒng)模型如圖6 所示，定位系統(tǒng)由G1狀態(tài)變化至G2狀態(tài)時，假設(shè)定位系統(tǒng)先以Δθ角度轉(zhuǎn)動，然后平動至G2位置（或者先平動或轉(zhuǎn)動）。這種模型的特點是默認編碼器記錄的里程值不包含轉(zhuǎn)動分量，只有平動分量。

圖6 傳統(tǒng)模型

設(shè)編碼器記錄的里程分別為La、Lb。定位系統(tǒng)中心的坐標增量：

傳統(tǒng)模型將旋轉(zhuǎn)運動和平移運動分開，算法簡單。這只適應(yīng)機器人在低速運動場景，若機器人進行旋轉(zhuǎn)兼平移運動，隨著速度的提高，編碼器記錄的里程數(shù)據(jù)中的轉(zhuǎn)動分量也會隨之增加。由于機器人在室內(nèi)運行，假定大于1 m／s左右的速度即為高速。

3.3 改進模型

若采樣時間很短，可將編碼器的移動軌跡近似成直線，如圖7 所示?？傻玫紸 編碼器位移的絕對值L1為

圖7 改進模型

編碼器記錄的里程是其位移在編碼器正方向上投影的長度［12］。對該移動過程按位移等分成N份，并假設(shè)機器人的偏航角相對于編碼器的位移是均勻變化的。以A編碼器為例進行分析。

將位移增量和角度增量N等分：

運動過程中A編碼器的正方向角

由幾何關(guān)系可以得到軌跡的正方向角

A編碼器的里程La的表達式為：

由式（9）～（12），可將式（13）改寫為

同理，B編碼器的里程表達式為

簡化參數(shù)，ka、kb、ca、cb定義如下：

最后將式（7）中ΔXa、ΔYa、ΔXb、ΔYb代入式（14）、（15）后整理得定位系統(tǒng)中心的坐標增量表達式為：

在模型中適當增大分割數(shù)N，可以提高該算法的精度。令分割數(shù)N趨于無窮，則ka、kb、ca、cb化簡為：

化簡后的坐標增量為：

模型將采樣間隔內(nèi)編碼器的運動軌跡近似為直線，因此也具有一定的模型誤差，在實際應(yīng)用時可將其結(jié)果與傳統(tǒng)模型得到的結(jié)果做加權(quán)濾波處理。

3.4 仿真結(jié)果

通過運動學(xué)仿真對改進后定位算法的性能進行驗證［13-16］。方法是，讓機器人旋轉(zhuǎn)兼平動運行10 s，得到理想曲線，兩種算法得到的預(yù)測曲線及預(yù)測誤差。設(shè)置采樣時間間隔為5 ms，分割數(shù)N＝3；設(shè)置機器人自轉(zhuǎn)速度為72 °／s，在X方向上最大速度為0.4 m／s，在Y方向上的最大速度為1.0 m／s，使其在X方向作余弦運動，Y方向作直線勻速兼正弦運動。得到的仿真結(jié)果如圖8 所示。

圖8 10 s內(nèi)兩種定位算法的仿真結(jié)果

對傳統(tǒng)算法與改進算法進行仿真，得到圖8（a）關(guān)于機器人在二維平面內(nèi)運動的理想坐標曲線、傳統(tǒng)算法及改進算法后的運動軌跡。在每個時刻，定位算法得到的位置與實際位置的距離作為結(jié)果誤差，得到圖8（b）。由圖8 可看出，改進后定位算法的定位效果明顯優(yōu)于傳統(tǒng)的定位算法，前者的誤差積累速度僅是后者的1／3 左右。由模型本身可知，如果使用分割數(shù)N趨于無窮的簡化式，還可以進一步提高該算法的精度。

4 實驗結(jié)果

將定位系統(tǒng)安裝在移動機器人底盤，開展曲線跑點實驗。機器人由原點出發(fā)，測試標準圓或不規(guī)則運動軌跡，然后回到原點。測試過程中的行車速度約為1 m／s。在機器人上安裝有DT35 激光測距模塊，其分辨率為0.1 mm。在場地四周架設(shè)擋板，由DT35 測距后無線傳輸至上位機，經(jīng)數(shù)據(jù)處理可實時獲得機器人的坐標，結(jié)果如圖9 和表1 所示。圖9 中，標準圓圓心坐標為（-1.5，1.0）m，半徑為1.0 m；不規(guī)則曲線為多段三階貝塞爾曲線拼接而成。由圖9 可看出，基于改進算法的定位系統(tǒng)實現(xiàn)的實際軌跡接近參考曲線。

圖9 改進算法定位系統(tǒng)現(xiàn)場測試結(jié)果

表1 設(shè)定與實際軌跡的偏差

在每個時刻，定位系統(tǒng)得到的坐標與激光測距得到的坐標距離作為偏差值，如表1 所示。為觀察整個運行結(jié)束后的誤差積累，在到達終點并穩(wěn)定后，定位系統(tǒng)得到的坐標與激光測距得到的坐標求差，對其取絕對值。由于機器人在運行過程中會存在晃動、激光照射的擋板不平滑、標定誤差等因素，偏差值會比理想情況大。但無論是從最大偏差還是從到達終點后的偏差來看，通過在采樣間隔內(nèi)對軌跡進行N等分、每段近似為若干微小直線的算法，有效降低了系統(tǒng)的誤差積累速度。

5 結(jié)語

本文設(shè)計了一種基于正交編碼器與慣性測量單元的機器人室內(nèi)定位系統(tǒng)。通過融合正交編碼器和慣性測量單元的數(shù)據(jù)，來解算機器人的坐標位置，并在傳統(tǒng)的坐標解算模型上提出了一種新的坐標解算模型。最后將定位系統(tǒng)安裝在移動機器人上，進行曲線實驗測試，驗證了該模型的可行性。

在實際應(yīng)用時，IMU 的陀螺儀漂移對定位系統(tǒng)產(chǎn)生的影響不可忽視。本文用的IMU JY901 除了陀螺儀，還自帶加速度計。IMU JY901 內(nèi)部已通過卡爾曼濾波等方法，對陀螺儀和加速度的數(shù)據(jù)進行融合，以期降低陀螺儀漂移對定位系統(tǒng)的影響。本文討論的重點為基于正交編碼器與慣性測量單元的定位系統(tǒng)的模型設(shè)計，因此在文中的仿真與實驗過程中，未就陀螺儀的誤差影響展開討論。