ROS计算TSP遍历顺序

前面介绍了ROS计算全覆盖路径的整体流程，在完成区间分割之后，就需要计算区间之间的遍历顺序，本篇会对这部分的代码功能做一个分析记录。

 计算各区间的遍历顺序，本质上可以看作是一个TSP问题（即旅行商问题），只不过不要求回到起点位置，故可以用求解TSP的方式求出各个区间的遍历顺序。求解TSP的方式分为两类：一类是求出最优解，另一种则是求出近似解。由于TSP属于NP完全问题，而且一般应用在智能扫地机上面的算法板算力并不高，故一般在实际应用中，不会要求求出最优解，能得到一个相对较合理的近似解即可。

ROS中调用计算TSP顺序的代码如下：

	std::vector<int> optimal_order;
	if (cell_visiting_order == OPTIMAL_TSP)
	{
		GeneticTSPSolver tsp_solver;
		optimal_order = tsp_solver.solveGeneticTSP(rotated_room_map, polygon_centers, 0.25, 0.0, map_resolution, start_cell_index, 0);
		if (optimal_order.size()!=polygon_centers.size())
		{
			std::cout << "=====================> Genetic TSP failed with 25% resolution, falling back to 100%. <=======================" << std::endl;
			optimal_order = tsp_solver.solveGeneticTSP(rotated_room_map, polygon_centers, 1.0, 0.0, map_resolution, start_cell_index, 0);
		}
	}
	else if (cell_visiting_order == LEFT_TO_RIGHT)
	{
        ...//此处省略
	}

这段代码在前面有简单分析过，ROS默认使用的计算TSP遍历顺序的算法是遗传算法，且会先将地图缩放0.25倍后进行计算。本篇中，主要讲另一种求解TSP遍历顺序的方式，其源码位置为autopnp/ipa_building_navigation/common/include/ipa_building_navigation/nearest_neighbor_TSP.h，这是一种计算最邻近区域的方式。算法思想很简单，每次都取离当前位置最近的区域为下一个清扫区域，到达下一个区域后，再取最近的区域为下一个清扫区域。

该方法的函数的接口定义如下：

	// compute TSP and distance matrix without cleaning it
	// this version does not exclude infinite paths from the TSP ordering
	std::vector<int> solveNearestTSP(const cv::Mat& original_map, const std::vector<cv::Point>& points, double downsampling_factor,
			double robot_radius, double map_resolution, const int start_node, cv::Mat* distance_matrix=0);

original_map 是经过预处理以及角度旋转后的地图数据，用来计算点跟点之间的路径长度；points 是前面做区间分割的时候，计算得出的每一个区间的中心点坐标数组；downsampling_factor 则是地图的缩放比率，即把传入的地图按照给定的缩放比率进行缩放，在缩放后的地图上寻找路径，需要注意的是，这种方式虽然用来处理大地图的时候，可以提高计算速度，但是存在一个隐患：当地图中存在窄通道时，缩放后该通道会闭合，从而导致两个区间不连通无法计算出区间距离；robot_radius 是机器人的半径；map_resolution 是栅格地图的分辨率；start_node 为初始区间序号，通过机器人当前坐标算出距离最近的区间；distance_matrix 则是用来返回距离矩阵。函数具体实现如下：

std::vector<int> NearestNeighborTSPSolver::solveNearestTSP(const cv::Mat& original_map, const std::vector<cv::Point>& points,
		double downsampling_factor, double robot_radius, double map_resolution, const int start_node, cv::Mat* distance_matrix)
{
	std::cout << "NearestNeighborTSPSolver::solveNearestTSP: Constructing distance matrix..." << std::endl;
	//calculate the distance matrix
	cv::Mat distance_matrix_ref;
	if (distance_matrix != 0)
		distance_matrix_ref = *distance_matrix;
	DistanceMatrix distance_matrix_computation;
	distance_matrix_computation.constructDistanceMatrix(distance_matrix_ref, original_map, points, downsampling_factor, robot_radius, map_resolution, pathplanner_);

	return solveNearestTSP(distance_matrix_ref, start_node);
}

distance_matrix_ref 是一个二维的距离矩阵，该矩阵保存各个区间的两两距离，在进行距离计算时，只需计算二维矩阵对角右上方的两点距离，计算完毕即可得出所有的区间之间的最短距离。

最短距离搜索策略则是：

（1）先将两点之间连一条直线，计算该直线上是否穿过障碍物，假如没有穿过障碍物，则直线距离即为两点之间的最短距离；

（2）假如两点之间的连线穿过障碍物，则使用A*算法计算出两点之间的最短距离。

计算生成距离矩阵后，就可以通过距离矩阵，计算得出最邻近点的遍历序列。这部分的代码如下：

std::vector<int> NearestNeighborTSPSolver::solveNearestTSP(const cv::Mat& path_length_matrix, const int start_node)
{
	std::vector<int> calculated_order; //solution order

	if(path_length_matrix.rows > 1) //check if clique has more than one member or else this algorithm produces a order of size=3
	{
		int last_node; //index of the last spectated node
		std::vector<bool> visited(path_length_matrix.rows, false);

		int current_node = start_node; //index of the current spectated node
		calculated_order.push_back(current_node);
		visited[current_node] = true;

		//check every Point for the next nearest neighbor and add it to the order
		do
		{
			int next_node; //saver for next node
			double min_distance = 1e100; //saver for distance to current next node
			for (int current_neighbor = 0; current_neighbor < path_length_matrix.cols; current_neighbor++)
			{
				if (visited[current_neighbor]==false) //check if current neighbor hasn't been visited yet
				{
					const double length = path_length_matrix.at<double>(current_node, current_neighbor);
					if (length < min_distance && length > 0)
					{
						next_node = current_neighbor;
						min_distance = length;
					}
				}
			}
			calculated_order.push_back(next_node); //add the found nearest neighbor to the order-vector
			visited[next_node] = true;
			current_node = next_node;
		} while (calculated_order.size() < path_length_matrix.rows); //when the order has as many elements as the pathlength Matrix has the solver is ready
	}
	else
	{
		calculated_order.push_back(start_node);
	}

	return calculated_order;
}

需要注意的是，这段代码实现存在一个隐患，上面已经讲到，调用该接口默认会将地图进行缩放，缩放地图可能就导致某一个区间与其他区间不连通，而这段代码里面没有对 next_node 进行初始化，假设由于缩放导致初始区间跟其他所有区间都不连通，则在执行完 for 循环后，next_node 的值是一个随机值，从而可能导致运行到 visited[next_node] = true; 这里发生程序崩溃。个人建议将这一段代码重写。

计算得出各区间的遍历顺序后，后面就会按照遍历顺序，依次生成各个区间内弓字形覆盖路径，这个具体实现以及存在的一些问题后续再补充。