基于DGPS-SINS組合導航系統的定日鏡清洗機器人設計

■徐能 宓霄凌＊ 張旭中 胡玉超 陳康立 薛剛強 熊步青

(1.浙江中控太陽能技術有限公司；2.青海中控太陽能發電有限公司)

0 引言

定日鏡作為塔式太陽能熱發電站中聚光集熱系統的核心部件，其光學效率會直接影響整個電站的性能。由于定日鏡在戶外環境運行，空氣中的粉塵及其他污染物等逐漸在其表面堆積，使其表面的光學反射率下降。因此，必須設計高效的定日鏡清洗方式，對定日鏡進行周期性清洗，以提升整個電站的發電效率。

目前，針對世界范圍內已投運的塔式太陽能熱發電站的定日鏡清洗方式，有大量的研究和試驗。Sener公司和Torresol Energy公司針對西班牙已投運的19.9 MW Gemasolar塔式熱發電站聯合開發了一款智能清洗機器人[1]，該機器人被放置在水平鏡面上，可自動行走完成清掃。美國Bright Source公司針對其2014年2月投運的392 MW Ivanpah塔式熱發電站設計了一款基于GPS定位的自動清洗車[2]，清洗車本體為一臺起重機，清洗刷輥通過一根長24.5m的可伸縮機械臂固定在本體上，在同一個位置可清洗約24.5m范圍內的定日鏡，并可通過增加機械臂長度來減少清洗車在鏡場內的行進里程。美國2015年3月投運的新月沙丘110MW塔式熱發電站采用的清洗車是通過在重型車輛本體上搭載水箱的方式，由人工操縱清洗定日鏡。

雖然上述清洗方式在清洗單個鏡面時可實現自動清洗，但在整個鏡場的清洗過程中仍需要人工干預，如人工控制機械臂的位置、控制執行機構的姿態、移動清洗車位置等。這樣既增加了運維成本，又使在清洗過程中待清洗的定日鏡必須由正常運行狀態切換至清洗狀態，導致這部分定日鏡無法用于“追日”，影響了鏡場的可利用率。

基于上述分析，浙江中控太陽能技術有限公司以中控德令哈50MW塔式太陽能熱發電站為背景，設計了一款全自主執行清洗任務的機器人，可用于塔式太陽能熱發電站定日鏡的清洗，結構如圖1所示。該清洗機器人通過在本體上搭載清洗水箱，并結合高壓噴嘴、刷輥及精密的距離傳感器控制執行機構主動靠近鏡面，以完成清洗任務。

圖1 定日鏡清洗機器人結構圖

由于該清洗機器人的行進和清洗任務均是在無人工干預的情況下自主完成的，因此可將清洗任務安排在鏡場關場后的非日間時段。這樣做避免了日間清洗時定日鏡處于待清洗狀態而造成的能量損失，有效提高了鏡場的運營效率，降低了運維成本。

移動機器人的自主導航方式一般包括GPS導航、慣性導航(INS)、激光雷達導航[3-9]等。GPS導航具有全球性、全天候、高精度和三維定位等優點，但也存在數據易丟失、可靠性差等缺點；INS完全依靠載體自身設備獨立導航；不與外界發生聯系，數據不易受到干擾，但是INS的誤差會隨著時間累積，不適合長距離導航。所以將兩者進行融合，可有效提高導航的精度和可靠性。然而在機器人行進過程中，不可避免地會出現GPS-INS信息丟失、行進軌跡發生偏移等情況，此時需要機器人具備感知周圍局部環境的能力，激光雷達導航因測量精度高、數據量小、不受環境中光線影響等優點，在機器人局部環境感知領域得到了廣泛應用[6]。

