三輪全向移動機器人里程計在線校正方法研究

簡傲，閔華松，黃文暉

(武漢科技大學機器人與智能系統(tǒng)研究院，湖北武漢 430081)

0 前言

移動機器人是當今機器人領(lǐng)域應用開發(fā)的熱點。在移動機器人應用開發(fā)過程中，會遇到一個關(guān)鍵性的問題，即定位和控制問題，具體表現(xiàn)為移動機器人在全局坐標系下的位姿矯正問題。移動機器人在復雜多變環(huán)境中如何獲得自身位置和姿態(tài)，以及通過怎樣的方式到達目標位置是定位問題的關(guān)鍵。解決這樣的問題常常需要用到多種類型的傳感器進行信息融合，并設計適當?shù)目刂扑惴āＲ苿訖C器人系統(tǒng)是一個非常復雜的系統(tǒng)，它需要融合對環(huán)境的感知能力、決策能力、自主規(guī)劃能力以及控制與執(zhí)行能力，具備相當高的智能性和自主性。

全向移動機器人是目前移動機器人中比較熱門的課題，相比擁有常規(guī)輪子結(jié)構(gòu)的移動機器人，它具備更優(yōu)良的機動性，能夠在狹窄空間內(nèi)轉(zhuǎn)彎以及向任何方向移動，這類機器人通常用于家庭、車間、醫(yī)療服務等。盡管全向移動機器人具有更好的機動性，但由于復雜的機械結(jié)構(gòu)和特殊的輪子屬性，使得全向移動機器人的控制變得極為復雜。全向移動機器人的位姿控制難度較大，所以市場上的全向移動機器人的精度普遍不高。對于如何提高全向移動機器人的定位精度，使它能夠為后續(xù)的導航和精準作業(yè)提供保障，一直是各大高校和科研機構(gòu)討論和研究的熱門課題。

移動機器人的定位精度必然受到兩類誤差的影響，即系統(tǒng)誤差和非系統(tǒng)誤差，且無法避免。系統(tǒng)誤差是由控制模型和機械裝配的不準確而產(chǎn)生的，例如機械器件的裝配誤差、車輪直徑不相等、輪距的不確定性、控制參數(shù)的不確定性等；非系統(tǒng)誤差則是由車輪打滑或者地面不平整等原因造成的。系統(tǒng)誤差是移動機器人本身的屬性，在長時間內(nèi)幾乎保持不變，而非系統(tǒng)誤差可表示成車輪與地面之間關(guān)系的函數(shù)。因此，利用有效的傳感器信息，對機器人的位置和姿態(tài)進行校準是有必要的。

在移動機器人經(jīng)過規(guī)劃導航到達目標點后，為使其在目標位置能夠正常地進行工作任務，需要保證它在目標點的位姿精度能適應正常工作的要求。但是，目前的規(guī)劃導航算法由于受系統(tǒng)誤差和非系統(tǒng)誤差的影響，會導致機器人在到達目標點的位姿偏差較大，需要經(jīng)過校正算法來提高機器人到達目標點時的位姿精度，其中比較有效的方法是里程計校正法。

