王昕煜，平雪良

（1.江南大學(xué)機(jī)械工程學(xué)院，江蘇無錫214122；2.江蘇省食品先進(jìn)制造裝備技術(shù)重點實驗室，江蘇無錫214122）

目前，移動機(jī)器人在特種作業(yè)、物流倉儲和安防服務(wù)等行業(yè)的需求非常大，但其在復(fù)雜環(huán)境下的控制性能與適應(yīng)能力仍面臨巨大的挑戰(zhàn)［1-2］。為提高移動機(jī)器人在復(fù)雜環(huán)境下完成指定任務(wù)的能力，須對其運(yùn)動性能進(jìn)行改善，這對其速度控制的精度與穩(wěn)健性提出了較高的要求?；谳喪嚼锍逃嬓畔⒌乃俣瓤刂品椒ㄒ蚴芑瞥潭炔淮_定的影響，存在速度控制精度低和環(huán)境適應(yīng)能力差的問題，導(dǎo)致移動機(jī)器人的速度控制性能難以提升［3］。

隨著視覺位姿估計技術(shù)［4-6］和慣性導(dǎo)航技術(shù)［7-8］的日益發(fā)展，基于視覺-慣性信息融合原理的位姿估計方法［9-13］逐漸應(yīng)用于移動機(jī)器人導(dǎo)航領(lǐng)域，其可有效改善動態(tài)環(huán)境下視覺位姿的估計性能和慣性導(dǎo)航數(shù)據(jù)的穩(wěn)定性。例如：Qin等人［12］提出了基于滑動窗口原理的視覺信息與慣性信息非線性優(yōu)化的方法，實現(xiàn)了精準(zhǔn)、實時的視覺-慣性信息融合；Mur-artal 等人［13-14］提出了基于ORB（oriented FAST and rotated BRⅠEF）視覺特征的視覺-慣性位姿非線性優(yōu)化估計方法。雖然基于非線性優(yōu)化理論的視覺-慣性位姿估計方法在精度上明顯優(yōu)于單一傳感器位姿估計方法，但移動機(jī)器人的運(yùn)動常呈勻速狀態(tài)且局限于平面，這會導(dǎo)致慣性測量單元（inertial measurement unit，ⅠMU）因缺乏激勵而出現(xiàn)測量失真的問題，從而限制了視覺-慣性位姿估計方法對移動機(jī)器人運(yùn)動的估計效果［15］，使得相關(guān)的運(yùn)動估計信息難以在移動機(jī)器人速度控制中應(yīng)用。

為提高移動機(jī)器人速度控制的精度與穩(wěn)健性，筆者提出了一種基于多傳感器融合信息的移動機(jī)器人速度控制方法。通過采用單目相機(jī)、輪式里程計和ⅠMU 來觀測移動機(jī)器人的運(yùn)動，并利用非線性優(yōu)化方法來實現(xiàn)多傳感器信息的融合，以提高移動機(jī)器人運(yùn)動狀態(tài)的估計精度。同時，將運(yùn)動狀態(tài)估計信息引入基于增量式PⅠD（proportion integration differentiation，比例積分微分）控制策略的移動機(jī)器人速度控制系統(tǒng)，以實現(xiàn)移動機(jī)器人速度控制精度與穩(wěn)健性的提升。

1 移動機(jī)器人運(yùn)動狀態(tài)優(yōu)化估計模型

1.1 運(yùn)動狀態(tài)非線性優(yōu)化方法

圖1 移動機(jī)器人運(yùn)動信息觀測模型Fig.1 Observation model of mobile robot motion information

