Nao机器人单目测距

1、视觉识别和测距局部：虽然这次的任务以机器人的步态规划和姿态控制等为主，但要准确实现寻找并踢到乒乓球的目标，通过摄像头图像的视觉识别和测距也是非常重要的。功能和策略 视觉局部的主要功能和策略包括：1、在初始姿态下寻找乒乓球，包括配合机器人的动作，如更换摄像头和转头来 实现对超出当前视野X围的目标的寻找。2、通过颜色阈值识别乒乓球，计算乒乓球和机器人之间的距离和角度，传递数 据给运动局部。3、在运动过程中对方向和距离进展屡次测量和校正，不断修正运动误差，保证 最终准确到达。4、到达目标位置后，通过对乒乓球位置的再次测量，选择机器人踢乒乓球所使 用的脚。测距方案的选择1、双目测距双目测距的主要原理是

2、利用位于不同空间位置，有重复的视角区域的两个摄像头同时进展拍摄。对得到的两 X图片进展一定的校正和变换，识别出位于两 X 图片中的乒乓球，分别得到对应的参数。然后根据几何关系推导出乒乓球和机器 人的距离。双目测距的准确度较高，但计算过程复杂，对图像的校正和变换处理要求高。 对于nao机器人来说，双目测距还存在一个问题，即上下两个摄像头的重合区域 较小，就可能导致可用测距 X围小，对于寻找到乒乓球有一定的困难。58.71此外考虑到这次任务对测距的准确度要求并不是那么高， 而且可以通过一定 的补偿方法来提高单目测距的准确度， 为了程序的简便起见，我们决定采用单目 测距的方法。2、单目测距单目测距的

3、原理比拟简单，如下列图:一般来说，就是根据图中的两个相似三角形的比例关系计算出距离乙但是在实际操作中发现，对于摄像头的焦距f的获取存在很大的问题和误差。此外，由于 摄像头的主光轴并不是水平的，用这种方法计算并不方便。所以我采用了另外一 种类似的计算方法： ，H为机器人高度。通过图像识别我们可以得到目标乒乓球的球心近似像素坐 标x, y，而图片总大小为640*480。这个方法的主要思路是图像中角度的比 例和长度的比例近似相等，那么根据公式：disty=y-480/2a=dist即可得到a的角度大小。机器人头部也可能有俯仰角度，设为m图中未标出， 那么c=a+b+m,机器人与乒乓球的前向距离为d1

4、=H/ tan(c)此外，我们还需要计算乒乓球和机器人的侧向距离2 / 8did d2bab为机器人头部左右方向的偏航角。那么distx=x-640/2d2=d1 cos(a) d=d2* cos(a+b)S =d2* si n( a+b)由此就可以得到目标乒乓球相对于机器人躯干的前向和侧向距离。如果需要计算角度的话，只要求距离的反正切就可以了。这一局部数学计算的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三、图像识别方法上文阐述了利用单目测距，在获得了目标乒乓球球心的像素坐标后如何计算 机器人和乒乓球之间距离的方法。下面就来详细说明通过图像识别获得乒乓球球 心坐标的具体方法。在本次任务中，我是通过识别乒乓球的颜色来实现定位乒乓球位置的目的 的。即当乒乓球颜色与背景颜色相差较大时， 设定需要的色彩阈值，就能过滤背 景中的其他内容，只识别出目标乒乓球的形状。此外，利用Python编写图像识别程序，需要用到O

6、pencv和PIL的相关功能。主要步骤:1、首先调用nao机器人内置的摄像头控制模块，对当前场景进展拍摄。由于在球距机器人较近时约小于 0.6m，机器人额头的摄像头无法看到，所以 需要变换使用下巴部位的摄像头。在这段程序中我就额外添加了更改摄像头的语 句，并通过设置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、设定

8、色彩阈值，识别乒乓球。第一步拍摄的图片保存为JPEG格式，色彩为RGB模式。在这一步利用Ope ncv命令读取图片后，可以将原来的三通道图像分 别保存为三个单通道的图像进展处理。逐个对像素点进展检查，符合阈值X围的 保存为1,不符合的为0,最终得到一 X二值图，此时乒乓球即以空白区域显示， 其他干扰的背景均为黑色。实际效果如如下图所：_=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 国片的三通道数址瑾弓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#设置 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#彳學劭二 tt 因，RG*cv.Nair.edWindow(nresultnf cv.CVJWINDOW_AUTOSIZEtcv. Shawlrtage( nres口_匸M r temp) tcv. WaitKey (01 retizrr temp该方法简单明了，运算速度较快，在光照良好，无明显干扰的情况下，设定 准确的阈值X围，如此识别可靠性较高。3、利用得到的二值图数据，可以计算出乒乓球的球心像素坐标。原理很简单， 即遍历各行各列像素点的数值的合，最大的组合即为球心坐标。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 沖导Jin 二=ry;rynjit-i

14、二 mrii 二匸 (cKnunir rynuni然后就可以利用得到的坐标数据，按照上文所述的方法去计算距离了。下面是将子函数综合起来的一次识别的程序：.Captut_FiGtuCIPr FORT, coir后匸耳HD#gartt塔二二匚二品)；SetHeadAngles (IF, PORT, angles)GetNaoIrrage (IPf PCRTf carreraID|image = cv LoadLmage fteir.p , jpqr,iitagsaiEe = cv.GetSize tiirage*返回数纯，两个元秦分别湄列数和行数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四、识别策略的优化和附加功能前面两局部介绍了图像识别和测距的最根底原理。除此之外，为了能够与机 器人运行的实际情况相结合，并实现更为丰富的功能，还需要进展其他的程序设 计。1、如上文所说，机器人默认使用额头部位的

16、摄像头，但当乒乓球距离机器人较 近，或超出水平视场X围时，如此需要通过判断程序和机器人的动作来进一步找 到球。其策略是，当额头摄像头无法找到目标时，先切换为下巴摄像头。然后再左 转机器人头部，右转机器人头部。在这过程中，如果发现目标，如此立即进展计 算并输出距离。假如始终未找到目标，如此输出距离为 0，等待下一步命令。:三三 Tarjet Detect and(IE, PCRr:parserr colors = 55f 150, 0)c ame xa I D=0卡券认氏需头相机开抬拍碍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、当机器人移动到目标区域，即乒乓球前时，尚存在一定位置偏差。此时就需 要判定乒乓球位于机器人前方哪一侧，再决定使用哪一只脚踢乒乓球。这一 功能的实现和之前的视觉识别根本一样，只是改变了一下判断和输出条件。 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