國內(nèi)外關(guān)于移動機器人里程計校正方法的研究較多，較為經(jīng)典的校正方法是1996年密歇根大學提出的針對兩輪差速驅(qū)動機器人的里程計誤差校正方法UMBmark。該方法指出了誤差來源為輪子直徑不等、輪子間距離存在不確定性，定量分析了系統(tǒng)誤差的大小。文獻[11]中將全向移動機器人的運動分為以下3種：純旋轉(zhuǎn)、直線運動、繞非零半徑點旋轉(zhuǎn)，使得機器人不僅能夠跟蹤所需方向也能跟蹤既定的軌跡，在引入瞬時曲率中心的坐標和瞬時旋轉(zhuǎn)半徑后，建立更為精準的運動學模型，確定了機器人調(diào)速范圍，設計出合適的模糊控制器。文獻[12]中提出了通過機載慣性傳感器來獲取機器人姿態(tài)，用機器人姿態(tài)角的偏移來推導校正方程，不需要做近似計算。文獻[13]中認為全向移動機器人的運動誤差來源于運動學模型的不準確，即全向輪有內(nèi)外圈，在機器人的運動過程中，內(nèi)外圈交替地與地面接觸，所以機器人中心到輪子的距離并不是一個固定值；為解決這一問題，對機器人的運動學模型進行改進，提高全向移動機器人的定位精度。文獻[14]中提出了一種單目相機與增量型編碼器相結(jié)合的視覺里程計，利用圖像特征獲取機器人的相對位姿并與編碼器測得的機器人轉(zhuǎn)向角結(jié)合，實現(xiàn)對車輛姿態(tài)和路標位置的實時估計。文獻[15]中針對視覺里程計(VO)因累積誤差導致機器人位姿估計存在偏差，設計了一種基于擴展卡爾曼濾波的位姿估計模型，利用慣性測量單元(IMU)結(jié)合重力加速度方向作為垂直方向參考，對視覺里程計航向、俯仰和側(cè)傾3個方向姿態(tài)估計進行解耦，修正姿態(tài)估計的累積誤差。文獻[16]中針對里程計在定位過程中存在累積誤差的問題，建立了一種通用的移動機器人里程計誤差模型，通過處理激光雷達反饋數(shù)據(jù)對里程計誤差進行實時反饋補償。三輪全向移動機器人的動力學分析有助于提高里程精度，合理的動力分配和控制器設計能夠大幅度提高機器人的里程計精度。文獻[19]中利用牛頓歐拉法和簡化的直流電機模型，在Simulink中建立一種簡化的三輪全向移動機器人的動力學控制器，實現(xiàn)了存在摩擦情況下的速度閉環(huán)控制。

針對機器人里程誤差的校正，比較成熟的方法是通過工程手段反復測量機器人的軌跡信息，然后根據(jù)里程誤差為機器人的運動學模型添加校正系數(shù)，從而達到修正里程誤差的目的。但是這種離線校正手段建立在地面平整且輪子與地面的摩擦始終不變的情況下，不具備魯棒性和普適性，不同的工作環(huán)境需再次進行大量的實驗，以得到校正系數(shù)，且無法應對地面環(huán)境的變化。針對離線校正方法的不足，本文作者采用擴展卡爾曼濾波算法與動力學分析相結(jié)合的方法，設計一種適用于三輪全向移動機器人在線里程計校正系統(tǒng)，通過仿真實驗驗證其有效性，并在實際平臺上取得了預期實驗效果。

1 三輪全向移動機器人建模

1.1 三輪全向移動機器人運動學

機器人的運動學特性是對機械系統(tǒng)工作原理的本質(zhì)表述。而移動機器人運動學模型是機器人運動控制的基礎(chǔ)，通過解析運動學方程，可以將機器人中心點的位移轉(zhuǎn)化為每個全向輪上各自的轉(zhuǎn)動，從而得到機器人的位姿、速度以及單個全向輪轉(zhuǎn)速之間的關(guān)系，有利于分析和解決問題。文中的三輪全向輪底盤如圖1所示。

圖1 三輪全向輪底盤模型

圖1中機身坐標系的原點與底盤幾何中心重合，在機器人自身坐標系下，建立三輪全向移動機器人運動學方程。其中，、、分別表示3個全向輪的線速度，m/s；、、表示輪子的角速度，rad/s；為輪子中心到機器人中心的距離，m；為機器人整體的前向速度，m/s;為機器人整體的法向速度,m/s;為底盤的旋轉(zhuǎn)角速度,rad/s。速度分解如公式(1)所示：

(1)

通過矩陣的逆變換可以求得機器人中心點的速度與全向輪線速度之間的關(guān)系，如公式(2)所示：

(2)

(3)

=()·

(4)

圖2 三輪全向輪坐標變換

得到運動學方程后，可以由上位機發(fā)布機器人的速度指令=[′,′,′],然后根據(jù)運動學方程轉(zhuǎn)換為全向輪附著電機所需要的PWM占空比，從而控制三輪全向移動機器人的運動。

1.2 三輪全向移動機器人動力學

運動學能夠?qū)崿F(xiàn)機器人整體速度和輪子速度之間的轉(zhuǎn)換，通過運動學逆解,上位機能夠及時發(fā)布控制指令使其運動到目標位置。然而，機器人受到自身和環(huán)境因素的影響，基于運動學的控制器并不能滿足系統(tǒng)高精度定位的要求，所以需要對機器人的動力學進行建模。

