Nao機(jī)器人單目測距

1、視覺識別和測距局部：雖然這次的任務(wù)以機(jī)器人的步態(tài)規(guī)劃和姿態(tài)控制等為主，但要準(zhǔn)確實(shí)現(xiàn)尋找并踢到乒乓球的目標(biāo)，通過攝像頭圖像的視覺識別和測距也是非常重要的。功能和策略 視覺局部的主要功能和策略包括：1、在初始姿態(tài)下尋找乒乓球，包括配合機(jī)器人的動(dòng)作，如更換攝像頭和轉(zhuǎn)頭來 實(shí)現(xiàn)對超出當(dāng)前視野X圍的目標(biāo)的尋找。2、通過顏色閾值識別乒乓球，計(jì)算乒乓球和機(jī)器人之間的距離和角度，傳遞數(shù) 據(jù)給運(yùn)動(dòng)局部。3、在運(yùn)動(dòng)過程中對方向和距離進(jìn)展屢次測量和校正，不斷修正運(yùn)動(dòng)誤差，保證 最終準(zhǔn)確到達(dá)。4、到達(dá)目標(biāo)位置后，通過對乒乓球位置的再次測量，選擇機(jī)器人踢乒乓球所使 用的腳。測距方案的選擇1、雙目測距雙目測距的主要原理是

2、利用位于不同空間位置，有重復(fù)的視角區(qū)域的兩個(gè)攝像頭同時(shí)進(jìn)展拍攝。對得到的兩 X圖片進(jìn)展一定的校正和變換，識別出位于兩 X 圖片中的乒乓球，分別得到對應(yīng)的參數(shù)。然后根據(jù)幾何關(guān)系推導(dǎo)出乒乓球和機(jī)器 人的距離。雙目測距的準(zhǔn)確度較高，但計(jì)算過程復(fù)雜，對圖像的校正和變換處理要求高。 對于nao機(jī)器人來說，雙目測距還存在一個(gè)問題，即上下兩個(gè)攝像頭的重合區(qū)域 較小，就可能導(dǎo)致可用測距 X圍小，對于尋找到乒乓球有一定的困難。58.71此外考慮到這次任務(wù)對測距的準(zhǔn)確度要求并不是那么高， 而且可以通過一定 的補(bǔ)償方法來提高單目測距的準(zhǔn)確度， 為了程序的簡便起見，我們決定采用單目 測距的方法。2、單目測距單目測距的

3、原理比擬簡單，如下列圖:一般來說，就是根據(jù)圖中的兩個(gè)相似三角形的比例關(guān)系計(jì)算出距離乙但是在實(shí)際操作中發(fā)現(xiàn)，對于攝像頭的焦距f的獲取存在很大的問題和誤差。此外，由于 攝像頭的主光軸并不是水平的，用這種方法計(jì)算并不方便。所以我采用了另外一 種類似的計(jì)算方法： ，H為機(jī)器人高度。通過圖像識別我們可以得到目標(biāo)乒乓球的球心近似像素坐 標(biāo)x, y，而圖片總大小為640*480。這個(gè)方法的主要思路是圖像中角度的比 例和長度的比例近似相等，那么根據(jù)公式：disty=y-480/2a=dist即可得到a的角度大小。機(jī)器人頭部也可能有俯仰角度，設(shè)為m圖中未標(biāo)出， 那么c=a+b+m,機(jī)器人與乒乓球的前向距離為d1

