張馳洲,熊根良,陳海初
(南昌大學(xué) 機電工程學(xué)院,江西 南昌330031)
掃地機器人是一個涵蓋機械、汽車、電子、自動控制及傳感器技術(shù)等科目的綜合系統(tǒng)[1]。其中,直線行走和路徑規(guī)劃是用來評價掃地機器人清掃任務(wù)的重要指標,這成為掃地機器人研究的重要部分[2]。自主智能的掃地機器人除擁有感知、規(guī)劃等功能外,還應(yīng)在清掃過程中擁有自主路徑規(guī)劃能力的同時完成清掃任務(wù),而其中實現(xiàn)機器人系統(tǒng)的穩(wěn)定性是偏航角數(shù)據(jù)的準確性與實時性。
本文采用卡爾曼濾波算法[3]對陀螺儀和編碼器數(shù)據(jù)進行融合得到最優(yōu)估計,利用最優(yōu)偏航角和機器人速度組成串級比例、積分、微分(PID)控制算法[4-5],實現(xiàn)機器人的直線行走和精準轉(zhuǎn)彎,最后利用弓字形行走方式實現(xiàn)全遍歷清掃,與隨機清掃方式相比,其清掃效率明顯提高。
掃地機器人的硬件系統(tǒng)是保障其實現(xiàn)清掃任務(wù)的重要前提,硬件設(shè)計的優(yōu)劣決定了掃地機的工作性能。因此,本文根據(jù)掃地機器人路徑規(guī)劃、避障等功能需求,自主搭建掃地機器人的硬件平臺,其框圖如圖1所示。掃地機器人的硬件實物圖如圖2所示。
圖1 掃地機器人的硬件電路結(jié)構(gòu)框圖
圖2 掃地機器人硬件實物圖
本系統(tǒng)采用ST公司的STM32F030R8T6作為主控制器,該芯片是基于Cortex-M0內(nèi)核的32位微處理器,具有功耗低,性能高,集成度高和接口豐富等特點[6]。片上集成了IIC,USART,SPI等通信接口, 包含有5個通用16位定時器和1個高級脈沖寬度調(diào)制(PWM)定時器,可輸出多路PWM信號從而控制電機轉(zhuǎn)速。
本系統(tǒng)采用的傳感器芯片有單軸陀螺儀GGPM01、光電編碼器。其中單軸陀螺儀[7]的角速度感測范圍為±250 (°)/s、±500 (°)/s、±1 000 (°)/s、±2 000 (°)/s。該款陀螺儀的工作電壓在3~3.6 V,本系統(tǒng)中為GGPM01陀螺儀提供了3.3 V直流電壓,電路圖如圖3所示,其中數(shù)據(jù)線SDA和時鐘線SCL與微控制器通過IIC進行通信,不斷讀取陀螺儀的數(shù)據(jù)。而光電編碼器的線數(shù)為12,分辨率為1 536(機器人齒輪減速比為128)。
圖3 GGPM01陀螺儀模塊驅(qū)動電路
本系統(tǒng)所選用的直流工作電壓為14.4 V,由3節(jié)4.8 V可充電鎳氫電池組成直流電源。而電機的工作電壓為8 V,因此,板載非同步降壓轉(zhuǎn)換器TPS54335降壓得到8 V電壓(同時還可得到5 V電壓)。陀螺儀需要的電壓為3.3 V,所以板載正向可調(diào)集成穩(wěn)壓器LM117實現(xiàn)5 V到3.3 V的電壓轉(zhuǎn)變。
本系統(tǒng)中電機驅(qū)動電路核心采用電機驅(qū)動AM2849。AM2849芯片內(nèi)有4個H橋電機驅(qū)動器,可以用來驅(qū)動有刷直流電機,其工作電壓在6.5~45 V,具有PWM輸出接口,通過改變高電平的占空比來實現(xiàn)電機調(diào)速。
本控制系統(tǒng)中軟件控制算法可分為兩部分:
1) 電機速度控制程序,它負責控制掃地機的速度。
2) 弓字形路徑規(guī)劃。
本系統(tǒng)電機速度控制的主程序流程圖如圖4所示,其中系統(tǒng)初始化包括系統(tǒng)時鐘初始化、中斷初始化、陀螺儀GGPM01初始化、定時器外部中斷初始化、IIC初始化、光電編碼器初始化等。STM32的工作頻率為48 MHz,IIC的串口波特率設(shè)置為115 200 bit/s。
圖4 電機控制的主程序流程圖
2.1.1 卡爾曼濾波數(shù)據(jù)融合算法
在對機器人偏航角的測量過程中,若出現(xiàn)地面不平整或從動輪打滑等現(xiàn)象,則會造成光電編碼器脈沖計數(shù)不準確,產(chǎn)生隨機誤差,而陀螺儀受隨機誤差、溫飄等影響也會產(chǎn)生數(shù)據(jù)漂移。同時碼盤的靜態(tài)穩(wěn)定性好,而在機器人運動時其數(shù)據(jù)相對不可靠;陀螺儀的動態(tài)穩(wěn)定性更好,但是靜止時數(shù)據(jù)相對不可靠。所以,可通過對碼盤和陀螺儀[8]的數(shù)據(jù)進行卡爾曼濾波融合來獲得較準確的偏航角。
本系統(tǒng)機器人采用三輪驅(qū)動方式,左、右兩側(cè)兩輪采用同軸相連,通過安裝在左、右兩側(cè)驅(qū)動輪的光電編碼器可測出左、右兩輪的線速度和兩輪連線中心的線速度。
通過下式且根據(jù)采樣時間Δt可計算出機器人的偏航角。
(1)
θ(k+1)=θ(k)+dθ(k)=θ(k)+wdΔt
(2)
式中:vl,vr分別為人的左、右輪線速度;B為機器人左、右輪間距;ωd為光電編碼器測得的機器人偏航變化的角速度;θ(k)為機器人的偏航角。
現(xiàn)取系統(tǒng)狀態(tài)變量X=(vl,vr,ωg)T,其中ωg為陀螺儀讀出的角速度。因?qū)ο到y(tǒng)的控制量為0且當前機器人的狀態(tài)與上一時刻相同,則系統(tǒng)的狀態(tài)方程為
Xk+1=Ak+1|kXk+ΓkWk
(3)
式中:Xk+1和Xk分別為tk+1時刻、tk時刻的系統(tǒng)狀態(tài)變量;Wk是協(xié)方差為Qk的系統(tǒng)過程激勵噪聲矩陣(是均值為0的高斯白噪聲);Гk為系統(tǒng)噪聲矩陣的驅(qū)動矩陣;Ak+1|k為tk~tk+1時刻的狀態(tài)轉(zhuǎn)移矩陣,即
(4)
式中α=1為根據(jù)實驗確定的碼盤修正系數(shù)。
系統(tǒng)的測量矩陣為
Zk+1=Hk+1Xk+1+Vk+1
(5)
式中:Hk+1為系統(tǒng)測量矩陣,本系統(tǒng)中為單位矩陣;Vk+1為Rk+1的系統(tǒng)測量噪聲矩陣(是均值為0的高斯白噪聲),Rk+1為系統(tǒng)測量噪聲矩陣協(xié)方差,本系統(tǒng)中陀螺儀測量的角速度作為預(yù)測值的控制量,光電編碼器測量的角速度作為預(yù)測值,則融合卡爾曼濾波方程為
(6)
其中
(7)
式中:Qk為系統(tǒng)過程激勵噪聲矩陣協(xié)方差;Rk+1=0.1;P為誤差協(xié)方差矩陣;K為卡爾曼增益矩陣;E為單位矩陣。
2.1.2 串級PID控制算法
掃地機器人的控制算法編寫是實現(xiàn)其保持直線行走的核心問題,也是軟件控制機器人的關(guān)鍵技術(shù)之一。本系統(tǒng)采用常用的離散型位置式PID算法和增量式PID[9]算法相結(jié)合,即用差分代替微分,用累加代替積分,實現(xiàn)串級PID控制與PWM信號的輸出,其結(jié)構(gòu)框圖如圖5所示,其中PID控制器的處理時間和卡爾曼濾波算法的步長為10 ms,光電編碼器的采樣時間為20 ms。
圖5 系統(tǒng)串級PID控制算法框圖
外環(huán)為機器人的姿態(tài)控制環(huán),是基于位置式PID控制的比例、積分及微分控制,用來控制機器人的直線行走過程,利用卡爾曼濾波算法輸出的機器人偏航角與期望偏航角進行PID運算后輸出PWM信號的占空比,則有
PWM(k)=Kp·e(k)+Ki·∑e(k)+
Kd·[e(k)-e(k-1)]
(8)
式中:e(k)為當前偏航角偏差;e(k-1)為上一次偏航角偏差;PWM(k)為當前控制器輸出的占空比。
內(nèi)環(huán)為機器人的速度環(huán),是基于離散式PID控制的比例、積分控制,對光電編碼器輸出的計數(shù)值經(jīng)計算得到機器人的速度與期望機器人速度進行PI控制后得到的PWM占空比的增量值,則
ΔPWM(k)=K′p·[e′(k)-e′(k-1)]+
K′i·e′(k)
(9)
外環(huán)偏航角控制與內(nèi)環(huán)速度控制相互耦合,將其融合后即可得到最終的當前PWM占空比為
PWMfinal(k)=PWM(k)+ΔPWM(k)
(10)
在掃地機器人開始移動前,以當前偏航角為參考方向,然后根據(jù)參考方向來調(diào)整掃地機的直線運動,當機器人左轉(zhuǎn)時偏航角減小,右轉(zhuǎn)時則增大。如果在直線行駛過程中偏航角發(fā)生了正向偏移,掃地機器人則應(yīng)向左轉(zhuǎn),此時采用串級PID控制算法來調(diào)小左輪PWM占空比,同時增大右輪PWM占空比以實現(xiàn)方向的修正;如果偏航角發(fā)生了負向偏移,機器人則應(yīng)向右轉(zhuǎn),此時PWM調(diào)整反向以確保掃地機保持當前路線。
機器人開機啟動時即進入弓字形清掃方式[10],由于機器人的兩輪集成有光電編碼器,所以可以通過編碼器來獲取里程信息,用于控制機器人的前進距離。掃地機的弓字形路徑規(guī)劃可分為4個部分:
1) 模式1。機器人向前直線至左右編碼器計數(shù)值的和為60 000時(實際距離約為3 m),掃地機轉(zhuǎn)向標志位設(shè)為1,機器人左轉(zhuǎn)90°。完成后,編碼器計數(shù)值清0并重新計數(shù),轉(zhuǎn)向標志位被設(shè)置為0,同時轉(zhuǎn)到模式2。
2) 模式2。機器人完成模式1后,將自動執(zhí)行模式2。掃地機直線向前移動至左、右編碼器計數(shù)值的和為1 800時(實際距離約為0.09 m),掃地機器人轉(zhuǎn)向標志位設(shè)為1,機器人左轉(zhuǎn)90°。完成后,編碼器計數(shù)值清0并重新計數(shù),轉(zhuǎn)向標志位被設(shè)置為0同時進入到模式3。
3) 模式3。模式3的實現(xiàn)過程與模式1基本相同。不同的是,模式3使機器人向右轉(zhuǎn),而不是向左轉(zhuǎn)。當此過程完成時,機器人進入模式4。
4) 模式4。模式4的實現(xiàn)過程與模式2基本相同。不同的是,模式4使機器人向右轉(zhuǎn),而不是向左轉(zhuǎn)。當此過程完成時,機器人重新啟動循環(huán)。
為驗證陀螺儀和編碼器數(shù)據(jù)融合算法的有效性,利用Matlab中進行仿真實驗。定義仿真初始變量X0|0=(0,0,0)T,誤差協(xié)方差P0|0=zeros(3),過程噪聲協(xié)方差Q=diag(0.01,0.01,0.01)T,測量噪聲協(xié)方差R=(0.01,0.01,0.01)。為了更好的模擬現(xiàn)實環(huán)境,編碼器加入均值為0,標準差為理論值1%的高斯白噪聲;陀螺儀加入均值為0,標準差為理論值1%的高斯白噪聲。角速度仿真結(jié)果如圖6所示。
圖6 卡爾曼濾波誤差補償后角速度數(shù)據(jù)
由圖6可知,未采用卡爾曼濾波前原始數(shù)據(jù)的均值為6.932×10-3(°)/s,方差為0.812 (°)2/s2;而經(jīng)過卡爾曼濾波后數(shù)據(jù)的均值為5.627×10-4(°)/s,方差為5.132×10-2(°)2/s2。顯然卡爾曼濾波算法對于陀螺儀與編碼器的數(shù)據(jù)融合是有效的,能夠補償角度誤差。
機器人系統(tǒng)采用串級PID控制算法,其PID參數(shù)需要多次實驗采用試湊法進行參數(shù)整定,整定出的參數(shù)能夠使機器人的速度穩(wěn)定且直線行駛能力良好。先調(diào)節(jié)內(nèi)環(huán)速度PID控制的參數(shù),待速度穩(wěn)定后再調(diào)節(jié)外環(huán)角度PID控制的參數(shù),最終PID參數(shù)如表1所示。
表1 串級PID參數(shù)整定表
為了驗證機器人的直線行駛能力,避障算法和路徑保持的能力[11],建立了掃地機器人測試平臺,完成了相應(yīng)的測試。
圖7 機器人直行中偏航角和速度波形圖(帶干擾)
在直線行駛時間為900 s的過程中,通過藍牙傳輸偏航角和速度數(shù)據(jù)至上位機(初始設(shè)定偏航角為0°,速度為0.2 m/s,采樣時間為10 ms),同時在4.60 s時加入瞬時干擾,使機器人的姿態(tài)角變?yōu)?0°,且速度降為0(見圖7),經(jīng)過實時對偏航角進行采樣可知,在0~46.0 s時,機器人的當前偏航角波動在-0.4°~0.4°,而機器人的速度經(jīng)過4 s后達到期望速度,且波動為-0.03~0.01 m/s,顯然控制效果較好,滿足直線行走要求;在46.0 s加入瞬時干擾后,經(jīng)過串級PID算法調(diào)節(jié)后,機器人的偏航角經(jīng)過2.01 s后穩(wěn)定到初始角度,而機器人的速度經(jīng)過1.35 s后進入穩(wěn)態(tài)速度。
機器人的弓字形過程中偏航角的變化曲線如圖8所示。實際路徑軌跡用油墨印記法顯示,如圖9所示。
圖8 弓字形清掃中偏航角變化曲線
圖9 掃地機器人的弓字形路徑軌跡
經(jīng)過多次實驗測試,本樣機運行時各項性能指標如表2所示,與同類別的陀螺儀導(dǎo)航產(chǎn)品相比,本樣機直線行走能力和對機器人速度的控制能力都優(yōu)于同類產(chǎn)品。
表2 各類掃地機器人系統(tǒng)性能指標
本文設(shè)計了一個基于STM32F030R8T6微處理器和多種傳感器的掃地機器人控制系統(tǒng),采用卡爾曼濾波算法和串級PID算法對機器人的姿態(tài)和速度進行控制,利用弓字形算法實現(xiàn)了有規(guī)劃式的清掃方式。仿真及實驗證明,采用卡爾曼濾波算法能夠有效地將陀螺儀和光電編碼器的數(shù)據(jù)進行融合,消除傳感器測量過程中的噪聲影響和隨機誤差干擾,獲得最優(yōu)偏航角估計值。而采用串級PID控制算法能夠?qū)C器人的當前偏航角、當前速度值與期望值進行比較,得出誤差信息后進行雙閉環(huán)控制能有效地使機器人的速度穩(wěn)定,同時維持相對平衡的姿態(tài)。最終機器人直線行駛的偏航角誤差為[-0.4°,0.4°],速度控制在10.2~12.6 m/min,通過弓字形路徑規(guī)劃完成了空間的全遍歷。在以后的工作中將會進一步對掃地機器人的硬件和控制算法進行優(yōu)化,如使用視覺SLAM技術(shù),盡可能減少偏航誤差。