機器人靠電機提供驅(qū)動力，全向機器人存在前向、法向的線加速度以及旋轉(zhuǎn)的角加速度，由此可得三輪全向輪底盤的受力分析如公式(5)所示：

(5)

式中：為車體的質(zhì)量，kg;為底盤整體的慣性力矩常數(shù)，kg·m；為底盤前向的滑動摩擦力，N；為底盤法向的滑動摩擦力，N；為底盤滑動摩擦力矩，N·m;為底盤前向的靜摩擦力，N;n為底盤法向的靜摩擦力，N；為底盤靜摩擦力矩，N·m。

滑動摩擦力與機器人的速度成正比，所以有:

=·n=n·=·

式中：、n分別是、方向上的滑動摩擦系數(shù)，N·s/m；是關(guān)于的滑動摩擦系數(shù)，N·m·s/rad。

靜摩擦力與接觸面材質(zhì)以及壓力有關(guān)，可表示為

=·sign()n=n·sign()

=·sign()=·sign()

式中：、n分別是、方向上的靜摩擦力，N；是關(guān)于的靜摩擦力矩，N·m。

底盤整體受到的牽引力由電機提供，圖1中已給出受力分析，進行矢量分解后如公式(6)所示:

(6)

輪子受到的牽引力以及牽引力矩如公式(7)所示：

(7)

為了更為便利地建模，需要對電機模型進行簡化：

(8)

其中：為輪子半徑，m;為減速比；為電機的轉(zhuǎn)矩常數(shù)，N·m/A;為電機電流，A；為電機內(nèi)部電阻，Ω。由此可以得到電機驅(qū)動力(N)與電壓(V)以及轉(zhuǎn)速(m/s)之間的表達式如公式(9)所示:

(9)

聯(lián)立公式(5)—(9),可以用狀態(tài)空間的形式來表示三輪全向輪底盤的動力學模型，如公式(10)所示。其中，()=[,,]、()=[,,]，為。

(10)

有了三輪全向輪底盤的動力學模型,可以更為精確地控制機器人。全向輪與地面的摩擦因數(shù)低，機器人自身慣量大，運動過程中容易發(fā)生偏移，且偏移誤差會不斷累積，如果將加速度控制在一定的范圍內(nèi)，能有效減小控制難度。

2 在線里程計校正方法

2.1 卡爾曼濾波算法

傳感器精度大多會受到自身刷新頻率和環(huán)境因素的影響，導致測量結(jié)果發(fā)生漂移，且誤差會隨著時間的推移累積，所以精確的測量需要利用合適的濾波器對數(shù)據(jù)進行處理?？柭鼮V波(Kalman Filter,KF)的核心思想是將測量更新與預測更新結(jié)合起來，得到最佳的狀態(tài)估計值。擴展卡爾曼濾波(Extended Kalman Filter ,EKF)和KF不同，EKF能夠處理非線性系統(tǒng)。兩者都需要建立系統(tǒng)的狀態(tài)方程和觀測方程，如公式(11)所示：

(11)

將KF的狀態(tài)估計分為兩個步驟，即狀態(tài)預測和狀態(tài)更新。其中，狀態(tài)預測主要是針對狀態(tài)向量和系統(tǒng)誤差，如公式(12)所示：

(12)

式中：為系統(tǒng)狀態(tài)量對應的協(xié)方差；為系統(tǒng)過程的協(xié)方差矩陣。而狀態(tài)更新主要分為以下3個方面。

更新KF增益：

=|-1(+)

其中：為觀測噪聲的協(xié)方差矩陣。

由測量值更新系統(tǒng)狀態(tài)估計：

|-1=-1|-1+(--1|-1)

更新誤差協(xié)方差：

|-1=(1-)-1|-1

2.2 基于EKF的里程計融合

使用KF來進行傳感器融合是一種比較常用的手段，其觀測方程有多個，即有多個傳感器，由不同傳感器的測量值來估計系統(tǒng)的狀態(tài)向量。其觀測方程可表示成公式(13)的形式。

(13)

圖3 三輪全向移動機器人狀態(tài)更新

文中通過擴展卡爾曼濾波算法將編碼器和IMU的數(shù)據(jù)進行融合，得到三輪全向移動機器人的里程信息，如圖4所示，以便設計合適的控制算法來校正機器人的里程誤差。

