我用python做了一个蒙特卡洛定位MCL的可视化系统

摘要：
1 项目背景和意义
2 需求分析
3 系统总框架设计
4 系统详细设计和代码实现
4.4 基于tkinter的可视化界面代码ui_view.py的实现
5 程序展示和测试
7 结论和未来方向
8 致谢
9 参考文献

摘要：

在本报告中介绍了一套基于Python的蒙特卡洛定位仿真可视化软件系统的需求分析、设计、实现和测试，基于tkinter图形化编程实现了一个完善的蒙特卡洛定位仿真演示系统，能很直观地显示定位的过程、粒子的权重变化、Loss的收敛情况等。还支持用户交互，用户可以自行修改蒙特卡洛定位的各种超参数去进行定位的实验操作。本系统能用于教学，给入门蒙特卡洛定位的同学更丰富得展示定位的过程，帮助理解。
关键字：蒙特卡洛定位；可视化系统；tkinter编程。
代码行数：1100行

1 项目背景和意义

蒙特卡洛定位通常又叫粒子滤波定位，是一种非常重要的定位算法，在著名的《Probabilistic robotics》[1]这本书专门展开了一章进行阐述。但对于初学者来说，书中的蒙特卡洛定位算法数学理论推导过于复杂抽象，主要是以公式推导和部分图片来展示定位过程，而且目前网络上没有找到比较完善的蒙特卡洛定位仿真演示系统。
为了解决这个痛点，本项目开发了一套基于Python的蒙特卡洛定位仿真可视化软件系统，不但支持可视化定位的直观过程、粒子的权重变化、Loss的收敛情况，系统还可以进行一定的交互，给用户提供了自己修改粒子数量、障碍物位置、运动速度等多种可调参数，相比仅仅直接看书较为理论的学习方式，结合本系统进行学习，能做到理论联系实践，丰富的可视化和交互功能能让入门学习蒙特卡洛定位算法的学生更易于理解，并且理解得更为深刻。

2 需求分析

用户在使用基于Python的蒙特卡洛定位仿真可视化软件系统时，需要的基本需求可以总结为以下几点：
1、要有良好的交互界面，能够对蒙特卡洛定位的实现过程进行界面可视化
2、要有用户操作的空间，允许用户对蒙特卡洛定位时的各种超参数自行调整。
3、蒙特卡洛定位可视化系统能很好地展示定位过程、能观测到粒子的位置变化、能观察到估计的轨迹、能观察到粒子的权重变化情况。
4、能显示定位的好坏，好坏可以用坐标的(x,y)收敛情况表示
5、最好有log展示，内容更丰富。

3 系统总框架设计

基于Python的蒙特卡洛定位仿真可视化软件系统，总体上可以分为两大部分进行实现，第一部分是蒙特卡洛定位仿真的原理实现，第二部分是定位的可视化界面实现，具体的系统框架图如图3-1所示。
对于第一部分，蒙特卡洛定位仿真的原理实现主要包括粒子初始化、运动模型、观测模型、粒子重采样、测量模型和画图函数，这一部分具体的原理第4节会详细展开描述。
对于第二部分，定位的可视化要写成界面交互的形式，本项目使用的是Python自带的tkinter库实现，主要把用户界面以画布的形式分为三大模块：左边画布界面、中间画布界面和右边画布界面。左边画布界面用于放置Log滚动条；中间画布界面放置参数设置，放置了各种可调参数和按钮；右边画布界面放置蒙特卡洛定位可视化的Matplotlib图形，包括实时定位图、粒子权重变化图、坐标x的Loss变化图和坐标y的Loss变化图。除了这三大模块外，用户界面左上角增加了一个“关于”菜单，用于查看软件信息和作者帮助链接。

图3-1 系统总框架设计图

4 系统详细设计和代码实现

4.1 架构设计

总结了系统的需求分析和总框架设计思想后，架构设计如下：项目总目录为mcl_gui，包含mcl、utils和view一共3个子目录。其中mcl子目录包含mcl.py代码文件，主要包含MCL()类，负责实现蒙特卡洛定位仿真的基本原理；utils子目录可以放置常用的工具函数，这里只有获取实时系统时间的get_data_time.py代码文件；view子目录包含ui_view.py代码文件，包含UIview()类，负责系统的整个界面极其可视化实现。系统的架构设计如图4-1所示。

