王道累, 孫嘉珺, 柴萍萍, 徐凌弘, 劉易騰, 韓 洋

(上海電力大學(xué) 能源與機(jī)械工程學(xué)院, 上海 200090)

隨著計(jì)算技術(shù)和微處理器等技術(shù)的發(fā)展,智能化技術(shù)發(fā)展越來越迅速,智能機(jī)器人的應(yīng)用也越來越廣泛。機(jī)器人的巡檢運(yùn)動和定位方式作為機(jī)器人的主要功能成為研究機(jī)器人學(xué)應(yīng)用的熱門方向。因此,如何運(yùn)用相應(yīng)的控制技術(shù)、傳感器融合技術(shù)和視覺技術(shù)等成為研究輪式巡檢機(jī)器人發(fā)展的主要課題之一。

目前相關(guān)人員在輪式移動發(fā)展[1]和傳統(tǒng)視覺目標(biāo)跟蹤算法的基礎(chǔ)上[2],針對視覺追蹤等問題提出了新的算法,以便克服干擾;同時(shí)也有在傳感器中使用數(shù)據(jù)融合技術(shù)[3],精準(zhǔn)獲得信息采集,再通過相關(guān)控制算法,結(jié)合信息融合技術(shù),以確定障礙物的種類。

本文研究的主要目標(biāo)是實(shí)現(xiàn)全向移動機(jī)器人的位姿確定和移動控制。在數(shù)據(jù)獲取方面,利用光流傳感器、慣性導(dǎo)航,通過各個(gè)傳感器讀取與處理信息;在結(jié)構(gòu)分析上,對麥克納姆輪的結(jié)構(gòu)和移動特點(diǎn)進(jìn)行了分析和建模,搭建了實(shí)物模型,并運(yùn)用相應(yīng)的傳感器技術(shù),獲得了傳感器相應(yīng)技術(shù)、單片機(jī)和全向機(jī)器人結(jié)合的現(xiàn)實(shí)成果;在控制上,以各個(gè)傳感器獲得的數(shù)據(jù)為基礎(chǔ),獲得了移動平臺所需的所有位姿信息,再利用雙閉環(huán)控制來實(shí)現(xiàn)移動平臺的相應(yīng)位姿變化控制,并通過實(shí)驗(yàn)調(diào)整控制參數(shù),以適應(yīng)最佳的控制要求。

1 基于光流傳感器的位置數(shù)據(jù)獲取

本文使用的光流傳感器為是ADNS-3080光流傳感器,能夠感應(yīng)速度高達(dá)101.6 cm/s和高速移動的加速度達(dá)15 m/s2。通過光學(xué)的技術(shù)獲取連續(xù)的表面圖像(幀)并在數(shù)學(xué)上確定移動的方向和大小,不需要精確的光學(xué)對準(zhǔn)。

在光流算法中,一般以兩個(gè)基本條件為假設(shè)前提:一是亮度恒定,隨著時(shí)間變化,鄰近幀間同點(diǎn)的光亮度不改變;二是小運(yùn)動,鄰近幀間物體的運(yùn)動持續(xù),變化非常小。

在光流算法中,以上兩個(gè)條件是光流法使用過程中都要遵循的假設(shè)條件,在現(xiàn)實(shí)實(shí)驗(yàn)中,需要基本符合這兩個(gè)條件。本文采用Lucas-Kanade(LK)光流算法[4]獲取瞬時(shí)速度數(shù)據(jù)。

基本的約束條件列寫方程為

I(x,y,t)=I(x+dx,y+dy,t+dt)

(1)

式中:I(x,y,z)——第一幀的光強(qiáng)度。

在假定運(yùn)動了(dx,dy)的間距到下一幀的前提下,應(yīng)用了dt的時(shí)間,又假定同一個(gè)像素前后光照度一致,因此式(1)等價(jià)條件成立。

將式(1)右端開展泰勒展開可得

(2)

式(2)化簡后同除以dt獲得式(3)

(3)

