基于改進導航圓算法的光伏陣列清掃機器人路徑跟蹤控制

丁 坤，王 立，張經煒，李辰陽，翁 帥

(1.河海大學機電工程學院，江蘇 常州 213022；2.常州市光伏系統集成與生產裝備技術重點實驗室，江蘇 常州 213022)

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

移動機器人的研究涉及諸多科學領域，其中智能控制是移動機器人的前沿課題，主要指移動機器人的定位導航、路徑規劃和路徑跟蹤等問題[8]。路徑跟蹤是指在平面坐標系下一條滿足任務需求的理想幾何路徑，移動機器人從平面上某一點出發，按照某種控制規律到達該路徑，并沿著該理想幾何路徑實現跟蹤的控制運動[9]。對于路徑跟蹤問題，國外的Kanayama等[10]提出了一種回旋曲線跟蹤路徑；而對于國內學者，郝存明等[11]提出了基于滑?？刂频氖覂纫苿訖C器人路徑跟蹤，尉成果等[12]提出了一種輪式移動機器人的軌跡跟蹤算法，第一次提出了導航圓的概念。本文在導航圓算法的基礎上進行改進，進一步提高移動機器人路徑跟蹤性能。

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

1 清掃機器人的運動數學模型

1.1 機器人基本機構

為了適應不同的工作環境，一般室內移動機器人通常采用輪式移動機構，室外移動機器人為了適應野外環境的需要，多采用履帶式移動機構[13]，所以本文機器人選擇履帶式移動機構，該機構相對輪式移動機構跨越障礙能力優越[14]，對組件表面壓強小且行走方式更靈活。

1.2 機器人運動數學模型

為了了解機器人行走方式與電機轉速的關系，從而更好地控制機器人的運動，需要對機器人進行運動分析。建立如圖1所示的光伏陣列清掃機器人運動數學模型，假定機器人以P點為圓心從C1點移動至C2點，C為機器人幾何中心。則機器人的運動數學模型[15]為

(1)

圖1 光伏陣列清掃機器人運動數學模型 Fig.1 Kinematic model of photovoltaic array cleaning robot

式中：x、y——機器人的位置；vl——左帶輪線速度；vr——右帶輪線速度；L——左右履帶間距；δ——機器人相對x軸的轉角；v、ω——C點線速度和角速度。

2 清掃機器人的超寬帶定位系統

UWB定位是一種基于極窄脈沖的無線技術，因其具有傳輸速率高(最高可達1 000 Mbps以上)、發射功率和功耗低，以及穿透性較強的特點而廣泛應用于科學研究[16-17]。

UWB定位系統一般設有主動發射標簽和多個接收基站。采用到達時間(time of arrival，TOA)算法或到達時間差(time difference of arrival，TDOA)算法來測距定位，接收基站接收來自發射標簽的超寬帶信號，過濾掉電磁波在傳輸過程中的噪聲干擾，得到信號中的有效信息[18]，利用CPU對信息進行解碼和計算，最終實現對發射標簽的定位[19]。

為實現機器人的定位功能，建立如圖2所示的光伏陣列清掃機器人超寬帶定位系統，將1個主動發射標簽設置在機器人的幾何中心，將4個接收基站設置在組件陣列四周，采用TOA算法進行位置計算，之后通過無線通信上傳到上位機中，用于后續跟蹤算法，并在顯示器上實時顯示機器人的位置信息。設計的UWB系統定位精度在20 cm之內。

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

3 改進的導航圓路徑跟蹤算法

3.1 導航圓路徑跟蹤算法

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

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

如圖3所示，在機器人逐漸靠近目標直線的過程中，糾偏角(機器人當前方向與目標方向的夾角)γ逐漸減小，當γ=0時，即可實現目標跟蹤。

已知機器人到目標直線y=k*x+b的距離h[12]為

(2)

式中：xc、yc——機器人實時位置坐標；k*、b——目標直線的斜率和縱截距。

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

θ=α-arcsin(h/R0)

(3)

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

(4)

式中：α——目標直線路徑傾角；β——機器人前進方向與x軸的夾角，即偏航角；θ——預測偏航角。