為保證移動機(jī)器人運(yùn)動狀態(tài)的估計效率，在單目相機(jī)采集到的視覺測量幀中提取視覺關(guān)鍵幀并進(jìn)行視覺運(yùn)動觀測。同時根據(jù)各視覺關(guān)鍵幀所在時刻下的輪式里程計關(guān)鍵幀和ⅠMU 關(guān)鍵幀，采用構(gòu)建滑動窗口的方法對各視覺關(guān)鍵幀間各傳感器的運(yùn)動觀測殘差進(jìn)行非線性優(yōu)化，以實現(xiàn)對單目相機(jī)、輪式里程計及ⅠMU運(yùn)動觀測信息的優(yōu)化融合。將滑動窗口內(nèi)各ⅠMU 關(guān)鍵幀運(yùn)動狀態(tài)、觀測偏差及視覺特征逆深度作為非線性優(yōu)化變量，構(gòu)建移動機(jī)器人運(yùn)動狀態(tài)優(yōu)化向量χ，表示為：及滑動窗口先驗殘差rP的移動機(jī)器人運(yùn)動觀測殘差優(yōu)化模型，可表示為：

根據(jù)移動機(jī)器人運(yùn)動觀測殘差優(yōu)化模型，構(gòu)建關(guān)于運(yùn)動狀態(tài)優(yōu)化增量δχ的優(yōu)化函數(shù)增量方程，對移動機(jī)器人運(yùn)動狀態(tài)優(yōu)化向量χ中各優(yōu)化變量進(jìn)行最小二乘優(yōu)化，以實現(xiàn)各視覺關(guān)鍵幀間移動機(jī)器人運(yùn)動狀態(tài)的優(yōu)化估計。優(yōu)化函數(shù)增量方程為：

1.2 視覺運(yùn)動約束

視覺運(yùn)動約束是指根據(jù)視覺運(yùn)動觀測信息對移動機(jī)器人多傳感器信息融合構(gòu)建的優(yōu)化約束條件。本文將移動機(jī)器人運(yùn)動過程中單目相機(jī)觀測到的Harris角點作為視覺特征。單目相機(jī)提取到的室內(nèi)、外環(huán)境視覺特征如圖2所示。

構(gòu)建單目相機(jī)視覺特征觀測模型，表示為：

1.3 輪式里程計運(yùn)動約束

移動機(jī)器人的運(yùn)動狀態(tài)可根據(jù)輪式里程計觀測到的驅(qū)動輪轉(zhuǎn)速信息與移動機(jī)器人結(jié)構(gòu)參數(shù)來計算。輪式里程計觀測到的運(yùn)動信息與真實運(yùn)動信息之間因滑移現(xiàn)象而存在誤差。在移動機(jī)器人運(yùn)動過程中，輪式里程計的運(yùn)動觀測信息為：

式中：v～、ψ～ 為輪式里程計的速度觀測值和角速度觀測值；v、ψ為速度真實值和角速度真實值；bv、bψ為輪式里程計的速度觀測偏差和角速度觀測偏差；nv、nψ為輪式里程計的速度觀測噪聲和角速度觀測噪聲。

根據(jù)相鄰輪式里程計關(guān)鍵幀Ok與Ok+1間的輪式里程計運(yùn)動觀測信息，得到其對移動機(jī)器人運(yùn)動狀態(tài)的約束為：

1.4 IMU運(yùn)動約束

ⅠMU運(yùn)動約束是指根據(jù)ⅠMU在視覺關(guān)鍵幀間的運(yùn)動觀測信息移動機(jī)器人的運(yùn)動狀態(tài)進(jìn)行約束。ⅠMU運(yùn)動觀測信息為：

式中：a～、ω～ 分別為ⅠMU 的加速度觀測值和角速度觀測值；a、ω分別為加速度真實值和角速度真實值；ba、bω為ⅠMU的加速度觀測偏差和角速度觀測偏差；na、nω為ⅠMU的加速度觀測噪聲和角速度觀測噪聲；RWB為ⅠMU 本體坐標(biāo)系至世界坐標(biāo)系W 的旋轉(zhuǎn)矩陣；gW為世界坐標(biāo)系W下的重力加速度。

根據(jù)ⅠMU 預(yù)積分模型理論［8］，相鄰ⅠMU 關(guān)鍵幀Bk與Bk+1間ⅠMU運(yùn)動觀測信息對移動機(jī)器人運(yùn)動狀態(tài)的約束為：

