python编写的2D激光扫描SLAM程序

#!/bin/env python # -*- coding:utf-8 -*- """ # _desc:SLAM # _author:james # _create_time:2020-08-01 """ import math import struct import sys import matplotlib.pyplot as plot import numpy as np import redis import xlrd np.set_printoptions(threshold=sys.maxsize) class Lidar: """ 激光雷达参数 """ def __init__(self): self.angle_min = -2.351831 # 最小角度-弧度值 self.angle_max = 2.351831 # 最大角度 self.angle_increment = 0.004363 # 角度增量 self.npoints = 1079 # 每一帧的节点数 self.range_min = 0.023 # 每一个测距值的最小值 self.range_max = 60 # 每一个测距值的最大值 self.scan_time = 0.025 # 扫描时间 self.time_increment = 1.736112e-05 # 时间增量 self.angles = np.transpose([np.arange(self.angle_min, self.angle_max, self.angle_increment)]) class Map: """ 地图 """ class MapStruct(): def __init__(self, points, connections, keyscans): self.points = points self.connections = connections self.keyscans = keyscans def set_map(self, pose, scan): """ 设置地图 :param pose: :param scan: :return: """ points = transform(pose, scan) connections = [] keyscans = [] m = dict() m["pose"] = pose m["i_begin"] = 0 m["i_end"] = scan.shape[0] - 1 # 行数 m["loop_closed"] = True m["loop_tried"] = False keyscans.append(m) return self.MapStruct(points, connections, keyscans) class GridMap: """ 栅格地图 """ class GridMapStruct(): def __init__(self, occGrid, metricMap, pixelSize, topLeftCorner): self.occGrid = occGrid self.metricMap = metricMap self.pixelSize = pixelSize self.topLeftCorner = topLeftCorner def create_grid(self, local_map, pixel_size): """ 从点集创建占用栅格地图 :param local_map: :param pixel_size: :return: """ # 网格尺寸 min_xy = local_map.min(axis=0) - 3 * pixel_size # min(local_map)：返回x的最小值和y的最小值构成的向量(不一定是对应左下角,可能图里面没有左下角) max_xy = local_map.max(axis=0) + 3 * pixel_size # 3*pixelSize：构成的栅格地图中占用栅格最边界离地图边界留有3个栅格单元的余量 s_grid = np.round((max_xy - min_xy) / pixel_size) + 1 # s_grid[0]为x轴向栅格数量,s_grid[1]为y轴向栅格数量 s_grid = s_grid.astype(np.int) # float转int n = local_map.shape[0] # hits为被占用的栅格的二维坐标 (第hits(1)块,第hits(2)块) # 这一步使得到的栅格地图会较原始地图出现一个翻转(当点集里不存在左下角时会出现翻转) hits = np.round((local_map - np.tile(min_xy, (n, 1))) / pixel_size) + 1 hits = hits.astype(np.int) # float转int # 构造一个空的栅格地图 grid = np.full((s_grid[1], s_grid[0]), False, dtype=bool) idx = np.transpose((hits[:, 0] - 1) * s_grid[1] + hits[:, 1] - 1).reshape(-1, 1) temp_grid = grid.flatten("F") # 按列拉平一维数组 temp_grid[idx] = True grid = temp_grid.reshape(s_grid[1], s_grid[0], order='F') # 重新组合 occGrid = grid.copy() # 深拷贝 from scipy import ndimage # 获取grid中零元素所在的位置靠近非零元素位置的最短距离构成的矩阵 temp_array = ndimage.distance_transform_edt(1 - grid) metricMap = np.minimum(temp_array, 10).copy() topLeftCorner = min_xy # 栅格地图的x最小值和y最小值构成的向量的全局坐标 return self.GridMapStruct(occGrid, metricMap, pixel_size, topLeftCorner) def pol2cart(rho, phi): """ 从极坐标转换为笛卡尔坐标 :param rho: 距离 :param phi: 角度 :return: """ x = rho * np.cos(phi) y = rho * np.sin(phi) return x, y def read_scan(lidar_data, idx, lidar, usableRange): """ 读取扫描数据 :param lidar_data: 测距数据 :param idx: 扫描数据索引，行标 :param lidar: 激光参数 :param usableRange:可使用的范围 :return: """ angles = lidar.angles ranges = np.transpose([lidar_data[idx, :]]) # 获取索引行扫描数据，数据行转列 maxRange = min(lidar.range_max, usableRange) isBad = np.where((ranges[:, 0] < lidar.range_min) | (ranges[:, 0] > maxRange)) # 获取不符合条件的索引 angles = np.delete(angles, isBad) # 移除angles中不符合条件的行 ranges = np.delete(ranges, isBad) # 移除ranges中不符合条件的行 x, y = pol2cart(ranges, angles) scan = np.c_[x, y] return scan def transform(pose, scan): """ 局部坐标转化为全局坐标 :param scan: 某次扫描数据的局部笛卡尔坐标 :param pose: 当前位姿(x坐标tx y坐标ty 旋转角theta) :return: """ tx = pose[0] ty = pose[1] theta = pose[2] ct = math.cos(theta) st = math.sin(theta) a = [ct, -st] b = [st, ct] R = np.vstack((a, b)) tscan = np.dot(scan, R.T) tscan[:, 0] = tscan[:, 0] + tx tscan[:, 1] = tscan[:, 1] + ty return tscan def get_local_map(points, pose, scan, border_size): """ 从全局地图中 提取当前扫描周围的局部地图 的全局坐标 :param points: :param pose: :param scan: :param border_size: :return: """ scan_w = transform(pose, scan) # 将当前扫描数据坐标scan转化为全局坐标scan_w # 设置左上角和右下角 min_x = min(scan_w[:, 0] - border_size) min_y = min(scan_w[:, 1] - border_size) max_x = max(scan_w[:, 0] + border_size) max_y = max(scan_w[:, 1] + border_size) # 提取位于范围内的全局地图中的点 is_around = np.where( (points[:, 0] > min_x) & (points[:, 0] < max_x) & (points[:, 1] > min_y) & (points[:, 1] < max_y)) local_map = points[is_around, :] return local_map def diff_pose(pose1, pose2): """ 计算位姿差 :param pose1: :param pose2: :return: """ dp = pose2 - pose1 dp[2] = pose2[2] - pose1[2] return dp def fast_match(grid_map, scan, pose, search_resolution): """ 根据当前位姿的栅格地图，优化预测的下一位姿，使下一位姿的栅格地图与当前位姿的栅格地图达到最大的重合度 :param grid_map: 局部栅格地图 :param scan: 构成grid_map的当前扫描点集的局部笛卡尔坐标 :param pose: 预测的下一位姿(预测得到的pose_guess) :param search_resolution: 搜索的分辨率 :return: pose 优化过后的下一位姿,使下一位姿的栅格地图与当前位姿的栅格地图达到最大的重合度 best_hits pose对应的最佳重合度score,对应的当前位姿栅格地图的原始距离矩阵 """ # <editor-fold desc="栅格地图信息"> metric_map = grid_map.metricMap # 栅格地图中0元素所在的位置靠近非零元素位置的最短栅格距离构成的矩阵 i_pixel = 1 / grid_map.pixelSize # 实际距离1m对应几个栅格单元边长 (栅格单元尺寸对应的实际距离的倒数) min_x = grid_map.topLeftCorner[0] # 栅格地图中的最左端的横坐标(全局) min_y = grid_map.topLeftCorner[1] # 栅格地图中的最下端的纵坐标(全局) n_cols = metric_map.shape[1] # metric_map的列数 n_rows = metric_map.shape[0] # metric_map的行数 # </editor-fold> # <editor-fold desc="爬山算法"> max_iter = 50 # 最大循环次数 50 max_depth = 3 # 提高分辨率次数的最大值 3 iter = 0 # 循环变量 depth = 0 # 分辨率提高次数变量 pixel_scan = scan * i_pixel # 将扫描数据实际坐标转为栅