图4-1 系统总框架设计图

4.2 获取实时时间和日期字符串代码get_data_time.py的实现

在用户界面的Log滚动条中，为了区分不同时间段的Log，需要实时获取系统日期和时间，可通过datetime库获取。这个不是本项目的重点，不展开阐述。

4.3 蒙特卡洛定位代码mcl.py的实现

在mcl.py中，主要函数结构图如图4-2所示，在mcl.py定义了MCL(object)这个类，在这个被用户界面的UIview的类实例化时，def init(self, ui_view)方法除了定义了一些属性外，还会把UIview的类实例化，从而实现蒙特卡洛定位于用户界面的数据传输。MCL(object)的类主要的方法还有用于初始化各种参数的def initialization_parameter(self)方法，运动模型方法def motion_model(self)，重采样方法def re_sampling(self)，观测模型方法def observation(self)，测量模型方法def measurement_model(self)，与UIview中开始MCL定位按钮绑定的方法接口def start_mcl(self)。还有其它很多相对不太重要的方法和画图函数，因为篇幅有限这里不一一展开。

图4-2 mcl.py的主要函数结构图

4.3.1 蒙特卡洛定位仿真模型假设

蒙特卡洛定位又叫粒子滤波定位，常用于机器人的定位问题。蒙特卡洛定位的基本思想是把定位问题转化为粒子概率问题，通过不断获取环境信息，然后利用这些有用的信息进行判断更新，从而实现定位。
刚开始机器人因为没有任何先验信息，我们可以认为机器人可能出现在地图上的任何位置。这时候我们在地图上均匀得撒下M个粒子并记录位姿，用于模拟机器人，每一个粒子都看做是一个机器人。后续会不断根据传感器的信息抛弃一些认为不太可能的粒子，也就是出现“权重”很小的粒子，那么最终问题就可以转变成哪一个位置的“粒子机器人”更多，我们就认为实际的机器人在这个位置上。
假设一个机器人在地图上运动，机器人其坐标为(x,y)，运动速度恒定为v m/s ，偏航角为yaw rad，计算状态时间间隔为dt，偏航角速度为yaw_rate rad/s。这里假设机器人身上的传感器为激光雷达，激光雷达可以观测到障碍物。并定义t-1时刻的机器人状态X为坐标(x,y)，角度yaw和运动速度v组为列向量Xt-1：

4.3.2 粒子初始化和障碍物初始化

刚开始在地图上，机器人的位置我们是完全不知道的，这时候可以认为机器人等概率地可能在地图上任意位置。粒子初始化可以根据粒子的数量M，使用Python的random库生成均匀分布的M个粒子的x、y坐标，yaw是根据用户设置的偏航率使用运动模型进行计算得出，速度v也是用户自己设置，本项目的速度v设置后为匀速。除了粒子的状态外，还需要初始化对应M个粒子各自的粒子权重，每一个粒子的权重初始化为1/M，如图4-3为M=1000个粒子初始化，颜色对应右边的轴代表着粒子的权重，可以看到粒子的权重是相等的。

图4-3 粒子初始化例子图
障碍物初始化为Nx2大小的numpy数组，N代表障碍物个数，每一个障碍物用户都可以自己设置具体的坐标，如图4-4为障碍物初始化粒子。

图4-4 障碍物初始化例子图

初始化关键代码如下：

# 粒子滤波参数
self.M = 100  # Number of Particle 粒子数目
self.X_particles = np.zeros((4, self.M))  # Particle store所有的粒子，一共M个，都有各自的状态
self.X_particles[0, :] = np.random.uniform(low=-10, high=10, size=self.M)  # 替换整一行的x值
self.X_particles[1, :] = np.random.uniform(low=0, high=20, size=self.M)  # 替换正一行的y值
self.particles_weight = np.zeros((1, self.M)) + 1.0 / self.M  # Particle weight粒子权重  刚开始为均匀概率为1/M
# 障碍物位置，激光雷达可以观测到(x,y)
self.barrier_position = np.array([[-10, 0],[-10, 10],[-8, 15],[-5, 20],[0, 15],[0, 5],[0, 0],[5, 15],[3, 18],[10, 0],[10, 5],])

