python实现Nao机器人的单目测距(机器人摄像头)

本文实例为大家分享了python实现Nao机器人单目测距的具体代码，供大家参考，具体内容如下

此代码适于用做对Nao机器人做视觉识别和测距实验,只提供关键代码部分,尝试利用cv2去优化代码会更加简洁哟！

此代码的主要功能：

1.初始姿态下，通过更换摄像头和转头去寻找目标
2.通过颜色阈值识别目标，计算目标与Nao的距离和角度

可以扩展功能：

1.在运动过程中对方向和距离进行多次测量和校正，提高准确度
2.找到目标后，通过对目标的测量，选择使用哪个脚去踢目标

#!/usr/bin/python2.7
#-*- encoding: UTF-8 -*-
import vision_definitions
#----------------------单目测距--------------------------------
#***********************************************
#@函数名：DistAndDirect_cal(cxnum,rynum,colsum,rowsum,Head_angle,cameraID)
#@参数：  (cxnum,rynum)是通过图像识别得到球心的像素点坐标
#  (colsum,rowsum)是图片总大小：640*480
#cameraID=0，取上摄像头；cameraID=1，取下摄像头
#@返回值：无
#@功能说明： 采用机器人的下摄像头进行测量球离机器人的相关角度与距离
def DistAndDirect_cal(cxnum,rynum,colsum,rowsum,Head_angle,cameraID):
 distx=-(cxnum-colsum/2)
 disty=rynum-rowsu/2
 print distx,disty
 Picture_angle=disty*47.64/480
 if cameraID ==0:
  h=0.62
  Camera_angle=12
 else:
  h=0.57
  Camera_angle=38
 #Head_angle[0]机器人仰俯角度
 Total_angle=math.pi*(Picture_angle+Camera_angle)/180+Head_angle[0]
 d1=h/math.tan(Total_angle)
 alpha=math.pi*(distx*60.92/640)/180
 d2=d1/math.cos(alpha)
 #Head_angle[1]机器人左右角度
 Forward_Distance=d2*math.cos(alpha+Head_angle[1])
 Sideward_Distance=-d2*math.sin(alpha+Head_angle[1])
#***********************************************
#@函数名：GetNaoImage(IP,PORT,cameraID)
#@参数：  略
#@返回值：无
#@功能说明： 采调用机器人内置摄像头控制模块，对当前场景进行拍摄并保持。
#  由于球距离机器人约小于0.6m时，机器人额头摄像头无法看到，
#  所以需要变换摄像头，cameraID=0，取上摄像头；
#  cameraID=1，取下摄像头
def Get NaoImage(IP,PORT,cameraID):
 camProxy=ALProxy("ALVideoDevice",IP,PORT)
 resolition=2 #VGA格式640*480
 colorSpace=11#RGB
 #选择并启用摄像头
 camProxy.setParam(vision_definitions.kCameraSelectID,cameraID)
 videoClient=camProxy.subscribe("python_client",resolition,colorSpace,5)
 #获取摄像机图像。
 #image [6]包含以ASCII字符数组形式传递的图像数据。
 naoImage=camProxy.getImageRemote(videoClient)
 camProxy.unsubscribe(videoClient)
 #获取图像大小和像素阵列。
 imageWidth=naoImage[0]
 imageHeight=naoImage[1]
 array=naoImage[6]
 #从我们的像素阵列创建一个PIL图像。
 im=Image.fromstring("RGB",(imageWidth,imageHeight),array)
 #保存图像。
 im.save("temp.jpg","JPEG")