2 移動機(jī)器人速度控制系統(tǒng)

2.1 速度控制系統(tǒng)框架

移動機(jī)器人速度控制是指通過對移動機(jī)器人各驅(qū)動電機(jī)的電壓進(jìn)行控制，以實現(xiàn)其在期望速度及角速度下的穩(wěn)定運(yùn)動。為保證移動機(jī)器人速度控制的精度，通過融合視覺信息、輪式里程計信息和ⅠMU信息，利用運(yùn)動狀態(tài)優(yōu)化估計模型實現(xiàn)其運(yùn)動狀態(tài)的優(yōu)化估計，并以此為基礎(chǔ)構(gòu)建速度控制系統(tǒng)框架，如圖3所示。

圖3 移動機(jī)器人速度控制系統(tǒng)框架Fig.3 Framework of mobile robot speed control system

從圖3中可以看出，移動機(jī)器人速度控制系統(tǒng)根據(jù)運(yùn)動狀態(tài)優(yōu)化估計模型中實時更新的優(yōu)化變量，實現(xiàn)移動機(jī)器人運(yùn)動速度的實時估計，并結(jié)合增量式PⅠD 控制策略，完成對移動機(jī)器人驅(qū)動電機(jī)的實時控制。

2.2 速度控制策略

為實現(xiàn)對移動機(jī)器人運(yùn)動速度的精確、實時估計，制定如下速度控制策略：當(dāng)移動機(jī)器人運(yùn)動狀態(tài)優(yōu)化變量更新時，將優(yōu)化更新的當(dāng)前運(yùn)動速度vWBt進(jìn)行坐標(biāo)變換，實現(xiàn)對移動機(jī)器人中心坐標(biāo)系下運(yùn)動速度的估計；當(dāng)移動機(jī)器人運(yùn)動狀態(tài)優(yōu)化變量未更新時，根據(jù)輪式里程計的當(dāng)前觀測速度vOt及上一次優(yōu)化更新的輪式里程計速度觀測偏差bvp來估計移動機(jī)器人的運(yùn)動速度。在移動機(jī)器人中心坐標(biāo)系下的估計速度ve可表示為：

基于輪式移動機(jī)器人運(yùn)動學(xué)模型，根據(jù)移動機(jī)器人左、右側(cè)驅(qū)動輪間距d以及估計速度ve的縱向分量vex與切向分量vey，可計算兩側(cè)驅(qū)動輪的估計速度；根據(jù)移動機(jī)器人的期望速度vt與期望角速度ωt，可計算驅(qū)動輪的目標(biāo)速度。結(jié)合移動機(jī)器人驅(qū)動輪的實時估計速度和目標(biāo)速度，計算得到其左、右側(cè)驅(qū)動電機(jī)的速度控制誤差el和er，分別為：

為降低運(yùn)動速度優(yōu)化變量對速度控制精度的影響，在對各驅(qū)動輪電機(jī)轉(zhuǎn)速進(jìn)行增量式PⅠD控制的過程中，對滑動窗口中運(yùn)動速度優(yōu)化變量的估計增量Δvs進(jìn)行補(bǔ)償。移動機(jī)器人速度控制策略為：

式中：Ph、Ph-1分別為當(dāng)前速度控制時刻h與上一速度控制時刻（h-1）的速度控制輸出；KP、KⅠ與KD分別為比例增益、積分增益與微分增益。

3 試驗驗證與結(jié)果分析

為驗證基于多傳感器融合信息的移動機(jī)器人速度控制系統(tǒng)的實際性能，搭建如圖4所示的移動機(jī)器人速度控制試驗平臺，并開展移動機(jī)器人速度估計精度驗證試驗及速度控制精度驗證試驗。該試驗平臺配備的單目相機(jī)、輪式里程計與ⅠMU 的主要性能參數(shù)如表1所示。

