Motion planning for self-driving cars课程笔记1：应用雷达数据生成占用栅格地图（Occupancy Grid Map）

此文章为Motion planning for self-driving cars上第二课的笔记，主要讲述占据栅格地图的生成。由于课程中算法也是参考《probability robot》这本书，书中对算法的解释更为详尽，因此本文同时参考了课程内容和中文版《概率机器人》。
**

1. 占用栅格地图算法简介

**
占用栅格的基本思想是用一系列随机变量来表示地图。每个随机变量是一个二值数据,表示该位置是否被占用，一般用{0,1}表示，1代表占用。占用栅格地图构建算法对以上随机变量进行近似后验估计，最终得到地图中每个栅格被占用的概率，即
式中m 为地图; Z_1:t为从开始到时刻 t 的所有测量值; X_1:t为用所有固定于车辆上的雷达位姿定义的路径。对于无人驾驶汽车来说，测量值Z表示测得的障碍物到雷达的距离，X包括车辆在二维平面内的全局坐标和航向角。
标准的占用栅格方法将构建地图这一问题划分为一些独立的间题,即为所有的栅格单元 m_i建立后验概率:
由于概率取值范围为[0,1]，直接用概率相乘在接近0或1处会出现不稳定且浪费计算资源，因此转换成对数占用概率表示方法
对数占用概率（log odds）取值范围为（-inf,inf)，如下图所示
应用二值贝叶斯滤波算法（具体推导过程见《概率机器人》第4章），概率更新算法步骤如下图所示：

其中，
注意式9.8中的下标为t，代表根据当前时刻的测量值和车辆位置计算的对数占用概率，下一节对此重点介绍。即若m_i在雷达测量范围内，则t时刻m_i的对数占用概率等于上一时刻的对数占用概率加上当前时刻测量计算的对数概率，减去初始对数概率。

根据上述算法得到每个栅格的对数占用概率后，再转换回概论值，就可以得到占用网格地图。
**

2. inverse_sensor_model算法

**
如图所示，t时刻雷达向四周发送一定数目的波束，到达障碍物后反射回来，整个地图可划分为三大区域
雷达探测到的障碍物点附近，为高概率区域；超过雷达检测范围或检测的障碍物点更远的位置为未知区域；车辆到检测到的障碍物点之间为低概率区域。其中，高概率区域范围的确认方法如下：

对于单个波束来说，波束角为β,α定义为高概率区域宽度，上下边界分别离雷达检测点α/2距离，如上图所示，若栅格中心落在这个红色区域内，则此栅格为高概率区域。
基于以上的概率区域划分方法，inverse_sensor_model算法如下：
第3，4步：计算当前栅格到车辆的距离r和在全局坐标下与车辆之间的角度
第5步：计算返回当前时刻离栅格和车辆连线最近的波束索引k
第6，7步：若r大于雷达测量范围，或者r超过了第k个波束的高概率区域，则将当前时刻此栅格对数占用概率设为l₀
第8，9步：若栅格中心落在高概率区域，则将当前时刻此栅格对数占用概率设为l_occ
第10，11步：若栅格中心落在低概率区域，则将当前时刻此栅格对数占用概率设为l_free

其中，l₀，l_occ，l_free为预先定义好的值。
**

3. 示例程序（课程第2课作业）

**
inverse_sensor_model实现函数，这里返回的是概率，后面主循环中会再转换为对数占用概率

def inverse_scanner(num_rows, num_cols, x, y, theta, meas_phi, meas_r, rmax, alpha, beta):
    m = np.zeros((M, N))
    for i in range(num_rows):
        for j in range(num_cols):
            # Find range and bearing relative to the input state (x, y, theta).
            r = math.sqrt((i - x)**2 + (j - y)**2)
            phi = (math.atan2(j - y, i - x) - theta + math.pi) % (2 * math.pi) - math.pi
            
            # Find the range measurement associated with the relative bearing.
            k = np.argmin(np.abs(np.subtract(phi, meas_phi)))
            
            # If the range is greater than the maximum sensor range, or behind our range
            # measurement, or is outside of the field of view of the sensor, then no
            # new information is available.
            if (r > min(rmax, meas_r[k] + alpha / 2.0)) or (abs(phi - meas_phi[k]) > beta / 2.0):
                m[i, j] = 0.5
            
            # If the range measurement lied within this cell, it is likely to be an object.
            elif (meas_r[k] < rmax) and (abs(r - meas_r[k]) < alpha / 2.0):
                m[i, j] = 0.7
            
            # If the cell is in front of the range measurement, it is likely to be empty.
            elif r < meas_r[k]:
                m[i, j] = 0.3
           
    return m

基于真值地图和某一时刻车辆位置生成的雷达测量值函数

