目录
1、前言
在现代科技的普及下，人们对于机器人的兴趣与期待日渐增加。然而，大多数人对机器人的印象仍停留在复杂、高度智能的形象上。而今天，我将重点介绍一个极简的栅格地图行走机器人，它不仅使用了简单的编程语言Python，而且只是一个基础的栅格地图行走算法的展示。这个机器人并不具备复杂的感知与决策能力，只能按照预定的规则在栅格地图上行走。然而，正是这种简单的机器人展示了编程的魅力与机器人的可能性。通过学习这个机器人的代码与原理，我们可以更好地理解机器人的宏观工作流程，并激发我们对机器人的创造力与想象力。无论是初学者还是有一定编程经验的人，都能从这个极简的栅格地图行走机器人中获得一些启发与收获。希望大家通过这篇博文来一窥机器人的奥秘，并能在编程与机器人领域中探索更多可能性。
2、增加的功能
本次增加了基于蒙特卡洛地标定位功能，通过机器人与地标点的距离测量，利用蒙特卡洛粒子定位，实现定位，如上图的绿色部分。此外还更新了一些小功能：
- 蒙特卡洛定位功能
- 编辑地图后保存地图
- 将保存的地图导入
- 更改了手动控制器
打开地图：
载入地图并定位：
3、主要算法python实现
本次蒙特卡洛算法主要流程如下：
通过在地图上随机撒粒子，以表示机器人的可能状态。每个粒子代表一个可能的机器人状态。通过若干运动、测量、更新后，粒子会聚在单个位置周围。主要过程如下：
如上
- 初始化粒子集：根据机器人所在环境的先验知识，随机生成一组粒子，每个粒子代表一个可能的机器人位置。
- 运动模型更新：根据机器人的运动模型，对每个粒子进行运动更新，以模拟机器人在运动后的位置。
- 传感器测量更新：根据机器人的传感器测量模型进行测量
- 计算权重：根据测量结果，计算每个粒子的权重，表示该粒子与测量结果的一致性。
- 重采样：根据粒子的权重，对粒子集进行重采样，使得具有较高权重的粒子被保留，而具有较低权重的粒子被替换掉。
重复步骤2至步骤4，直到达到预定的迭代次数或满足停止条件。
通过不断迭代运动模型更新和传感器测量更新，MCL算法可以逐渐减小粒子集的方差，从而得到机器人的最可能位置和方向。
首先本篇所使用的测量传感器为定位标签传感器，即机器人通过对固定位置的标签的距离测量，获取机器人当前相对于固定标签的各距离。运动模型还是采用速度模型。并在运动和观测模型中加入不确定噪声。这个定位算法其实可以用于uwb、wifi、蓝牙的定位。以下是最核心部分：
3.1定义一个地图和固定标签
地图为100m*100m的方形，在地图中设置8各定位标签（其实3个就够了）。
# Landmarks定义4个标签标签，可在地图范围内随便设置
self.landmarks=np.array([[1, -1],
[len(self.map_data[0])-1, -1],
[20, -20],
[1, -(len(self.map_data)-1)],
[len(self.map_data[0])-1, -(len(self.map_data)-1)]
])
#定义栅格地图跟原来一样随机生成
if randommap:
#创建随机地图
self.map_data=self.generate_randommap(mapdatasize)
3.2定义一个粒子
定义一个粒子机器人，里面有用于栅格地图的位姿信息，和设置获取pose方法
class particle:
def __init__(self,x,y,iid,do=None,turn_noise=0.05):
# Constructor
self.x = x # robot's x coordinate
self.y = y # robot's y coordinate
if do==None:
self.orient = random.Random().uniform(0.0, 1.0) * 2.0 * math.pi # robot's orientation
else:
self.orient = self.gassion(do,turn_noise)[0]
self.id=iid
def setpose(self,pose):
if isinstance(pose,type(np.array([]))):
if pose.shape==(4,1):
self.x = pose[0,0] # robot's x coordinate
self.y = pose[1,0] # robot's y coordinate
self.orient =pose[2,0]
return True
else:
raise ValueError("setpose error,pose shape error")
return False
def getpose(self):
return np.array([[self.x],[self.y],[self.orient],[1]])
def gassion(self,mean,std_dev,num_samples=1):
"""
生成高斯分布，
mean:设置峰值
std_dev:设置标准差
num_samples:设置生成的样本数量
return:list
"""
samples = np.random.normal(mean, std_dev, num_samples)
return samples.tolist()
3.3定义一个粒子管理类
在mclrobot中用于管理粒子，用于完成创建，复制等，地图的读取和障碍信息提取等操作。
class mclrobot:
def __init__(self,mapdata=None,r=2,N_SAMPLE=1000):
# Constructor
self.x = gen_real_random() * world_size # robot's x coordinate
self.y = gen_real_random() * world_size # robot's y coordinate
self.orient = gen_real_random() * 2.0 * math.pi # robot's orientation
self.forward_noise = 0.0 # noise of the forward movement
self.turn_noise = 0.0 # noise of the turn
self.sense_noise = 0.0 # noise of the sensing
self.N_SAMPLE=N_SAMPLE#粒子数
self.obstacle_x_list, self.obstacle_y_list=self.get_obstacles(mapdata)
self.obstacle_kd_tree = KDTree(np.vstack((self.obstacle_x_list, self.obstacle_y_list)).T)
#机器人安全距离单位grid
self.r=2
def ncopy(self,p):#粒子拷贝方法
pose=p.getpose().copy()
iid= random.random() * 1000
p0=particle(pose[0,0],pose[1,0],iid)
p0.setpose(pose.copy())
return p0
def create_particles(self, do=0,rng=None):
"""
根据地图障碍物生产合法粒子
rr:安全半径
"""
max_x = max(self.obstacle_x_list)
max_y = max(self.obstacle_y_list)
min_x = min(self.obstacle_x_list)
min_y = min(self.obstacle_y_list)
particles=[]
#sample_x, sample_y = [], []
if rng is None:
rng = np.random.default_rng()
iid=0
while len(particles) <= self.N_SAMPLE:
tx = (rng.random() * (max_x - min_x)) + min_x
ty = (rng.random() * (max_y - min_y)) + min_y
dist, index = self.obstacle_kd_tree.query([tx, ty])
#判断采样位置有效性
if dist > self.r+0.01:
particles.append(particle(tx,ty,iid,do=do,turn_noise=self.turn_noise))
iid+=1
return particles
3.4定义粒子运动模型
根据栅格机器人的速度运动模型改造，增加了随机性。
def pnext_state(self, u,x0=None,DT=None):
#运动模型
if type(x0)==type(None):
x0=self.posexy.copy()
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]])
#加噪声
u[0,0]=u[0,0]+self.gen_gauss_random(0.0, self.myrobot.forward_noise)
u[1,0]=u[1,0]+self.gen_gauss_random(0.0, self.myrobot.turn_noise)
#print("B:",B.dot(u))
x = F.dot(x0) + B.dot(u)
x[0, 0]=self.mod(x[0, 0], len(self.map_data[0])-1)
x[1, 0]=self.mod(x[1, 0], -(len(self.map_data)-1))
#障碍物限制
ix=math.ceil(x[0, 0])
iy=-1*math.ceil(x[1, 0])
if self.map_data[iy][ix]==1:
x=x+B.dot(u)
x[0, 0]=self.mod(x[0, 0], len(self.map_data[0])-1)
x[1, 0]=self.mod(x[1, 0], -(len(self.map_data)-1))
#print("cannt go")
return x.copy()
3.5定义观测模型
观测这里比较简单，直接计算当前点到各标签的距离，并加入随机噪声
def measurement(self,p=None):
if p==None:
pose=self.posexy
x=pose[0,0]
y=pose[1,0]
else:
x=p.x
y=p.y
z=[]
for i in range(len(self.landmarks)):
dist = math.sqrt((x - self.landmarks[i, 0])**2 + (y - self.landmarks[i, 1])**2)
dist += self.gen_gauss_random(0.0, self.myrobot.sense_noise)
z.append(dist)
return z
3.6定义权重计算
计算用于更新粒子的权重，这个函数的原理如下：
- 首先，我们初始化一个概率 prob 为1.0，一个距离 dist 为0.0。
- 然后，我们遍历所有的地标（landmarks）。对于每个地标，我们计算机器人到这个地标的距禮 dist。
- 接下来，我们计算一个高斯概率，表示机器人的测量值 measurement 在这个地标的距禮 dist 上的概率。这个高斯概率的均值是 dist，方差是 self.sense_noise。
- 最后，我们将这个高斯概率乘以 prob，并将结果赋给 prob。这相当于将机器人的测量值在所有地标上的概率相乘，得到机器人的位置的概率。
- 最后，我们返回 prob。
这个函数的目的是计算机器人的测量值在所有地标上的概率，这个概率可以用来更新粒子的权重。
def landmark_prob(self,p,measurement):
# Calculates how likely a measurement should be
prob = 1.0
dist = 0.0
for i in range(len(self.landmarks)):
dist = math.sqrt((p.x - self.landmarks[i, 0])**2 + (p.y - self.landmarks[i, 1])**2)
prob *= self.gaussian(dist, self.myrobot.sense_noise, measurement[i])
return prob
3.6更新粒子重采样
在蒙特卡洛定位中，每个粒子都有一个权重，表示这个粒子的位置有多大可能性。根据多个时间步序的计算，每个个步序都需要根据权重对粒子的存活进行筛选，原则上权重大的被存活下来，权重小的进行重新采样。以下是一种实现方式：
def localizing(self,u):
"""
粒子更新，计算位置
u:控制量
"""
# Move the robot and sense the environment afterwards
w=[]
if self.local_mod==1:#landmark模式
z0 = self.measurement()#地标测量
elif self.local_mod==2: #雷达模式
z0 = self.sensorby()#雷达测量
self.sensor_data0=z0
print("z0:",z0)
zz=[]
n=len(self.p)
# Simulate a robot motion for each of these particles
for i in range(n):
x0=self.p[i].getpose()
nextpose=self.pnext_state(u,x0=x0)
self.p[i].setpose(nextpose)
self.p[i]=self.myrobot.ncopy(self.p[i])
# Generate particle weights depending on robot's measurement
if self.local_mod==2:
z1=self.sensorby(pose=nextpose)
self.sensor_data1=z1
zz.append(z1)
w.append(self.measurement_prob(z0,z1))#雷达测量
elif self.local_mod==1:
w.append(self.landmark_prob(self.p[i],z0))#地标测量
self.rep = self.myrobot.create_particles()
beta = 0.0
mw=float(np.array(w).max())
print("maxmw:",mw)
if self.local_mod==1:
index = int(self.gen_real_random() * n)
for i in range(n):
beta += self.gen_real_random() * 2.0 * mw
while (beta > w[index]):
beta -= w[index]
index = self.mod((index + 1), n)
self.rep[i] = self.p[index]
elif self.local_mod==2:
pass
self.p=self.rep
较详细的原理可以看博文《蒙特卡罗定位（Particle Filter Localization）2D定位算法的python实现》
4 总结
通过本文的介绍，我们对Python编写一个简单的栅格地图行走机器人进行了功能升级。是不是感觉越来越好玩了，目前实现了从“智障”驾驶，到了一点“智能”驾驶。
那么我们接下来还可以对他做点啥呢？
- 通过雷达数据，自主进行定位？
- 通过这个仿真，研究研究强化学习？
- 再加入一台机器人，多个机器人进行协作？
- 接入大模型？
5、python源码
所有源码已上传:下载链接
关注公众号获取更多资料。