基于上述分析，結合清洗機器人需具備高精度運行軌跡的要求，本文擬采用差分全球定位系統(Differential Global Positioning System，DGPS)與捷聯慣性導航系統(Strapdown Inertial Navigation System，SINS)相組合的導航方式，以獲得機器人cm級的定位精度。同時，結合二維激光雷達完成障礙物檢測，并充分利用鏡場運行環境特性，開發了基于定日鏡立柱識別的同時定位和建圖(Simultaneous localization and mapping,SLAM)[7-10]，對機器人清洗過程中的路徑進行優化，降低了因DGPS-SNIS故障或漂移導致清洗機器人與定日鏡發生碰撞的風險。該清洗機器人在行進過程中，結合紅外相機、可見光相機等測量單元，對定日鏡相對機器人的姿態進行識別，完成鏡面防碰撞檢測，確保自主執行任務期間的穩定性和安全性。

本文主要針對定日鏡清洗機器人的組合導航系統進行設計和驗證。首先，建立了機器人系統地面導航系統坐標系，給出了WGS-84坐標系至當地導航系統坐標系的轉換方程；其次，建立了基于擴展卡爾曼濾波(Extended Kalman Filer，EKF)算法的機器人導航方程；然后，介紹了二維激光雷達測量原理，對點云數據聚類算法進行了設計和測試，同時根據聚類結果對立柱特征進行模板匹配，基于對立柱的觀測信息研究了清洗機器人即時定位與地圖構建(SLAM)算法；最后，基于模型樣車對設計的導航系統在模擬鏡場環境下進行測試驗證。

1 地面導航系統坐標系(ENU)

由于GPS廣播星歷采用WGS-84坐標系，其輸出為測點的經度B、緯度L和高度H信息，不利于地面導航系統方程的建立，因此需要將其轉換至地面導航系統坐標系ON-ENU下。該坐標系原點為地面上1個已知精確經度、緯度、高度信息的測站點ON；ONN軸為過點ON的子午線切線，指向正北方向；ONE軸為過點ON的橢球平行圈切線，指向正東方向；ONU由ONE、ONN根據右手定則確定，指向天頂方向。具體如圖2所示。

圖2 地面導航系統坐標系

將WGS-84坐標系轉換到地面導航系統坐標系，需要將基準站ON的經度、緯度和高度信息()B,L,H轉換至O-XYZ直角坐標系下，即：

式中，e為WGS-84坐標系橢球對應的第一偏心率；N為緯度L處卯酉圈曲率半徑。

式中，a、b分別為WGS-84坐標系橢球對應的長半軸、短半軸。

在機器人實時測量時，將獲取的WGS-84坐標系下的經度、緯度、高度坐標(Bm,Lm,Hm)按照式(1)計算得到其空間直角坐標為(Xm,Ym,Zm)。

移動站和基站間的相對坐標[ΔX,ΔY,ΔZ]T為：

式中，T為轉置。

則移動站在地面導航系統坐標系下的坐標由式(5)確定[11]：

式中，

DGPS-SINS差分定位定向時，通過解析包含有UTC時間、速率、航向角等數據的$GPFPD報文，按照式(1)將與機器人運行相關的狀態信息轉換至地面導航系統坐標系下，以進行清洗機器人系統的導航與控制。

2 擴展卡爾曼濾波(EKF)方程

全局坐標系下機器人位姿如圖3所示，圖中L1、C、Po、Φ分別為機器人輪基長度、參考控制點、質心與前輪的偏角。機器人采用后輪驅動前輪轉向的結構，其運動學方程滿足Ackerman轉向幾何約束[12]。DGPS-SNIS系統在運動過程中，由于受路面顛簸、發動機震動等干擾因素的影響，會出現噪聲及野值，使獲取的位姿數據受到干擾，進而影響機器人的控制。考慮到機器人運動方程的非線性特性，本文采用EKF算法對機器人狀態數據進行濾波。由于機器人系統在地面上運動，所以這里暫不考慮其在高度方向的狀態變化。

圖3 全局坐標系下機器人位姿

機器人系統運動學模型可表示為[13]：

式中，v、ω分別為機器人運行的速度、本體轉動角速度[13-14]；為機器人在參考點C的速度在地面導航系統坐標系下的分量；θ為車體航向角。

式中，為車體航向角速度；Ts為采樣周期；aω為角加速度；k為時刻；ax為x方向的加速度；ay為y方向的加速度。

設系統噪聲W為白噪聲，則式(7)可表示為普遍狀態方程的形式，即：

