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)
以上就是本文的全部內容，希望對大傢的學習有所幫助，也希望大傢多多支持WalkonNet。
推薦閱讀：
- Python中turtle繪圖模塊的詳細講解
- C++實現LeetCode(48.旋轉圖像)
- C++實現LeetCode(73.矩陣賦零)
- Python的turtle繪圖庫使用基礎
- C++提取文件名與提取XML文件的方法詳解