#***********************************************
#@函数名：findColorPattern(img,pattern)
#@参数：  略
#@返回值：无
#@功能说明：  将RGB图像转化为二值图：此方法用的是cv,可以尝试用cv2代码会更加简洁
def  findColorPattern(img,pattern):
 channels=[None,None,None]
 channels[0]=cv.CreateImage(cv.GetSize(img),8,1)
 channels[1]=cv.CreateImage(cv.GetSize(img),8,1)
 channels[2]=cv.CreateImage(cv.GetSize(img),8,1)
 ch0=cv.CreateImage(cv.GetSize(img),8,1)
 ch1=cv.CreateImage(cv.GetSize(img),8,1)
 ch2=cv.CreateImage(cv.GetSize(img),8,1)
 cv.Split(img,ch0,ch1,ch2,None)
 dest=[None,None,None,None]
 dest[0]=cv.CreateImage(cv.GetSize(img),8,1)
 dest[1]=cv.CreateImage(cv.GetSize(img),8,1)
 dest[2]=cv.CreateImage(cv.GetSize(img),8,1)
 dest[3]=cv.CreateImage(cv.GetSize(img),8,1)
 cv.Smooth(ch0,channels[0],cv.CV_GAUSSIAN,3,3,0)
 cv.Smooth(ch1,channels[1],cv.CV_GAUSSIAN,3,3,0)
 cv.Smooth(ch2,channels[2],cv.CV_GAUSSIAN,3,3,0)
 for i in range(3):
  k=2-i
  lower=pattern[k]-75#设置阈值
  upper=pattern[k]+75
  cv.InRangeS(channels[i],lower,upper,dest[i])
 cv.And(dest[0],dest[1],dest[3])
 temp=cv.CreateImage(cv.GetSize(img),8,1)
 cv.And(dest[2],dest[3],temp)
 '''
 cv.NameWindow("result",cv.CV_WINDOW_AUTOSIZE)
 cv.ShowImage("result",temp)
 cv.WaitKey(0)
 '''
 return temp
#***********************************************
#@函数名：xyProject(matrix,imgaesize)
#@参数：  matrix
#  imgaesize
#@返回值：无
#@功能说明： 利用二值图，计算球的像素坐标。其原理是：遍历各行各列
#  像素的数值的和，最大的组合即为球心坐标
def xyProject(matrix,imagesize):
 #声明一个数据类型为8位型单通道的imagessize[1]*1/1*imagessize[0]矩阵(初始值为 0)。
 colmask=cv.CreateMat(imagessize[1],1,cv.CV_8UC1)
 rowmask=cv.CreateMat(1,imagessize[0],cv.CV_8UC1)
 cv.Set(colmask,1)
 cv.Set(rowmask,1)
 colsum=[]
 for i in range(imagesize[0]):
  col=cv.GetCol(matrix,i)
  #计算向量点积
  a=cv.DotProduct(colmask,col)
  colsum.append(a)
 rowsum=[]
 for i in range(imagesize[1]):
  row=cv.GetRow(matrix,i)
  a=cv.DotProduct(rowmask,row)
  rowsum.append(a)
 return(colsum,rowsum)#得到各行各列“1”值的和
def crMax(colsum,rowsum):
 cx=max(colsum)
 ry=max(rowsum)
 for i in range(len(colsum)):
  if colsum[i]==cx:
cxnum=i
 for i in range(len(rowsum)):
  if rowsum[i]==ry:
rynum=i
 return(cxnum,rynum)
#***********************************************
#@函数名：  GetHeadAngles(robotIP,PORT)
#@参数： 略
#@返回值：无
#@功能说明：
def GetHeadAngles(robotIP,PORT):
 motionProxy=ALProxy("ALMotion",robotIP,PORT)
 names=["HeadPitch","HeadYaw"]
 useSensors=1
 sensorAngles=motionProxy.getAngles(names,useSensors)
 return sensorAngles
#***********************************************
#@函数名：  SetHeadAngles(robotIP,PORT,angles)
#@参数： 略
#@返回值：无
#@功能说明：
def SetHeadAngles(robotIP,PORT,angles):
 motionProxy=ALProxy("ALMotion",robotIP,PORT)
 motionProxy.setStiffnesses("Head",1.0)
 names=["HeadPitch","HeadYaw"]
 fractionMaxSpeed=0.2
 motionProxy.setAngles(names,angles,fractionMaxSpeed)
 time.sleep(2.0)
 motionProxy.setStiffnesses("Head",0.0)
#***********************************************
#@函数名：Capture_Picture(IP,PORT,cameraID,angles,pattern_colors)
#@参数：  angles
#  pattern_colors
#@返回值：无
#@功能说明： 将上面的一系列函数整合起来
def Capture_Picture(IP,PORT,cameraID,angles,pattern_colors):
 SetHeadAngles(IP,PORT,angles)
 GetNaoImage(IP,PORT,cameraID)
 image=cv.LoadImage("temp.jpg")
 imagesize=cv.GetSize(image) #返回数值，两个元素分别为列数和行数
 matrix=findColorPattern(image,pattern_colors)
 (colsum,rowsum)=xyProject(matrix,imagesize)
 (cxnum,rynum)=crMax(colsum,rowsum)
 cv.SaveImage("result.jpg",matrix)
 return (cxnum,rynum,colsum,rowsum)
 