圖4 移動機(jī)器人速度控制試驗平臺Fig.4 Mobile robot speed control test platform

表1 試驗平臺中各傳感器的主要性能參數(shù)Table1 Main performance parameters of each sensor in the test platform

在速度估計精度驗證試驗中，移動機(jī)器人在恒定速度激勵下勻速運(yùn)動?；诓杉脑囼灁?shù)據(jù)，分別采用本文的速度估計方法、輪式里程計速度估計方法和ⅤⅠNS-Mono視覺慣性位姿估計方法［12］對移動機(jī)器人的實時運(yùn)動速度進(jìn)行估計。根據(jù)移動機(jī)器人的實際運(yùn)動距離及運(yùn)動時間，計算得到其平均運(yùn)動速度為0.445 m/s。將0.445 m/s 作為參考速度，對各速度估計方法的精度進(jìn)行評估。不同方法估計得到的移動機(jī)器人運(yùn)動速度及其均方根誤差（root mean squared error，RMSE）分別如圖5和圖6所示。

圖5 不同方法估計的移動機(jī)器人運(yùn)動速度對比Fig.5 Comparison of mobile robot motion speed estimated by different methods

圖6 不同方法估計的移動機(jī)器人運(yùn)動速度均方根誤差對比Fig.6 Comparison of RMSE of mobile robot motion speed estimated by different methods

在速度控制精度驗證試驗中，在室內(nèi)、外環(huán)境下采用不同速度控制方法（本文速度控制方法和傳統(tǒng)的基于輪式里程計信息的速度控制方法）對移動機(jī)器人進(jìn)行速度控制，并對不同速度控制方法的控制精度進(jìn)行比較。在指定的控制時間tc內(nèi)，移動機(jī)器人在不同期望線速度vt下作直線運(yùn)動，根據(jù)實際運(yùn)動距離計算移動機(jī)器人線速度的平均估計誤差Δvc，結(jié)果如表2所示。在指定的控制時間tc內(nèi)，移動機(jī)器人在相同期望線速度vt=0.5 m/s、不同期望角速度ωt下作圓周運(yùn)動，根據(jù)實際運(yùn)動角度計算移動機(jī)器人角速度的平均估計誤差Δωc，結(jié)果如表3所示。

表2 基于不同速度控制方法的移動機(jī)器人線速度平均估計誤差對比Table 2 Comparison of average linear speed estimation errors of mobile robot based on different speed control methods 單位：m/s

表3 基于不同速度控制方法的移動機(jī)器人角速度平均估計誤差對比Table 3 Comparison of average angular velocity estimation errors of mobile robot based on different speed control methods 單位：rad/s

分析表2和表3可知：基于輪式里程計信息的速度控制方法的線速度估計誤差與角速度估計誤差均較大，且其控制精度隨地面摩擦性質(zhì)不同呈現(xiàn)較大的隨機(jī)差異性；本文速度控制方法的速度估計過程雖受到各種環(huán)境因素的影響，但因采用了多傳感器融合方法，使得在速度估計上具有較好的穩(wěn)健性，在不同環(huán)境下仍具有較高的精度與適應(yīng)能力，其控制精度較基于輪式里程計信息的速度控制方法有顯著提高。

4 結(jié) 論

針對傳統(tǒng)移動機(jī)器人速度控制方法控制精度低和環(huán)境適應(yīng)能力差的問題，提出了一種基于多傳感器融合信息的移動機(jī)器人速度控制方法。該方法采用非線性優(yōu)化方法對移動機(jī)器人的視覺信息、輪式里程計信息及ⅠMU 信息進(jìn)行優(yōu)化融合，并結(jié)合運(yùn)動狀態(tài)估計信息與增量式PⅠD控制策略對移動機(jī)器人的速度進(jìn)行控制。試驗結(jié)果表明，相較于其他速度估計方法，本文速度估計方法有效減小了速度估計誤差；相較于基于輪式里程計信息的速度控制方法，本文速度控制方法在控制精度和穩(wěn)健性方面有較大提升。