張馳洲，熊根良，陳海初

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

0 引言

掃地機(jī)器人是一個(gè)涵蓋機(jī)械、汽車、電子、自動(dòng)控制及傳感器技術(shù)等科目的綜合系統(tǒng)[1]。其中，直線行走和路徑規(guī)劃是用來評(píng)價(jià)掃地機(jī)器人清掃任務(wù)的重要指標(biāo)，這成為掃地機(jī)器人研究的重要部分[2]。自主智能的掃地機(jī)器人除擁有感知、規(guī)劃等功能外，還應(yīng)在清掃過程中擁有自主路徑規(guī)劃能力的同時(shí)完成清掃任務(wù),而其中實(shí)現(xiàn)機(jī)器人系統(tǒng)的穩(wěn)定性是偏航角數(shù)據(jù)的準(zhǔn)確性與實(shí)時(shí)性。

本文采用卡爾曼濾波算法[3]對(duì)陀螺儀和編碼器數(shù)據(jù)進(jìn)行融合得到最優(yōu)估計(jì)，利用最優(yōu)偏航角和機(jī)器人速度組成串級(jí)比例、積分、微分(PID)控制算法[4-5]，實(shí)現(xiàn)機(jī)器人的直線行走和精準(zhǔn)轉(zhuǎn)彎，最后利用弓字形行走方式實(shí)現(xiàn)全遍歷清掃，與隨機(jī)清掃方式相比，其清掃效率明顯提高。

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

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

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

圖2 掃地機(jī)器人硬件實(shí)物圖

1.1 主控芯片

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

1.2 傳感器芯片

本系統(tǒng)采用的傳感器芯片有單軸陀螺儀GGPM01、光電編碼器。其中單軸陀螺儀[7]的角速度感測(cè)范圍為±250 (°)/s、±500 (°)/s、±1 000 (°)/s、±2 000 (°)/s。該款陀螺儀的工作電壓在3～3.6 V，本系統(tǒng)中為GGPM01陀螺儀提供了3.3 V直流電壓，電路圖如圖3所示，其中數(shù)據(jù)線SDA和時(shí)鐘線SCL與微控制器通過IIC進(jìn)行通信，不斷讀取陀螺儀的數(shù)據(jù)。而光電編碼器的線數(shù)為12，分辨率為1 536(機(jī)器人齒輪減速比為128)。

圖3 GGPM01陀螺儀模塊驅(qū)動(dòng)電路

1.3 電源模塊

本系統(tǒng)所選用的直流工作電壓為14.4 V，由3節(jié)4.8 V可充電鎳氫電池組成直流電源。而電機(jī)的工作電壓為8 V，因此,板載非同步降壓轉(zhuǎn)換器TPS54335降壓得到8 V電壓(同時(shí)還可得到5 V電壓)。陀螺儀需要的電壓為3.3 V，所以板載正向可調(diào)集成穩(wěn)壓器LM117實(shí)現(xiàn)5 V到3.3 V的電壓轉(zhuǎn)變。

1.4 電機(jī)驅(qū)動(dòng)模塊

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

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

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

1) 電機(jī)速度控制程序，它負(fù)責(zé)控制掃地機(jī)的速度。

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

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

本系統(tǒng)電機(jī)速度控制的主程序流程圖如圖4所示，其中系統(tǒng)初始化包括系統(tǒng)時(shí)鐘初始化、中斷初始化、陀螺儀GGPM01初始化、定時(shí)器外部中斷初始化、IIC初始化、光電編碼器初始化等。STM32的工作頻率為48 MHz，IIC的串口波特率設(shè)置為115 200 bit/s。

圖4 電機(jī)控制的主程序流程圖

2.1.1 卡爾曼濾波數(shù)據(jù)融合算法

在對(duì)機(jī)器人偏航角的測(cè)量過程中，若出現(xiàn)地面不平整或從動(dòng)輪打滑等現(xiàn)象,則會(huì)造成光電編碼器脈沖計(jì)數(shù)不準(zhǔn)確,產(chǎn)生隨機(jī)誤差，而陀螺儀受隨機(jī)誤差、溫飄等影響也會(huì)產(chǎn)生數(shù)據(jù)漂移。同時(shí)碼盤的靜態(tài)穩(wěn)定性好，而在機(jī)器人運(yùn)動(dòng)時(shí)其數(shù)據(jù)相對(duì)不可靠；陀螺儀的動(dòng)態(tài)穩(wěn)定性更好，但是靜止時(shí)數(shù)據(jù)相對(duì)不可靠。所以，可通過對(duì)碼盤和陀螺儀[8]的數(shù)據(jù)進(jìn)行卡爾曼濾波融合來獲得較準(zhǔn)確的偏航角。

