基于改進(jìn)導(dǎo)航圓算法的光伏陣列清掃機(jī)器人路徑跟蹤控制

丁 坤，王 立，張經(jīng)煒，李辰陽(yáng)，翁 帥

(1.河海大學(xué)機(jī)電工程學(xué)院，江蘇 常州 213022；2.常州市光伏系統(tǒng)集成與生產(chǎn)裝備技術(shù)重點(diǎn)實(shí)驗(yàn)室，江蘇 常州 213022)

在戶外環(huán)境中，光伏電站中的光伏組件表面極易堆積灰塵等不潔物，而灰塵因?yàn)榫哂蟹瓷?、散射和吸收太?yáng)輻射的作用，使得組件前蓋透光率下降，降低了太陽(yáng)輻射的通過(guò)率，從而導(dǎo)致光伏系統(tǒng)的發(fā)電效率和經(jīng)濟(jì)效益降低[1-2]。目前針對(duì)光伏組件表面的不潔物，主要存在4種清除方式：人工擦拭、人工水洗、半自動(dòng)機(jī)械清洗、自動(dòng)機(jī)械清洗[3-6]。其中采用人工擦拭、人工水洗的方式，清潔效率低，并且清潔過(guò)程難以控制。半自動(dòng)機(jī)械清洗多采用帶有滾刷臂的清掃卡車，其場(chǎng)地適應(yīng)能力有限，無(wú)法在屋頂電站或者山地電站工作。自動(dòng)機(jī)械清洗是將清潔裝備技術(shù)與移動(dòng)機(jī)器人技術(shù)結(jié)合，即移動(dòng)機(jī)器人在光伏組件表面工作，通過(guò)自身配備的滾刷來(lái)清潔組件。由于光伏組件表面材料為玻璃，組件間存在邊框，所以要求清掃機(jī)器人對(duì)組件表面的壓強(qiáng)要小，并且具有一定的越障能力，而履帶式移動(dòng)結(jié)構(gòu)因其與組件接觸面積大，以及優(yōu)秀的越障性能，十分契合光伏陣列清掃機(jī)器人的要求。光伏陣列清掃機(jī)器人具有清潔效率高，操作靈活和場(chǎng)地適應(yīng)能力強(qiáng)等優(yōu)勢(shì)，以及巨大的市場(chǎng)潛力和經(jīng)濟(jì)價(jià)值，已引起許多專家和學(xué)者的重視[7]。

移動(dòng)機(jī)器人的研究涉及諸多科學(xué)領(lǐng)域，其中智能控制是移動(dòng)機(jī)器人的前沿課題，主要指移動(dòng)機(jī)器人的定位導(dǎo)航、路徑規(guī)劃和路徑跟蹤等問(wèn)題[8]。路徑跟蹤是指在平面坐標(biāo)系下一條滿足任務(wù)需求的理想幾何路徑，移動(dòng)機(jī)器人從平面上某一點(diǎn)出發(fā)，按照某種控制規(guī)律到達(dá)該路徑，并沿著該理想幾何路徑實(shí)現(xiàn)跟蹤的控制運(yùn)動(dòng)[9]。對(duì)于路徑跟蹤問(wèn)題，國(guó)外的Kanayama等[10]提出了一種回旋曲線跟蹤路徑；而對(duì)于國(guó)內(nèi)學(xué)者，郝存明等[11]提出了基于滑?？刂频氖覂?nèi)移動(dòng)機(jī)器人路徑跟蹤，尉成果等[12]提出了一種輪式移動(dòng)機(jī)器人的軌跡跟蹤算法，第一次提出了導(dǎo)航圓的概念。本文在導(dǎo)航圓算法的基礎(chǔ)上進(jìn)行改進(jìn)，進(jìn)一步提高移動(dòng)機(jī)器人路徑跟蹤性能。

