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

01

引言

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

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

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

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

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

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

02

里程計(jì)模型分析

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

2.1 速度計(jì)算

2.1.1 輪速計(jì)算

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

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

（b）編碼器輸出波形

圖 2.1 光電編碼器原理

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

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

2.1.2 Odom速度計(jì)算

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

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

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

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

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

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

其中，vr和vl分別表示兩驅(qū)動(dòng)輪的線速度，是可以通過章節(jié)2.1.1中的方法計(jì)算出來的，因此幾何中心的線速度vc和角速度w就可以通過公式（4）計(jì)算出來。公式（4）中的vc也就是Odom坐標(biāo)系x軸的線速度vx，由于兩輪差速驅(qū)動(dòng)機(jī)器人的非完整性約束（具體分析可參考《兩輪差速驅(qū)動(dòng)機(jī)器人運(yùn)動(dòng)模型及應(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ū)動(dòng)機(jī)器人Twist信息

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

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

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

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

表 2?2 麥輪移動(dòng)平臺Twist信息

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

2.2 位姿計(jì)算

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

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

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

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

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

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

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

03

案例分析

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

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

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

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

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

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

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

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

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

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

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

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

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

04

結(jié)論及展望

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

審核編輯：劉清