利用u和v分別設(shè)定x軸和y軸的速度矢量

(4)

分別用Ix,Iy,It表示圖像中像素點(diǎn)灰度沿x,y,t方向的偏導(dǎo)數(shù)

(5)

綜上可以得出式(6),即

Ixu+Iyv+It=0

(6)

在實(shí)際使用中,均需要從圖像數(shù)據(jù)中獲得u和v,但在當(dāng)前約束等式下,沒辦法解出u和v的準(zhǔn)確值。這時(shí)要引進(jìn)其他約束來開展求解。

而在LK光流算法中,在兩種假設(shè)的基礎(chǔ)條件下,又添加了第3個(gè)條件——一個(gè)幀的圖像具備近似同樣的運(yùn)動,以確保全部像素均有相似的運(yùn)動。以此假設(shè)來解決式(6)無法求解的問題。文獻(xiàn)[5]研究了利用3×3的窗口內(nèi)的9個(gè)像素點(diǎn)來建立9個(gè)方程,可以寫成如下形式

(7)

寫成矩陣形式

(8)

應(yīng)用最小二乘法對式(8)開展綜合性求解

(9)

通過上述累加鄰域像素點(diǎn)在3個(gè)維度的偏導(dǎo)數(shù)并做矩陣運(yùn)算,可以有效獲得光流矢量數(shù)值(u,v)。

2 基于慣性導(dǎo)航的角度數(shù)據(jù)獲取

為了獲得全向移動平臺的全部姿態(tài)信息,需要額外的傳感器來獲取移動平臺轉(zhuǎn)角信息。

MPU-6050運(yùn)動處理傳感器集成了三軸MEMS陀螺儀和三軸MEMS加速度計(jì),以及一個(gè)內(nèi)嵌的數(shù)字運(yùn)動處理器DMP。下面將從陀螺儀和加速度計(jì)的原理和數(shù)據(jù)處理算法展開。

2.1 基本原理

三軸陀螺儀原理為:通過測量三維坐標(biāo)系內(nèi)陀螺轉(zhuǎn)子的垂直軸與設(shè)備之間的夾角,并計(jì)算角速度,通過夾角和角速度來判別物體在三維空間的運(yùn)動狀態(tài)[5]。

加速度計(jì)原理為:加速度計(jì)精確測量一條直線上的加速度,一般在靜止?fàn)顟B(tài)下,可以通過計(jì)算重力加速度的各個(gè)角度關(guān)系來檢測出所需要的當(dāng)前被測物體在空間中的旋轉(zhuǎn)狀態(tài)[5]。

2.2 數(shù)據(jù)處理算法

在應(yīng)用MPU6050中的MEMS陀螺儀時(shí),普通情況下歐拉角可以通過MEMS陀螺儀直接來計(jì)算加速度,具體為

(10)

?t=?t-1+v×ΔT

(11)

式中:v角——實(shí)際角速度;

v輸出——陀螺儀的輸出值;

?t,?t-1——當(dāng)前和上一時(shí)刻累計(jì)的角度;

ΔT——采樣固定周期。

通過式(10)中陀螺儀的輸出值v輸出獲得的實(shí)際角速度v角,再根據(jù)式(11)來獲得角度。下面將詳細(xì)描述通過三軸加速度計(jì)所獲得的數(shù)據(jù)來解算出歐拉角的一個(gè)過程。

首先根據(jù)加速度的輸出值,計(jì)算得到當(dāng)前的實(shí)際加速度a為

(12)

在各個(gè)方向上獲取的數(shù)據(jù)需滿足條件:模長|a|=g,與重力加速度大小相等、方向相反。

(13)

式中:x,y,z——重力三軸的分量。

(14)

(15)

式中:?——翻轉(zhuǎn)角Roll角,為加速度向量與其在xz平面上投影的夾角;

在通過三軸陀螺儀和三軸加速度計(jì)分別獲得所需的轉(zhuǎn)角信息后,就可以應(yīng)用互補(bǔ)融合算法來獲得移動平臺所需的精確轉(zhuǎn)角信息以提供控制所用。