本文使用的光伏陣列清掃機(jī)器人采用履帶式移動(dòng)結(jié)構(gòu)并帶有滾刷，工作時(shí)行駛在光伏組件表面，在遇到凸起的組件邊框或者在光滑組件表面上時(shí)容易出現(xiàn)打滑現(xiàn)象，導(dǎo)致偏離原始路徑的問(wèn)題。針對(duì)這一問(wèn)題，首先給出機(jī)器人的運(yùn)動(dòng)學(xué)模型和適用的超寬帶(ultra wide band，UWB)定位系統(tǒng)，然后提出改進(jìn)的導(dǎo)航圓光伏陣列清掃機(jī)器人路徑跟蹤算法。該算法在原有導(dǎo)航圓算法的基礎(chǔ)上，對(duì)其關(guān)鍵變量進(jìn)行重新修正。驗(yàn)證結(jié)果表明，采用這一控制方法，機(jī)器人在短時(shí)間內(nèi)即可調(diào)整到目標(biāo)路徑上，較導(dǎo)航圓算法有了顯著提升。

1 清掃機(jī)器人的運(yùn)動(dòng)數(shù)學(xué)模型

1.1 機(jī)器人基本機(jī)構(gòu)

為了適應(yīng)不同的工作環(huán)境，一般室內(nèi)移動(dòng)機(jī)器人通常采用輪式移動(dòng)機(jī)構(gòu)，室外移動(dòng)機(jī)器人為了適應(yīng)野外環(huán)境的需要，多采用履帶式移動(dòng)機(jī)構(gòu)[13]，所以本文機(jī)器人選擇履帶式移動(dòng)機(jī)構(gòu)，該機(jī)構(gòu)相對(duì)輪式移動(dòng)機(jī)構(gòu)跨越障礙能力優(yōu)越[14]，對(duì)組件表面壓強(qiáng)小且行走方式更靈活。

1.2 機(jī)器人運(yùn)動(dòng)數(shù)學(xué)模型

為了了解機(jī)器人行走方式與電機(jī)轉(zhuǎn)速的關(guān)系，從而更好地控制機(jī)器人的運(yùn)動(dòng)，需要對(duì)機(jī)器人進(jìn)行運(yùn)動(dòng)分析。建立如圖1所示的光伏陣列清掃機(jī)器人運(yùn)動(dòng)數(shù)學(xué)模型，假定機(jī)器人以P點(diǎn)為圓心從C1點(diǎn)移動(dòng)至C2點(diǎn)，C為機(jī)器人幾何中心。則機(jī)器人的運(yùn)動(dòng)數(shù)學(xué)模型[15]為

(1)

圖1 光伏陣列清掃機(jī)器人運(yùn)動(dòng)數(shù)學(xué)模型 Fig.1 Kinematic model of photovoltaic array cleaning robot

式中：x、y——機(jī)器人的位置；vl——左帶輪線速度；vr——右?guī)л喚€速度；L——左右履帶間距；δ——機(jī)器人相對(duì)x軸的轉(zhuǎn)角；v、ω——C點(diǎn)線速度和角速度。

2 清掃機(jī)器人的超寬帶定位系統(tǒng)

UWB定位是一種基于極窄脈沖的無(wú)線技術(shù)，因其具有傳輸速率高(最高可達(dá)1 000 Mbps以上)、發(fā)射功率和功耗低，以及穿透性較強(qiáng)的特點(diǎn)而廣泛應(yīng)用于科學(xué)研究[16-17]。

UWB定位系統(tǒng)一般設(shè)有主動(dòng)發(fā)射標(biāo)簽和多個(gè)接收基站。采用到達(dá)時(shí)間(time of arrival，TOA)算法或到達(dá)時(shí)間差(time difference of arrival，TDOA)算法來(lái)測(cè)距定位，接收基站接收來(lái)自發(fā)射標(biāo)簽的超寬帶信號(hào)，過(guò)濾掉電磁波在傳輸過(guò)程中的噪聲干擾，得到信號(hào)中的有效信息[18]，利用CPU對(duì)信息進(jìn)行解碼和計(jì)算，最終實(shí)現(xiàn)對(duì)發(fā)射標(biāo)簽的定位[19]。

