前言
在現(xiàn)代科技的普及下，人們對于機器人的興趣與期待日漸增加。然而，大多數(shù)人對機器人的印象仍停留在復雜、高度智能的形象上。而今天，我將重點介紹一個極簡的柵格地圖行走機器人，它不僅使用了簡單的編程語言Python，而且只是一個基礎的柵格地圖行走算法的展示。這個機器人并不具備復雜的感知與決策能力，只能按照預定的規(guī)則在柵格地圖上行走。然而，正是這種簡單的機器人展示了編程的魅力與機器人的可能性。通過學習這個機器人的代碼與原理，我們可以更好地理解機器人的宏觀工作流程，并激發(fā)我們對機器人的創(chuàng)造力與想象力。無論是初學者還是有一定編程經(jīng)驗的人，都能從這個極簡的柵格地圖行走機器人中獲得一些啟發(fā)與收獲。希望大家通過這篇博文來一窺機器人的奧秘，并能在編程與機器人領域中探索更多可能性。
本文使用Python編寫的行走機器人，可以通過監(jiān)盤操作方向，在給定的地圖上進行移動，并根據(jù)地圖上的障礙物確定運動的邊界。我們將使用OpenCV和NumPy庫來處理地圖和機器人的移動，還模擬了一個只有四個方向的測距雷達。
效果如下：
代碼思路
在代碼的結構中，我們定義了一個機器人類，其中包含了機器人的位置、地圖數(shù)據(jù)和時間間隔等屬性。
然后是實現(xiàn)了一個運動模型，和雷達的觀測。
并提供了一個地圖數(shù)據(jù)更新函數(shù)。
class agnet:
def __init__(self,location=(1,1),map_data=None,DT=1):
if type(map_data)==type(None):
#先驗地圖
self.map_data= [
[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,1],
[1, 9, 0, 0, 0, 0, 1, 0, 0, 0, 0,1],
[1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,1],
[1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0,1],
[1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0,1],
[1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,1],
[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,1],
[1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1,1],
[1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0,1],
[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,1],
[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,1],
[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,1],
]
else:
self.map_data=map_data
self.lifevalue=100
#雷達數(shù)據(jù)
self.sensordata=[0,0,0,0]
#機器人位姿xy坐標系下
self.posexy=np.array([[1],[-1],[-np.pi/2],[1]])#x寬,y長,yaw為逆時針方向弧度[rad/s]
#機器人位姿圖像坐標系下
self.poseuv=np.array([[1],[1],[-np.pi/2],[1]])
self.Dt=DT#時間步長
def sensor(self):
#雷達返回四個方向的障礙向量
###
print("雷達數(shù)據(jù):",self.sensordata)
return ds
def xy2uv(self,x):
#將xy坐標系統(tǒng)轉化為uv坐標系
uv=x.copy()
uv[1,0]=-1*x[1,0]
return uv
def flash_mapdata(self,u,Dt=None):
#刷新地圖數(shù)據(jù)
if Dt==None:
pass
else:
self.Dt=Dt
x=self.next_state(u)
xuv=self.xy2uv(x)
#print("xuv",xuv)
#print("x",x)
#print("posex",self.posex)
x0=self.poseuv
iu0=round(x0[0, 0])
iv0=round(x0[1, 0])
self.map_data[iv0][iu0]=0
iu=round(xuv[0, 0])
iv=round(xuv[1, 0])
self.map_data[iv][iu]=9
#更新位姿
self.poseuv=xuv
self.posexy=x
#測量
self.sensor()
#print("raidar:",self.sensor())
return self.map_data
def radiar(self, direct=0,pose=None,DT=1):
#模擬雷達,遇到障礙物返回1
####
return ishit,x
def next_state(self, u,x0=None,DT=None):
#機器人運動模型
if type(x0)==type(None):
x0=self.posexy
if DT==None:
DT=self.Dt
F = np.array([[1.0, 0, 0, 0],
[0, 1.0, 0, 0],
[0, 0, 1.0, 0],
[0, 0, 0, 0]])
B = np.array([[DT * math.cos(x0[2, 0]), 0],
[DT * math.sin(x0[2, 0]), 0],
[0.0, DT],
[1.0, 0.0]])
#print("B:",B.dot(u))
x = F.dot(x0) + B.dot(u)
###
#障礙限制
return x
def do_u(self,v=None,yaw_rate=None):
#控制量，里程計
if v==None:
v = 1.0 # [m/s]
if yaw_rate==None:
yaw_rate = 0 # [rad/s]
u = np.array([[v, yaw_rate]]).T
#print("u",u)
return u
主程序中，提供了一個根據(jù)地圖數(shù)據(jù)用opencv繪制柵格地圖，和機器人位置，方向和雷達線的程序：
def flashmap(img,map_data):
# 根據(jù)map_test數(shù)組繪制柵格地圖
newimg=img.copy()
for y in range(len(map_data)):
for x in range(len(map_data[y])):
if map_data[y][x] == 1:
cv2.rectangle(img, pt1=(x * grid_size[0], y * grid_size[1]), pt2=(x * grid_size[0] + grid_size[0], y * grid_size[1] + grid_size[1]), color=(255,255,255), thickness=-1) # 使用黑色填充矩形表示黑色柵格
cv2.rectangle(img, pt1=(x * grid_size[0], y * grid_size[1]), pt2=(x * grid_size[0] + grid_size[0], y * grid_size[1] + grid_size[1]), color=(100,100,100), thickness=1) # 使用黑色填充矩形表示黑色柵格
elif map_data[y][x] == 0:
cv2.rectangle(img, pt1=(x * grid_size[0], y * grid_size[1]), pt2=(x * grid_size[0] + grid_size[0], y * grid_size[1] + grid_size[1]), color=(0,0,0), thickness=cv2.FILLED) # 使用白色填充矩形表示白色柵格
cv2.rectangle(img, pt1=(x * grid_size[0], y * grid_size[1]), pt2=(x * grid_size[0] + grid_size[0], y * grid_size[1] + grid_size[1]), color=(100,100,100), thickness=1) # 使用白色填充矩形表示白色柵格
for y in range(len(map_data)):
for x in range(len(map_data[y])):
if map_data[y][x] == 9:
#機器人
# 三角形的三個頂點
w=x * grid_size[0] + x * grid_size[0] + grid_size[0]
h=y * grid_size[1]+ y * grid_size[1] + grid_size[1]
cv2.circle(img, (int(w/2),int(h/2)), 30, (0, 0, 255), thickness=5)
# 在圖像上畫直線代表方向
theta=robot.poseuv[2,0]
xx=int(30*math.cos(theta))
yy=int(30*math.sin(theta))
start_point=(int(w/2),int(h/2))
end_point=(int(w/2)+xx,int(h/2)-yy)
cv2.line(img, start_point, end_point, (0, 200, 255), thickness=6)
#畫雷達光線
for i, data in enumerate(robot.sensordata):
xx=int((40+(data-1)*80)*math.cos(theta+i*np.pi/2))
yy=int((40+(data-1)*80)*math.sin(theta+i*np.pi/2))
end_point=(int(w/2)+xx,int(h/2)-yy)
cv2.line(img, start_point, end_point, (255, 200, 0), thickness=1)
cv2.line(img, start_point, end_point, (255, 200, 0), thickness=1)
return newimg
此外，目前實現(xiàn)了一個手動監(jiān)盤控制機器人移動的功能，可以通過監(jiān)盤按鍵“e”,“d”,“s”,"f"進行上下左右操作：
while True:
k=cv2.waitKey(1)#10ms一循環(huán)
if k == ord("q"):
break
if k== ord('e'):
yaw=np.pi/2
print('e')
if k==ord('s'):
yaw=np.pi
print('s')
if k==ord('f'):
yaw=0
print('f')
if k==ord('d'):
yaw=-np.pi/2
print('d')
if nowtime>=DT*100: #1s更新一次
robot.posexy[2,0]=yaw
#print("+")
md=robot.flash_mapdata(robot.do_u(yaw_rate=0))
nowtime=0
cv2.imshow("Grid Map", flashmap(image,md))
nowtime+=1
核心代碼解釋
機器人運動模型：
機器人狀態(tài)向量我們用：
表示，python如下：
# Vector [x y yaw v]
self.posexy=np.array([[1],[-1],[-np.pi/2],[1]])#x坐標,y坐標,yaw為逆時針方向弧度[rad/s]，v為速度uint/s
運動模型簡單采用：
其中，u為控制量，矩陣：
python實現(xiàn)運動模型：
def next_state(self, u,x0=None,DT=None):
#運動模型
if type(x0)==type(None):
x0=self.posexy
if DT==None:
DT=self.Dt
F = np.array([[1.0, 0, 0, 0],
[0, 1.0, 0, 0],
[0, 0, 1.0, 0],
[0, 0, 0, 0]])
B = np.array([[DT * math.cos(x0[2, 0]), 0],
[DT * math.sin(x0[2, 0]), 0],
[0.0, DT],
[1.0, 0.0]])
#print("B:",B.dot(u))
x = F.dot(x0) + B.dot(u)
#print("xnext:",x)
#x邊界限制
if round(x[0, 0])>=len(self.map_data[0])-1:
x[0, 0]=len(self.map_data[0])-2
elif round(x[0, 0])<=0:
x[0, 0]=1
#y邊界限制
if round(x[1, 0])<=-1*(len(self.map_data)-1):
x[1, 0]=-1*(len(self.map_data)-2)
elif round(x[1, 0])>=0:
x[1, 0]=-1
#障礙物限制
ix=round(x[0, 0])
iy=-1*round(x[1, 0])
if self.map_data[iy][ix]==1:
x=x0
print("cannt go")
#障礙限制
return x
如上所示，在完成公式的同時，我們對運動的邊界進行了限制，機器人要根據(jù)地圖的結構來移動，如果碰到墻或者障礙物就不能前進了。
機器人的雷達模擬：
雷達也是根據(jù)以上的運動模型來模擬的，思路是根據(jù)當前的位姿，假設機器人向四個方向移動，撞到障礙物就說明觀測到了物體，然后返回這個距離，即為雷達的四個方向的觀測距離：
def radiar(self, direct=0,pose=None,DT=1):
#模擬雷達,遇到障礙物返回1
ishit=0
if type(pose)==type(None):
pose0=self.posexy.copy()
else:
pose0=pose.copy()
pose0[2,0]=direct
u=self.do_u()
F = np.array([[1.0, 0, 0, 0],
[0, 1.0, 0, 0],
[0, 0, 1.0, 0],
[0, 0, 0, 0]])
B = np.array([[DT * math.cos(pose0[2, 0]), 0],
[DT * math.sin(pose0[2, 0]), 0],
[0.0, DT],
[1.0, 0.0]])
x = F.dot(pose0) + B.dot(u)
#x邊界限制
if round(x[0, 0])>=len(self.map_data[0])-1:
x[0, 0]=len(self.map_data[0])-1
elif round(x[0, 0])<=0:
x[0, 0]=0
#y邊界限制
if round(x[1, 0])<=-1*(len(self.map_data)-1):
x[1, 0]=-1*(len(self.map_data)-1)
elif round(x[1, 0])>=0:
x[1, 0]=0
#障礙物限制
ix=round(x[0, 0])
iy=-1*round(x[1, 0])
if self.map_data[iy][ix]==1:
print("hitted")
ishit=1
#print("xnext:",x)
return ishit,x
機器人的控制：
目前是手動控制的，即通過監(jiān)盤控制機器人的方向，然后默認機器人向前一直走，代碼簡單不再贅述。
總結
通過本文的介紹，我們學習了如何使用Python編寫一個簡單的柵格地圖行走機器人。目前這個機器人是不具備智能的。
那么我們接下來可以對他做點啥呢？
修改一下地圖數(shù)據(jù)，讓它自己走迷宮？
通過雷達數(shù)據(jù)，自主進行定位？
通過這個仿真，研究研究強化學習？
再加入一臺機器人，多個機器人進行協(xié)作？
…
文章來源：http://www.zghlxwxcb.cn/news/detail-831236.html
源碼
本文全部源碼已經(jīng)上傳
點擊獲取源碼地址，
或者關注公眾號獲取。文章來源地址http://www.zghlxwxcb.cn/news/detail-831236.html
到了這里，關于python做了一個極簡的柵格地圖行走機器人，到底能干啥？的文章就介紹完了。如果您還想了解更多內容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關文章，希望大家以后多多支持TOY模板網(wǎng)！
