基于DGPS-SINS組合導(dǎo)航系統(tǒng)的定日鏡清洗機(jī)器人設(shè)計(jì)

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

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

0 引言

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

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

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

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

圖1 定日鏡清洗機(jī)器人結(jié)構(gòu)圖

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

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

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

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

1 地面導(dǎo)航系統(tǒng)坐標(biāo)系(ENU)

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

圖2 地面導(dǎo)航系統(tǒng)坐標(biāo)系

將WGS-84坐標(biāo)系轉(zhuǎn)換到地面導(dǎo)航系統(tǒng)坐標(biāo)系，需要將基準(zhǔn)站ON的經(jīng)度、緯度和高度信息()B,L,H轉(zhuǎn)換至O-XYZ直角坐標(biāo)系下，即：

式中，e為WGS-84坐標(biāo)系橢球?qū)?yīng)的第一偏心率；N為緯度L處卯酉圈曲率半徑。

式中，a、b分別為WGS-84坐標(biāo)系橢球?qū)?yīng)的長半軸、短半軸。

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

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

式中，T為轉(zhuǎn)置。

則移動站在地面導(dǎo)航系統(tǒng)坐標(biāo)系下的坐標(biāo)由式(5)確定[11]：

式中，

DGPS-SINS差分定位定向時，通過解析包含有UTC時間、速率、航向角等數(shù)據(jù)的$GPFPD報文，按照式(1)將與機(jī)器人運(yùn)行相關(guān)的狀態(tài)信息轉(zhuǎn)換至地面導(dǎo)航系統(tǒng)坐標(biāo)系下，以進(jìn)行清洗機(jī)器人系統(tǒng)的導(dǎo)航與控制。

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

全局坐標(biāo)系下機(jī)器人位姿如圖3所示，圖中L1、C、Po、Φ分別為機(jī)器人輪基長度、參考控制點(diǎn)、質(zhì)心與前輪的偏角。機(jī)器人采用后輪驅(qū)動前輪轉(zhuǎn)向的結(jié)構(gòu)，其運(yùn)動學(xué)方程滿足Ackerman轉(zhuǎn)向幾何約束[12]。DGPS-SNIS系統(tǒng)在運(yùn)動過程中，由于受路面顛簸、發(fā)動機(jī)震動等干擾因素的影響，會出現(xiàn)噪聲及野值，使獲取的位姿數(shù)據(jù)受到干擾，進(jìn)而影響機(jī)器人的控制?？紤]到機(jī)器人運(yùn)動方程的非線性特性，本文采用EKF算法對機(jī)器人狀態(tài)數(shù)據(jù)進(jìn)行濾波。由于機(jī)器人系統(tǒng)在地面上運(yùn)動，所以這里暫不考慮其在高度方向的狀態(tài)變化。

圖3 全局坐標(biāo)系下機(jī)器人位姿

機(jī)器人系統(tǒng)運(yùn)動學(xué)模型可表示為[13]：

式中，v、ω分別為機(jī)器人運(yùn)行的速度、本體轉(zhuǎn)動角速度[13-14]；為機(jī)器人在參考點(diǎn)C的速度在地面導(dǎo)航系統(tǒng)坐標(biāo)系下的分量；θ為車體航向角。

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

設(shè)系統(tǒng)噪聲W為白噪聲，則式(7)可表示為普遍狀態(tài)方程的形式，即：

式中，Xk為第k時刻計(jì)算的X值。

式(8)為非線性方程，在當(dāng)前狀態(tài)處Taylor公式展開進(jìn)行線性化處理，得到系統(tǒng)的Jacobian矩陣，可表示為：

式中，F(xiàn)v為X的偏導(dǎo)數(shù)。

系統(tǒng)觀測方程為：

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

Kalman濾波方程式[15]為：

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

3 機(jī)器人SLAM算法

3.1 激光雷達(dá)測量模型

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

圖4 激光雷達(dá)測量模型

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

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

圖5 試驗(yàn)環(huán)境下對隨機(jī)設(shè)置的不同形狀的物體激光雷達(dá)數(shù)據(jù)聚類結(jié)果

3.2 EKF-SLAM算法

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

行進(jìn)過程中，由運(yùn)動學(xué)方程可將機(jī)器人位姿xv,k表示為：

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

圖6 鏡場SLAM場景示意圖

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

式中，Vk為零均值，協(xié)方差為Rk的高斯白噪聲；對應(yīng)的轉(zhuǎn)換矩陣。

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

預(yù)測階段需要在車輛運(yùn)動后更新系統(tǒng)狀態(tài)向量和協(xié)方差矩陣。

系統(tǒng)協(xié)方差矩陣Pk可表示為：

系統(tǒng)協(xié)方差矩陣Pk還可表示為：

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

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

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

協(xié)方差矩陣Pzz,k可表示為：

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

式中，Pk+1|k為根據(jù)k時刻的Pk值及系統(tǒng)模型預(yù)測的第k+1時刻的P值。

▽Hk可表示為：

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

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

其他情況以此類推。

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

新狀態(tài)向量的協(xié)方差矩陣為：

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

清洗機(jī)器人現(xiàn)場運(yùn)行時，通過傳感器不斷測量，將新觀測到的立柱向量加入到聯(lián)合狀態(tài)向量，并對清洗機(jī)器人估計(jì)的位置進(jìn)行更新，完成清洗機(jī)器人局部環(huán)境定位和建圖，對清洗機(jī)器人局部運(yùn)行軌跡進(jìn)行修正優(yōu)化，降低清洗機(jī)器人與定日鏡碰撞的風(fēng)險。

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

4 仿真測試及分析

對設(shè)計(jì)的組合導(dǎo)航算法進(jìn)行了基于模型機(jī)器人的圓形軌跡跟蹤測試，并對基于激光雷達(dá)的SLAM算法進(jìn)行了仿真驗(yàn)證。

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

圖7 圓形軌跡局部放大圖

圖8 Kalman濾波誤差

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

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

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

從圖7和圖10可以看出，算法對設(shè)備輸出的具有劇烈抖動特性的原始數(shù)據(jù)進(jìn)行了有效地濾波和平滑，保證了機(jī)器人導(dǎo)航和控制系統(tǒng)的精度和穩(wěn)定性。在實(shí)際測試過程中也發(fā)現(xiàn)，這種高頻抖動的原始數(shù)據(jù)也會給控制執(zhí)行機(jī)構(gòu)帶來劇烈抖動，會嚴(yán)重影響控制執(zhí)行機(jī)構(gòu)的安全性能，設(shè)計(jì)的濾波算法有效地解決了上述問題。

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

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

圖12 清洗機(jī)器人SLAM路徑導(dǎo)航結(jié)果

5 結(jié)論

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