為實(shí)現(xiàn)機(jī)器人的定位功能，建立如圖2所示的光伏陣列清掃機(jī)器人超寬帶定位系統(tǒng)，將1個(gè)主動(dòng)發(fā)射標(biāo)簽設(shè)置在機(jī)器人的幾何中心，將4個(gè)接收基站設(shè)置在組件陣列四周，采用TOA算法進(jìn)行位置計(jì)算，之后通過(guò)無(wú)線通信上傳到上位機(jī)中，用于后續(xù)跟蹤算法，并在顯示器上實(shí)時(shí)顯示機(jī)器人的位置信息。設(shè)計(jì)的UWB系統(tǒng)定位精度在20 cm之內(nèi)。

圖2 光伏陣列清掃機(jī)器人超寬帶定位系統(tǒng)示意圖Fig.2 Schematic map of ultra-wide band positioning system for photovoltaic array cleaning robot

3 改進(jìn)的導(dǎo)航圓路徑跟蹤算法

3.1 導(dǎo)航圓路徑跟蹤算法

導(dǎo)航圓路徑跟蹤算法，是將獲取的位置信息轉(zhuǎn)化成與目標(biāo)路徑的距離偏差，與當(dāng)前行進(jìn)方向和目標(biāo)路徑方向的角度偏差綜合成一個(gè)角度，對(duì)該角度進(jìn)行比例積分微分 (proportional-integral-derivative，PID)調(diào)節(jié)，以使機(jī)器人回到路徑上。尉成果等[12]提出的導(dǎo)航圓算法，本質(zhì)上是將一個(gè)假想圓作為機(jī)器人的導(dǎo)航范圍來(lái)計(jì)算路徑跟蹤時(shí)的糾偏角度，通過(guò)PID控制調(diào)節(jié)糾偏角，直至機(jī)器人回歸直線。具體算法如圖3所示，導(dǎo)航圓的圓心C為機(jī)器人的幾何中心，半徑為R0；機(jī)器人處于遠(yuǎn)離目標(biāo)直線的位置C1，逐漸調(diào)整至C2位置，D1和D2分別表示當(dāng)前時(shí)刻機(jī)器人的目標(biāo)點(diǎn)。

圖3 直線跟蹤示意圖Fig.3 Sketch map of straight-line tracking

如圖3所示，在機(jī)器人逐漸靠近目標(biāo)直線的過(guò)程中，糾偏角(機(jī)器人當(dāng)前方向與目標(biāo)方向的夾角)γ逐漸減小，當(dāng)γ=0時(shí)，即可實(shí)現(xiàn)目標(biāo)跟蹤。

已知機(jī)器人到目標(biāo)直線y=k*x+b的距離h[12]為

(2)

式中：xc、yc——機(jī)器人實(shí)時(shí)位置坐標(biāo)；k*、b——目標(biāo)直線的斜率和縱截距。

由幾何關(guān)系可得各角度信息[12]，各角度方向均如圖3中所示：

θ=α-arcsin(h/R0)

(3)

γ=θ-β=α-arcsin(h/R0)-β

(4)

式中：α——目標(biāo)直線路徑傾角；β——機(jī)器人前進(jìn)方向與x軸的夾角，即偏航角；θ——預(yù)測(cè)偏航角。

利用PID控制器對(duì)γ反饋調(diào)節(jié)，直至γ為0，機(jī)器人回歸目標(biāo)直線。PID控制的原理為

(5)

式中：u(t)——控制量；Kp——比例系數(shù)；TI——積分常數(shù)；TD——微分常數(shù)；e(t)——糾偏角γ;t——時(shí)間。

對(duì)γ采用增量式PID控制，將PID控制算法離散化為

Δu(k)=Kp[e(k)-e(k-1)]+KIe(k)+KD[e(k)-2e(k-1)+e(k-2)]

(6)

其中KI=KPT/TIKD=KPTD/T

式中：KI——積分系數(shù)；KD——微分系數(shù)；T——采樣周期。由式(6)得

