python怎么实现Nao机器人的单目测距功能
发表于:2024-11-26 作者:千家信息网编辑
千家信息网最后更新 2024年11月26日,本篇内容介绍了"python怎么实现Nao机器人的单目测距功能"的有关知识,在实际案例的操作过程中,不少人都会遇到这样的困境,接下来就让小编带领大家学习一下如何处理这些情况吧!希望大家仔细阅读,能够学
千家信息网最后更新 2024年11月26日python怎么实现Nao机器人的单目测距功能
本篇内容介绍了"python怎么实现Nao机器人的单目测距功能"的有关知识,在实际案例的操作过程中,不少人都会遇到这样的困境,接下来就让小编带领大家学习一下如何处理这些情况吧!希望大家仔细阅读,能够学有所成!
此代码的主要功能:
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) ch2=cv.CreateImage(cv.GetSize(img),8,1) ch3=cv.CreateImage(cv.GetSize(img),8,1) cv.Split(img,ch0,ch2,ch3,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(ch2,channels[1],cv.CV_GAUSSIAN,3,3,0) cv.Smooth(ch3,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"python怎么实现Nao机器人的单目测距功能"的内容就介绍到这里了,感谢大家的阅读。如果想了解更多行业相关的知识可以关注网站,小编将为大家输出更多高质量的实用文章!
摄像
功能
摄像头
机器
机器人
函数
参数
目标
图像
像素
角度
坐标
过程
测量
输出
代码
内容
大小
数值
数据
数据库的安全要保护哪些东西
数据库安全各自的含义是什么
生产安全数据库录入
数据库的安全性及管理
数据库安全策略包含哪些
海淀数据库安全审计系统
建立农村房屋安全信息数据库
易用的数据库客户端支持安全管理
连接数据库失败ssl安全错误
数据库的锁怎样保障安全
电子城科技大厦 互联网公司
南昌服务器租用商有哪些
网络安全漫画简单图小学二年级
北京钟爱网络技术有限公司
软件开发中的生产环境搭建
数据库专升本2020海南
软件开发培训学费多少
本机代理服务器
数据库建设技术特点
sql 数据库路径
网络安全链接是什么
软件开发思路保护
淮南市舍得网络技术有限公司
艾尔登法环登陆服务器有什么用
备份的数据库是啥
数据库不支持mvcc
qq开黑服务器是什么原因
重庆猎头软件开发有限公司
通用数据库是啥
徐州品牌网络技术服务费
医学影像数据库知乎
软件开发工程师访谈
dell t700服务器
景德镇正规服务器找哪家
个人做股票软件开发
ugg仿牌服务器
党委党组加强网络安全会议
原神分辨服务器
苏州有多少家做软件开发的
网络安全边界设施