張馳洲，熊根良，陳海初

(南昌大學(xué) 機電工程學(xué)院，江西 南昌330031)

0 引言

掃地機器人是一個涵蓋機械、汽車、電子、自動控制及傳感器技術(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)全遍歷清掃，與隨機清掃方式相比，其清掃效率明顯提高。

1 系統(tǒng)的硬件設(shè)計

掃地機器人的硬件系統(tǒng)是保障其實現(xiàn)清掃任務(wù)的重要前提，硬件設(shè)計的優(yōu)劣決定了掃地機的工作性能。因此，本文根據(jù)掃地機器人路徑規(guī)劃、避障等功能需求，自主搭建掃地機器人的硬件平臺，其框圖如圖1所示。掃地機器人的硬件實物圖如圖2所示。

圖1 掃地機器人的硬件電路結(jié)構(gòu)框圖

圖2 掃地機器人硬件實物圖

1.1 主控芯片

本系統(tǒng)采用ST公司的STM32F030R8T6作為主控制器，該芯片是基于Cortex-M0內(nèi)核的32位微處理器，具有功耗低，性能高，集成度高和接口豐富等特點[6]。片上集成了IIC，USART，SPI等通信接口， 包含有5個通用16位定時器和1個高級脈沖寬度調(diào)制(PWM)定時器，可輸出多路PWM信號從而控制電機轉(zhuǎn)速。

1.2 傳感器芯片

本系統(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ū)動電路

1.3 電源模塊

本系統(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)變。

1.4 電機驅(qū)動模塊

本系統(tǒng)中電機驅(qū)動電路核心采用電機驅(qū)動AM2849。AM2849芯片內(nèi)有4個H橋電機驅(qū)動器，可以用來驅(qū)動有刷直流電機，其工作電壓在6.5～45 V，具有PWM輸出接口，通過改變高電平的占空比來實現(xiàn)電機調(diào)速。

2 系統(tǒng)的軟件設(shè)計

本控制系統(tǒng)中軟件控制算法可分為兩部分：

1) 電機速度控制程序，它負責控制掃地機的速度。

2) 弓字形路徑規(guī)劃。

2.1 電機速度控制總體流程設(shè)計

本系統(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)

2.2 機器人路徑規(guī)劃算法

在掃地機器人開始移動前，以當前偏航角為參考方向，然后根據(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)。

3 實驗

3.1 卡爾曼濾波融合算法仿真實驗

為驗證陀螺儀和編碼器數(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ù)融合是有效的，能夠補償角度誤差。

3.2 系統(tǒng)的串級PID參數(shù)整定

機器人系統(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ù)整定表

3.3 機器人路徑規(guī)劃測試實驗

為了驗證機器人的直線行駛能力，避障算法和路徑保持的能力[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 掃地機器人的弓字形路徑軌跡

3.4 機器人系統(tǒng)性能指標

經(jīng)過多次實驗測試，本樣機運行時各項性能指標如表2所示，與同類別的陀螺儀導(dǎo)航產(chǎn)品相比，本樣機直線行走能力和對機器人速度的控制能力都優(yōu)于同類產(chǎn)品。

表2 各類掃地機器人系統(tǒng)性能指標

4 結(jié)束語

本文設(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ù)，盡可能減少偏航誤差。