式中，Xk為第k時刻計算的X值。

式(8)為非線性方程，在當前狀態處Taylor公式展開進行線性化處理，得到系統的Jacobian矩陣，可表示為：

式中，Fv為X的偏導數。

系統觀測方程為：

式中，Zk為k時刻的測量值(觀測值)，是濾波的輸入為狀態變量到測量(觀測)的轉換矩陣，H1=[I5×505×1]，其中I為單位矩陣；Vk為觀測噪聲。

Kalman濾波方程式[15]為：

初始時刻給定狀態和誤差矩陣初值與P0，結合濾波方程式(11)在k-1時刻的狀態預測值為,k-1，通過測量值Zk對預測值修正即可獲得狀態估值。

3 機器人SLAM算法

3.1 激光雷達測量模型

單線激光雷達測量原理如圖4所示。

圖4 激光雷達測量模型

圖4中，ρi為第i個測點pi到測量探頭的距離；φi為pi與x軸方向的夾角。單線激光雷達的掃描角度范圍為-45°～225°，掃描角分辨率Δφ=0.25°，完成一次掃描共返回1081個以極坐標(ρi,φi)形式表示的數據點。

本文采用基于密度分布的聚類(Density-based spatialclusteringofapplicationswithnoise，DBSCAN)算法[16-17]，對激光雷達測量的原始數據進行聚類，并進行識別和模板匹配，基本的模板包括基于圓形輪廓辨識的圓柱形目標、基于直線識別[18]的規則目標物和其他不規則物體，以區分出立柱和障礙物。由于在鏡場建設時已獲得全鏡場立柱的精確坐標，所以對立柱的識別可用于機器人局部環境定位和建圖；對障礙物進行識別用于機器人防撞。圖5為試驗環境下對隨機設置的不同形狀的物體聚類測試結果，其中白色“o”為原始數據中的測量噪點，不同顏色類代表掃描范圍內不同的物體。該算法能對目標物有效聚類，并濾除測量到的噪聲。因此，可根據掃描的立柱信息，獲得目標相對于機器人的位置和方位。

圖5 試驗環境下對隨機設置的不同形狀的物體激光雷達數據聚類結果

3.2 EKF-SLAM算法

鏡場SLAM問題的基本場景如圖6所示：清洗機器人在鏡場內清洗，通過DGPS-SINS獲得自己的全局位姿，同時利用測到的立柱信息遞增的建立機器人導航地圖，完成機器人即時建圖和定位，對機器人運行軌跡進行優化確認。

行進過程中，由運動學方程可將機器人位姿xv,k表示為：

式中，xv,k、yv,k分別為k時刻x方向、y方向的速度；Qv,k為k時刻的航向角；Wk為零均值，協方差為Qk的高斯白噪聲。

圖6 鏡場SLAM場景示意圖

將第fi個立柱在機器人移動過程中的觀測向量Zk的方程可表示為距離ρi和方位φi的形式，即：

式中，Vk為零均值，協方差為Rk的高斯白噪聲；對應的轉換矩陣。

本節基于式(11)的EKF基本框架推導機器人的SLAM算法。根據機器人運動時序，將SLAM算法分成預測、觀測、更新和增廣4個階段。

預測階段需要在車輛運動后更新系統狀態向量和協方差矩陣。

系統協方差矩陣Pk可表示為：

系統協方差矩陣Pk還可表示為：

式中，Pvv為機器人位姿協方差矩陣；Pvm為機器人和立柱的互協方差矩陣，且有Pmm為立柱協方差矩陣。

觀測階段通過激光雷達掃描獲取立柱的狀態信息，觀測向量的預測值線性化后形成式(16)：

式中，▽Hk為觀測方程的Jacobian矩陣。

協方差矩陣Pzz,k可表示為：

式(17)展開后可簡化為：

式中，Pk+1|k為根據k時刻的Pk值及系統模型預測的第k+1時刻的P值。

▽Hk可表示為：

針對第1根觀測到的立柱，則有：

對第2根觀測到的立柱，則有：

其他情況以此類推。

