python中outside loop_未知环境下的Lidar概率占位栅格图(Occupancy Grid Map) Python代码实现...

自动驾驶Mapping-占位栅格图(Occupancy Grid Map)中介绍了概率占位栅格地图(Probabilistic Occupancy Grid)的原理，并推导了如何利用贝叶斯理论(Bayes Theorem)更新生成概率占位栅格地图。下面看看如何用Python代码实现未知环境中的运动车辆上安装的激光雷达(lidar)生成概率占位栅格图。

１、构建环境地图和车辆运动模型

在生成栅格地图之前，首先需要构造一个用于车辆运动的环境地图(这个地图是用于仿真的真值，对于车辆来说是未知的环境)。我们用０和１值来构造Ｍ*N的环境地图，０表示可行驶区域，１表示占用区域。

M = 50

N = 60

true_map = np.zeros((M, N))

true_map[0:10, 0:10] = 1

true_map[30:35, 40:45] = 1

true_map[3:6,40:60] = 1;

true_map[20:30,25:29] = 1;

true_map[40:50,5:25] = 1;

然后构建车辆的运动模型。这里实现了一个简单的运动模型：车辆遇到障碍物或者到达地图边界之前，沿一个方向一直行驶；遇到障碍物或者到达地图边界之后，调整方向继续行驶。

# Initializing the robot's location.

x_0 = [30, 30, 0]

# The sequence of robot motions.

u = np.array([[3, 0, -3, 0], [0, 3, 0, -3]])

u_i = 1

# Initialize the vector of states for our simulation.

x = np.zeros((3, len(time_steps)))

x[:, 0] = x_0

while(Some Conditon...) :

# Perform robot motion.

move = np.add(x[0:2, t-1], u[:, u_i])

# If we hit the map boundaries, or a collision would occur, remain still.

if (move[0] >= M - 1) or (move[1] >= N - 1) or (move[0] <= 0) or (move[1] <= 0) or true_map[int(round(move[0])), int(round(move[1]))] == 1:

x[:, t] = x[:, t-1]

u_i = (u_i + 1) % 4

else:

x[0:2, t] = move

车辆的运动效果如下所示：

最后要构建激光雷达(Lidar)的旋转模型。这里假设在车辆运动过程中，激光雷达(lidar)以0.3/Step的速度持续旋转，对周围的环境进行扫描。

x[2, t] = (x[2, t-1] + w[t]) % (2 * math.pi)

２、生成激光雷达(Lidar)测量数据

有了地图和车辆运动模型，我们看看如何生成运动车辆上的激光雷达(lidar)扫描数据。

首先，我们需要搞清楚激光雷达的外参和内参，并以此推导出激光雷达(lidar)在Map坐标系下的姿态(x, y,

)和激光雷达(lidar)的激光束的水平和垂直角度分布(激光束的水平和垂直角度分布跟激光雷达自身的硬件属性相关，一般可以从Lidar产品说明书中获取)。

其次，我们需要知道激光雷达(Lidar)的最大扫描范围，超出该范围的区域不能被当前位置的Lidar扫描到，因而是定义为未知区