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

01

引言

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

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

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

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

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

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

02

里程計模型分析

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

2.1 速度計算

2.1.1 輪速計算

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

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

（b）編碼器輸出波形

圖 2.1 光電編碼器原理

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

假設(shè)編碼器在t1時刻的計數(shù)是n1，經(jīng)過△t時間后，在t2時刻的計數(shù)是n2，因此可知電機(jī)轉(zhuǎn)軸在△t時間內(nèi)轉(zhuǎn)過的角度△φ，表示為

那么角速度便可表示為

輪直徑是可以直接測量出來的，但需要校準(zhǔn)（可參考《常見移動機(jī)器人輪直徑校準(zhǔn)》），則可計算出驅(qū)動輪轉(zhuǎn)動的線速度v，可表示為

式中，dw為輪直徑。

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

2.1.2 Odom速度計算

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

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

圖 2.2 差速驅(qū)動機(jī)器人運(yùn)動及其軌跡

這里需要正運(yùn)動學(xué)模型來計算Odom坐標(biāo)系原點(diǎn)的運(yùn)動速度，以兩輪差速驅(qū)動機(jī)器人為例，可表示為

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

運(yùn)動學(xué)模型寫為

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

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

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

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

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

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

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

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

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

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

2.2 位姿計算

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

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

圖 2.5 兩輪差速驅(qū)動機(jī)器人運(yùn)動過程. 藍(lán)色弧線表示運(yùn)動軌跡

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

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

化簡為

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

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

03

案例分析

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

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

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

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

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

則，左輪和右輪的運(yùn)動軌跡長度可寫為

結(jié)合公式（8）和（9），有

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

可見，這里還存在一個系統(tǒng)誤差，也就是圓弧長度△Sc和直線段長度△dc并不相等，要得到直線段長度△dc，還需要再做一個轉(zhuǎn)換（三角變換，也可以使用余弦定理）

需要回顧的是，這里有個假設(shè)，dt時間間隔極短，意味著△θ非常小，所以公式（12）可寫作

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

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

通過公式（9）計算△θ，得

將公式（10）和公式（15）帶入公式（14），可知

因此，可狀態(tài)轉(zhuǎn)換方程可寫作

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

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

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

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

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

04

結(jié)論及展望

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

審核編輯 ：李倩