地图构建 -- occupancy grid mapping

在机器人运动过程中，我们一直假设它有一个地图。但是很少有真实世界的应用能够满足这一假设，因为地图通常都是先验的， 或者是人们手工创建的。在一些应用领域，没有过多的资源来建立先验地图。所以，对于机器人而言，能够自行通过移动感知周围环境自建地图的能力是很重要的。

因为一些因素的制约，移动机器人自动建图是一项有挑战的工作:

  • 计算量问题，由于机器人可能的运动环境不定，也许是一个小小的空间，也许是一个足球场，在这种不定的环境中，可能的地图数量有可能是无穷的。举个例子：4宫格地图，可能的地图状态就有 [公式] 个。如果将连续的现实场景下的地图离散化，比如有1500割网格分隔表示一个连续的场景地图，仅仅是如此高维空间的大小就使得计算所有可能的地图的完整后验概率充满了挑战。因此在定位问题中工作的很好的贝叶斯滤波器就不再适合学习地图了， 至少是这些算法的朴素实现方式不再适用了。
  • 学习地图是一个"先有鸡还是先有蛋(chicken-and-egg)"的问题，因而人们通常称之为即时定位与建图问题simutaneous localization and mapping(SLAM)或者说是concurrent mapping and localization。首先它是一个定位问题。机器人在环境中运动，其里程计在不断的累计误差，使得机器人逐渐的迷失了。如果有了一幅地图，就有很多方法来确定机器人的位置。 其次它还是一个建图的问题。如果知道了机器人的位置，那么构建一幅地图也是一件相对容易的事情，这点将在本章和后续的几章中得到证实。如果缺失了初始地图，也没有确切的位置信息， 那么机器人就必须同时处理这两件事情，估计地图，并确定自己相对于该地图的位置。

显然，不是所有的建图问题的难度都一样。建图问题的难度取决于一系列的因素，其中最重要的有:

  1. 尺寸。相比于机器人的感知范围，环境越大就越难建图。
  2. 感知和执行噪声。如果机器人的传感器和执行器都是无噪声的，那么建图就会是一个简单的问题。噪声越大，问题越难。
  3. 感知confuse。 如果有很多地方长的都差不多，那么就很难及时的确定这些地方的地图。
  4. 环路。如果环境中有很多环路，那么构建地图就会尤其困难。如果只是让机器人在走廊里来来回回的走，在每次返回的时候，机器人都可以修正一下里程计的误差。 但是如果有环路的存在，那么机器人可以通过不同的路径回到原点，如此形成的闭环将不利于里程计的修正，最终导致累计误差增大。

下图就是在一个很大的环境下，（a）表示传感器扫描到的所有地图 （b）表示最终通过算法获取的最终地图

我们首先研究在已知机器人位姿的假设下进行建图的问题。换句话说，我们先回避SLAM问题中的难点，在建图的时候有神谕(oracle)告知我们机器人的正确路径。

占用网格构图算法，主要是根据t时刻的有噪声的测量数据和机器人当前的位姿来生成连续地图。如下：

任何占用栅格建图的黄金法则都是给定数据计算地图的后验概率

[公式]

我们可以发现，地图的构建就是寻找判断是否是障碍物，这样就把问题分解成为了一个二值概率的问题，讨论过这个问题的一个滤波器——二值贝叶斯滤波器binary Bayes filter。

ARUANTOU：静态二值贝叶斯滤波器1 赞同 · 0 评论文章

occupancy grid mapping伪代码：

原理还是很简单的，就是遍历所有的网格，判断当前网格cell是否在传感器的扫描范围内，如果在的话，根据 [公式] 和 [公式] 来计算对数差异。

对数差异的逆模型的算法：

这样通过不断的获取扫描的传感器信息和机器人的位姿信息，就可以更新差异值。然后就可以计算 [公式] 来获取最终的后验概率。

下面是一个伪代码的算法实现：

#include <iostream>
#include <math.h>
#include <vector>
using namespace std;

