本發(fā)明涉及變電站智能巡檢
技術(shù)領(lǐng)域：
，是一種變電站巡檢機(jī)器人定位方法及定位裝置。
背景技術(shù)：
：現(xiàn)有的移動(dòng)機(jī)器人的定位有基于里程計(jì)的定位、基于慣性傳感器的定位、視覺(jué)定位、超聲定位、地圖匹配定位、基于多種傳感信息的定位、路標(biāo)與燈塔的定位和同時(shí)定位與地圖構(gòu)建(SLAM)等定位方式。目前，變電站智能巡檢機(jī)器人主要包括軌道式巡檢機(jī)器人和輪式巡檢機(jī)器人。軌道式巡檢機(jī)器人系統(tǒng)是由設(shè)置在軌道上的定位片對(duì)機(jī)器人進(jìn)行定位，定位精度高，但其能夠巡檢的空間有限，只能在固定的軌道中移動(dòng)。輪式巡檢機(jī)器人定位方式主要是依靠磁軌道進(jìn)行定位和利用自身傳感器系統(tǒng)進(jìn)行定位，依靠磁軌道進(jìn)行定位需要事先在底下鋪設(shè)磁軌道，成本較高，行動(dòng)范圍有限；依靠自身傳感器進(jìn)行定位可以使機(jī)器人在大范圍內(nèi)進(jìn)行移動(dòng)，但傳統(tǒng)的方式是使用激光掃描雷達(dá)使機(jī)器人進(jìn)行自定位，成本高昂，機(jī)器人不能實(shí)現(xiàn)智能定位。發(fā)明專(zhuān)利201210186563.5公布了一種基于二維碼的移動(dòng)機(jī)器人定位系統(tǒng)和方法，將視覺(jué)定位技術(shù)、二維碼定位技術(shù)及兩自由度測(cè)速技術(shù)有機(jī)結(jié)合在一起，實(shí)現(xiàn)了對(duì)移動(dòng)機(jī)器人的準(zhǔn)確定位功能，解決了傳統(tǒng)視覺(jué)定位在圖像處理上過(guò)于復(fù)雜和定位精度不夠準(zhǔn)確的問(wèn)題。該方法存在的問(wèn)題是：1、施工難度大，對(duì)人工和環(huán)境設(shè)備要求非?？量?，形成人工成本和設(shè)備成本的高投入；2、容錯(cuò)性差，移動(dòng)機(jī)器人運(yùn)行非常不穩(wěn)定：由于采用一個(gè)二維碼讀碼器定位的方法不能容錯(cuò)人工和設(shè)備的誤差而常常造成誤判斷，致使移動(dòng)機(jī)器人不得不長(zhǎng)期處于大幅度位姿調(diào)整的狀態(tài)，移動(dòng)機(jī)器人運(yùn)行時(shí)機(jī)體晃動(dòng)、搖擺不定，嚴(yán)重影響了穩(wěn)定性。3、該方法由于采用隨動(dòng)輪測(cè)速機(jī)構(gòu)來(lái)檢測(cè)移動(dòng)機(jī)器人的速度，沒(méi)有對(duì)隨動(dòng)輪打滑的情況進(jìn)行考慮，會(huì)造成測(cè)速機(jī)構(gòu)所測(cè)得的速度不夠準(zhǔn)確。發(fā)明專(zhuān)利201210472984.4公布了一種基于多個(gè)二維碼讀碼器的移動(dòng)機(jī)器人定位系統(tǒng)及方法。該方法是在機(jī)器人與地面上安裝至少兩個(gè)二維碼讀碼器和分布于地面上的二維碼標(biāo)簽，每次至少同時(shí)讀取兩個(gè)二維碼標(biāo)簽來(lái)實(shí)現(xiàn)對(duì)移動(dòng)機(jī)器人的定位，有效容錯(cuò)性差，以及讀取單個(gè)二維碼標(biāo)簽所帶來(lái)的不足。但是該方法仍然存在以下缺點(diǎn)與不足：1、由于二維碼張貼在地面上，因此對(duì)于地面的平整度要求較高，對(duì)環(huán)境要求較為苛刻，并且該方法只能適用于室內(nèi)移動(dòng)機(jī)器人的定位，在室外等情況下無(wú)法適用；2、該方法在張貼二維碼時(shí)施工難度較大，對(duì)人工和環(huán)境設(shè)備要求較為嚴(yán)格，容易形成較高的人工成本與設(shè)備成本。技術(shù)實(shí)現(xiàn)要素：本發(fā)明提供了一種變電站巡檢機(jī)器人定位裝置和方法，克服了上述現(xiàn)有技術(shù)之不足，其能有效解決現(xiàn)有技術(shù)中存在的移動(dòng)機(jī)器人穩(wěn)定性差、容錯(cuò)性差以及移動(dòng)機(jī)器人不能適用于室外移動(dòng)定位的問(wèn)題。本發(fā)明的技術(shù)方案之一是通過(guò)以下措施來(lái)實(shí)現(xiàn)的：變電站巡檢機(jī)器人定位方法包括以下步驟：第一步，建立坐標(biāo)系，建立世界坐標(biāo)系OXYZ，二維碼坐標(biāo)系o1x1y1z1，攝像機(jī)坐標(biāo)系o2x2y2z2，云臺(tái)a部分坐標(biāo)系o3x3y3z3，云臺(tái)b部分坐標(biāo)系o3x4y4z4和機(jī)器人坐標(biāo)系o5x5y5z5；第二步，計(jì)算里程計(jì)的定位結(jié)果M1，對(duì)移動(dòng)機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)建模，使用三輪全向輪移動(dòng)機(jī)器人作為移動(dòng)平臺(tái)，XOY為世界坐標(biāo)系，xoy為機(jī)器人坐標(biāo)系，移動(dòng)機(jī)器人的三個(gè)輪子分別標(biāo)記為i,j,k，三個(gè)輪子各自的線速度分別為Vi,Vj,Vk，在機(jī)器人坐標(biāo)系中，移動(dòng)機(jī)器人在X軸和Y軸兩個(gè)坐標(biāo)上的速度分別為vx，vy，方向與坐標(biāo)軸方向相同，機(jī)器人自身旋轉(zhuǎn)的角速度為ω，R為輪子到機(jī)器人中心點(diǎn)O的距離；在世界坐標(biāo)系中機(jī)器人沿兩坐標(biāo)軸的速度分別為VX,VY，通過(guò)分析可得在機(jī)器人坐標(biāo)系中根據(jù)機(jī)器人三個(gè)全向輪的線速度可以計(jì)算出機(jī)器人的速度為：vxvyω=23-13-130-333313R13R13R*ViVjVk]]>機(jī)器人坐標(biāo)系與世界坐標(biāo)系的夾角為φ，由機(jī)器人坐標(biāo)系中的速度可以計(jì)算得到在世界坐標(biāo)系下的速度為：VxVyω=cosφ-sinφ0sinφcosφ0001*vxvyω]]>移動(dòng)機(jī)器人的初始姿態(tài)為(x0，y0，φ0)，Δt時(shí)間后移動(dòng)機(jī)器人的姿態(tài)為(x1，y1，φ1)，計(jì)算公式如下：x1y1φ1=Δt000Δt000Δt*VxVyω+x0y0φ0]]>因移動(dòng)機(jī)器人在地面運(yùn)動(dòng)，移動(dòng)機(jī)器人的位姿用M1表示為：M1＝(x1,y1,φ1)T；第三步，通過(guò)二維碼圖像計(jì)算移動(dòng)機(jī)器人的位姿M2，包括以下步驟：(1)通過(guò)圖像處理，讀取二維碼的信息，包括以下步驟：(a)相機(jī)拍照，(b)灰度化，(c)二值化，(d)形態(tài)學(xué)濾波，(e)邊緣檢測(cè)，(f)霍夫直線變換，(g)透視變換，(h)二維碼信息讀?。?2)通過(guò)二維碼計(jì)算攝像機(jī)坐標(biāo)系o2x2y2z2相對(duì)于二維碼坐標(biāo)系o1x1y1z1的旋轉(zhuǎn)矩陣21R以及平移矩陣P12，計(jì)算方法如下：二維碼貼在x1o1y1平面中，z1軸垂直于墻面向外，移動(dòng)機(jī)器人相機(jī)鏡頭的朝向?yàn)閦2軸，鏡體方向?yàn)閤2軸，圖像信息位于x2o2y2坐標(biāo)中；設(shè)邊AO長(zhǎng)度為lengthAO，邊A1O1的長(zhǎng)度為標(biāo)定時(shí)假設(shè)當(dāng)物體距離相機(jī)距離為1m時(shí)，實(shí)際中物體的長(zhǎng)度和圖像中物體長(zhǎng)度的比為m，相機(jī)與二維碼之間的距離計(jì)算公式如下：l=lengthAOlengthA1O1*m]]>已知二維碼張貼高度h，移動(dòng)機(jī)器人高清攝像機(jī)距離二維碼的長(zhǎng)度為l，則：γ=arcsinhl]]>在世界坐標(biāo)系中∠AOB＝90°在相機(jī)坐標(biāo)系中∠A1O1B1＝θ，o2X2y2z2相對(duì)于o1x1y1z1繞z軸旋轉(zhuǎn)-90°,然后在z1軸方向不再轉(zhuǎn)動(dòng)，在移動(dòng)機(jī)器人運(yùn)動(dòng)和相機(jī)云臺(tái)運(yùn)動(dòng)過(guò)程中，相對(duì)于z1軸不再旋轉(zhuǎn)，只相對(duì)于x1軸和y1軸轉(zhuǎn)動(dòng)，繞x1轉(zhuǎn)動(dòng)用θx表示，繞y1軸用θy表示。o2x2y2z2中的坐標(biāo)在o1x1y1z1坐標(biāo)中如下表示：x1y1z1=M-1*x2y2z2+-Δx-Δy-Δz;]]>其中L*sinβ=(l*cosγ*sinα)2+h2]]>α=arcsin(L*sinβ)2-h2l*cosγ]]>Δz＝hΔx＝l*sinαΔy＝l*cosα3個(gè)未知數(shù)θx，θy和β,需要3個(gè)方程，則OA,OB在x1o1y1平面中可表示為：OA→=(a,b,0),OB→=(c,d,0)]]>在o2x2y2z2坐標(biāo)系中:OA→(x2y2z2)=(a,b,0)T*M+ΔxΔyΔz]]>OB→(x2y2z2)=(c,d,0)T*M+ΔxΔyΔz]]>OA→(x2y2z2)*OB→(x2y2z2)=0---(1)]]>在z2方向?yàn)?z2軸與x1軸之間的夾角為(cosθz1x)2+cosβ2+cosγ2=1]]>其中r已知，可用β表示；根據(jù)z2軸在o1x1y1z1坐標(biāo)系中的向量得到關(guān)于β的方程；(0,0,1)T*M+ΔxΔyΔz=(cosβ,cos(90-γ),cosθz1x)2---(3)]]>聯(lián)立方程(1)(2)(3)可求得θx，θy，和β，得到o2x2y2到o1x1y1z1的轉(zhuǎn)換矩陣以及平移矩陣P12；(3)各個(gè)坐標(biāo)系之間旋轉(zhuǎn)矩陣與平移矩陣的計(jì)算：在世界坐標(biāo)系中，設(shè)定二維碼的位置為(x1,y1,z1)，二維碼坐標(biāo)系o1x1y1z1相對(duì)于世界坐標(biāo)系OXYZ的平移矩陣為PO1的計(jì)算如下：P01＝(X1,Y1,Z1)T二維碼坐標(biāo)系的三個(gè)坐標(biāo)軸與世界坐標(biāo)系的三個(gè)軸之間的夾角組成的3×3的矩陣，各個(gè)夾角所對(duì)應(yīng)的余弦值所組成的矩陣為二維碼坐標(biāo)系o1x1y1z1相對(duì)于世界坐標(biāo)系OXYZ的旋轉(zhuǎn)矩陣為如下：R1O=x1→*XO→y1→*XO→z1→*XO→x1→*YO→y1→*YO→z1→*YO→x1→*ZO→y1→*ZO→z1→*ZO→]]>移動(dòng)機(jī)器人各個(gè)坐標(biāo)系相對(duì)于相鄰的坐標(biāo)系的旋轉(zhuǎn)角度以及平移距離在移動(dòng)機(jī)器人系統(tǒng)均已知，其他坐標(biāo)系的旋轉(zhuǎn)矩陣以及平移矩陣也按照上述方法來(lái)計(jì)算；二維碼坐標(biāo)系o1x1y1z1相對(duì)于世界坐標(biāo)系OXYZ有平移，相對(duì)于Z軸旋轉(zhuǎn)，旋轉(zhuǎn)矩陣為平移矩陣為PO1；攝像機(jī)坐標(biāo)系o2x2y2z2相對(duì)于二維碼坐標(biāo)系o1x1y1z1為平移和旋轉(zhuǎn)，旋轉(zhuǎn)矩陣為平移矩陣為P12；云臺(tái)a部分坐標(biāo)系o3x3y3z3相對(duì)于攝像機(jī)坐標(biāo)系o2x2y2z2只是相對(duì)于x2軸的平移，旋轉(zhuǎn)矩陣為平移矩陣為P23；云臺(tái)a部分坐標(biāo)系o3x3y3z3相對(duì)于云臺(tái)b部分坐標(biāo)系o3x4y4z4只是相對(duì)于z3軸的旋轉(zhuǎn)，所以他們共用一個(gè)坐標(biāo)系原點(diǎn)o3，旋轉(zhuǎn)矩陣為平移矩陣為P34；移動(dòng)機(jī)器人坐標(biāo)系o5x5y5z5相對(duì)于云臺(tái)b部分坐標(biāo)系o3x4y4z4只是相對(duì)于軸方向的平移和旋轉(zhuǎn)，并且在云臺(tái)b部分的坐標(biāo)系o3x4y4z4下，移動(dòng)機(jī)器人的坐標(biāo)為4O5；(4)通過(guò)旋轉(zhuǎn)矩陣與平移矩陣計(jì)算移動(dòng)機(jī)器人的位姿：通過(guò)計(jì)算表示出云臺(tái)b部分坐標(biāo)系相對(duì)于世界坐標(biāo)系的旋轉(zhuǎn)矩陣為平移矩陣為PO1+P12+P23+P34，移動(dòng)機(jī)器人坐標(biāo)系原點(diǎn)O5就是移動(dòng)機(jī)器人的位置；根據(jù)坐標(biāo)系變換規(guī)則，可以計(jì)算出移動(dòng)機(jī)器人位置：OO5=R1OR21R32R43O45+PO1+P12+P23+P34=(x,y,z)T]]>移動(dòng)機(jī)器人坐標(biāo)系相對(duì)于世界坐標(biāo)系沿著Z軸方向旋轉(zhuǎn)，就是移動(dòng)機(jī)器人坐標(biāo)系相對(duì)于世界坐標(biāo)系的旋轉(zhuǎn)矩陣，通過(guò)旋轉(zhuǎn)矩陣，可以計(jì)算出移動(dòng)機(jī)器人坐標(biāo)系相對(duì)于世界坐標(biāo)系的夾角θ，移動(dòng)機(jī)器人的位姿用M2矩陣表示為：M2＝(x,y,θ)T；第四步，二維碼定位數(shù)據(jù)對(duì)里程計(jì)定位結(jié)果修正，使用二維碼的定位結(jié)果來(lái)修正里程計(jì)的數(shù)據(jù)，則：M1＝M2＝(x,y,θ)T。本發(fā)明的技術(shù)方案之二是通過(guò)以下措施來(lái)實(shí)現(xiàn)的：一種變電站巡檢機(jī)器人定位裝置，包括驅(qū)動(dòng)機(jī)器人運(yùn)動(dòng)的輪式平臺(tái)、設(shè)置在輪式平臺(tái)內(nèi)的機(jī)器人運(yùn)動(dòng)控制器、編碼器、通信模塊、攝像機(jī)和云臺(tái)，所述輪式平臺(tái)沿圓周等間距安裝有三個(gè)全向輪，三個(gè)全向輪上各安裝有一個(gè)編碼器，所述云臺(tái)底部設(shè)有支撐軸，支撐軸固定安裝在輪式平臺(tái)上，所述攝像機(jī)安裝在云臺(tái)的頂部，所述機(jī)器人運(yùn)動(dòng)控制器與通信模塊雙向通信連接，編碼器與機(jī)器人運(yùn)動(dòng)控制器雙向電連接，攝像機(jī)和云臺(tái)均與通信模塊雙向通信連接。上述通信模塊包括無(wú)線路由器和串口聯(lián)網(wǎng)模塊，所述攝像機(jī)、云臺(tái)、無(wú)線路由器和機(jī)器人運(yùn)動(dòng)控制器均與串口聯(lián)網(wǎng)模塊通信連接。上述還包括遠(yuǎn)程PC機(jī)，所述遠(yuǎn)程PC機(jī)與無(wú)線路由器無(wú)線通信連接。上述攝像機(jī)包括紅外攝像機(jī)和高清攝像機(jī)，紅外攝像機(jī)和高清攝像機(jī)均與串口聯(lián)網(wǎng)模塊通信連接。上述編碼器A、編碼器B和編碼器C均為光電編碼器。本發(fā)明通過(guò)使用三個(gè)全向輪移動(dòng)機(jī)器人平臺(tái)比其他普通輪移動(dòng)機(jī)器人平臺(tái)運(yùn)動(dòng)靈活，可以向任意方向運(yùn)動(dòng)。通過(guò)使用云臺(tái)，對(duì)二維碼的張貼位置沒(méi)有特殊的要求，實(shí)施會(huì)更加簡(jiǎn)單高效。通過(guò)采用編碼器與二維碼方式相結(jié)合定位，即使在二維碼有障礙物遮擋的情況下仍然可以知道移動(dòng)機(jī)器人的位置。變電站巡檢機(jī)器人使用二維碼的定位方法，在室內(nèi)與室外環(huán)境下都可以使用。附圖說(shuō)明附圖1為本發(fā)明實(shí)施例1全方位移動(dòng)機(jī)器人定位方法流程圖。附圖2為本發(fā)明全方位移動(dòng)機(jī)器人定位坐標(biāo)系示意圖。附圖3為本發(fā)明全方位移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)簡(jiǎn)化模型示意圖。附圖4為本發(fā)明高清攝像機(jī)與二維碼位置關(guān)系示意圖。附圖5為本發(fā)明高清攝像機(jī)相對(duì)于世界坐標(biāo)系的定位坐標(biāo)示意圖。附圖6為本發(fā)明二維碼讀取過(guò)程示意圖。附圖7為本發(fā)明全方位移動(dòng)機(jī)器人運(yùn)動(dòng)路線及二維碼張貼位置示意圖。附圖8為本發(fā)明全方位移動(dòng)機(jī)器人依據(jù)二維碼位置校正自身位置示意圖。附圖9為本發(fā)明全方位移動(dòng)機(jī)器人定位系統(tǒng)框圖。附圖10為本發(fā)明全方位移動(dòng)機(jī)器人平臺(tái)效果示意圖。附圖中的編碼分別為：1為輪式平臺(tái)，2為機(jī)器人運(yùn)動(dòng)控制器，3為云臺(tái)，4為支撐軸，5為全向輪，6為無(wú)線路由器，7為編碼器A，8為編碼器B，9為編碼器C，10為串口聯(lián)網(wǎng)模塊，,11為遠(yuǎn)程PC機(jī),12為紅外攝像機(jī)，13為高清攝像機(jī)。具體實(shí)施方式本發(fā)明不受下述實(shí)施例的限制，可根據(jù)本發(fā)明的技術(shù)方案與實(shí)際情況來(lái)確定具體的實(shí)施方式。在本發(fā)明中，為了便于描述，各部件的相對(duì)位置關(guān)系的描述均是根據(jù)說(shuō)明書(shū)附圖1的布圖方式來(lái)進(jìn)行描述的，如：前、后、上、下、左、右等的位置關(guān)系是依據(jù)說(shuō)明書(shū)附圖1的布圖方向來(lái)確定的。下面結(jié)合實(shí)施例及附圖對(duì)本發(fā)明作進(jìn)一步描述：實(shí)施例1：如附圖1、2、3、4、5、6、7、8所示，變電站巡檢機(jī)器人定位方法包括以下步驟：第一步，建立坐標(biāo)系，建立世界坐標(biāo)系OXYZ，二維碼坐標(biāo)系o1x1y1z1，攝像機(jī)坐標(biāo)系o2x2y2z2，云臺(tái)a部分坐標(biāo)系o3x3y3z3，云臺(tái)b部分坐標(biāo)系o3x4y4z4和機(jī)器人坐標(biāo)系o5x5y5z5；這里的二維碼坐標(biāo)系o1x1y1z1相對(duì)于世界坐標(biāo)系OXYZ有平移，相對(duì)于Z軸旋轉(zhuǎn)。攝像機(jī)坐標(biāo)系o2x2y2z2相對(duì)于二維碼坐標(biāo)系o1x1y1z1為平移和旋轉(zhuǎn)。云臺(tái)A部分坐標(biāo)系o3x3y3z3相對(duì)于攝像機(jī)坐標(biāo)系o2x2y2z2只是相對(duì)于x2軸的平移。云臺(tái)A部分坐標(biāo)系o3x3y3z3相對(duì)于云臺(tái)B部分坐標(biāo)系o3x4y4z4只是相對(duì)于z3軸的旋轉(zhuǎn)，所以他們共用一個(gè)坐標(biāo)系原點(diǎn)o3。云臺(tái)B部分坐標(biāo)系o3x4y4z4相對(duì)于移動(dòng)機(jī)器人坐標(biāo)系o5x5y5z5只是相對(duì)于軸方向的平移和旋轉(zhuǎn)。第二步，計(jì)算里程計(jì)的定位結(jié)果M1，對(duì)移動(dòng)機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)建模，使用三輪全向輪移動(dòng)機(jī)器人作為移動(dòng)平臺(tái)，XOY為世界坐標(biāo)系，xoy為機(jī)器人坐標(biāo)系，移動(dòng)機(jī)器人的三個(gè)輪子分別標(biāo)記為i,j,k，三個(gè)輪子各自的線速度分別為Vi,Vj,Vk，在機(jī)器人坐標(biāo)系中，移動(dòng)機(jī)器人在X軸和Y軸兩個(gè)坐標(biāo)上的速度分別為vx，vy，方向與坐標(biāo)軸方向相同，機(jī)器人自身旋轉(zhuǎn)的角速度為ω，R為輪子到機(jī)器人中心點(diǎn)O的距離；在世界坐標(biāo)系中機(jī)器人沿兩坐標(biāo)軸的速度分別為VX,VY，通過(guò)分析可得在機(jī)器人坐標(biāo)系中根據(jù)機(jī)器人三個(gè)全向輪的線速度可以計(jì)算出機(jī)器人的速度為：vxvyω=23-13-130-333313R13R13R*ViVjVk]]>機(jī)器人坐標(biāo)系與世界坐標(biāo)系的夾角為φ，由機(jī)器人坐標(biāo)系中的速度可以計(jì)算得到在世界坐標(biāo)系下的速度為：VxVyω=cosφ-sinφ0sinφcosφ0001*vxvyω]]>移動(dòng)機(jī)器人的初始姿態(tài)為(x0，y0，φ0)，Δt時(shí)間后移動(dòng)機(jī)器人的姿態(tài)為(x1，y1，φ1)，計(jì)算公式如下：x1y1φ1=Δt000Δt000Δt*VxVyω+x0y0φ0]]>因移動(dòng)機(jī)器人在地面運(yùn)動(dòng)，移動(dòng)機(jī)器人的位姿用M1表示為：M1＝(x1,y1,φ1)T；第三步，通過(guò)二維碼圖像計(jì)算移動(dòng)機(jī)器人的位姿M2，包括以下步驟：(1)通過(guò)圖像處理，讀取二維碼的信息，包括以下步驟：(a)相機(jī)拍照，(b)灰度化，(c)二值化，(d)形態(tài)學(xué)濾波，(e)邊緣檢測(cè)，(f)霍夫直線變換，(g)透視變換，(h)二維碼信息讀??；(2)通過(guò)二維碼計(jì)算攝像機(jī)坐標(biāo)系o2x2y2z2相對(duì)于二維碼坐標(biāo)系o1x1y1z1的旋轉(zhuǎn)矩陣以及平移矩陣P12，計(jì)算方法如下：二維碼貼在x1o1y1平面中，z1軸垂直于墻面向外，移動(dòng)機(jī)器人相機(jī)鏡頭的朝向?yàn)閦2軸，鏡體方向?yàn)閤2軸，圖像信息位于x2o2y2坐標(biāo)中；設(shè)邊AO長(zhǎng)度為lengthAO，邊A1O1的長(zhǎng)度為標(biāo)定時(shí)假設(shè)當(dāng)物體距離相機(jī)距離為1m時(shí)，實(shí)際中物體的長(zhǎng)度和圖像中物體長(zhǎng)度的比為m，相機(jī)與二維碼之間的距離計(jì)算公式如下：l=lengthAOlengthA1O1*m]]>已知二維碼張貼高度h，移動(dòng)機(jī)器人高清攝像機(jī)距離二維碼的長(zhǎng)度為l，則：γ=arcsinhl]]>在世界坐標(biāo)系中∠AOB＝90°在相機(jī)坐標(biāo)系中∠A1O1B1＝θ，o2X2y2z2相對(duì)于o1x1y1z1繞z軸旋轉(zhuǎn)-90°,然后在z1軸方向不再轉(zhuǎn)動(dòng)，在移動(dòng)機(jī)器人運(yùn)動(dòng)和相機(jī)云臺(tái)運(yùn)動(dòng)過(guò)程中，相對(duì)于z1軸不再旋轉(zhuǎn)，只相對(duì)于x1軸和y1軸轉(zhuǎn)動(dòng)，繞x1轉(zhuǎn)動(dòng)用θx表示，繞y1軸用θy表示。o2x2y2z2中的坐標(biāo)在o1x1y1z1坐標(biāo)中如下表示：x1y1z1=M-1*x2y2z2+-Δx-Δy-Δz;]]>其中L*sinβ=(l*cosγ*sinα)2+h2]]>α=arcsin(L*sinβ)2-h2l*cosγ]]>Δz＝hΔx＝l*sinαΔy＝l*cosα3個(gè)未知數(shù)θx，θy和β,需要3個(gè)方程，則OA,OB在x1o1y1平面中可表示為：OA→=(a,b,0),OB→=(c,d,0)]]>在o2x2y2z2坐標(biāo)系中:OA→(x2y2z2)=(a,b,0)T*M+ΔxΔyΔz]]>OB→(x2y2z2)=(c,d,0)T*M+ΔxΔyΔz]]>OA→(x2y2z2)*OB→(x2y2z2)=0---(1)]]>在z2方向?yàn)?z2軸與x1軸之間的夾角為(cosθz1x)2+cosβ2+cosγ2=1]]>其中r已知，可用β表示；根據(jù)z2軸在o1x1y1z1坐標(biāo)系中的向量得到關(guān)于β的方程；(0,0,1)T*M+ΔxΔyΔz=(cosβ,cos(90-γ),cosθz1x)2---(3)]]>聯(lián)立方程(1)(2)(3)可求得θx，θy，和β，得到o2x2y2到o1x1y1z1的轉(zhuǎn)換矩陣以及平移矩陣P12；(3)各個(gè)坐標(biāo)系之間旋轉(zhuǎn)矩陣與平移矩陣的計(jì)算：在世界坐標(biāo)系中，設(shè)定二維碼的位置為(x1,y1,z1)，二維碼坐標(biāo)系o1x1y1z1相對(duì)于世界坐標(biāo)系OXYZ的平移矩陣為PO1的計(jì)算如下：P01＝(X1,Y1,Z1)T二維碼坐標(biāo)系的三個(gè)坐標(biāo)軸與世界坐標(biāo)系的三個(gè)軸之間的夾角組成的3×3的矩陣，各個(gè)夾角所對(duì)應(yīng)的余弦值所組成的矩陣為二維碼坐標(biāo)系o1x1y1z1相對(duì)于世界坐標(biāo)系OXYZ的旋轉(zhuǎn)矩陣為如下：R1O=x1→*XO→y1→*XO→z1→*XO→x1→*YO→y1→*YO→z1→*YO→x1→*ZO→y1→*ZO→z1→*ZO→]]>移動(dòng)機(jī)器人各個(gè)坐標(biāo)系相對(duì)于相鄰的坐標(biāo)系的旋轉(zhuǎn)角度以及平移距離在移動(dòng)機(jī)器人系統(tǒng)均已知，其他坐標(biāo)系的旋轉(zhuǎn)矩陣以及平移矩陣也按照上述方法來(lái)計(jì)算；二維碼坐標(biāo)系o1x1y1z1相對(duì)于世界坐標(biāo)系OXYZ有平移，相對(duì)于Z軸旋轉(zhuǎn)，旋轉(zhuǎn)矩陣為平移矩陣為PO1；攝像機(jī)坐標(biāo)系o2x2y2z2相對(duì)于二維碼坐標(biāo)系o1x1y1z1為平移和旋轉(zhuǎn)，旋轉(zhuǎn)矩陣為平移矩陣為P12；云臺(tái)a部分坐標(biāo)系o3x3y3z3相對(duì)于攝像機(jī)坐標(biāo)系o2x2y2z2只是相對(duì)于x2軸的平移，旋轉(zhuǎn)矩陣為平移矩陣為P23；云臺(tái)a部分坐標(biāo)系o3x3y3z3相對(duì)于云臺(tái)b部分坐標(biāo)系o3x4y4z4只是相對(duì)于z3軸的旋轉(zhuǎn)，所以他們共用一個(gè)坐標(biāo)系原點(diǎn)o3，旋轉(zhuǎn)矩陣為平移矩陣為P34；移動(dòng)機(jī)器人坐標(biāo)系o5x5y5z5相對(duì)于云臺(tái)b部分坐標(biāo)系o3x4y4z4只是相對(duì)于軸方向的平移和旋轉(zhuǎn)，并且在云臺(tái)b部分的坐標(biāo)系o3x4y4z4下，移動(dòng)機(jī)器人的坐標(biāo)為4O5；(4)通過(guò)旋轉(zhuǎn)矩陣與平移矩陣計(jì)算移動(dòng)機(jī)器人的位姿：通過(guò)計(jì)算表示出云臺(tái)b部分坐標(biāo)系相對(duì)于世界坐標(biāo)系的旋轉(zhuǎn)矩陣為平移矩陣為PO1+P12+P23+P34，移動(dòng)機(jī)器人坐標(biāo)系原點(diǎn)O5就是移動(dòng)機(jī)器人的位置；根據(jù)坐標(biāo)系變換規(guī)則，可以計(jì)算出移動(dòng)機(jī)器人位置：OO5=R1OR21R32R43O45+PO1+P12+P23+P34=(x,y,z)T]]>移動(dòng)機(jī)器人坐標(biāo)系相對(duì)于世界坐標(biāo)系沿著Z軸方向旋轉(zhuǎn)，就是移動(dòng)機(jī)器人坐標(biāo)系相對(duì)于世界坐標(biāo)系的旋轉(zhuǎn)矩陣，通過(guò)旋轉(zhuǎn)矩陣，可以計(jì)算出移動(dòng)機(jī)器人坐標(biāo)系相對(duì)于世界坐標(biāo)系的夾角，移動(dòng)機(jī)器人的位姿用M2矩陣表示為：M2＝(x,y,θ)T；第四步，二維碼定位數(shù)據(jù)對(duì)里程計(jì)定位結(jié)果修正，使用二維碼的定位結(jié)果來(lái)修正里程計(jì)的數(shù)據(jù)，則：M1＝M2＝(x,y,θ)T。這里考慮到機(jī)器人在移動(dòng)的過(guò)程中存在打滑的情況，所以里程計(jì)的定位結(jié)果存在誤差。二維碼的位置在世界坐標(biāo)系中是已知的，所以使用二維碼的定位結(jié)果來(lái)修正里程計(jì)的數(shù)據(jù)；當(dāng)移動(dòng)機(jī)器人移動(dòng)到?jīng)]有二維碼的地方時(shí)采用里程計(jì)的定位結(jié)果M1，當(dāng)移動(dòng)機(jī)器人運(yùn)動(dòng)到有二維碼的地方時(shí)，使用二維碼的定位結(jié)果M2來(lái)修正里程計(jì)的定位結(jié)果M1。因移動(dòng)機(jī)器人在移動(dòng)的過(guò)程中存在打滑的情況，里程計(jì)的定位結(jié)果存在誤差，通過(guò)二維碼定位數(shù)據(jù)對(duì)里程計(jì)定位結(jié)果進(jìn)行修正移動(dòng)機(jī)器人不需要進(jìn)行大幅度位姿調(diào)整，有效提高了移動(dòng)機(jī)器人的容錯(cuò)性和穩(wěn)定性。實(shí)施例2：如附圖9、10所示，一種變電站巡檢機(jī)器人定位裝置，包括驅(qū)動(dòng)機(jī)器人運(yùn)動(dòng)的輪式平臺(tái)1、設(shè)置在輪式平臺(tái)1內(nèi)的機(jī)器人運(yùn)動(dòng)控制器2、編碼器、通信模塊、攝像機(jī)和云臺(tái)3，所述輪式平臺(tái)1沿圓周等間距安裝有三個(gè)全向輪5，三個(gè)全向輪5上各安裝有一個(gè)編碼器，所述云臺(tái)3底部設(shè)有支撐軸4，支撐軸4固定安裝在輪式平臺(tái)1上，所述攝像機(jī)安裝在云臺(tái)3的頂部，所述機(jī)器人運(yùn)動(dòng)控制器2與通信模塊雙向通信連接，編碼器與機(jī)器人運(yùn)動(dòng)控制器2雙向電連接，攝像機(jī)和云臺(tái)3均與通信模塊雙向通信連接。在實(shí)際工作時(shí)，輪式平臺(tái)1起到驅(qū)動(dòng)變電站巡檢機(jī)器人運(yùn)動(dòng)和支撐云臺(tái)的作用，在每個(gè)全向輪5上分別安裝有一個(gè)電機(jī)，機(jī)器人運(yùn)動(dòng)控制器2控制電機(jī)的轉(zhuǎn)向和轉(zhuǎn)速，電機(jī)驅(qū)動(dòng)車(chē)輪轉(zhuǎn)動(dòng)，從而實(shí)現(xiàn)機(jī)器人的運(yùn)動(dòng)；這里采用三個(gè)全向輪5帶動(dòng)旋轉(zhuǎn)機(jī)器人的運(yùn)動(dòng)，能夠快速實(shí)現(xiàn)機(jī)器人的360°轉(zhuǎn)向，比起四輪驅(qū)動(dòng)更方便靈活。這里的電機(jī)、機(jī)器人運(yùn)動(dòng)控制器2、通信模塊均由固定在輪式平臺(tái)內(nèi)的大容量電池提供電源；編碼器分別為編碼器A7、編碼器B8和編碼器C9，機(jī)器人運(yùn)動(dòng)控制器2接收編碼器A7、編碼器B8和編碼器C9發(fā)送的全向輪5的位置信息。這里的云臺(tái)3為兩自由度云臺(tái)，包括云臺(tái)a部分和云臺(tái)b部分，云臺(tái)a部分的兩側(cè)臂安裝在云臺(tái)b部分上，云臺(tái)a部分和云臺(tái)b部分均能夠在機(jī)器人運(yùn)動(dòng)控制器的控制指令下旋轉(zhuǎn)，云臺(tái)a部分能夠帶動(dòng)攝像機(jī)轉(zhuǎn)動(dòng)，隨時(shí)調(diào)整攝像頭的角度，便于尋找設(shè)備二維碼位置。如附圖9、10所示，通信模塊包括無(wú)線路由器6和串口聯(lián)網(wǎng)模塊10，所述攝像機(jī)、云臺(tái)3、無(wú)線路由器6和機(jī)器人運(yùn)動(dòng)控制器2均與串口聯(lián)網(wǎng)模塊10通信連接。串口聯(lián)網(wǎng)模塊10的作用是將串口轉(zhuǎn)化為RJ-45的網(wǎng)絡(luò)接口，攝像機(jī)、串口聯(lián)網(wǎng)模塊10與無(wú)線路由器6之間均通過(guò)網(wǎng)口連接，云臺(tái)3與串口聯(lián)網(wǎng)模塊10之間使用RS485通信接口連接，機(jī)器人運(yùn)動(dòng)控制器2與串口聯(lián)網(wǎng)模塊10之間使用RS232通信接口連接。如附圖9、10所示，還包括遠(yuǎn)程PC機(jī)11，所述遠(yuǎn)程PC機(jī)11與無(wú)線路由器6無(wú)線通信連接。遠(yuǎn)程PC機(jī)11與無(wú)線路由器6之間采用無(wú)線網(wǎng)絡(luò)通信連接，用戶(hù)通過(guò)遠(yuǎn)程PC機(jī)11控制巡檢機(jī)器人運(yùn)動(dòng)，實(shí)現(xiàn)對(duì)設(shè)備的遠(yuǎn)程監(jiān)控。如附圖9、10所示，所述攝像機(jī)包括紅外攝像機(jī)12和高清攝像機(jī)13，紅外攝像機(jī)12和高清攝像機(jī)13均與串口聯(lián)網(wǎng)模塊10通信連接。高清攝像機(jī)13上設(shè)有網(wǎng)絡(luò)接口和USB接口，主要用于采集變電站內(nèi)的二維碼圖像信息，這里的二維碼圖像均為矩形；紅外攝像機(jī)13主要利用紅外采集二維碼的圖像，便于更清楚的采集二維碼信息。如附圖9、10所示，編碼器均為光電編碼器。這里的光電編碼器A7、光電編碼器B8和光電編碼器C9分別安裝在驅(qū)動(dòng)移動(dòng)機(jī)器人全向輪5運(yùn)動(dòng)的三個(gè)電機(jī)上，用于檢測(cè)電機(jī)的轉(zhuǎn)速，便于計(jì)算，從而實(shí)現(xiàn)對(duì)移動(dòng)機(jī)器人進(jìn)行精確定位。實(shí)施例3：如附圖1、2、3、4、5、6、7、8、9、10所示，基于二維碼信息對(duì)變電站巡檢機(jī)器人精確定位方法，包括以下步驟：(1)使用二維碼生成器生成帶有編號(hào)的變電站設(shè)備二維碼信息；(2)將生成的二維碼圖片裝訂好，貼在二維碼信息對(duì)應(yīng)的變電站相應(yīng)設(shè)備上并獲得每個(gè)二維碼在整個(gè)地圖中的坐標(biāo)；(3)由計(jì)算機(jī)自動(dòng)生成機(jī)器人的運(yùn)動(dòng)軌跡圖，將軌跡圖通過(guò)遠(yuǎn)程PC機(jī)以無(wú)線的形式發(fā)送給移動(dòng)機(jī)器人；(4)將搭載云臺(tái)及高清攝像機(jī)的移動(dòng)機(jī)器人放在變電站中按軌跡圖進(jìn)行移動(dòng)，在機(jī)器人運(yùn)動(dòng)過(guò)程中，根據(jù)編碼器得到機(jī)器人的位置信息，編碼器具有累計(jì)誤差，機(jī)器人運(yùn)動(dòng)越久，定位誤差越大；當(dāng)誤差增大時(shí)，機(jī)器人自身檢測(cè)到的自身位置和實(shí)際機(jī)器人所在的位置就會(huì)不同；(5)在沒(méi)有二維碼區(qū)域移動(dòng)機(jī)器人利用編碼器進(jìn)行定位，當(dāng)捕捉到二維碼時(shí)，機(jī)器人利用二維碼信息進(jìn)行識(shí)別，定位自身所在位置；二維碼所代表的就是地圖中坐標(biāo)信息，設(shè)定移動(dòng)機(jī)器人設(shè)置有兩種路線，第一種路線依序經(jīng)過(guò)的設(shè)備二維碼位置為是0001至0002至0003至0004至0005至0006至0007至0008至0009,第二種路線是0001至0004至0002至0003至0005至0006至0008至0007至0009，在兩種路線中，機(jī)器人在運(yùn)動(dòng)過(guò)程中自身定位誤差會(huì)不同，但移動(dòng)機(jī)器人根據(jù)二維碼的設(shè)備位置信息調(diào)整后，可以糾正其定位誤差；因?yàn)槎S碼自身所包含的信息中有自身的編號(hào)，機(jī)器人通過(guò)無(wú)線通信將編號(hào)信息發(fā)送給遠(yuǎn)程PC機(jī)11，遠(yuǎn)程PC機(jī)11遍尋編號(hào)后，得知該編號(hào)二維碼在地圖中的位置信息；(6)遠(yuǎn)程PC機(jī)11查得二維碼所在位置信息后，將信息發(fā)送給移動(dòng)機(jī)器人，移動(dòng)機(jī)器人根據(jù)自己相對(duì)于二維碼的位置，從而得到移動(dòng)機(jī)器人在全局中的準(zhǔn)確位置，將自身的位置信息進(jìn)行糾正，實(shí)現(xiàn)對(duì)移動(dòng)機(jī)器人的全局精確定位。例如：0012號(hào)二維碼所在位置是(X1,Y1),然后根據(jù)機(jī)器人相對(duì)二維碼的距離和方位，確定機(jī)器人自身位置(X2,Y2)。以上技術(shù)特征構(gòu)成了本發(fā)明的實(shí)施例，其具有較強(qiáng)的適應(yīng)性和實(shí)施效果，可根據(jù)實(shí)際需要增減非必要的技術(shù)特征，來(lái)滿足不同情況的需求。當(dāng)前第1頁(yè)1 2 3