介紹一種基于編碼器合成里程計的方案

摘要：本文主要分析輪式移動機器人的通用里程計模型，并以兩輪差速驅(qū)動機器人的里程計計算為案例，給出簡化后的兩輪差速驅(qū)動機器人里程計模型。

01

引言

里程計是機器人導航系統(tǒng)中非常重要的一環(huán)，那么何為里程計？

里程計，可簡單理解為記錄機器人在一段時間內(nèi)的運動狀態(tài)，是基于時間戳的數(shù)據(jù)幀，該數(shù)據(jù)幀包含機器人位置、姿態(tài)及運動速度、角速度，具體結(jié)構(gòu)對應下圖 1.1。

圖 1.1 ROS里程計數(shù)據(jù)結(jié)構(gòu)

里程計的英文單詞是Odometry，在ROS小車坐標系樹里面有一個里程計專用坐標系，叫做Odom。

但凡涉及到可移動的機器人的導航系統(tǒng)，大概率會涉及到里程計的計算，比如輪式移動機器人、無人機、無人艇，以及多足機器人等，而計算里程計的方案也有很多種，比如基于編碼器合成里程計的方案、基于視覺的里程計方案（ORB SLAM）、基于激光雷達的里程計方案（Hector SLAM）、基于IMU的里程計方案，以及多傳感器融合的方案。

本文主要以輪式移動機器人為載體介紹基于編碼器合成里程計的方案，這是非常經(jīng)典常用的里程計計算方法之一，在輪式移動機器人領(lǐng)域有著廣泛的應用。

02

里程計模型分析

從圖 1.1可以看出，里程計主要包含的信息有兩部分：速度和位姿，因此接下來將從這兩個方面展開介紹。

2.1 速度計算

2.1.1 輪速計算

輪式機器人的驅(qū)動輪是由伺服電機驅(qū)動的，而電機轉(zhuǎn)軸后安裝有編碼器，通過編碼器采集的數(shù)據(jù)可以計算出電機輸出軸轉(zhuǎn)過的角度。

（a）編碼器結(jié)構(gòu)

（b）編碼器輸出波形

圖 2.1 光電編碼器原理

編碼器的種類很多，比如增量式光電編碼器（圖 2.1），紅色的光源是固定不動的，當光柵（圖中灰色帶孔圓盤）轉(zhuǎn)動的同時，其電路結(jié)構(gòu)會輸出A、B兩相的方波，使用單片機GPIO的中斷模式可采集到方波邊沿，進而進行計數(shù)，當電機軸轉(zhuǎn)動一圈，可獲取總計數(shù)為N（可能是512、也可能是1024，這與編碼器分辨率有關(guān)）。

這樣就可以將所有驅(qū)動輪的線速度計算出來，如果驅(qū)動輪與地面之間沒有相對滑動（也就是打滑），那么該線速度也就是驅(qū)動輪在地面上的運動線速度。

2.1.2 Odom速度計算

Odom坐標系一般與建立機器人運動學模型時的坐標系相同，也就是機器人零半徑轉(zhuǎn)彎的旋轉(zhuǎn)中心為坐標系原點，具體可參考《常見移動機器人運動學模型總結(jié)》。

需要注意的是，Odom是一個運動的坐標系，會隨著機器人的運動而運動，但是Odom記錄的是運動過程中的Odom坐標系狀態(tài)參數(shù)（速度、位姿）是在初始位姿的Odom的坐標系中描述的，如下圖所示：記錄的是Odom(tk)在Odom(t0)坐標系下的位姿和速度。

圖 2.2 差速驅(qū)動機器人運動及其軌跡

這里需要正運動學模型來計算Odom坐標系原點的運動速度，以兩輪差速驅(qū)動機器人為例，可表示為

圖 2.3 兩輪差速模型.機器人的外輪廓可視為圓形，直徑為dwb，ICR表示瞬時旋轉(zhuǎn)中心，藍色圓弧表示機器人運動路徑，點L和R分別為左右輪與地面的接觸點。

運動學模型寫為

其中，vr和vl分別表示兩驅(qū)動輪的線速度，是可以通過章節(jié)2.1.1中的方法計算出來的，因此幾何中心的線速度vc和角速度w就可以通過公式（4）計算出來。公式（4）中的vc也就是Odom坐標系x軸的線速度vx，由于兩輪差速驅(qū)動機器人的非完整性約束（具體分析可參考《兩輪差速驅(qū)動機器人運動模型及應用分析》），線速度vy和vz均為0，而公式（4）中的角速度w是Odom坐標系z軸的角速度wz，其余角速度wx和wy均為0。

從公式（4）中可以看出，除了前面提到的輪直徑需要校準之外，還需對輪間距dwb進行校準，由此實現(xiàn)

由此，圖 1.1中的的Odom結(jié)構(gòu)體中的Twist子結(jié)構(gòu)體可寫為

表 2?1 兩輪差速驅(qū)動機器人Twist信息

以上是兩輪差速驅(qū)動機器人的速度計算思路，而麥輪移動平臺與之不同，屬于全向移動機器人，是包含3個自由度，對應的運動學模型也是不一樣的，寫為

圖 2.4 麥輪平臺運動分解示意圖. 圖中r表示中心點CENTER到麥輪與地面接觸點之間的距離，rx和ry分別表示r在CENTER坐標系x軸和y軸上的投影距離（均為正值）.

采用同樣的思路，計算出四個驅(qū)動輪的線速度，結(jié)合公式（5）便可得到麥輪平臺中心的速度信息。