圖4 獲取機器人位姿信息

選取適當?shù)淖兞孔鳛镋KF的系統(tǒng)狀態(tài)，三輪全向移動機器人運動在二維平面上，選擇=[,,,,,]作為系統(tǒng)變量。如圖5所示，、表示機器人在平面上的坐標，表示平面上相對于初始位姿的角度,、表示平面上沿、方向的速度，表示機器人自身的旋轉(zhuǎn)角速度。

圖5 機器人運動狀態(tài)

單獨用編碼器充當移動機器人的里程計來反饋機器人的位姿效果不夠好，因為機器人的系統(tǒng)誤差和非系統(tǒng)誤差在實際運動過程中無法避免，打滑等情況會造成編碼器推算的姿態(tài)誤差累積，而加入IMU和編碼器的信息融合后測量得到的姿態(tài)信息能有效減少誤差。

(14)

(15)

=，表示系統(tǒng)狀態(tài)的不確定度的協(xié)方差矩陣。里程計提供的位置信息的準確度高于速度信息，所以>>。編碼器的測量誤差需要傳感器廠家提供。

(16)

()=[,,]=

(17)

IMU的EKF狀態(tài)更新的主要方程如下：

2.3 校正方法

本文作者通過EKF與三輪全向移動機器人動力學結(jié)合的方法來校正機器人的里程誤差。首先是獲取機器人在運動過程中在全局坐標系下的位姿和姿態(tài)，這里只需要通過EKF對編碼器和IMU數(shù)據(jù)作處理就可以得到位姿信息以及速度信息，將位置和速度信息與參考值作比較，設計位姿控制器和速度控制器。文中為PID控制器，根據(jù)誤差值并結(jié)合前面的受力分析，對機器人作新的動力分配，減小誤差值。算法原理如圖7所示。

圖6 三輪全向移動機器人里程誤差

圖7 校正算法原理

3 實驗結(jié)果與分析

3.1 三輪全向移動機器人V-REP仿真

三輪全向輪底盤的特殊機械結(jié)構(gòu)，使它在平面內(nèi)擁有3個自由度，機器人可以在不改變自身姿態(tài)的情況下朝平面任意方向運動。導入移動底盤模型后，可以在V-REP中構(gòu)造全向輪的仿真模型，并添加約束。

全向輪的運動學本質(zhì)上是在靠輪轂外圍安裝一周與輪轂軸線呈一定角度的無動力輥子作為輪胎，該輥子不僅可繞輪轂軸公轉(zhuǎn)，也能在地面摩擦力作用下繞各自的支撐芯軸自轉(zhuǎn)。兩種運動的合成使得接觸地面的輥子中心合速度與輪轂軸有一定的夾角，通過調(diào)節(jié)輪轂速度可改變輥子中心合速度的大小和方向。

在V-REP中，復雜的機械結(jié)構(gòu)會使得仿真不夠流暢，控制難度增大，所以對全向輪模型進行簡化。由于全向輪與底盤的接觸是一個點，這個點既可以平動也可以滑動，所以將全向輪簡化成兩個材料不同的球體和2個電機關(guān)節(jié)。電機互成90°，一個使能作為驅(qū)動輪，一個不使能作為滑動輪，如圖8所示。

圖8 全向輪簡化模型

在V-REP中搭建的三輪全向輪底盤仿真模型，與實物的屬性保持一致。仿真下能直觀地模擬三輪全向移動機器人的運動過程，在模型中添加多種傳感器信息，能夠及時地反饋機器人的里程信息，分析算法的有效性，如圖9所示。

圖9 三輪全向移動機器人V-REP仿真

3.2 對比實驗及評價指標

本文作者利用V-REP與ROS進行聯(lián)合仿真，能夠利用上一章節(jié)提出的算法計算機器人自身在全局坐標系下的位姿，并結(jié)合動力學作動力分配，以及時地修正里程誤差，最后在ROS中發(fā)布校正后的機器人坐標。

移動機器人的運動可以分為直線運動和弧線運動，所以移動機器人的里程誤差校正，一般是讓機器人作正方形軌跡和圓弧軌跡運動。與傳統(tǒng)的差速機器人不同，三輪全向移動機器人在平面內(nèi)具有3個自由度，進行正方形軌跡運動的時候，在轉(zhuǎn)角處可以在不改變自身姿態(tài)的情況下作橫向直線運動。