4、=H/ tan(c)此外，我們還需要計(jì)算乒乓球和機(jī)器人的側(cè)向距離2 / 8did d2bab為機(jī)器人頭部左右方向的偏航角。那么distx=x-640/2d2=d1 cos(a) d=d2* cos(a+b)S =d2* si n( a+b)由此就可以得到目標(biāo)乒乓球相對于機(jī)器人軀干的前向和側(cè)向距離。如果需要計(jì)算角度的話，只要求距離的反正切就可以了。這一局部數(shù)學(xué)計(jì)算的Python程序如下所示：f(cxnuui, rynum, colsum, rowsuir Head_angle caineralD):diBrx=- (cxnuiE-calsuin/ 2di3ty= rynuni- rows jil

5、/ 2print di日匚齊print disyPicture aiigle-disty* ( 64/80i_f camera芷D = 0 :h=0,2Camera_anglE = 12:h-0三、圖像識別方法上文闡述了利用單目測距，在獲得了目標(biāo)乒乓球球心的像素坐標(biāo)后如何計(jì)算 機(jī)器人和乒乓球之間距離的方法。下面就來詳細(xì)說明通過圖像識別獲得乒乓球球 心坐標(biāo)的具體方法。在本次任務(wù)中，我是通過識別乒乓球的顏色來實(shí)現(xiàn)定位乒乓球位置的目的 的。即當(dāng)乒乓球顏色與背景顏色相差較大時(shí)， 設(shè)定需要的色彩閾值，就能過濾背 景中的其他內(nèi)容，只識別出目標(biāo)乒乓球的形狀。此外，利用Python編寫圖像識別程序，需要用到O

6、pencv和PIL的相關(guān)功能。主要步驟:1、首先調(diào)用nao機(jī)器人內(nèi)置的攝像頭控制模塊，對當(dāng)前場景進(jìn)展拍攝。由于在球距機(jī)器人較近時(shí)約小于 0.6m，機(jī)器人額頭的攝像頭無法看到，所以 需要變換使用下巴部位的攝像頭。在這段程序中我就額外添加了更改攝像頭的語 句，并通過設(shè)置cameralD來控制，0為額頭攝像頭，1為下巴攝像頭Jef GetMaoIxage (IPr PORTr cameraIE:camPioXy = ALPrdaty (rAL -z deoil, 工F” PORT)resoluriaa = 2舉.GAcolorSpac = 114 RGB*逢擇幷啟用握像頭camProxy, aetP

7、arax (visioTi_definition5 *cameraLD)video匚liErt = camPEUJiH.日uDsuzdJc殳= resolution colorSpacer 5)aoZiaage canProxy-getlmageRemote(videaClient)1 mPzA匸上丄bu j V 口Cl丄LiEdceWidtsi = naoTmage 0 ir.a ge H e i 1 nao Tillage 1 arr&y = naolinage 6 jim =- FramsEtringght) f aTray)IniiBave (麗二巴皿丘 jpgf JPEG J2、設(shè)定

8、色彩閾值，識別乒乓球。第一步拍攝的圖片保存為JPEG格式，色彩為RGB模式。在這一步利用Ope ncv命令讀取圖片后，可以將原來的三通道圖像分 別保存為三個(gè)單通道的圖像進(jìn)展處理。逐個(gè)對像素點(diǎn)進(jìn)展檢查，符合閾值X圍的 保存為1,不符合的為0,最終得到一 X二值圖，此時(shí)乒乓球即以空白區(qū)域顯示， 其他干擾的背景均為黑色。實(shí)際效果如如下圖所：_=1 findCoLczFacEern(imgF paCEern):try 匸口 teat if one pixel is in 口口r patternpi H Vfchann皂丄s = Noner Nonj Nan皂channels 0 = cv-Create

9、 Image (cv. GetSise (iw) * Sf 1) tblne ctiBnnelsl = cv.Createlrcage (cv.GecSiea (iw r Sr 1 green channels 2 = cv. Create Image (cv_ Ger SizerSr1tr&dchO = cv- Zt已注匸(d甘-G色匸51.之2 丄建口)Br 1 H鬥 1二巳 chi = cv. Create Image (cv Ge 匸 5i.z 巳(inio)1 #0 匕已巳 n ch2 = cv*CreateImaqe (cvG亡匕呂i.畫已(丨加# 丁日尸1.日ev, Split

10、(inrgr ehO,. ehl,. eh2f 饑“襯fimg 國片的三通道數(shù)址瑾弓GBfitb1dest = (Hone, Mane, Nor*Ler Maneldes七0=Createlmage (cv(久比牙 1 尸 1)dest1= uyCreatelmage(cv2豐七1云比甘)十包土1dest2 = cv - Createlmage (cv.GetSize (ling)8r 1 de st 3 = cv - Createlirage ( cv -. SetSize (img f 8,-1 cv. Smooth Gh0fchannels 0fcv CV_GAU5SIAN,3f3f0*

11、平滑田像cv,Smooth(chlfchannelsl)fcv.CVGAUSSIAN,3f3,0“ 5nOQDh(ch2rchannels 2 rcy.CV_GAUSSIAN,-3r3rOJ*二u二 1 -r.3:k=2-ilower - patterned - 75#設(shè)置 Rltt?5tipper - natcernk + 75cv-InRengeS(channelsiflowerr upper,dest(ij)cv.And(desttdear1tdear3temp = cv. Crear-elmage (cv. GecSize (lmg F 0,1 gv.And (dest 2 dest

12、3 F Cenp#彳學(xué)劭二 tt 因，RG*cv.Nair.edWindow(nresultnf cv.CVJWINDOW_AUTOSIZEtcv. Shawlrtage( nres口_匸M r temp) tcv. WaitKey (01 retizrr temp該方法簡單明了，運(yùn)算速度較快，在光照良好，無明顯干擾的情況下，設(shè)定 準(zhǔn)確的閾值X圍，如此識別可靠性較高。3、利用得到的二值圖數(shù)據(jù)，可以計(jì)算出乒乓球的球心像素坐標(biāo)。原理很簡單， 即遍歷各行各列像素點(diǎn)的數(shù)值的合，最大的組合即為球心坐標(biāo)。4ef icy Exomat x lx f IjelsJcs i z c);rr n nprDjier

13、ewaun =fer 1 土n range imagsa-ize |i F i xotf V .GetRudW (tuaLXJLJL xI v*EctP：oausc(zc4SFrow) tQwi jm亠呂匚pendrctuzn (oalaun tgwaur:)解耳到各行各列的和le f crMax (colsiim, rowBin):(匚aljurn)rymax(rowsumfor 1 j_ 匸 range Lan | calsunl): col3jm i = ex:cxauin-ifar i i_n range (J_en (rowsLzir)、:to 沖導(dǎo)Jin 二=ry;rynjit-i

14、二 mrii 二匸 (cKnunir rynuni然后就可以利用得到的坐標(biāo)數(shù)據(jù)，按照上文所述的方法去計(jì)算距離了。下面是將子函數(shù)綜合起來的一次識別的程序：.Captut_FiGtuCIPr FORT, coir后匸耳HD#gartt塔二二匚二品)；SetHeadAngles (IF, PORT, angles)GetNaoIrrage (IPf PCRTf carreraID|image = cv LoadLmage fteir.p , jpqr,iitagsaiEe = cv.GetSize tiirage*返回?cái)?shù)純，兩個(gè)元秦分別湄列數(shù)和行數(shù)itiatrx = fj_ndColcrPatter

15、n tnra.ge,pa.ct：eni colors)Ccclsun;, xowsujn) = nyProj ect (m.atrix, iiraeaize)Ccxnumz rynum) czMax colsum rowfum)-cv_ Save Inage (,r lesulc.pg M r natrix)二三 J：: (cxnuCTif rynwi,col3Uin, Towsurn四、識別策略的優(yōu)化和附加功能前面兩局部介紹了圖像識別和測距的最根底原理。除此之外，為了能夠與機(jī) 器人運(yùn)行的實(shí)際情況相結(jié)合，并實(shí)現(xiàn)更為豐富的功能，還需要進(jìn)展其他的程序設(shè) 計(jì)。1、如上文所說，機(jī)器人默認(rèn)使用額頭部位的

16、攝像頭，但當(dāng)乒乓球距離機(jī)器人較 近，或超出水平視場X圍時(shí)，如此需要通過判斷程序和機(jī)器人的動(dòng)作來進(jìn)一步找 到球。其策略是，當(dāng)額頭攝像頭無法找到目標(biāo)時(shí)，先切換為下巴攝像頭。然后再左 轉(zhuǎn)機(jī)器人頭部，右轉(zhuǎn)機(jī)器人頭部。在這過程中，如果發(fā)現(xiàn)目標(biāo)，如此立即進(jìn)展計(jì) 算并輸出距離。假如始終未找到目標(biāo)，如此輸出距離為 0，等待下一步命令。:三三 Tarjet Detect and(IE, PCRr:parserr colors = 55f 150, 0)c ame xa I D=0卡券認(rèn)氏需頭相機(jī)開抬拍礙angles-TO, 0(cxnuHijrynum, colsuni, rowaun) Capture_Pic

17、ture (IP, PORT* xam雯rTD” ar|rxrun rynum)=(63 5 斗7超:cur.*x*ID 1(sxnuxn synuzn calsumT xswaun) Captdxc_PiatuEC (IP/ rORT, cacncralr( cxnujx zynum) (633 473 ji ；u-aJUE上a.TD ODiigl.ca- 0, 0,7(jxiimt.,丄推：山t:ci丄出- CgluLlI丄匕_Flc:i,3i匕Tr PORTr ceQLtidl,ll (cxnuji, rynumj =-( 63 3,479):angles= _0,-0. J(zxzil

18、, rynjni, cQlsuirr r jwaum) = Capture Ficrure IIP, PORTr canueral3,HeadAzgles = GtHeadAzigl&s (IE, POR.T1Sarwaxd Dis-ance,Sidewizrd Distac已 = Dist2LndEirect cal cxzuil, zyziui(cxnuitj rynum) = (633, 479):(Forward Distance,5Ldwaxd Dtsance= (0,OJprint mForward_Disrance=n, Forward_Di5tance, meters 十前向距

19、蔑 print SideirCisTaic1,5ideward_Distance豐仲向距鬲2、當(dāng)機(jī)器人移動(dòng)到目標(biāo)區(qū)域，即乒乓球前時(shí)，尚存在一定位置偏差。此時(shí)就需 要判定乒乓球位于機(jī)器人前方哪一側(cè)，再?zèng)Q定使用哪一只腳踢乒乓球。這一 功能的實(shí)現(xiàn)和之前的視覺識別根本一樣，只是改變了一下判斷和輸出條件。 Final_See robo-IPf PORT:良t二erE_cclors _ (255,150 50 0.5,0SeceatiAnglea (robocIP, FORT, angles)cair.raID=LGetMa0lniag& (rabatIFf PORTf caireralD)imajsiz

20、s = cvGetSize (iirafle)n;at rix = findCalorPattern (image, pattsrn_color3) (colsuiLf rawsumt - xyProjecr (matrixr irna?eBiz)i(cxnuinrynmn) crMax (colBuinrowauiii)cv. Savelmage (二亡占It, jzg inaDrixiHeaziAzi51e,s = GsriteadAnglea (zobflcIP, PS?RTj(forward Di s t an c e f S1 de ward Distance) = DistAndDirect cal (cxnun,Len