// Sensor characteristic: Min and Max ranges of the beams
double Zmax = 5000, Zmin = 170;
// Defining free cells(lfree), occupied cells(locc), unknown cells(l0) log odds values
double l0 = 0, locc = 0.4, lfree = -0.4;
// Grid dimensions
double gridWidth = 100, gridHeight = 100;
// Map dimensions
double mapWidth = 30000, mapHeight = 15000;
// Robot size with respect to the map 
double robotXOffset = mapWidth / 5, robotYOffset = mapHeight / 3;
// Defining an l vector to store the log odds values of each cell
vector< vector<double> > l(mapWidth/gridWidth, vector<double>(mapHeight/gridHeight));

double inverseSensorModel(double x, double y, double theta, double xi, double yi, double sensorData[])
{
    // You will be coding this section in the upcoming concept! 
	double Zk, thetaK, sensorTheta;
    double minDelta = -1;
    double alpha = 200, beta = 20;

    //******************Compute r and phi**********************//
    double r = sqrt(pow(xi - x, 2) + pow(yi - y, 2));
    double phi = atan2(yi - y, xi - x) - theta;

    //Scaling Measurement to [-90 -37.5 -22.5 -7.5 7.5 22.5 37.5 90]
    for (int i = 0; i < 8; i++) {
        if (i == 0) {
            sensorTheta = -90 * (M_PI / 180);
        }
        else if (i == 1) {
            sensorTheta = -37.5 * (M_PI / 180);
        }
        else if (i == 6) {
            sensorTheta = 37.5 * (M_PI / 180);
        }
        else if (i == 7) {
            sensorTheta = 90 * (M_PI / 180);
        }
        else {
            sensorTheta = (-37.5 + (i - 1) * 15) * (M_PI / 180);
        }

        if (fabs(phi - sensorTheta) < minDelta || minDelta == -1) {
            Zk = sensorData[i];
            thetaK = sensorTheta;
            minDelta = fabs(phi - sensorTheta);
        }
    }

    //******************Evaluate the three cases**********************//
    if (r > min((double)Zmax, Zk + alpha / 2) || fabs(phi - thetaK) > beta / 2 || Zk > Zmax || Zk < Zmin) {
        return l0;
    }
    else if (Zk < Zmax && fabs(r - Zk) < alpha / 2) {
        return locc;
    }
    else if (r <= Zk) {
        return lfree;
    }

}

void occupancyGridMapping(double Robotx, double Roboty, double Robottheta, double sensorData[])
{
    //1 - TODO: Generate a grid (size 300x150) and then loop through all the cells
            //2- TODO: Compute the center of mass of each cell xi and yi 
            //double xi = x * gridWidth + gridWidth / 2 - robotXOffset;
            //double yi = -(y * gridHeight + gridHeight / 2) + robotYOffset;
            //3- TODO: Check if each cell falls under the perceptual field of the measurements
    
	for(int i = 0; i < mapWidth/gridWidth;; i++)
	{
		for(int j = 0 ; j < mapHeight/gridHeight; j++)
		{
			double xi = i * gridWidth + gridWidth / 2 - robotXOffset;
			double yi = -(j * gridHeight + gridHeight / 2) + robotYOffset;
			if (sqrt(pow(xi - Robotx, 2) + pow(yi - Roboty, 2)) <= Zmax) {
                l[i][j] = l[i][j] + inverseSensorModel(Robotx, Roboty, Robottheta, xi, yi, sensorData) - l0;
            }
		}
	}
}

int main()
{
    double timeStamp;
    double measurementData[8];
    double robotX, robotY, robotTheta;

    FILE* posesFile = fopen("poses.txt", "r");
    FILE* measurementFile = fopen("measurement.txt", "r");

    // Scanning the files and retrieving measurement and poses at each timestamp
    while (fscanf(posesFile, "%lf %lf %lf %lf", &timeStamp, &robotX, &robotY, &robotTheta) != EOF) {
        fscanf(measurementFile, "%lf", &timeStamp);
        for (int i = 0; i < 8; i++) {
            fscanf(measurementFile, "%lf", &measurementData[i]);
        }
        occupancyGridMapping(robotX, robotY, (robotTheta / 10) * (M_PI / 180), measurementData);
    }
    
    // Displaying the map
    for (int x = 0; x < mapWidth / gridWidth; x++) {
        for (int y = 0; y < mapHeight / gridHeight; y++) {
            cout << l[x][y] << " ";
        }
    }
    
    return 0;
}