VO避障 算法 python

VFH避障核心代码讲解，以及避障流程

VFH是一种由人工势场法改进而来的机器人导航算法。在机器人移动的过程中， 利用传感器探测周围障碍物信息（图1），生成极坐标直方图（图2）。图2中x轴是以机器人为中心感知到的障碍物的角度（逆时钟方向为正），y轴表示在该方向存在障碍物的障碍物密度值（距离越近，密度值越大；3个峰值ABC分别代表障碍物ABC）。低于阈值的波谷成为备选通过方案，再根据具体情况（如目标角度、当前朝向）选择通过的角度（1/2）。
避障主要分为两个阶段，第一个阶段是扫描障碍物距离信息，第二个阶段寻找合适的机器人驱动方向。

图1

图2

横坐标为0-360度的方向，纵轴坐标是该角度下的行进叠加。柱状图超越，表示向该方向行进的重叠取代，也表明越不可能通过。理论上，这个柱状图低的区域是合理行进的，但可能会改变目标方向。
需要一个平衡函数来平衡目标方向。最终，会选择一个相对最方便的方向行进。于是这个函数便是整个算法的核心。
它执行过程中针对移动机器人的周围环境创建了一个基于极坐标表示的局部直方图，这个局部使用直方图的表示方法，会被最近的一些传感器数据所更新、会根据这个直方图首先识别出允许机器人通过的足够大的所有空隙，然后对所有这些空隙计算其代价函数，
最终选择具有最低代价函数的通路通过。代价函数受三个因素影响：目标方向，机器人当前方向，之前选择的方向

障碍物扫描

将360度扫描范围 数据 存入一个宽度区间为30度向量，共12维。这个数据是利用优化过的数据，不是真实的距离数据。每30°一个统一数据。这个数据是对实际距离数据做的一个反向（最大值减去实际扫描值）。如果激光雷达实际探测距离越大，则该值越小。

void ComputeMV(vector<float> r) {//计算直方图
	float dist[360] = { 0 };
	ranges.clear();
	map_cv.clear();
	int range_size = r.size(); //有多少条激光

	for (size_t i = 0; i < range_size; i++)
	{
		//A non-zero value (true) if x is a NaN value; and zero (false) otherwise.

		//isinf A non-zero value (true) if x is an infinity; and zero (false) otherwise.
		if (!std::isnan(r[i]) && !std::isinf(r[i]))//取出有效值
		{
			float scan_distance = r[i];
			//0-30都是一个sector
			int sector_index = std::floor((i*angle_resolution) / sector_value);//(i*1)/30 sector_index:[0 12]
			if (scan_distance >= scan_distance_max || scan_distance < scan_distance_min)
				scan_distance = 0;
			else
				scan_distance = scan_distance_max - scan_distance;//距离为雷达探测最大距离减去扫描距离  做了一个反向。实际距离越大，scan_distence 越小

			dist[sector_index] += scan_distance; //设定柱状高度
		}
		ranges.push_back(r[i]);//ranges存放360度距离信息
	}

	for (int j = 0; j < (int)(360 / sector_value); j++)//把uav四周分为12个sector，每个sector的值越小，代表越安全。
	{
		map_cv.push_back(dist[j]);
	}

确定机头方向

首先会通过当前点与目标点的坐标值，计算目标点相对Uav的夹角，即与y轴的夹角。

其次对柱状图的高度进行统一处理。

当障碍物扫描后处理的数据小于0.1时，柱状图高度为0，

数据为0.1到0.3之间时，柱状图高度为2，

其他形式高度为4。

for (int i = 0; i < map_cv.size(); i++) //确定12个柱状图高度值，CV值越大，存在障碍物可能性越大
	{
		if (map_cv[i] < 0.1)
			mesh.push_back(0);
		else if (map_cv[i] >= 0.1 && map_cv[i] < 0.3)
			mesh.push_back(2);
		else
			mesh.push_back(4);
	}

将障碍物的距离在直方图处理以后，有12个直方柱，有11个波谷。其次是确定有哪几个波谷可以飞行，最后确定从第几个波谷走。

">	for (int j = 0; j < mesh.size(); j++)
	{
		if (j == mesh.size() - 1) //if(j == 11)
		{
			if (mesh[0] + mesh[mesh.size() - 1] == 0)
				cand_dir.push_back(0.0);
		}
		else
		{
			if (mesh[j] + mesh[j + 1] == 0)//11个波谷
				cand_dir.push_back((j + 1)*sector_value);//寻找安全角度，机体坐标系下；即确定波谷
		}
	}

if (cand_dir.size() != 0) {
	vector<float> delta;
	for (auto &dir_ite : cand_dir) {
		float delte_theta1 = fabs(dir_ite - goal_ori);
		float delte_theta2 = 360 - delte_theta1;
		float delte_theta = delte_theta1 < delte_theta2 ? delte_theta1 : delte_theta2;
		delta.push_back(delte_theta);
	}//寻找接近目标区域的波谷
	int min_index = min_element(delta.begin(), delta.end()) - delta.begin();
	ori = cand_dir.at(min_index);//从第几个波谷走

	ori += heading;
	ori = (int)ori % 360;
	return ori;//确定机头方向
}

确定飞行速度

每次确定障碍物的飞行方向后，其次是确定无人机的飞行速度。

将机器人当前的飞行速度进行x和y轴的速度分解。

mavros_msgs::PositionTarget pos_target;
					pos_target.coordinate_frame = 1;
		            pos_target.type_mask = 1 + 2 + 4 +/* 8 + 16 + 32 +*/ 64 + 128 + 256 + 512 + 1024 + 2048;
					pos_target.velocity.x = 0.5* cos(arc);
					pos_target.velocity.y = 0.5* sin(arc);
				local_pos_pub.publish(pos_target);