u(k)=u(k-1)+Δu(k)

(7)

則機(jī)器人在糾偏過(guò)程中任意k時(shí)刻速度分配為

(8)

對(duì)于目標(biāo)路徑為圓弧的導(dǎo)航圓跟蹤算法，其思想與直線跟蹤相同，只是由跟蹤已知直線變?yōu)楦檮?dòng)態(tài)直線，動(dòng)態(tài)直線是跟蹤圓弧上的切線，但光伏陣列清掃機(jī)器人主要是直線運(yùn)動(dòng)，所以不具體分析。

假設(shè)路徑跟蹤的目標(biāo)直線l：y=500 cm，機(jī)器人左右履帶間距L=50 cm，起點(diǎn)為(50，700)，初始速度為0.1 m/s，初始前進(jìn)方向?yàn)閤軸正方向。若使導(dǎo)航圓算法成立，則導(dǎo)航圓與目標(biāo)直線必須存在交點(diǎn)，即R0≥h=200 cm，設(shè)R0為200 cm和300 cm，采用Matlab分別模擬仿真，導(dǎo)航圓算法直線跟蹤仿真結(jié)果如圖4所示。

圖4 導(dǎo)航圓算法直線跟蹤仿真結(jié)果Fig.4 Simulation results of straight-line tracking based on navigation round algorithm

由圖4(a)位置變化過(guò)程可知，當(dāng)R0=200 cm時(shí)，所需調(diào)節(jié)距離為533 cm，當(dāng)R0=300 cm，所需調(diào)節(jié)距離為924 cm。由圖4整體結(jié)果可知，在h>80 cm時(shí)，兩種參數(shù)下的β均收斂迅速，在h<80 cm時(shí)，兩種參數(shù)下的β均較為平滑收斂?？傻贸鰧?dǎo)航圓算法在R0≥h條件下，導(dǎo)航圓R0越小，收斂速度越快，調(diào)節(jié)距離越短，而且在接近目標(biāo)直線時(shí)變化緩慢，不會(huì)產(chǎn)生超調(diào)量。

3.2 改進(jìn)的導(dǎo)航圓路徑跟蹤算法

由仿真結(jié)果可知，在滿足R0≥h的條件下，R0越小，調(diào)節(jié)距離越短；不同R0條件下，當(dāng)h<80 cm時(shí)，β均較為平滑收斂。所以動(dòng)態(tài)地調(diào)整導(dǎo)航圓半徑可以有效地改善調(diào)節(jié)速度。另外機(jī)器人在最終接近目標(biāo)直線時(shí)，需要使機(jī)器人緩慢靠近目標(biāo)直線。因此當(dāng)h>80 cm時(shí)，令R0=h，即arcsin(h/R0)=90°，機(jī)器人垂直地靠近目標(biāo)直線；當(dāng)h≤80 cm時(shí)，令R0=80 cm，機(jī)器人平緩安全地接近目標(biāo)直線，則改進(jìn)后的γ為

(9)

對(duì)首次改進(jìn)導(dǎo)航圓算法進(jìn)行模擬仿真，其中機(jī)器人自身物理參數(shù)和初始姿態(tài)參數(shù)與導(dǎo)航圓算法相同，如圖5(a)所示，發(fā)現(xiàn)x軸方向的調(diào)節(jié)距離大幅縮短至172 cm，但是如圖5(b)所示，β相對(duì)導(dǎo)航圓算法變化劇烈，容易使機(jī)器人在行進(jìn)時(shí)發(fā)生抖動(dòng)，導(dǎo)致機(jī)器人穩(wěn)定性變差。

圖5 首次改進(jìn)導(dǎo)航圓算法直線跟蹤仿真結(jié)果Fig.5 Simulation results of straigh-line tracking based on improved navigation circle algorithm for first time

