本發(fā)明涉及車式機器人控制領(lǐng)域，具體的說是一種面向輸電線路巡檢應(yīng)用的無人直升機地面控制系統(tǒng)。
背景技術(shù)：
：管道作為一種重要的傳輸媒介在很多方面發(fā)揮著越來越重要的作用。由于管道較窄，在惡劣的環(huán)境下有效地對管道進(jìn)行除塵、清洗在預(yù)防安全事故、減少環(huán)境污染以及避免經(jīng)濟損失等方面具有重要的價值。然而維持車體的直線行駛是車體進(jìn)入管道的基本要求，也是有效除塵以及清洗的重要保證。針對方式管道，車輛在進(jìn)入管道以及行走過程中不可避免地存在跑偏問題，為了避免車輛與管道內(nèi)壁發(fā)生不必要的碰撞，導(dǎo)致管道以及車體零部件受損，將車體與管道中心線保持高度的重合性尤為重要。然而當(dāng)前方法的處理方式主要是解決有明顯管道道路信息以及標(biāo)記信息的情況，對于缺少道路信息以及反光較嚴(yán)重的特殊管道，現(xiàn)有方法無法解決。技術(shù)實現(xiàn)要素：針對現(xiàn)有技術(shù)中存在的上述不足之處，本發(fā)明要解決的技術(shù)問題是提供一種基于視覺傳感器系統(tǒng)的管道車輛糾偏方法，實現(xiàn)車式機器人在管道中直線行走本發(fā)明為實現(xiàn)上述目的所采用的技術(shù)方案是：一種基于視覺傳感器系統(tǒng)的管道車輛糾偏方法，包括以下步驟：將兩個線結(jié)構(gòu)光傳感器平行放置于相機兩側(cè)，垂直于車體中軸，安裝在車體一側(cè)；用圓形標(biāo)定板標(biāo)定相機的內(nèi)參數(shù)和線結(jié)構(gòu)光傳感器的光平面參數(shù)；利用相機對圖像進(jìn)行采集以及數(shù)字化存儲，對采集到的圖像進(jìn)行處理，得到去除噪聲和干擾的感興趣區(qū)域圖像；將感興趣區(qū)域圖像的二值輪廓中所有非零點圖像像素坐標(biāo)值按橫坐標(biāo)區(qū)分 為兩組非零點，去畸變，并將其轉(zhuǎn)換到相機坐標(biāo)系下；分別對區(qū)分后的兩組非零點的三維點坐標(biāo)值求取平均值，得到相應(yīng)的兩個點的坐標(biāo)值；車體在管道中運行過程中，實時計算車體的偏移角度以及偏移量，并將偏移角度以及偏移量轉(zhuǎn)換到舵機中心點坐標(biāo)系上；同時，將車體的姿態(tài)進(jìn)行在線反饋，進(jìn)行姿態(tài)補償，不斷消除位姿偏差以使車體與管道中心線保持重合。所述對采集到的圖像進(jìn)行處理包括以下步驟：對采集的圖像進(jìn)行高斯平滑處理；將平滑后的圖像進(jìn)行全局閾值分割，即對整幅圖像使用同一閾值做分割處理，提取出感興趣的前景光條紋區(qū)域；對分割后的圖像做兩次腐蝕操作，去除前景光條紋區(qū)域邊緣的雜點，進(jìn)而提取圖像中的輪廓，去除輪廓區(qū)域面積較大和較小的區(qū)域。所述將感興趣區(qū)域圖像的二值輪廓中所有非零點圖像像素坐標(biāo)值進(jìn)行區(qū)分，去畸變，并將其轉(zhuǎn)換到相機坐標(biāo)系下，包括以下步驟：令Pi(x,y)是二值化圖像后的非零點，其中i＝1,2,3…W，W表示點數(shù)，x,y表示像素坐標(biāo)值；遍歷所有的非零點，如果x＞width/2，其中，width表示圖像寬度，則將該非零點作為相機右側(cè)結(jié)構(gòu)光傳感器發(fā)射到管道壁上的點；如果x＜width/2，將該非零點作為相機左側(cè)結(jié)構(gòu)光傳感器發(fā)射到管道壁上的點；利用相機內(nèi)參數(shù)對圖像進(jìn)行去畸變，并轉(zhuǎn)換到圖像的物理坐標(biāo)系下，然后通過標(biāo)定好的兩組光平面參數(shù)將結(jié)構(gòu)光傳感器發(fā)射到管道壁上的點轉(zhuǎn)換到相機坐標(biāo)系下；將Pr經(jīng)轉(zhuǎn)換后的點記為Pri(x,y,z)，i＝1,2,3…m，Pl經(jīng)轉(zhuǎn)換后的點記為Pli(x,y,z)，i＝1,2,3…n，按照下式求取點prp和plp：prpx=Σi=1mPrixm,prpy=0,prpz=Σi=1mPrizm---(1)]]>plpx=Σi=1nPlixn,prpy=0,prpz=Σi=1nPlizn---(2)]]>其中，prpx,prpy,prpz是點prp的坐標(biāo)值，Prix,Priz分別是Pri的x,z坐標(biāo)值，plpx,plpy,plpz是點plp的坐標(biāo)值，Plix,Pliz分別是Pli的x,z坐標(biāo)值。所述車體的偏移角度以及偏移量通過下式計算：θ=(P1z-P2z)(P1x-P2x)P1x≠P2xθ=0P1x=P2x---(3)]]>d=(P2z+|P2x|*tanθ1)*cosθ1-LP1z>P2zd=(P1z+|P1x|*tanθ1)*cosθ1-LP1z<P2z---(4)]]>其中，θ是相機坐標(biāo)系下車體偏轉(zhuǎn)角，P1x,P1z分別是相機右側(cè)線結(jié)構(gòu)光平均x和z坐標(biāo)值，P2x,P2z分別是相機左側(cè)線結(jié)構(gòu)光平均x和z坐標(biāo)值，d是相機光心位置處距基準(zhǔn)位置的偏移量，L是車體在管道中心線基準(zhǔn)位置處相機光心離管道壁的距離。所述將偏移角度以及偏移量轉(zhuǎn)換到舵機中心點坐標(biāo)系上，具體為：θ2＝θ1(5)d2＝d+m*sinθ2+n*(cosθ2-1)(6)其中，d2是以舵機中心點為參考點的偏移量，m是舵機的中心點離相機中心點的縱向距離，θ2是以舵機中心點為參考點的車體偏移角度，n是舵機的中心點離相機中心點的橫向距離。所述將車體的姿態(tài)進(jìn)行在線反饋，進(jìn)行姿態(tài)補償，包括以下步驟：對車體的姿態(tài)進(jìn)行檢測，獲得當(dāng)前時刻i的偏差信息di,θi；通過驅(qū)動電機的反饋信息及內(nèi)存的存儲信息獲得此時的速度信息與舵角信息μi,φi；將上述信息作為控制的輸入量，利用糾偏控制算法得到為消除上述偏差信 息舵機需要轉(zhuǎn)過的角度信息φi+1，利用公式(7)計算舵角信息對應(yīng)的脈沖寬度，控制舵機以實現(xiàn)一次糾偏流程；pwi＝(((90-φi)/180)*2+0.5)*1000+50(7)其中φi是舵角大小，pwi是舵角大小對應(yīng)的脈沖寬度；通過實時閉環(huán)控制，在有限時間內(nèi)使得θ→0,d→0，不斷消除位姿偏差以使車體與管道中心線保持重合。本發(fā)明具有以下優(yōu)點及有益效果：(1)本發(fā)明所述的車輛糾偏方法采用一個相機和兩個線結(jié)構(gòu)光進(jìn)行特殊組合形成視覺傳感器系統(tǒng)，成本低。(2)本發(fā)明所述的車輛糾偏方法利用視覺傳感器系統(tǒng)進(jìn)行導(dǎo)航，對管道有無道路信息沒有要求，僅需要將車體放置于方形管道中，車體即可沿直線行走，避免車體和管道壁進(jìn)行碰撞，避免損壞車體以及管道壁。(3)本發(fā)明所述的車輛糾偏方法采用了有效的圖像處理技術(shù)，可以將車體的姿態(tài)實時反饋給控制系統(tǒng)，便于車體進(jìn)行及時糾偏，因此車體在行駛過程中不會有較大的偏移，操作速度較快，節(jié)省了操作時間。附圖說明圖1為本發(fā)明流程圖；圖2為車體導(dǎo)航示意圖；圖3為視覺控制對于車體偏移量以及偏移角度的影響示意圖；圖4為車體前進(jìn)中的視覺導(dǎo)航控制曲線圖；圖5為車體后退中的視覺導(dǎo)航控制曲線圖；圖6為車體前進(jìn)和后退中的視覺導(dǎo)航控制曲線圖。具體實施方式下面結(jié)合附圖及實施例對本發(fā)明做進(jìn)一步的詳細(xì)說明。采用結(jié)合一個相機和兩個線結(jié)構(gòu)光的視覺測量技術(shù)，標(biāo)定相機內(nèi)參數(shù)以及線結(jié)構(gòu)光光平面參數(shù)，然后利用相機實時采集管道內(nèi)壁圖像，對采集的圖像進(jìn) 行閾值分割、形態(tài)學(xué)分析等一系列圖像處理技術(shù)，根據(jù)車輛行駛過程中跑偏定義，對車體的姿態(tài)(偏移量和偏移角度)進(jìn)行測量，進(jìn)而對車體進(jìn)行動力學(xué)分析，利用有效的控制算法將車輛實時糾正到準(zhǔn)確的基準(zhǔn)位置，以避免車體與管道內(nèi)壁發(fā)生不必要的接觸。該系統(tǒng)檢測速度快、精度高，能滿足車輛在管道中進(jìn)行直線行走的要求。參照附圖1和2，本發(fā)明所述基于視覺傳感器系統(tǒng)的管道車輛糾偏方法的具體步驟如下：步驟1：設(shè)計視覺傳感器系統(tǒng)，將兩個線結(jié)構(gòu)光平行放置于相機兩側(cè)，垂直于車體中軸，安裝在車體一側(cè)；步驟2：用100mm*80mm規(guī)格的圓形標(biāo)定板標(biāo)定相機的內(nèi)參數(shù)和結(jié)構(gòu)光的光平面參數(shù)；步驟3：利用相機對圖像進(jìn)行采集以及數(shù)字化存儲，對采集到的圖像進(jìn)行閾值分割以及形態(tài)學(xué)分析等圖像處理；對采集的圖像進(jìn)行高斯平滑處理，減少圖像噪聲。其中，采用大小為3*3的高斯濾波器模板，高斯內(nèi)核在X方向的標(biāo)準(zhǔn)偏差為1.2，高斯內(nèi)核在Y方向的標(biāo)準(zhǔn)偏差為0.5。將平滑后的圖像進(jìn)行最大類間方差OSTU算法處理，使得前景和背景兩類的類間方差最大，提取出感興趣的前景光條紋區(qū)域。對分割后的圖像做兩次腐蝕操作，去除光條紋區(qū)域邊緣的雜點。為了避免現(xiàn)實中存在水跡、光照、管道表面反光性較強等干擾，進(jìn)而提取圖像中的輪廓，去除輪廓區(qū)域面積較大和較小的區(qū)域，本發(fā)明中給定的閾值為Tmax＝2400，Tmin＝50；步驟4：將得到的二值輪廓中所有非零點圖像像素坐標(biāo)值進(jìn)行區(qū)分，去畸變，同時將其轉(zhuǎn)換到相機坐標(biāo)系下；令Pi(x,y)是檢測到的點，其中i＝1,2,3…W，W表示點數(shù)，x,y表示像素坐標(biāo)值；遍歷所有的點，如果x＞width/2，Pr＝Pi，其中，width表示圖像寬度，Pr表示 相機右側(cè)結(jié)構(gòu)光發(fā)射到管道壁上的點；如果x＜width/2，Pl＝Pi，其中，Pl表示相機左側(cè)結(jié)構(gòu)光發(fā)射到管道壁上的點；利用相機對上述得到的點去畸變，轉(zhuǎn)換到圖像的物理坐標(biāo)系下，然后通過標(biāo)定好的兩組光平面參數(shù)將其分別轉(zhuǎn)換到相機坐標(biāo)系下；將Pr經(jīng)轉(zhuǎn)換后的點記為Pri(x,y,z)，i＝1,2,3…m，Pl經(jīng)轉(zhuǎn)換后的點記為Pli(x,y,z)，i＝1,2,3…n，按照下式求取點prp和plp：prpx=Σi=1mPrixm,prpy=0,prpz=Σi=1mPrizm---(1)]]>plpx=Σi=1nPlixn,prpy=0,prpz=Σi=1nPlizn---(2)]]>其中，prpx,prpy,prpz是點prp的三個坐標(biāo)分量，Prix,Priz分別是Pri的x,z方向的坐標(biāo)分量，plpx,plpy,plpz是點plp的三個坐標(biāo)分量，Plix,Pliz分別是Pli的x,z方向的坐標(biāo)分量。步驟5：計算相機坐標(biāo)系下車體偏移角度和偏移量：θ=(P1z-P2z)(P1x-P2x)P1x≠P2xθ=0P1x=P2x---(3)]]>d=(P2z+|P2x|*tanθ1)*cosθ1-LP1z>P2zd=(P1z+|P1x|*tanθ1)*cosθ1-LP1z<P2z---(4)]]>其中，θ是相機坐標(biāo)系下車體偏轉(zhuǎn)角，P1x,P1z分別是相機右側(cè)線結(jié)構(gòu)光平均x和z坐標(biāo)值，P2x,P2z分別是相機左側(cè)線結(jié)構(gòu)光平均x和z坐標(biāo)值，d是相機光心位置處距基準(zhǔn)位置的偏移量，L是車體在管道中心線基準(zhǔn)位置處相機光心離管道壁的距離。為了計算車體處于管道中心線基準(zhǔn)位置處相機光心離管道壁的距離，已知管道的寬度W，將車體放置在管道的中心位置處，利用相機采集圖像，用圖像處理技術(shù)檢測光條上點相對于相機的三維點坐標(biāo)，可以得到光心點離管道壁的距離(L＝103)。相機光心點與車體的中心位置橫向距離N＝W/2-L。本發(fā)明以車體 前端的舵機中心點為轉(zhuǎn)動控制點，因此需要將上述計算得到的偏移角度和偏移量轉(zhuǎn)換到舵機的中心點位置處，轉(zhuǎn)換關(guān)系如下：θ2＝θ1(5)d2＝d+m*sinθ2+n*(cosθ2-1)(6)其中，θ2是以舵機中心點為參考點的車體偏移角度，d2是以舵機中心點為參考點的偏移量。步驟6：如圖2所示，將計算得到的車體姿態(tài)在線反饋，進(jìn)行姿態(tài)補償，通過控制算法不斷消除位姿偏差以達(dá)到車體與管道中心線保持高度重合性的目的。對車體的姿態(tài)進(jìn)行檢測，通過視覺傳感器系統(tǒng)獲得當(dāng)前時刻(記為i)的偏差信息di,θi，通過驅(qū)動電機的反饋信息及內(nèi)存的存儲信息獲得此時的速度信息與舵角信息μi,φi；將上述信息作為控制的輸入量，利用糾偏控制算法(可參考Pere等人發(fā)表的論文“AutomaticPlanningofManipulatorMovements”)進(jìn)行分析，得到為消除上述偏差信息舵機需要轉(zhuǎn)過的角度信息φi+1，利用公式(7)計算舵角信息對應(yīng)的脈沖寬度，控制舵機以實現(xiàn)一次控制流程。pwi＝(((90-φi)/180)*2+0.5)*1000+50(7)其中φi是舵角大小，pwi是舵角大小對應(yīng)的脈沖寬度。通過實時閉環(huán)控制，不斷消除偏差，在有限時間內(nèi)使得θ→0,d→0。實現(xiàn)車體精確跟蹤管道中心線行走的目的。下面結(jié)合附圖對本發(fā)明的效果做進(jìn)一步的描述。為驗證本發(fā)明的有效性和正確性，采用AVT公司的Manta系列相機(分辨率：2048×1088)和兩個COHERENT公司的中心波長為630nm的一字線激光器構(gòu)成一個視覺傳感器系統(tǒng)。同時，為了保證車體在偏移時，光條仍在視野內(nèi),相 機配置了Computar公司的6mm精工鏡頭，相機視野較大。所有的仿真實驗均在Windows7操作系統(tǒng)下采用VisualStudio2008軟件實現(xiàn)。仿真1圖3顯示視覺控制對于車體偏移量以及偏移角度的影響。橫坐標(biāo)表示時間Time(s),左側(cè)縱坐標(biāo)表示偏移量Displacement(mm)，右側(cè)縱坐標(biāo)表示偏移角度Offsetangle(度)。從圖中可以看出，借助視覺系統(tǒng)的控制，車體的偏移量以及偏移角度隨時間收斂。偏移量精度小于1mm，偏移角度精度小于1°，因此本發(fā)明方法在管道小車糾偏中起到重要的作用。仿真2圖4和圖5分別顯示了小車進(jìn)入和退出2m的實驗管道，當(dāng)小車前進(jìn)和后退時，視覺控制糾偏示意圖。橫坐標(biāo)表示時間Time(s)，左側(cè)縱坐標(biāo)表示偏移量Displacement(mm)，右側(cè)紅色縱坐標(biāo)表示偏移角度Offsetangle(度)，右邊藍(lán)色縱坐標(biāo)表示脈沖寬度(ms)。小車前進(jìn)速度為0.06m/s，后退速度為0.025m/s。圖5顯示了小車后退,也就是返回管道口時，利用閉環(huán)控制，不斷消除偏差，當(dāng)時間為45s時，小車的偏移量以及偏移角度趨向于0，脈沖寬度也趨向于1550，達(dá)到了小車糾偏的目的。仿真3圖6顯示了小車進(jìn)入和退出2m的實驗管道，當(dāng)小車前進(jìn)和后退時，視覺控制糾偏示意圖。橫坐標(biāo)表示時間Time(s)，左側(cè)縱坐標(biāo)表示偏移量Displacement(mm)，右側(cè)紅色縱坐標(biāo)表示偏移角度Offsetangle(度)，右邊藍(lán)色縱坐標(biāo)表示脈沖寬度(ms)。小車前進(jìn)速度為0.06m/s，后退速度為0.025m/s。圖6和圖4、圖5比較可以看出，小車最終都會達(dá)到平衡穩(wěn)定狀態(tài)，與小車進(jìn)入管道的初始位置無關(guān)?？梢詫崿F(xiàn)在線糾偏，偏移量精度為0.3mm，偏移角度精度為1°。當(dāng)前第1頁1 2 3