基于多傳感器融合信息的移動機器人速度控制方法

王昕煜，平雪良

（1.江南大學機械工程學院，江蘇無錫214122；2.江蘇省食品先進制造裝備技術重點實驗室，江蘇無錫214122）

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

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

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

1 移動機器人運動狀態優化估計模型

1.1 運動狀態非線性優化方法

圖1 移動機器人運動信息觀測模型Fig.1 Observation model of mobile robot motion information

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

根據移動機器人運動觀測殘差優化模型，構建關于運動狀態優化增量δχ的優化函數增量方程，對移動機器人運動狀態優化向量χ中各優化變量進行最小二乘優化，以實現各視覺關鍵幀間移動機器人運動狀態的優化估計。優化函數增量方程為：

1.2 視覺運動約束

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

構建單目相機視覺特征觀測模型，表示為：

1.3 輪式里程計運動約束

移動機器人的運動狀態可根據輪式里程計觀測到的驅動輪轉速信息與移動機器人結構參數來計算。輪式里程計觀測到的運動信息與真實運動信息之間因滑移現象而存在誤差。在移動機器人運動過程中，輪式里程計的運動觀測信息為：

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

根據相鄰輪式里程計關鍵幀Ok與Ok+1間的輪式里程計運動觀測信息，得到其對移動機器人運動狀態的約束為：

1.4 IMU運動約束

ⅠMU運動約束是指根據ⅠMU在視覺關鍵幀間的運動觀測信息移動機器人的運動狀態進行約束。ⅠMU運動觀測信息為：

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

根據ⅠMU 預積分模型理論［8］，相鄰ⅠMU 關鍵幀Bk與Bk+1間ⅠMU運動觀測信息對移動機器人運動狀態的約束為：

2 移動機器人速度控制系統

2.1 速度控制系統框架

移動機器人速度控制是指通過對移動機器人各驅動電機的電壓進行控制，以實現其在期望速度及角速度下的穩定運動。為保證移動機器人速度控制的精度，通過融合視覺信息、輪式里程計信息和ⅠMU信息，利用運動狀態優化估計模型實現其運動狀態的優化估計，并以此為基礎構建速度控制系統框架，如圖3所示。

圖3 移動機器人速度控制系統框架Fig.3 Framework of mobile robot speed control system

從圖3中可以看出，移動機器人速度控制系統根據運動狀態優化估計模型中實時更新的優化變量，實現移動機器人運動速度的實時估計，并結合增量式PⅠD 控制策略，完成對移動機器人驅動電機的實時控制。

2.2 速度控制策略

為實現對移動機器人運動速度的精確、實時估計，制定如下速度控制策略：當移動機器人運動狀態優化變量更新時，將優化更新的當前運動速度vWBt進行坐標變換，實現對移動機器人中心坐標系下運動速度的估計；當移動機器人運動狀態優化變量未更新時，根據輪式里程計的當前觀測速度vOt及上一次優化更新的輪式里程計速度觀測偏差bvp來估計移動機器人的運動速度。在移動機器人中心坐標系下的估計速度ve可表示為：

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

為降低運動速度優化變量對速度控制精度的影響，在對各驅動輪電機轉速進行增量式PⅠD控制的過程中，對滑動窗口中運動速度優化變量的估計增量Δvs進行補償。移動機器人速度控制策略為：

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

3 試驗驗證與結果分析

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

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

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

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

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

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

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

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

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

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

4 結 論

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