考慮到機(jī)器人采用履帶式移動(dòng)機(jī)構(gòu)，該機(jī)構(gòu)因其具有轉(zhuǎn)向特性，即在轉(zhuǎn)向過(guò)程中存在較大的轉(zhuǎn)向阻力，導(dǎo)致機(jī)器人軌跡可控性差[20]，所以當(dāng)機(jī)器人β變化劇烈時(shí)，車身會(huì)出現(xiàn)比較大的抖動(dòng)，因此在機(jī)器人靠近目標(biāo)直線時(shí)偏航角需要平緩變化，避免因抖動(dòng)慣性導(dǎo)致震蕩的發(fā)生。

更進(jìn)一步發(fā)現(xiàn)γ=θ-β=α-arcsin(h/R0)-β中，α和β是不可更改量，影響γ的主要因素是反正弦函數(shù)arcsin(h/R0)，而R0又受h的制約，因此嘗試修正γ的計(jì)算公式，并引入一個(gè)新的調(diào)節(jié)參數(shù)ω，使γ為

γ=α-arctan(h/ω)-β

(11)

如圖6所示，ω不受h的制約，在[0，100]的范圍內(nèi)反正切函數(shù)arctan(h/ω)均大于反正弦函數(shù)arcsin(h/R0)；并且ω越小，反正切函數(shù)越大，則糾偏角變化越大，調(diào)節(jié)距離越短?？紤]到機(jī)器人的履帶移動(dòng)機(jī)構(gòu)特性以及運(yùn)動(dòng)慣性，ω的范圍設(shè)定在[50，80]之間，一般情況下ω取值為60，使得機(jī)器人既能快速調(diào)節(jié)，又符合實(shí)際要求。

圖6 反正切函數(shù)與反正弦函數(shù)對(duì)比Fig.6 Comparison between arctan function and arcsine function

采用最終改進(jìn)的導(dǎo)航圓算法進(jìn)行Matlab仿真，x軸方向調(diào)節(jié)距離如圖7(a)所示，調(diào)節(jié)距離為180 cm，β變化過(guò)程如圖7(b)所示，相對(duì)導(dǎo)航圓算法的首次改進(jìn)，位置和β變化更為平緩，機(jī)器人在跟蹤直線時(shí)可以更加穩(wěn)定和安全。

圖7 改進(jìn)后的導(dǎo)航圓算法直線跟蹤仿真結(jié)果Fig.7 Simulation results of straight-line tracking based on improved navigation round algorithm

圖8 光伏清掃機(jī)器人跟蹤系統(tǒng)工作流程Fig.8 Workflow of photovoltaic cleaning robot tracking system

4 跟蹤系統(tǒng)工作流程

根據(jù)所述原理，在硬件上有機(jī)器人機(jī)械本體、各類傳感器以及控制電路，軟件上有機(jī)器人控制算法、超寬帶定位算法，以及改進(jìn)后的導(dǎo)航圓算法，建立如圖8所示的基于改進(jìn)導(dǎo)航圓算法的光伏陣列清掃機(jī)器人路徑跟蹤系統(tǒng)工作流程。該流程解釋如下：在超寬帶定位系統(tǒng)和姿態(tài)傳感器使能的情況下，判斷當(dāng)前機(jī)器人車身狀態(tài)。當(dāng)機(jī)器人車身狀態(tài)為前進(jìn)，以每200 ms的間隔計(jì)算γ，之后利用PID控制，循環(huán)修正目標(biāo)偏航角，最終實(shí)現(xiàn)對(duì)目標(biāo)路徑的跟蹤。

對(duì)于PID的KI、KP和KD參數(shù)整定，首先將KI和KD取零，即消除微分和積分的作用，使用純比例控制，不斷增大KP的數(shù)值，觀察機(jī)器人調(diào)節(jié)響應(yīng)速度，直至達(dá)到一定范圍的超調(diào)。針對(duì)機(jī)器人出現(xiàn)的靜態(tài)誤差，不斷增大KI直至消除靜差，發(fā)現(xiàn)機(jī)器人超調(diào)量增大且有振蕩，引入KD并不斷微調(diào)KI、KP直至效果理想，機(jī)器人最終選擇PID參數(shù)分別為KP=16、KI=6、KD=2。