本系統(tǒng)機(jī)器人采用三輪驅(qū)動(dòng)方式，左、右兩側(cè)兩輪采用同軸相連，通過安裝在左、右兩側(cè)驅(qū)動(dòng)輪的光電編碼器可測(cè)出左、右兩輪的線速度和兩輪連線中心的線速度。

通過下式且根據(jù)采樣時(shí)間Δt可計(jì)算出機(jī)器人的偏航角。

(1)

θ(k+1)=θ(k)+dθ(k)=θ(k)+wdΔt

(2)

式中：vl，vr分別為人的左、右輪線速度；B為機(jī)器人左、右輪間距；ωd為光電編碼器測(cè)得的機(jī)器人偏航變化的角速度；θ(k)為機(jī)器人的偏航角。

現(xiàn)取系統(tǒng)狀態(tài)變量X=(vl,vr,ωg)T，其中ωg為陀螺儀讀出的角速度。因?qū)ο到y(tǒng)的控制量為0且當(dāng)前機(jī)器人的狀態(tài)與上一時(shí)刻相同，則系統(tǒng)的狀態(tài)方程為

Xk+1=Ak+1|kXk+ΓkWk

(3)

式中:Xk+1和Xk分別為tk+1時(shí)刻、tk時(shí)刻的系統(tǒng)狀態(tài)變量;Wk是協(xié)方差為Qk的系統(tǒng)過程激勵(lì)噪聲矩陣(是均值為0的高斯白噪聲);Гk為系統(tǒng)噪聲矩陣的驅(qū)動(dòng)矩陣;Ak+1|k為tk～tk+1時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣，即

(4)

式中α=1為根據(jù)實(shí)驗(yàn)確定的碼盤修正系數(shù)。

系統(tǒng)的測(cè)量矩陣為

Zk+1=Hk+1Xk+1+Vk+1

(5)

式中：Hk+1為系統(tǒng)測(cè)量矩陣，本系統(tǒng)中為單位矩陣；Vk+1為Rk+1的系統(tǒng)測(cè)量噪聲矩陣(是均值為0的高斯白噪聲)，Rk+1為系統(tǒng)測(cè)量噪聲矩陣協(xié)方差,本系統(tǒng)中陀螺儀測(cè)量的角速度作為預(yù)測(cè)值的控制量，光電編碼器測(cè)量的角速度作為預(yù)測(cè)值，則融合卡爾曼濾波方程為

(6)

其中

(7)

式中:Qk為系統(tǒng)過程激勵(lì)噪聲矩陣協(xié)方差;Rk+1=0.1；P為誤差協(xié)方差矩陣;K為卡爾曼增益矩陣;E為單位矩陣。

2.1.2 串級(jí)PID控制算法

掃地機(jī)器人的控制算法編寫是實(shí)現(xiàn)其保持直線行走的核心問題，也是軟件控制機(jī)器人的關(guān)鍵技術(shù)之一。本系統(tǒng)采用常用的離散型位置式PID算法和增量式PID[9]算法相結(jié)合，即用差分代替微分，用累加代替積分，實(shí)現(xiàn)串級(jí)PID控制與PWM信號(hào)的輸出，其結(jié)構(gòu)框圖如圖5所示，其中PID控制器的處理時(shí)間和卡爾曼濾波算法的步長(zhǎng)為10 ms，光電編碼器的采樣時(shí)間為20 ms。

圖5 系統(tǒng)串級(jí)PID控制算法框圖

外環(huán)為機(jī)器人的姿態(tài)控制環(huán)，是基于位置式PID控制的比例、積分及微分控制，用來控制機(jī)器人的直線行走過程，利用卡爾曼濾波算法輸出的機(jī)器人偏航角與期望偏航角進(jìn)行PID運(yùn)算后輸出PWM信號(hào)的占空比，則有

PWM(k)=Kp·e(k)+Ki·∑e(k)+

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

(8)

式中：e(k)為當(dāng)前偏航角偏差；e(k-1)為上一次偏航角偏差；PWM(k)為當(dāng)前控制器輸出的占空比。