#***********************************************
#@函数名：Target_Detect_and_Distance(IP,PORT)
#@参数：
#@返回值：无
#@功能说明： 当上摄像头无法找到球时，切换到下摄像头，然后在左转右转。
# 在这个过程中，如果发现目标，则计算距离并输出距离
# 若始终未找到目标，则输出距离为0。
def Target_Detect_and_Distance(IP,PORT):
 pattern_colors=(255,150,50)
 cameraID=0# 默认上摄像头
 angles=[0,0]
 (cxnum,rynum,colsum,rowsum)=Capture_Picture(IP,PORT,cameraID,angles)
 if(cxnum,rynum)==(639,479):
  cameraID=1
  (cxnum,rynum,colsum,rowsum)=Capture_Picture(IP,PORT,cameraID,angles)
 if(cxnum,rynum)==(639,479):
  cameraID=0
  angles=[0.0.7]
  (cxnum,rynum,colsum,rowsum)=Capture_Picture(IP,PORT,cameraID,angles)
 if(cxnum,rynum)==(639,479):
  cameraID=0
  angles=[0,-0.7]
  (cxnum,rynum,colsum,rowsum)=Capture_Picture(IP,PORT,cameraID,angles)
 HeadAngles-GetHeadAngles(IP,PORT)
 ###############
 (Forward_Distance,Sideward_Distance)=DistAndDirect_cal(cxnum,rynum,colsum,rowsum,Head_angle,cameraID)
 if(cxnum,rynum)==(639,479):
  (Forward_Distance,Sideward_Distance)=(0,0)
 print "Forward_Distance=",Forward_Distance,"meters"
 print "Sideward_Distance="+Sideward_Distance+"meters"
#***********************************************
#@函数名：Target_Detect_and_Distance(IP,PORT)
#@参数：
#@返回值：无
#@功能说明： 当找到球后，可能会存在一定的误差。
#  因此需要判断球位于机器人前方的哪一侧，再来确定用哪只脚踢球
def Final_See(robotIP,PORT):
 pattern_colors=(255,150,50)
 angles=[0.5,0]
 SetHeadAngles(robotIP,PORT,angles)
 cameraID=1
 GetNaoImage(robotIP,PORT,cameraID)
 image=cv.LoadImage("temp.jpg")
 imagesize=cv.GetNaoImage(image)
 matrix=findColorPattern(image,pattern_colors)
 (colsum,rowsum)=xyProject(matrix,imgaesize)
 (cxnum,rynum)=crMax(colsum,rowsum)
 cv.SaveImage("result.jpg",matrix)
 HeadAngles=GetHeadAngles(robotIP,PORT)
 #########################
 (Forward_Distance,Sideward_Distance)=DistAndDirect_cal(cxnum,rynum,colsum,rowsum,Head_angle,cameraID)
 if cxnum<len(colsum)/2:
  side=0#左脚
 else:
  side=1#右脚
 print "side=",side
 print "last distance=",Forward_Distance
 return (side,Forward_Distance)

以上就是本文的全部内容，希望对大家的学习有所帮助，也希望大家多多支持本站。

香港服务器租用

动态拨号：关键词排名下降是啥缘故，快速提高排名怎样做

排名优化：网站排名优化方法有什么，如何做有效果

老域名：怎样才算老域名，老域名建站有什么影响

内容优化：关键字排名要做哪些方面的优化，怎样做

技巧：网站转化率究竟是什么，有什么提升的技巧

一下吧：外贸站优化有哪些基本的做法和注意事项

概要：竞价推广费用大概要多少呢，竞价推广好不好

一下吧：SEO中site是什么意思，作用和应用是怎样的

邮箱：付费邮箱有哪些优势，付费邮箱挑选要考虑什么

集群是什么意思：集群是什么意思，都有哪些优势呢