def get_ranges(true_map, X, meas_phi, rmax):
    (M, N) = np.shape(true_map)
    x = X[0]
    y = X[1]
    theta = X[2]
    meas_r = rmax * np.ones(meas_phi.shape)
    
    # Iterate for each measurement bearing.
    for i in range(len(meas_phi)):
        # Iterate over each unit step up to and including rmax.
        for r in range(1, rmax+1):
            # Determine the coordinates of the cell.
            xi = int(round(x + r * math.cos(theta + meas_phi[i])))
            yi = int(round(y + r * math.sin(theta + meas_phi[i])))
            
            # If not in the map, set measurement there and stop going further.
            if (xi <= 0 or xi >= M-1 or yi <= 0 or yi >= N-1):
                meas_r[i] = r
                break
            # If in the map, but hitting an obstacle, set the measurement range
            # and stop ray tracing.
            elif true_map[int(round(xi)), int(round(yi))] == 1:
                meas_r[i] = r
                break
                
    return meas_r

仿真参数设置，包括仿真步数，车辆初始位置，车辆运动参数，雷达参数及真值地图定义

# Simulation time initialization.
T_MAX = 150
time_steps = np.arange(T_MAX)#[0 1 2 ...149]

# 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

# Robot sensor rotation command
w = np.multiply(0.3, np.ones(len(time_steps)))

# True map (note, columns of map correspond to y axis and rows to x axis, so 
# robot position x = x(1) and y = x(2) are reversed when plotted to match
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;

# Initialize the belief map.
# We are assuming a uniform prior.
m = np.multiply(0.5, np.ones((M, N)))#置信度地图，初始化都为0.5

# Initialize the log odds ratio.
L0 = np.log(np.divide(m, np.subtract(1, m)))#初始化l0
L = L0

# Parameters for the sensor model.
meas_phi = np.arange(-0.4, 0.4, 0.05)#[-0.4,-0.35,...0,0.05,...0.35],弧度
rmax = 30 # Max beam range.
alpha = 1 # Width of an obstacle (distance about measurement to fill in).
beta = 0.05 # Angular width of a beam.

# Initialize the vector of states for our simulation.
x = np.zeros((3, len(time_steps)))
x[:, 0] = x_0

主程序，循环更新地图中每个栅格占用概率

meas_rs = []
meas_r = get_ranges(true_map, x[:, 0], meas_phi, rmax)#0时刻测量半径范围
meas_rs.append(meas_r)
invmods = []
invmod = inverse_scanner(M, N, x[0, 0], x[1, 0], x[2, 0], meas_phi, meas_r, \
                         rmax, alpha, beta)#0时刻概率图
invmods.append(invmod)
ms = []
ms.append(m)

# Main simulation loop.
for t in range(1, len(time_steps)):
    # 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
    x[2, t] = (x[2, t-1] + w[t]) % (2 * math.pi)
    
    # TODO Gather the measurement range data, which we will convert to occupancy probabilities
    # using our inverse measurement model.
    meas_r = get_ranges(true_map, x[:, t], meas_phi, rmax)#t时刻测量半径范围
    meas_rs.append(meas_r)
    
    # TODO Given our range measurements and our robot location, apply our inverse scanner model
    # to get our measure probabilities of occupancy.
    invmod = inverse_scanner(M, N, x[0, t], x[1, t], x[2, t], meas_phi, meas_r, \
                         rmax, alpha, beta)#t时刻概率图
    invmods.append(invmod)
    
    # TODO Calculate and update the log odds of our occupancy grid, given our measured
    # occupancy probabilities from the inverse model.
    L = np.log(np.divide(invmod, np.subtract(1, invmod)))#t时刻inverse_sensor_model
    
    # TODO Calculate a grid of probabilities from the log odds.
    Lt = np.subtract(np.add(np.log(np.divide(ms[t-1], np.subtract(1, ms[t-1]))),L),L0)
    m=np.subtract(1,np.divide(1,np.add(1,np.exp(Lt))))
    ms.append(m)

**

4. 仿真效果验证

**
真值地图和车辆行驶路径如下所示

选取[10, 40]、[40, 30]、[40, 35]、[50, 0]、[5, 10]、[15, 20]、[50, 25]这几个点进行计算，得出的占用概率分别为：
0.9857478005865102
0.9988631799564817
0.8448275862068966
0.5
0.072972972972973
0.00020899763468718024
1.2953015129379963e-06

可以看出，计算结果与实际相符，前三个点被占用，第4个点因为雷达未扫描到，占用是否未知，后3个点为未占用点。

最终根据概率绘制的占用栅格地图如下图所示，可以看出，很接近真值地图，算法效果较好。

**

5. 参考

**
[1].Coursera无人驾驶课程：Motion planning for self-driving cars
[2].《概率机器人》第4、9章