3 全向移動平臺機(jī)械結(jié)構(gòu)分析

本次實(shí)驗(yàn)采用配備有4個(gè)麥克納姆輪[6]為移動機(jī)構(gòu)的全向移動平臺作為主要的研究對象。以此平臺裝配各個(gè)傳感器、主控和電機(jī)等部件,最后裝配形成本次試驗(yàn)所需的硬件設(shè)施。麥克納姆輪結(jié)構(gòu)如圖1所示。

圖1 麥克納姆輪結(jié)構(gòu)

3.1 速度解算

當(dāng)4個(gè)麥克納姆輪組成底盤時(shí),計(jì)算麥克納姆輪的速度是實(shí)現(xiàn)對各個(gè)輪子的調(diào)速來控制移動機(jī)構(gòu)整體中心速度的基礎(chǔ)。

(16)

圖2 麥克納姆輪運(yùn)動速度計(jì)算

同時(shí)再分別計(jì)算x軸和y軸的分量[3]

(17)

式中:ry,rx——x軸和y軸的間距的一半。

(18)

再通過式(18)計(jì)算最終需要的輪子的轉(zhuǎn)速[3]

-vx+vy

(19)

將式(17)代入式(19)進(jìn)行等式整理,獲得單個(gè)輪子的最終轉(zhuǎn)速信息[3]為

(20)

式(20)為O-長方形麥輪底盤的逆運(yùn)動學(xué)模型方程[7-10]。綜合上述等式,根據(jù)底盤的3個(gè)狀態(tài)信息來解算出4個(gè)輪子各自的轉(zhuǎn)速信息。

4 仿真和實(shí)驗(yàn)

本文采用ADNS3080光流傳感器布置于平臺底部中心,與地面距離0.12 m。為充分保證周圍的環(huán)境光照強(qiáng)度,在移動平臺底部的ADNS3080周圍布置2個(gè)5 W的LED燈,同時(shí)設(shè)置紋理分明且較為平坦的桌面作為采樣面。

首先對x軸和y軸速度和距離進(jìn)行單獨(dú)正向采樣測試,采樣點(diǎn)數(shù)量大致在200,得出的數(shù)據(jù)曲線如圖3所示。同時(shí)對一段軌跡進(jìn)行測試,得出的數(shù)據(jù)曲線如圖4所示。用以上同樣的方式來測試YAW偏航角,測試結(jié)果如圖5所示。

圖3 x軸和y軸的數(shù)據(jù)曲線

圖4 對一段軌跡測試得出的數(shù)據(jù)曲線

圖5 YAW偏航角測試結(jié)果

單環(huán)控制PID算法[6]為

Ppwm=Kp×e(k)+Ki×∑e(k)+

Kd×[e(k)-e(k-1)]

(21)

式中:e(k)——當(dāng)前計(jì)算偏差值;

e(k-1)——前一次計(jì)算偏差值。

在此次雙閉環(huán)控制的參數(shù)調(diào)節(jié)中,調(diào)節(jié)兩環(huán)的參數(shù)以先內(nèi)環(huán)后外環(huán)為準(zhǔn)則。此次控制實(shí)驗(yàn)主要通過Simulink仿真的形式來進(jìn)行,以模擬速度解算環(huán)節(jié)和雙閉環(huán)控制環(huán)節(jié)。其仿真示意如圖6所示。

圖6 Simulink仿真

本研究中,單獨(dú)對YAW的控制進(jìn)行參數(shù)整定,設(shè)定移動平臺的位置目標(biāo)值為20。在對雙閉環(huán)進(jìn)行參數(shù)整定時(shí),首先對外環(huán)的位置環(huán)進(jìn)行參數(shù)整定。將外環(huán)控制器調(diào)整保持比例調(diào)節(jié)系數(shù)Kp=1,再對內(nèi)環(huán)的控制參數(shù)進(jìn)行整定。在位置環(huán)的調(diào)節(jié)中,對Kp進(jìn)行單獨(dú)調(diào)節(jié),分別選擇不同的Kp來觀測YAW的角度和角速度的變化情況,結(jié)果如圖7所示。