為驗證算法的優(yōu)越性和有效性，仿真中在機器人的前進路徑上設置微小的不平物。

本文作者提出的在線校正方法基本不受地面環(huán)境的影響。文獻[20]中的離線校正方法會嚴重偏離期望軌跡，由于無法很好地感知機器人自身位姿信息，不能及時調(diào)整自身姿態(tài)，以對軌跡作校正。為判定算法，設計評價指標，進行仿真實驗，結(jié)果如圖11所示。仿真結(jié)果證實了該方法的魯棒性和普適性。

圖10 對比實驗效果

圖11 仿真實驗效果

正方形軌跡可以反映機器人直線運動過程中橫向和縱向的誤差校正效果，圓弧軌跡能反映機器人姿態(tài)誤差校正效果。仿真中，機器人作正方形運動，橫向和縱向的誤差都歸類為徑向誤差，誤差改善率為。徑向誤差的定義如公式(18)所示：

(18)

式中：為實驗次數(shù)?？赏ㄟ^比較誤差改善率來判斷校正方法的有效性，其定義如公式(19)所示：

(19)

其中：和分別表示校正前和校正后徑向誤差的平均值。

機器人沿著圓形軌跡運動，為得到姿態(tài)誤差的改善效果，可以用期望圓弧半徑、校正前的平均圓弧半徑和校正后的平均圓弧半徑，來評價姿態(tài)誤差改善率。則可以表示為

(20)

從仿真結(jié)果看，機器人的位置誤差和姿態(tài)誤差均得到了改善，采集仿真數(shù)據(jù)求取平均值可得為68.14%、為72.33%，驗證了算法的有效性。

3.3 實際平臺效果

以實驗室搭建的實物機器人為基礎(chǔ)，搭建了模型并進行實驗。實物部分參數(shù)如表1所示。

表1 全向移動機器人部分參數(shù)

全向輪底盤的3個電機互成120°進行安裝，將全向輪底盤分為3個關(guān)鍵模塊：電源轉(zhuǎn)換模塊、驅(qū)動模塊、控制模塊。電源轉(zhuǎn)換模塊實現(xiàn)電壓的轉(zhuǎn)換，驅(qū)動模塊控制電機的運作，控制模塊發(fā)布運動指令和傳感器信息采集，實物如圖12所示。

實物實驗分為徑向誤差改善實驗以及弧度誤差改善實驗。徑向誤差實驗步驟如下：

步驟(1)，如圖6所示，設置控制程序，使機器人從原點運動到，機器人運動速度為0.2 m/s、運動距離為40 cm；

步驟(2)，機器人停止移動后，測量機器人相對于參考坐標系的絕對位置并記錄；

步驟(3)，重復10次步驟(1)、(2)，根據(jù)公式(18)，求出校正前的徑向平均誤差;

步驟(4)，在程序中引入文中所提的在線校正方法，調(diào)試控制器參數(shù)，重復10次步驟(1)和步驟(2)，求出校正后的徑向平均誤差;

步驟(5)，根據(jù)公式(19)計算誤差改善率。

整合測量結(jié)果可得，校正前的徑向平均誤差為2.41 cm、校正后的徑向平均誤差為0.82 cm、徑向誤差改善率為65.98%。

圖12 三輪全向移動機器人實物

4 結(jié)論

本文作者提出了一種減少移動機器人里程誤差的在線校正方法。相比于傳統(tǒng)的離線校正方法，該方法能夠及時地計算機器人的位姿，并根據(jù)位姿信息和速度信息及時校正機器人的里程誤差，能夠適應地面環(huán)境多變的情況，具有普適性和魯棒性。在仿真和實際平臺上的實驗效果證明了所提校正方法的有效性。

文中算法可以有效解決機器人自身定位精度問題。除了自身定位外，還需要對外部環(huán)境進行感知，使機器人獲得更強大的功能和更高精度的定位效果。因此，后期的工作考慮為機器人添加3D相機和激光雷達等傳感器，擴展其外部感知能力，使它能夠適應更多變的工作環(huán)境和更復雜的任務需求。