前言
在现代科技的普及下，人们对于机器人的兴趣与期待日渐增加。然而，大多数人对机器人的印象仍停留在复杂、高度智能的形象上。而今天，我将重点介绍一个极简的栅格地图行走机器人，它不仅使用了简单的编程语言Python，而且只是一个基础的栅格地图行走算法的展示。这个机器人并不具备复杂的感知与决策能力，只能按照预定的规则在栅格地图上行走。然而，正是这种简单的机器人展示了编程的魅力与机器人的可能性。通过学习这个机器人的代码与原理，我们可以更好地理解机器人的宏观工作流程，并激发我们对机器人的创造力与想象力。无论是初学者还是有一定编程经验的人，都能从这个极简的栅格地图行走机器人中获得一些启发与收获。希望大家通过这篇博文来一窥机器人的奥秘，并能在编程与机器人领域中探索更多可能性。
本文使用Python编写的行走机器人，可以通过监盘操作方向，在给定的地图上进行移动，并根据地图上的障碍物确定运动的边界。我们将使用OpenCV和NumPy库来处理地图和机器人的移动，还模拟了一个只有四个方向的测距雷达。
效果如下：
代码思路
在代码的结构中，我们定义了一个机器人类，其中包含了机器人的位置、地图数据和时间间隔等属性。
然后是实现了一个运动模型，和雷达的观测。
并提供了一个地图数据更新函数。
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
#雷达数据
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("雷达数据:",self.sensordata)
return ds
def xy2uv(self,x):
#将xy坐标系统转化为uv坐标系
uv=x.copy()
uv[1,0]=-1*x[1,0]
return uv
def flash_mapdata(self,u,Dt=None):
#刷新地图数据
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
主程序中，提供了一个根据地图数据用opencv绘制栅格地图，和机器人位置，方向和雷达线的程序：
def flashmap(img,map_data):
# 根据map_test数组绘制栅格地图
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
此外，目前实现了一个手动监盘控制机器人移动的功能，可以通过监盘按键“e”,“d”,“s”,"f"进行上下左右操作：
while True:
k=cv2.waitKey(1)#10ms一循环
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
核心代码解释
机器人运动模型：
机器人状态向量我们用：
表示，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实现运动模型：
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
如上所示，在完成公式的同时，我们对运动的边界进行了限制，机器人要根据地图的结构来移动，如果碰到墙或者障碍物就不能前进了。
机器人的雷达模拟：
雷达也是根据以上的运动模型来模拟的，思路是根据当前的位姿，假设机器人向四个方向移动，撞到障碍物就说明观测到了物体，然后返回这个距离，即为雷达的四个方向的观测距离：
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
机器人的控制：
目前是手动控制的，即通过监盘控制机器人的方向，然后默认机器人向前一直走，代码简单不再赘述。
总结
通过本文的介绍，我们学习了如何使用Python编写一个简单的栅格地图行走机器人。目前这个机器人是不具备智能的。
那么我们接下来可以对他做点啥呢？
修改一下地图数据，让它自己走迷宫？
通过雷达数据，自主进行定位？
通过这个仿真，研究研究强化学习？
再加入一台机器人，多个机器人进行协作？
…
文章来源：https://www.toymoban.com/news/detail-831236.html
源码
本文全部源码已经上传
点击获取源码地址，
或者关注公众号获取。文章来源地址https://www.toymoban.com/news/detail-831236.html
到了这里，关于python做了一个极简的栅格地图行走机器人，到底能干啥？的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！