4.3.3 运动模型

在实现运动模型代码时，首先介绍机器人运动的基本模型。假设机器人在t-1时刻的坐标为(x,y)，那么机器人在时间间隔dt的运动情况可以简化如图4-5所示。

图4-5 机器人运动分析图
根据上图我们很容易可以得到t时刻机器人的状态Xt：

一般为了方便改动速度和航向角速度去改变轨迹，在蒙特卡定位中运动模型更多地会习惯封装成def self.motion_model(Xt-1, U)，然后return Xt的形式，其中定义速度v和航向角速度yaw_rate的列向量U为：

定义当前时刻的状态X为坐标(x,y)，航向角yaw和运动速度v组为列向量：

定义观察矩阵F为：

定义机器人运动轨迹矩阵B为：

则t时刻机器人的状态Xt可以推导为

如图4-6为蒙特卡洛定位的运动模型计算的轨迹例子。

图4-6 运动模型计算机器人轨迹例子
运动模型关键代码如下，和上述公式推导一样：

def motion_model(self, x, 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([[self.dt * math.cos(x[2, 0]), 0],
                  [self.dt * math.sin(x[2, 0]), 0],
                  [0.0, self.dt],
                  [1.0, 0.0]])
    x = F.dot(x) + B.dot(u)
return x
# 另一个写法
X_ground_truth[0] = X_ground_truth[0] + math.cos(X_ground_truth[2]) * self.dt * self.v
X_ground_truth[1] = X_ground_truth[1] + math.sin(X_ground_truth[2]) * self.dt * self.v
X_ground_truth[2] = X_ground_truth[2] + self.yaw_rate * self.dt
X_ground_truth[3] = self.v

4.3.4 观测模型

观测模型用于计算轨迹坐标与地图信息障碍物的观测z，这里的坐标可以是ground truth轨迹坐标，也可以是每一个粒子的坐标。以ground truth轨迹坐标为例，假设障碍物坐标为(x,y)，观测信息我们可以定义为矩阵Z：

其中d为ground truth轨迹坐标与地图信息障碍物的欧氏距离，观测信息Z以矩阵的形式存储了每一个轨迹坐标(ground_truth_x,ground_truth_y)到每一个障碍物坐标(x,y)的距离d，图4-7为坐标与障碍物之间的连线欧式距离的例子：

图4-7 运动模型计算机器人轨迹例子
观测模型关键代码如下：

    for i in range(len(barrier_position[:, 0])):
        # 求真实标签坐标点———到———障碍物坐标点——的欧氏距离，也就是真实的点距离障碍物有多远的问题
        distance = math.sqrt(pow(X_ground_truth[0, 0] - barrier_position[i, 0], 2) +pow(X_ground_truth[1, 0] - barrier_position[i, 1], 2))
        zi = np.array([[distance, barrier_position[i, 0], barrier_position[i, 1]]])  # [真实的点距离障碍物距离，真实的点x坐标，真实的点y坐标]
        z = np.vstack((z, zi))  # 按垂直方向（行顺序）堆叠数组构成一个新的数组，堆叠的数组需要具有相同的维度
    return X_ground_truth, z

4.3.5 测量模型

粒子的测量模型主要是计算和更新粒子的权重，假设一共有M个粒子，n个障碍物，第m个粒子第t-1时刻的权重为wmt-1，该粒子的观测信息为Zm，真实标签轨迹坐标的观测信息为Zgt。
因为观测模型使用的是粒子坐标到障碍物的欧式距离度量，因此越靠近真实机器人位置的粒子，距离应该要越小，那么映射粒子权重应该要设计一个粒子权重与距离成反比的函数。映射粒子权重的方法或函数有很多，常见的可以用log函数、高斯分布函数等，在用户界面用户可以自由选择，图4-8为log函数图像，图4-9为高斯分布函数图像。

图4-8 log函数图像例子

图4-9 高斯分布函数图像例子
以log函数为例，则t时刻第m个粒子的权重更新为：

测量模型关键代码如下：

dz = distance - z[i, 0]  # z[i, 0]是真实标签到各个障碍物的距离。dz反应了粒子和标签的偏离程度
    if self.w_mapping_function == "负指数函数":
        p = p + self.exp_likelihood(x=dz)
    elif self.w_mapping_function == "高斯分布函数":
        p = p + self.gauss_likelihood(x=dz, sigma=1 / np.sqrt(2 * np.pi))
w = w * p

4.3.6 粒子重采样

粒子的重采样是“适者生存”的过程，权重概率大的粒子应该多点保留，权重概率小的粒子应该舍弃，类似于“轮盘大抽奖”，这里使用np.random.choice根据概率进行重采样。如图4-10所示，左边是重采样之前的，右边的重采样之后的，可视化可以很清晰地看出，地图中上面权重比较小的粒子（可以根据概率颜色柱子看出来）在采样后变少了，只剩零零星星的几个粒子。粒子向底部权重较大的粒子聚集，最终走向收敛。

图4-10 粒子重采样例子

粒子重采样的关键代码如下：

def re_sampling(self, X_particles, particles_weight):
    # 索引 0-粒子最后一个索引
    indexes = np.arange(len(X_particles[0, :]))
    # 根据概率进行重采样
    indexes_re_sampling=np.random.choice(a=indexes, size=self.re_sampling_M, replace=True,p=particles_weight[0, :])
    indexes_re_sampling.sort()
    # 重采样后根据重采样的索引产生新的粒子
    X_particles_re_sampling = X_particles[:, indexes_re_sampling]
    particles_weight_re_sampling = particles_weight[:, indexes_re_sampling]
    return X_particles_re_sampling, particles_weight_re_sampling

4.3.7 蒙特卡洛定位及其实时画图
蒙特卡洛定位和实时画图写在了mcl.py的def start_mcl(self)函数中，因为代码比较长，这里给出对应简洁版的伪代码思路。

def start_mcl(self)基本流程
While 累计仿真次数 < 仿真次数：
If(界面勾选了随机轨迹按钮)：
运动模型随机产生轨迹
  
# 画MCL定位过程图 画图对象对应self.ui_view_obj.mcl_plt_obj_subplot
画障碍物位置的点
画M个粒子点

通过观测模型得到当前时刻标签值X_ground_truth, 观测信息z
通过测量模型得到当前时刻的定位坐标估计值x_est, 粒子状态self.X_particles和粒子权重self.particles_weight

画ground_truth坐标点
画ground_truth坐标点与障碍物坐标点之间的连线
画定位出来的坐标估计值点

当前时刻估计值x_est存储到历史估计值中
当前时刻ground_truth坐标点存储到历史ground_truth坐标点中

画图例、网格
给画布加线程锁，保证同一时刻只有一个线程可以修改数据
刷新画布
解锁画布线程锁

# 画粒子的权重图——概率柱状图
使用self.ui_view_obj.particles_weight_plt_obj_subplot.bar(range(self.M), self.particles_weight[0])之间画
线程锁操作同上

# 画坐标x的损失loss x的图
这里的损失使用直接相减加绝对值的形式
loss_x = abs(history_X_estimation[0, :] - history_X_ground_truth[0, :])

# 画坐y的损失loss y的图
原理同上

延迟0.05s，让tk和matplotlib之间数据传输保证不出错

If（点击了用户界面的中断定位按钮）：
    提前结束定位，直接return

4.4 基于tkinter的可视化界面代码ui_view.py的实现

在ui_view.py中，主要函数结构图如图4-11所示，在ui_view.py定义了UIview(object)这个类。类中主要包含调节窗口大小、标题的系统主窗口参数方法def mcl_ui_config(self)；系统菜单栏的函数方法def menu_pack(self)，包含“关于”菜单栏，可以查看软件的开发信息；放置顶端画布的欢迎栏方法def top_part_view(self)；放置左边画布的函数方法def left_part_view(self)，用于实时显示Log信息；放置中间画布的函数方法def center_part_view(self)，显示用户参数设置和功能按钮；放置右边画布的函数方法def right_part_view(self)，显示实时定位图、粒子权重变化图、坐标x的Loss变化图和坐标y的Loss变化图，主要显示；更新滚动栏记录的函数def insert_log(self)；与用户界面的“开始MCL定位”按钮绑定的方法def start_mcl(self)；与用户界面的“手动终止定位”按钮绑定的方法def stop_mcl(self)；与用户界面的“点击修改参数”按钮绑定的方法def save_parameters(self)等。类UIview(object)还有别的函数，但不是这么重点的就不详细展开阐述。

图4-11 ui_view.py的主要函数结构图

4.4.1 “关于”菜单栏def menu_pack(self)实现

图4-12菜单栏界面

图4-13菜单栏—软件信息界面
菜单栏的实现首先是调用了tk.Menu()方法，同时tk.Menu()继承的是self.mcl_ui，也就是总的界面对象。在菜单栏Menu创建后，使用add_cascade()
方法添加菜单栏的“关于”选项，在“关于”选项下再使用add_command()方法添加“软件信息”和“帮助”两个子菜单栏，这两个菜单栏分别绑定了self.show_info()和self.open_homepage()方法。
关于”菜单栏def menu_pack(self)实现的关键代码如下：

def menu_pack(self):
    self.context_menu = tk.Menu(self.mcl_ui)
    self.menubar = tk.Menu(self.mcl_ui)  # 上方菜单栏
    self.author_info_menu = tk.Menu(self.menubar, tearoff=False)
    self.menubar.add_cascade(label="关于(G)", menu=self.author_info_menu, underline=3)
    self.author_info_menu.add_command(label="软件信息", command=self.show_info)
    self.author_info_menu.add_command(label="帮助——请访问叶昌鑫的官方博客——代码已开源", command=self.open_homepage)
    # 创建菜单栏完成后，配置让菜单栏menubar显示出来
    self.mcl_ui.config(menu=self.menubar)

4.4.2 左边画布的函数方法def left_part_view(self)实现

左边画布放置的是实时log画布，效果如图4-14所示。实现的思路是首先使用tk.Frame()方法创建左画布对象self.frame_left，并且继承了顶端的欢迎画布self.frame_top对象。
滚动栏的标签使用 tk.Label()方法实现，继承self.frame_left画布对象。滚动栏使用 tk.Text实现，同样继承self.frame_left画布对象，并且让滚动栏log绑定了右键鼠标事件，可以对log进行复制、剪切、全选等操作。最后，为了滚动栏的内容方便上下拉动，使用tk.Scrollbar方法创建了滚动条。

左边画布的函数方法def left_part_view(self)实现的关键代码如下：

def left_part_view(self):
    self.frame_left = tk.Frame(master=self.frame_top)
    # 滚动栏标签
    self.label_bulletin = tk.Label(master=self.frame_left, text='log',
                                   font=('微软雅黑', 20), bg='lightskyblue', fg='black')
    # 左边栏设计-滚动log
    self.frame_log_scroll = tk.Frame(master=self.frame_left)  # , bg='green')
    self.txt_log_history = tk.Text(self.frame_log_scroll, font=('微软雅黑', 11), state=tk.DISABLED,
    self.txt_log_history.bind("<Button-3>", lambda event: self.rightKey(event, self.txt_log_history))  # 绑定右键鼠标事件
    # 创建滚动条
    self.txt_scroll = tk.Scrollbar(self.frame_log_scroll, orient="vertical", command=self.txt_log_history.yview)

图4-14实时log滚动栏图

4.4.3 中间画布的函数方法def center_part_view(self)实现

中间画布的函数方法def center_part_view(self)实现的总体结构如图4-15所示。中间部分包括了“参数设置”标签、粒子参数设置画布、障碍物参数画布、运动模型参数设置画布、其它超参数设置画布和功能按钮。其中“参数设置”标签使用tk.Label()方法实现；粒子参数设置画布包括粒子数量子画布，通过tk.StringVar()方法实现，可以实现用户自行输入内容；障碍物参数设置画布包括了温馨提示、障碍物位置x、障碍物位置y；运动模型参数设置画布包括了运动速度v和偏航率yaw_rate；其它超参数设置画布包括了仿真单位时间dt、仿真次数、是否开启随机轨迹、选择权重映射函数、选择权重归一化函数和粒子估计值取法，其中“是否开启随机轨迹”使用tk.Checkbutton()方法实现，用于勾选操作，“选择权重映射函数”使用Combobox()下拉框方法实现；功能按钮设置包括点击修改参数、开启MCL定位和手动中止定位，功能按钮都是使用tk.Button()方法实现。

图4-15参数设置结构图

图4-16参数设置效果图
权重映射函数可以选择负指数函数和高斯分布函数。

图4-17权重映射函数选择图
权重归一化函数可以选择直接归一化和sofxmax归一化。

图4-18权重归一化函数选择图
选择粒子估计值取法可以选择根据权重取平均值和直接取概率最大的粒子。

图4-粒子估计值取法选择图
因为代码太多，且实现的方法具有相似性，下面就几个经典的举例讲解：
（1）粒子数量设置实现：
粒子数量设置实现首先使用tk.Frame()方法构建画布，在这个画布上构建2个子元素。元素1使用tk.Label()方法实现显示“粒子数量”，元素2使用 tk.Entry()方法构建输入框，并在输入框显示粒子的数量，即self.mcl_obj.M。
粒子数量设置实现的关键代码如下：

self.frame_particle_number = tk.Frame(master=self.frame_particles_parameters)
self.label_particle_number = tk.Label(master=self.frame_particle_number,
                                        text='粒子数量(个):', width=10, height=1, font=('微软雅黑', 11))
self.var_particle_number = tk.StringVar()
self.var_particle_number.set(self.mcl_obj.M)
self.entry_particle_number = tk.Entry(self.frame_particle_number,
                                  textvariable=self.var_particle_number,font=('微软雅黑', 11), width=20)

（2）是否开机随机轨迹勾选框实现
是否开启随机轨迹的勾选框使用 tk.BooleanVar()方法实现，然后绑定到tk.Checkbutton方法中。
是否开启随机轨迹勾选框实现的关键代码如下：

self.frame_is_random_trajectory = tk.Frame(master=self.frame_others_parameters)
self.var_is_random_trajectory = tk.BooleanVar()
self.var_is_random_trajectory.set(self.mcl_obj.is_random_trajectory)
self.checkbutton_is_random_trajectory = tk.Checkbutton(master=self.frame_is_random_trajectory,
                                  text="是否开启随机轨迹",
                                  variable=self.var_is_random_trajectory,
                                  onvalue=True, offvalue=False, width=12,
                                  font=('微软雅黑', 11))

（3）选择权重映射函数下拉框实现
选择权重映射函数实现的关键是使用Combobox()方法，这个方法在ttk中，随后根据self.mcl_obj.w_mapping_function的具体值去判断，决定下拉框显示什么值。
选择权重映射函数实现的关键代码如下：

self.cmb_w_mapping_function = Combobox(self.frame_w_mapping_function, width=15, font=('微软雅黑', 11))
self.cmb_w_mapping_function['value'] = ('负指数函数', '高斯分布函数')
if self.mcl_obj.w_mapping_function == "负指数函数":
    self.cmb_w_mapping_function.current(0)
elif self.mcl_obj.w_mapping_function == "高斯分布函数":
    self.cmb_w_mapping_function.current(1)

（4）开启MCL定位按钮实现
开启MCL定位按钮功能实现使用的是 tk.Button()方法，创建了一个按钮，然后使用command=lambda: thread_it(self.start_mcl)绑定了开始启动定位的函数。
值得注意的是，这里使用了多线程编程，这样子才能让用户能选择去随时中止MCL定位的操作。
开启MCL定位按钮功能实现的关键代码如下：

self.btn_start_mcl = tk.Button(self.frame_button, text='开始MCL定位', font=('微软雅黑', 12),command=lambda: thread_it(self.start_mcl), fg="black",
                                bg='springgreen',
                                borderwidth=5)
self.btn_start_mcl.pack(side=tk.LEFT, fill=tk.X, expand=tk.YES)

def thread_it(func, *args):
    t = threading.Thread(target=func, args=args)
    t.setDaemon(True)
    t.start()
    return t

4.4.4 放置右边画布的函数方法def right_part_view(self)实现

右边画布的实现如图4-19所示，显而易见包括了4张图：MCL定位过程、粒子权重变化图、坐标x的loss变化图和坐标y的loss变化图。因为每一个图构建的思想类似，这里以第一幅图为例说明实现思想。
MCL定位过程的图形展示思想为：首先使用plt.Figure()构建一个画图对象self.mcl_plt_obj，然后使用对象的.add_subplot(111)方法增加了1个子图作为画图的操作对象self.mcl_plt_obj_subplot。为了matpllotlib画图能和tkinter界面绑定，需要把画图对象self.mcl_plt_obj绑定到FigureCanvasTkAgg()方法中。最后，上述提的操作对象self.mcl_plt_obj_subplot就能进行matplotlib画图的xaxis.grid()、yaxis.grid()、set_xlabel()、plt()等各种接口调用进行画图。
MCL定位过程实现图实现的关键代码如下：

self.mcl_plt_obj = plt.Figure()
self.mcl_plt_obj_subplot = self.mcl_plt_obj.add_subplot(111)
# 创建画布
self.mcl_plt_canvas = FigureCanvasTkAgg(self.mcl_plt_obj, master=self.frame_mcl_figure)
self.mcl_plt_canvas.get_tk_widget().pack(side=tk.TOP, fill=tk.BOTH, expand=tk.YES)

图4-19右边画布MCL可视化图

5 程序展示和测试

5.1 默认参数下，定位的前期、中期和后期结果展示

在定位前期，粒子比较均匀分布，此时还无法确定机器人的位置；在定位中期，粒子渐渐聚集，但此时的loss还比较大；在定位后期，粒子聚集，并且机器人的位置估计值(x,y)的loss收敛，这时可以以很大概率认为已经确定了机器人的位置。

图5-1默认参数定位前期图

图5-2默认参数定位中期图

图5-3默认参数定位后期图

5.2 修改部分参数的展示

修改部分参数主要是为了展示用户交互功能，这里把粒子数量从100个手动改为了1000个，仿真的单位时间从0.1s改为了1s，测试结果如下。

图5-4修改部分参数定位前期图

图5-5修改部分参数定位中期图

图5-6修改部分参数定位后期图

5.3 勾选随机轨迹后的展示

勾选随机轨迹后，机器人走的路径很乱，如图4-所示。但是在100多次定位迭代后，坐标(x,y)的收敛效果一样很好。

图5-7勾选随机轨迹定位情况图

7 结论和未来方向

本项目开发了一个基于Python的蒙特卡洛定位仿真可视化软件系统，解决了网络上没有找到比较完善的蒙特卡洛定位仿真演示系统的痛点，不但支持可视化定位的直观过程、粒子的权重变化、Loss的收敛情况，系统还可以进行一定的交互，给用户提供了自己修改粒子数量、障碍物位置、运动速度等多种可调参数，希望丰富的可视化和交互功能能帮助到入门学习蒙特卡洛定位算法的同学。
目前蒙特卡洛定位的基本原理已经研究明白，未来下一步的工作是去阅读最新的论文，探索蒙特卡洛定位算法的改进方式有什么。

8 致谢

感谢这门课，让我学到了很多知识；感谢网络上各种写tkinter的经验贴，让我在踩了无数坑后，参照了很多前人的经验才把本项目完整得写完。

9 参考文献

[1]Thrun, Sebastian. Probabilistic robotics[J]. Communications of the Acm, 2005, 45(3):52-57.

posted @ 2023-05-15 03:01 JaxonYe 阅读(210) 评论(0) 编辑收藏举报

会员力量，点亮园子希望

刷新页面返回顶部

Jaxon

版权所有，转载请注明出处