內(nèi)環(huán)為機(jī)器人的速度環(huán)，是基于離散式PID控制的比例、積分控制，對(duì)光電編碼器輸出的計(jì)數(shù)值經(jīng)計(jì)算得到機(jī)器人的速度與期望機(jī)器人速度進(jìn)行PI控制后得到的PWM占空比的增量值，則

ΔPWM(k)=K′p·[e′(k)-e′(k-1)]+

K′i·e′(k)

(9)

外環(huán)偏航角控制與內(nèi)環(huán)速度控制相互耦合，將其融合后即可得到最終的當(dāng)前PWM占空比為

PWMfinal(k)=PWM(k)+ΔPWM(k)

(10)

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

在掃地機(jī)器人開始移動(dòng)前，以當(dāng)前偏航角為參考方向，然后根據(jù)參考方向來調(diào)整掃地機(jī)的直線運(yùn)動(dòng)，當(dāng)機(jī)器人左轉(zhuǎn)時(shí)偏航角減小，右轉(zhuǎn)時(shí)則增大。如果在直線行駛過程中偏航角發(fā)生了正向偏移，掃地機(jī)器人則應(yīng)向左轉(zhuǎn)，此時(shí)采用串級(jí)PID控制算法來調(diào)小左輪PWM占空比，同時(shí)增大右輪PWM占空比以實(shí)現(xiàn)方向的修正；如果偏航角發(fā)生了負(fù)向偏移，機(jī)器人則應(yīng)向右轉(zhuǎn)，此時(shí)PWM調(diào)整反向以確保掃地機(jī)保持當(dāng)前路線。

機(jī)器人開機(jī)啟動(dòng)時(shí)即進(jìn)入弓字形清掃方式[10]，由于機(jī)器人的兩輪集成有光電編碼器，所以可以通過編碼器來獲取里程信息，用于控制機(jī)器人的前進(jìn)距離。掃地機(jī)的弓字形路徑規(guī)劃可分為4個(gè)部分：

1) 模式1。機(jī)器人向前直線至左右編碼器計(jì)數(shù)值的和為60 000時(shí)(實(shí)際距離約為3 m)，掃地機(jī)轉(zhuǎn)向標(biāo)志位設(shè)為1，機(jī)器人左轉(zhuǎn)90°。完成后，編碼器計(jì)數(shù)值清0并重新計(jì)數(shù)，轉(zhuǎn)向標(biāo)志位被設(shè)置為0,同時(shí)轉(zhuǎn)到模式2。

2) 模式2。機(jī)器人完成模式1后，將自動(dòng)執(zhí)行模式2。掃地機(jī)直線向前移動(dòng)至左、右編碼器計(jì)數(shù)值的和為1 800時(shí)(實(shí)際距離約為0.09 m)，掃地機(jī)器人轉(zhuǎn)向標(biāo)志位設(shè)為1，機(jī)器人左轉(zhuǎn)90°。完成后，編碼器計(jì)數(shù)值清0并重新計(jì)數(shù)，轉(zhuǎn)向標(biāo)志位被設(shè)置為0同時(shí)進(jìn)入到模式3。

3) 模式3。模式3的實(shí)現(xiàn)過程與模式1基本相同。不同的是，模式3使機(jī)器人向右轉(zhuǎn)，而不是向左轉(zhuǎn)。當(dāng)此過程完成時(shí)，機(jī)器人進(jìn)入模式4。

4) 模式4。模式4的實(shí)現(xiàn)過程與模式2基本相同。不同的是，模式4使機(jī)器人向右轉(zhuǎn)，而不是向左轉(zhuǎn)。當(dāng)此過程完成時(shí)，機(jī)器人重新啟動(dòng)循環(huán)。

3 實(shí)驗(yàn)

3.1 卡爾曼濾波融合算法仿真實(shí)驗(yàn)

為驗(yàn)證陀螺儀和編碼器數(shù)據(jù)融合算法的有效性，利用Matlab中進(jìn)行仿真實(shí)驗(yàn)。定義仿真初始變量X0|0=(0,0,0)T，誤差協(xié)方差P0|0=zeros(3)，過程噪聲協(xié)方差Q=diag(0.01，0.01，0.01)T，測(cè)量噪聲協(xié)方差R=(0.01，0.01，0.01)。為了更好的模擬現(xiàn)實(shí)環(huán)境，編碼器加入均值為0，標(biāo)準(zhǔn)差為理論值1%的高斯白噪聲;陀螺儀加入均值為0，標(biāo)準(zhǔn)差為理論值1%的高斯白噪聲。角速度仿真結(jié)果如圖6所示。

