python做了一个极简的栅格地图行走机器人，第五弹-解锁蒙特卡洛定位功能，python源码

# -*- coding: utf-8 -*- """ Created on Wed Dec 20 08:00:19 2023 @author: JAMES FEI <https://i.csdn.net/#/user-center/profile?spm=1001.2014.3001.5111> Copyright (C) FEI PANFENG, All rights reserved. THIS SOFTEWARE, INCLUDING DOCUMENTATION,IS PROTECTED BY COPYRIGHT CONTROLLED BY FEI PANFENG ALL RIGHTS ARE RESERVED. this program is for store robot status,and some realtime data for robot and also can save all the datacenter calss as a dist file for next usage. """ import os import cv2 import numpy as np import math from collections import deque import PySimpleGUI as sg import keyboard from PIL import Image, ImageTk import csv import matplotlib.pyplot as plt from scipy.interpolate import interp1d import random from scipy.spatial.distance import cosine, euclidean, cityblock #依赖文件 from tkcanvas import CanvasPlane from robotplanner import prm_planner from robotlocalization import mclrobot class agnet: def __init__(self,location=(3,3),randommap=False,R=0.1,mapdatasize=(10,10),pgrid=0.1,map_data=None,DT=1,viewmapsize=(1000,800)): self.goalxy=[None,None]#目标 self.occ=40 #障碍物的数量 self.radarmaxd=10 self.map_editer_enable=False self.pgrid=pgrid#栅格分辨率m self.R=R #机器人半径单位m self.Rg=R/pgrid #机器人半径单位grid,网格 self.path=[None,None] self.pp=[None,None] self.isdead=0 # 前一时刻位置 self.pre_posexy=np.array([[3],[-3],[-np.pi/2],[1]]) # 当前位置 self.posexy=np.array([[3],[-3],[-np.pi/2],[1]])#x宽单位grid(pgrid),y长,yaw为逆时针方向弧度[rad/s] self.poseuv=np.array([[3],[3],[-np.pi/2],[1]]) if type(map_data)==type(None): if randommap: #创建随机地图 self.map_data=self.generate_randommap(mapdatasize) #self.map_data[u][v]=9 else: self.map_data= [ [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,1,1,1], [1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,0,0,1], [1, 0, 9, 0, 0, 0, 1, 0, 0, 0, 0,0,0,1], [1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,0,0,1], [1, 0, 0, 0, 0, 0, 1, 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, 0, 0, 0, 0, 0, 0, 0,0,0,1], [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0,0,1], [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0,0,1], [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0,0,1], [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,1,1,1], ] else: self.map_data=map_data #安全距离高，单位grid self.HHalarm=0.25/self.pgrid #安全距离高高高，单位grid self.Halarm=0.3/self.pgrid self.stop_num=0 self.prev_pose=np.array([[0],[0],[0],[1]])#判断死区用 print("创建背景图") self.grid_size=None self.map_view=None self.create_map_view_backgroud(viewmapsize=viewmapsize) self.lifevalue=100 #雷达数据[角度，距离] self.sensordata=[ [0,0], [math.pi/16,0], [math.pi/8,0], [math.pi*3/16,0], [math.pi/4,0], [math.pi*5/16,0], [math.pi*3/8,0], [math.pi*7/16,0], [math.pi/2,0], [-math.pi/16,0], [-math.pi/8,0], [-math.pi*3/16,0], [-math.pi/4,0], [-math.pi*5/16,0], [-math.pi*3/8,0], [-math.pi*7/16,0], [-math.pi/2,0] ] self.u=np.array([[0, 0]]).T#当前控制量 self.u_max=np.array([[1, math.pi/8]]).T#最大控制量 self.Dt=DT#时间步长 #增加路径规划器 self.pathplanner=prm_planner(self.map_data,self.Rg) #路径任务点集 self.pathtask=deque([]) self.taskachieved=deque([]) #任务允许开关 self.taskenable=False #添加定位器 self.myrobot=mclrobot(self.map_data,r=self.Rg,N_SAMPLE=1000) # Create a set of particles self.p=self.myrobot.create_particles() self.myrobot.set_noise(0.5, 0.5, 1) self.rep=self.p#重采样 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)] ]) #定位模式 self.local_mod=1 self.sensor_data0=[] self.sensor_data1=[] def gaussian(self, mu, sigma, x): # Probability of x for 1-dim Gaussian with mean mu and var. sigma return math.exp(-((mu - x)**2) / (sigma**2) / 2.0) / math.sqrt(2.0 * math.pi * (sigma**2)) def measurement_prob(self,z0,z1): """ 测量权重 """ data1 = np.array(z0) data2 = np.array(z1) prob = 1 / (1 + euclidean(data1, data2)) """ prob = 1.0 for i in range(len(z0)): dist = z0[i] dmeasure = z1[i] prob *= self.gaussian(dist, self.myrobot.sense_noise, dmeasure) """ return prob 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 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 def localizing(self,u): """ 粒子更新，计算位置 u:控制量 """ # Move the robot and sense the environment afterwards w=[] if self.local_mod==1: 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))#地标测量 #print("p[10]:",self.p[i].getpose()) #print("z:",(np.array(z0)-np.array(z1))[:4])