基于編碼器與慣性測(cè)量單元的定位系統(tǒng)設(shè)計(jì)

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

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

0 引言

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

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

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

1 機(jī)械方案

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

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

2 硬件方案

2.1 IMU

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

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

主要使用JY901 來測(cè)量機(jī)器人的偏航角。

2.2 編碼器

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

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

2.3 STM32F103 核心板

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

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

3 軟件方案

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

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

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

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

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

G1狀態(tài)表示為

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

同理B編碼器的坐標(biāo)及其正方向角為

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

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

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

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

圖6 傳統(tǒng)模型

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

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

3.3 改進(jìn)模型

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

圖7 改進(jìn)模型

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

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

運(yùn)動(dòng)過程中A編碼器的正方向角

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

A編碼器的里程La的表達(dá)式為：

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

同理，B編碼器的里程表達(dá)式為

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

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

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

化簡(jiǎn)后的坐標(biāo)增量為：

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

3.4 仿真結(jié)果

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

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

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

4 實(shí)驗(yàn)結(jié)果

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

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

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

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

5 結(jié)語

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

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