由圖7可知,隨著內(nèi)環(huán)比例調(diào)節(jié)系數(shù)Kp的增加,系統(tǒng)的調(diào)節(jié)時(shí)間和上升時(shí)間都有非常顯著的提升,在選擇內(nèi)環(huán)比例環(huán)節(jié)調(diào)節(jié)系數(shù)Kp=0.3時(shí),可以在不產(chǎn)生超調(diào)量的同時(shí)又保持較好的響應(yīng)速度。

在單獨(dú)測試完內(nèi)環(huán)的調(diào)節(jié)后,將較為適合的比例調(diào)節(jié)系數(shù)Kp=0.3作為外環(huán)控制器的固定值。以此為基礎(chǔ)進(jìn)行外環(huán)的測試,速度環(huán)也將從純比例環(huán)節(jié)開始進(jìn)行。不同外環(huán)比例調(diào)節(jié)系數(shù)Kp下的角度和角速度變化情況如圖8所示。

由圖8可知,外環(huán)的純比例調(diào)節(jié)系數(shù)Kp的改變能夠非常明顯地改變底盤角度的輸出特性。當(dāng)比例調(diào)節(jié)系數(shù)Kp從1到2.2變化時(shí),YAW的響應(yīng)速度有著非常明顯的提升,但達(dá)到目標(biāo)速度后存在穩(wěn)態(tài)誤差。因此,本試驗(yàn)中內(nèi)環(huán)控制還需要引入積分環(huán)節(jié)用以消除穩(wěn)態(tài)誤差。

圖7 不同內(nèi)環(huán)比例調(diào)節(jié)系數(shù)Kp下的角度和角速度變化曲線

圖8 不同外環(huán)比例調(diào)節(jié)系數(shù)Kp下的角度和角速度變化曲線

首先固定比例環(huán)節(jié)Kp=1.8,再加入積分環(huán)節(jié),通過調(diào)節(jié)后,選擇積分調(diào)節(jié)系數(shù)Ki=0.1,其響應(yīng)曲線如圖9所示。由圖9可知,加入積分環(huán)節(jié)后能夠非常有效地消除穩(wěn)態(tài)誤差,提高系統(tǒng)整體的穩(wěn)定性,并在運(yùn)行過程中能夠非常有效地將YAW穩(wěn)定至20,且保持良好。

圖9 不同外環(huán)積分調(diào)節(jié)系數(shù)Ki下角度和角速度變化曲線

5 結(jié) 論

本文基于多傳感器數(shù)據(jù)獲取和雙閉環(huán)控制,通過對移動平臺的速度解算獲得底盤與四輪子轉(zhuǎn)速之間的轉(zhuǎn)換關(guān)系,然后利用獲得的全向移動平臺姿態(tài)信息,通過雙閉環(huán)串級控制來進(jìn)行姿態(tài)控制。在研究的整個(gè)過程中,得出以下結(jié)論:

(1) 通過光流傳感器來獲得所需要的x軸和y軸距離數(shù)據(jù),在使用LED燈提供持續(xù)光照條件下移動10 cm存在1～2 mm的誤差,因此短距離移動對實(shí)驗(yàn)的影響非常小;

(2) 本次實(shí)驗(yàn)中,底盤的各個(gè)坐標(biāo)軸方向上的速度能通過運(yùn)動學(xué)的方法解算到每一個(gè)輪子上,麥克納姆輪存在的結(jié)構(gòu)優(yōu)勢也是全向移動實(shí)現(xiàn)的基礎(chǔ);

(3) 雙閉環(huán)控制能夠在系統(tǒng)中同時(shí)控制兩個(gè)關(guān)聯(lián)的狀態(tài),可以通過內(nèi)外環(huán)參數(shù)的調(diào)整來改善系統(tǒng)整體的性能,同時(shí)擾動和誤差也能得到非常好的解決。