下一步利用預測值和觀測值來更新系統的狀態向量和協方差矩陣，具體的步驟為式(22)～式(24)：

新狀態向量的協方差矩陣為：

式中，▽gx，▽gz分別為g對xv,k和zj,k的Jacobian矩陣。

清洗機器人現場運行時，通過傳感器不斷測量，將新觀測到的立柱向量加入到聯合狀態向量，并對清洗機器人估計的位置進行更新，完成清洗機器人局部環境定位和建圖，對清洗機器人局部運行軌跡進行修正優化，降低清洗機器人與定日鏡碰撞的風險。

由于機器人執行清洗任務的清洗行有幾百甚至上千個被掃描的“路標”立柱，隨著移動機器人觀測路標的增多，系統狀態向量的維數不斷增加，協方差矩陣和Jacobian矩陣的計算量急劇加大，從而導致算法運行的效率和定位精度[19]降低，因此本方案采用以DGPS-SINS導航為主，以基于激光雷達的機器人SLAM導航為輔的組合導航方式，有效完成清洗機器人全局和局部環境下的自主導航，提高了系統的精度、穩定性和容錯魯棒性。

4 仿真測試及分析

對設計的組合導航算法進行了基于模型機器人的圓形軌跡跟蹤測試，并對基于激光雷達的SLAM算法進行了仿真驗證。

模型機器人基本參數為輪基長度L1=1.02 m，GPS幀率為20Hz，控制周期為0.1s，機器人名義線速度vn=0.4m/s，圓形軌跡半徑R=7.48m，測試結果如圖7～圖11所示。

圖7 圓形軌跡局部放大圖

圖8 Kalman濾波誤差

圖9 車體航向角θ跟蹤曲線

圖10 車體航向角θ跟蹤曲線局部放大圖

圖11 車體航向角θ跟蹤誤差曲線

從圖7和圖10可以看出，算法對設備輸出的具有劇烈抖動特性的原始數據進行了有效地濾波和平滑，保證了機器人導航和控制系統的精度和穩定性。在實際測試過程中也發現，這種高頻抖動的原始數據也會給控制執行機構帶來劇烈抖動，會嚴重影響控制執行機構的安全性能，設計的濾波算法有效地解決了上述問題。

仿真開始前約15s內，機器人位于圓形軌跡外任意一點并處于靜止的預備狀態。15s以后，機器人開始進行軌跡跟蹤，測試結果表明，Kalman濾波器對機器人位置的估計誤差均值為0.023m，對航向角估計誤差均值為0.0043°。由于模型機器人GPS天線基線長度與機器人輪基長度均為1.02 m，所以機器人在靜止狀態下，前后天線的定位波動會導致約±1.5°的航向角計算誤差；一旦機器人處于移動狀態，該誤差會降低且趨于穩定(如圖11所示)。而實際的清洗機器人輪基長度約為2m，因此估計的航向角精度將優于此次測試的情況。

基于二維激光雷達的清洗機器人SLAM算法仿真測試結果如圖12所示。清洗機器人在行進過程中，不斷掃描獲取清洗機器人周圍的立柱信息并完成對立柱的匹配；進而結合該觀測量對清洗機器人進行局部環境下的定位和建圖，有效降低了系統的運行風險。

圖12 清洗機器人SLAM路徑導航結果

5 結論

針對現有塔式太陽能熱發電站定日鏡清洗方案存在的運維成本高、影響鏡場利用率、人工干預清洗效率低等突出問題，本文設計了一款全自主清洗機器人，重點設計了基于DGPS-SINS組合導航系統的機器人，同時針對機器人導航系統設計了EKF算法，對系統狀態進行濾波和最優估計，有效提高了導航系統的精度和穩定性；并結合太陽能熱發電站定日鏡布局特性和機器人運行環境特點，提出了采用二維激光雷達實現機器人SLAM算法，通過多傳感器融合方案，優化了機器人清洗過程中的位置精度，降低了機器人本體與鏡面碰撞的風險。最后，對設計的方案進行了樣機測試仿真驗證，結果表明，設計的方案和算法能夠滿足鏡場環境下清洗機器人的運行需求。