利用PID控制器對γ反饋調節，直至γ為0，機器人回歸目標直線。PID控制的原理為

(5)

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

對γ采用增量式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——積分系數；KD——微分系數；T——采樣周期。由式(6)得

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

(7)

則機器人在糾偏過程中任意k時刻速度分配為

(8)

對于目標路徑為圓弧的導航圓跟蹤算法，其思想與直線跟蹤相同，只是由跟蹤已知直線變為跟蹤動態直線，動態直線是跟蹤圓弧上的切線，但光伏陣列清掃機器人主要是直線運動，所以不具體分析。

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

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

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

3.2 改進的導航圓路徑跟蹤算法

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

(9)

對首次改進導航圓算法進行模擬仿真，其中機器人自身物理參數和初始姿態參數與導航圓算法相同，如圖5(a)所示，發現x軸方向的調節距離大幅縮短至172 cm，但是如圖5(b)所示，β相對導航圓算法變化劇烈，容易使機器人在行進時發生抖動，導致機器人穩定性變差。

圖5 首次改進導航圓算法直線跟蹤仿真結果Fig.5 Simulation results of straigh-line tracking based on improved navigation circle algorithm for first time

考慮到機器人采用履帶式移動機構，該機構因其具有轉向特性，即在轉向過程中存在較大的轉向阻力，導致機器人軌跡可控性差[20]，所以當機器人β變化劇烈時，車身會出現比較大的抖動，因此在機器人靠近目標直線時偏航角需要平緩變化，避免因抖動慣性導致震蕩的發生。

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

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

(11)

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

圖6 反正切函數與反正弦函數對比Fig.6 Comparison between arctan function and arcsine function

采用最終改進的導航圓算法進行Matlab仿真，x軸方向調節距離如圖7(a)所示，調節距離為180 cm，β變化過程如圖7(b)所示，相對導航圓算法的首次改進，位置和β變化更為平緩，機器人在跟蹤直線時可以更加穩定和安全。

圖7 改進后的導航圓算法直線跟蹤仿真結果Fig.7 Simulation results of straight-line tracking based on improved navigation round algorithm

圖8 光伏清掃機器人跟蹤系統工作流程Fig.8 Workflow of photovoltaic cleaning robot tracking system

4 跟蹤系統工作流程

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

對于PID的KI、KP和KD參數整定，首先將KI和KD取零，即消除微分和積分的作用，使用純比例控制，不斷增大KP的數值，觀察機器人調節響應速度，直至達到一定范圍的超調。針對機器人出現的靜態誤差，不斷增大KI直至消除靜差，發現機器人超調量增大且有振蕩，引入KD并不斷微調KI、KP直至效果理想，機器人最終選擇PID參數分別為KP=16、KI=6、KD=2。

5 試 驗 驗 證

為了驗證改進導航圓算法的優勢和正確性，如圖9所示，令機器人工作在光伏組件表面上，設置兩種不同的試驗條件，并通過超寬帶定位系統得到機器人的實時位置信息，最后對位置信息進行對比分析。

圖9 試驗平臺Fig.9 Experimental platform

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

圖10 直線路徑跟蹤對比實驗結果Fig.10 Experimental results of linear path tracking

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

圖11 擾動下直線路徑跟蹤對比實驗結果Fig.11 Experimental results of linear path tracking under disturbance

2種試驗方法均表明，改進后的導航圓算法相對原算法有更好的調節性，調節距離縮減50%以上，清掃機器人可以更為迅速地回到目標路徑上完成相應的工作。

6 結 語

本文在原有導航圓算法的基礎上，根據對算法原理的進一步分析，提出了一種改進后的導航圓路徑跟蹤算法。首先給出針對清掃機器人的運動學模型和定位系統，之后分析原有導航圓算法，提出假設并改進算法，同時對算法進行Matlab仿真。最終的對比試驗結果表明，改進后的導航圓算法較原有算法具有調節距離短、前后時刻偏航角變化量小的特點，機器人可以更加迅速和安全地回到原始路徑上。