由此，圖 1.1中的的Odom結(jié)構(gòu)體中的Twist子結(jié)構(gòu)體可寫為

表 2?2 麥輪移動平臺Twist信息

除了兩輪差速驅(qū)動機器人和麥輪移動平臺，均可以通過上述思路計算得到里程計中的Twist信息。

2.2 位姿計算

計算Odom的位姿是依賴于章節(jié)2.1.2中的速度信息的，我們知道速度對時間積分就是位移，角速度對時間積分就是轉(zhuǎn)角。而里程計記錄的是機器人在某一時間段內(nèi)的運動軌跡，也就是位姿變換過程（如圖 2.2所示），需要用遞推方程表示，如果是兩輪差速驅(qū)動機器人，寫為：

式中，[x y θ]分別表示兩輪差速驅(qū)動機器人在某時刻的位置和姿態(tài)角

圖 2.5 兩輪差速驅(qū)動機器人運動過程. 藍色弧線表示運動軌跡

結(jié)合圖 2.5來解釋公式（6），從公式（6）可以看出是做了近似處理，包括：dt時間內(nèi)假設(shè)機器人是以[vc w]的速度進行勻速運動，但實際速度肯定不是勻速運動的，這存在誤差；另外，驅(qū)動輪可能存在微弱打滑的情況，也會引入誤差；最后，兩輪差速驅(qū)動機器人運動軌跡是圓弧曲線，速度方向是沿著圓弧切線方向的變化而變化的，但這里假設(shè)dt時間間隔很短，故“以直代曲”，認為機器人運動軌跡是直線段、速度方向也是不變的，在dt時間內(nèi)保持著θ的方向，所以也會引入誤差。

如果是麥輪移動平臺，原理也是和兩輪差速驅(qū)動機器人一致的，但又有些區(qū)別：麥輪平臺多了一個自由度，但可以從速度分解（或運動合成）的角度分析

麥輪移動平臺公式也包含了兩輪差速驅(qū)動機器人的近似處理思想。

綜合上述分析，可以計算得到t時刻下機器人的運動速度、位姿信息，合在一起也就是里程計信息了。

03

案例分析

從前面的分析可以看出，在計算速度、位姿時，經(jīng)過多次運算，位姿計算精度有所降低，此外，計算速度和位姿時，都需要一個時間間隔dt，那么可以從這個dt入手，進一步化簡，得到更簡潔、優(yōu)雅的公式。

這里以兩輪差速機器人為例，計算化簡里程計公式。

假設(shè)差速機器人在極短的dt時間內(nèi)運動了一小節(jié)距離，故可視為在dt時間內(nèi)機器人做勻速運動，如圖 3.1所示，在極短時間內(nèi)做勻速運動，可寫為

圖 3.1 差速驅(qū)動機器人里程計示意圖

如果機器人做勻速運動，且存在角速度，那么機器人中心的運動軌跡一定是一段標準的圓弧，其長度表示為△Sc，對應的圓心角為△θ，因此有下面的這個關(guān)系式

為后續(xù)計算，這里需要對公式（6）做一個修正，細心的讀者可能已經(jīng)發(fā)現(xiàn)問題了，在公式（6）中，方向角使用的是θt，也就是速度vct的方向，這個方向和△dc的方向之間存在一個夾角，這個夾角的大小是△θ/2，使用△dc的方向比速度vct的方向更加準確，因此修正后的公式（6）為

從公式（13）可以看出，兜兜轉(zhuǎn)轉(zhuǎn)，又回到了公式（8），這里就需要做一個權(quán)衡，如果dt時間間隔極短，那么可以取近似值，也就是認為△dc≈△Sc，以直代曲，如果dt時間間隔偏長，那么可以采用公式（12）計算，計算中包含正弦函數(shù)，但會降低計算效率、損失計算精度。

這里就采用“以直代曲”的方式來做進一步計算：

從公式（17）可知，僅需要計算左右驅(qū)動輪在dt時間內(nèi)運動過路程，便可得到dt時刻的位姿，回顧到本文的章節(jié)2.1.1的輪速計算，根據(jù)公式（1）可以計算驅(qū)動輪轉(zhuǎn)過的角度，因此可以得到運動的路程

式中，nl1、nl2分辨表示左輪在時間dt前后采集的左輪編碼器讀數(shù)，nr1、nr2分辨表示右輪在時間dt前后采集的右輪編碼器讀數(shù)。

所以，將公式（17）和（18）相結(jié)合，僅需要定期采集編碼器讀數(shù)，即可更新里程計中的位姿信息，而速度信息也是可以根據(jù)公式（2）和（3），結(jié)合運動學模型計算出來。而在實際應用中，編碼器采集頻率和里程計更新頻率保持一致（或整數(shù)倍），這樣更利于計算，減少重復計算帶來的精度損失和時間消耗。

從前面的公式可以看出，要得到更準確的里程計參數(shù)，需要對差速驅(qū)動機器人的輪直徑和輪間距進行校準。

其他的機器人里程計模型也可以參考兩輪差速驅(qū)動機器人案例，做進一步化簡，減少重復計算。

04

結(jié)論及展望

本文主要從編碼器原理介紹、輪速計算及位姿計算等方面分析了輪式移動機器人的通用里程計模型，并以兩輪差速驅(qū)動機器人的里程計計算為案例，詳細分析了兩輪差速驅(qū)動機器人運動規(guī)律及其位姿變換的等效思想，最后給出了簡化后的兩輪差速驅(qū)動機器人里程計模型。

審核編輯：劉清