python做了一个极简的栅格地图行走机器人，到底能干啥？

于 2024-01-31 11:30:00 发布
阅读量919 收藏 29
点赞数 11
分类专栏： python实战 文章标签： python 机器人 开发语言
未经同意，不得转载
本文链接：https://blog.csdn.net/kanbide/article/details/135937927
版权

目录

前言

在现代科技的普及下，人们对于机器人的兴趣与期待日渐增加。然而，大多数人对机器人的印象仍停留在复杂、高度智能的形象上。而今天，我将重点介绍一个极简的栅格地图行走机器人，它不仅使用了简单的编程语言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编写一个简单的栅格地图行走机器人。目前这个机器人是不具备智能的。
那么我们接下来可以对他做点啥呢？
修改一下地图数据，让它自己走迷宫？
通过雷达数据，自主进行定位？
通过这个仿真，研究研究强化学习？
再加入一台机器人，多个机器人进行协作？
…
在这里插入图片描述

源码

本文全部源码已经上传
点击获取源码地址，
或者关注公众号获取。

JAMES费
关注
专栏目录
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

JAMES费

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值