5 試 驗(yàn) 驗(yàn) 證

為了驗(yàn)證改進(jìn)導(dǎo)航圓算法的優(yōu)勢(shì)和正確性，如圖9所示，令機(jī)器人工作在光伏組件表面上，設(shè)置兩種不同的試驗(yàn)條件，并通過(guò)超寬帶定位系統(tǒng)得到機(jī)器人的實(shí)時(shí)位置信息，最后對(duì)位置信息進(jìn)行對(duì)比分析。

圖9 試驗(yàn)平臺(tái)Fig.9 Experimental platform

試驗(yàn)一：機(jī)器人以坐標(biāo)(30，700)處為起點(diǎn)，以0.1 m/s的速度沿著x軸正方向前行，分別采用導(dǎo)航圓算法和改進(jìn)導(dǎo)航圓算法跟蹤目標(biāo)直線y=500 cm，調(diào)節(jié)結(jié)果如圖10所示。通過(guò)分析試驗(yàn)結(jié)果，可知采用導(dǎo)航圓算法跟蹤目標(biāo)路徑，x軸方向的調(diào)節(jié)距離為513 cm，而改進(jìn)后的導(dǎo)航圓算法在相同的條件下，x軸方向的調(diào)節(jié)距離為247 cm。由試驗(yàn)可得，改進(jìn)后的導(dǎo)航圓算法調(diào)節(jié)距離為原導(dǎo)航圓算法調(diào)節(jié)距離的48.1%，機(jī)器人迅速回到目標(biāo)路徑上，并且?guī)缀鯚o(wú)超調(diào)量的產(chǎn)生。

圖10 直線路徑跟蹤對(duì)比實(shí)驗(yàn)結(jié)果Fig.10 Experimental results of linear path tracking

試驗(yàn)二：機(jī)器人起點(diǎn)為(30，500)，以0.1 m/s的速度沿x軸正方向前行，在行進(jìn)中人為轉(zhuǎn)動(dòng)30°，模擬機(jī)器人在組件上出現(xiàn)打滑的現(xiàn)象，分別采用兩種算法跟蹤目標(biāo)直線y=500 cm，擾動(dòng)下的調(diào)節(jié)結(jié)果如圖11所示，采用導(dǎo)航圓算法調(diào)節(jié)，調(diào)節(jié)距離為246 cm，而采用改進(jìn)的導(dǎo)航圓算法調(diào)節(jié)，調(diào)節(jié)距離為86 cm，可知改進(jìn)后的導(dǎo)航圓算法調(diào)節(jié)距離為原導(dǎo)航圓算法的34.9%。

圖11 擾動(dòng)下直線路徑跟蹤對(duì)比實(shí)驗(yàn)結(jié)果Fig.11 Experimental results of linear path tracking under disturbance

2種試驗(yàn)方法均表明，改進(jìn)后的導(dǎo)航圓算法相對(duì)原算法有更好的調(diào)節(jié)性，調(diào)節(jié)距離縮減50%以上，清掃機(jī)器人可以更為迅速地回到目標(biāo)路徑上完成相應(yīng)的工作。

6 結(jié) 語(yǔ)

本文在原有導(dǎo)航圓算法的基礎(chǔ)上，根據(jù)對(duì)算法原理的進(jìn)一步分析，提出了一種改進(jìn)后的導(dǎo)航圓路徑跟蹤算法。首先給出針對(duì)清掃機(jī)器人的運(yùn)動(dòng)學(xué)模型和定位系統(tǒng)，之后分析原有導(dǎo)航圓算法，提出假設(shè)并改進(jìn)算法，同時(shí)對(duì)算法進(jìn)行Matlab仿真。最終的對(duì)比試驗(yàn)結(jié)果表明，改進(jìn)后的導(dǎo)航圓算法較原有算法具有調(diào)節(jié)距離短、前后時(shí)刻偏航角變化量小的特點(diǎn)，機(jī)器人可以更加迅速和安全地回到原始路徑上。