圖6 卡爾曼濾波誤差補(bǔ)償后角速度數(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。顯然卡爾曼濾波算法對(duì)于陀螺儀與編碼器的數(shù)據(jù)融合是有效的，能夠補(bǔ)償角度誤差。

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

機(jī)器人系統(tǒng)采用串級(jí)PID控制算法，其PID參數(shù)需要多次實(shí)驗(yàn)采用試湊法進(jìn)行參數(shù)整定，整定出的參數(shù)能夠使機(jī)器人的速度穩(wěn)定且直線行駛能力良好。先調(diào)節(jié)內(nèi)環(huán)速度PID控制的參數(shù)，待速度穩(wěn)定后再調(diào)節(jié)外環(huán)角度PID控制的參數(shù)，最終PID參數(shù)如表1所示。

表1 串級(jí)PID參數(shù)整定表

3.3 機(jī)器人路徑規(guī)劃測(cè)試實(shí)驗(yàn)

為了驗(yàn)證機(jī)器人的直線行駛能力，避障算法和路徑保持的能力[11]，建立了掃地機(jī)器人測(cè)試平臺(tái)，完成了相應(yīng)的測(cè)試。

圖7 機(jī)器人直行中偏航角和速度波形圖(帶干擾)

在直線行駛時(shí)間為900 s的過程中，通過藍(lán)牙傳輸偏航角和速度數(shù)據(jù)至上位機(jī)(初始設(shè)定偏航角為0°，速度為0.2 m/s,采樣時(shí)間為10 ms)，同時(shí)在4.60 s時(shí)加入瞬時(shí)干擾，使機(jī)器人的姿態(tài)角變?yōu)?0°，且速度降為0(見圖7)，經(jīng)過實(shí)時(shí)對(duì)偏航角進(jìn)行采樣可知，在0～46.0 s時(shí)，機(jī)器人的當(dāng)前偏航角波動(dòng)在-0.4°～0.4°，而機(jī)器人的速度經(jīng)過4 s后達(dá)到期望速度，且波動(dòng)為-0.03～0.01 m/s，顯然控制效果較好，滿足直線行走要求；在46.0 s加入瞬時(shí)干擾后，經(jīng)過串級(jí)PID算法調(diào)節(jié)后，機(jī)器人的偏航角經(jīng)過2.01 s后穩(wěn)定到初始角度，而機(jī)器人的速度經(jīng)過1.35 s后進(jìn)入穩(wěn)態(tài)速度。

機(jī)器人的弓字形過程中偏航角的變化曲線如圖8所示。實(shí)際路徑軌跡用油墨印記法顯示，如圖9所示。

圖8 弓字形清掃中偏航角變化曲線

圖9 掃地機(jī)器人的弓字形路徑軌跡

3.4 機(jī)器人系統(tǒng)性能指標(biāo)

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

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

4 結(jié)束語

本文設(shè)計(jì)了一個(gè)基于STM32F030R8T6微處理器和多種傳感器的掃地機(jī)器人控制系統(tǒng)，采用卡爾曼濾波算法和串級(jí)PID算法對(duì)機(jī)器人的姿態(tài)和速度進(jìn)行控制，利用弓字形算法實(shí)現(xiàn)了有規(guī)劃式的清掃方式。仿真及實(shí)驗(yàn)證明，采用卡爾曼濾波算法能夠有效地將陀螺儀和光電編碼器的數(shù)據(jù)進(jìn)行融合，消除傳感器測(cè)量過程中的噪聲影響和隨機(jī)誤差干擾，獲得最優(yōu)偏航角估計(jì)值。而采用串級(jí)PID控制算法能夠?qū)C(jī)器人的當(dāng)前偏航角、當(dāng)前速度值與期望值進(jìn)行比較，得出誤差信息后進(jìn)行雙閉環(huán)控制能有效地使機(jī)器人的速度穩(wěn)定，同時(shí)維持相對(duì)平衡的姿態(tài)。最終機(jī)器人直線行駛的偏航角誤差為[-0.4°，0.4°]，速度控制在10.2～12.6 m/min，通過弓字形路徑規(guī)劃完成了空間的全遍歷。在以后的工作中將會(huì)進(jìn)一步對(duì)掃地機(jī)器人的硬件和控制算法進(jìn)行優(yōu)化，如使用視覺SLAM技術(shù)，盡可能減少偏航誤差。