⭐ 欢迎来到虫虫下载站！ | 📦 资源下载 📁 资源专辑 ℹ️ 关于我们
⭐ 虫虫下载站
📤 上传资源

📄 apfpathplan.cpp

📁 路径规划源程序
💻 CPP
📖 第 1 页 / 共 3 页
字号:
🔍
123 
		}
		FindGap=false;
		if (!RightForbidden) ///<如果允许右方搜索即-方向搜索的话
		{
			SearchFreeAngleInHalfPlane(freeNumDec,NumCountDec,FindGap,i,GapBeginAngle,j_PassBound,-1);
		//	robot_console_printf("*************************LeftnotForbid");	
		}
		FindTargetAngleInCandidate(NumCountAdd,NumCountDec,LeftForbidden,RightForbidden);//选出最终的目标路径角
	}	
	CheckAngleLimitedBeyond(m_iTargetAngle,0);//限幅
	m_iPreTargetAngle=m_iTargetAngle;///<保存目标路径角
	
	return true;
}

//bool ApfPathPlan::CheckAngleLimitedBeyond(double & Angle, int Add_Pos_Dec_Neg)


bool ApfPathPlan::CheckCurAngFindNexAng(int& AngTobeCheck,int& NearestObstacleIndex,
										int& DisBetweenTarA2SelfA,int SearchInLeftOrRightPlaneFlag)
{
//__asm int 3;
	if (m_nFreRoadMap[AngTobeCheck]==0) 
	{AngTobeCheck=AngTobeCheck+SearchInLeftOrRightPlaneFlag;}
	else
	{
		if (m_dMinObstalceDis>m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].distance)//找到更小的距离 
		{
			m_dMinObstalceDis=m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].distance;
			NearestObstacleIndex=m_nFreRoadMap[AngTobeCheck];
			// 	robot_console_printf("NearObstacleIndex=%d\n",m_nFreRoadMap[i]);
		}
		//////////////////////////////////////////////////////////////////////////
		/* 检查下一个角度,依据物体边界的方法，目标极快速移动，
		可能导致死循环， 
		仅在应用动态障碍物时出现,因此当应用于动态障碍物时改为挨个角度搜索*/
		//////////////////////////////////////////////////////////////////////////
		if (
			CheckObstacleIsBehind(m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].LBoundaryAngle,
			m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].angle,
			m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].RBoundaryAngle)
			)//->检查该障碍物是否处于后面,
			
		{
			
			if (SearchInLeftOrRightPlaneFlag>0)//+方向
			{
				//return true;
				/*当搜索自己当前朝向到目标角间的障碍物最小距离时，若碰到一个在背后横跨360分界线的障碍物时，
			因目标角〉=自身朝向，所以不用再搜索了，它就是最后一个障碍物了，可以跳出.不对!!!*/
			/*	robot_console_printf("++AngTobeCheck=%d\n",AngTobeCheck);
				robot_console_printf("OBjLBoundary=%f",m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].LBoundaryAngle);
				robot_console_printf("  OBjRBoundary=%f\n",m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].RBoundaryAngle);*/
				AngTobeCheck=(int)m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].LBoundaryAngle+1;///<AngTobeCheck指向下一个
			}
			else
			{
			/*	robot_console_printf("--AngTobeCheck=%d\n",AngTobeCheck);
				robot_console_printf("OBjLBoundary=%f",m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].LBoundaryAngle);
				robot_console_printf("OBjAngle=%f",m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].angle);
				robot_console_printf("  OBjRBoundary=%f\n",m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].RBoundaryAngle);*/

				AngTobeCheck=(int)m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].RBoundaryAngle-1;///<AngTobeCheck指向下一个
			}
			/*AngTobeCheck=(int)m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].LBoundaryAngle+1;///<AngTobeCheck是当前扫描到的角度，移位到新的位置*/
		}
		else
		{
			if (SearchInLeftOrRightPlaneFlag>0)//+方向
			{
				AngTobeCheck=(int)m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].RBoundaryAngle+1;///<AngTobeCheck不可能过界
			}
			else
			{
				AngTobeCheck=(int)m_aObstacleStruct[m_nFreRoadMap[AngTobeCheck]-1].LBoundaryAngle-1;///<AngTobeCheck指向下一个
			}				
		}
		/*robot_console_printf("LeftOrRight=%d,AngTobeCheck=%d\n",SearchInLeftOrRightPlaneFlag,AngTobeCheck);
		robot_console_printf("ObNum=%d",m_nFreRoadMap[AngTobeCheck]);*/
		/*现改为挨个角度搜索*/
		//if (SearchInLeftOrRightPlaneFlag>0)//+方向
		//{
		//	AngTobeCheck++;///<AngTobeCheck指向下一个
		//}
		//else
		//{
		//	AngTobeCheck--;///<AngTobeCheck指向下一个
		//}
	}
	CheckAngleLimitedBeyond(AngTobeCheck,SearchInLeftOrRightPlaneFlag);
	DisBetweenTarA2SelfA=int(m_iTargetAngle-AngTobeCheck);//不可取绝对值
	//CheckAngleLimitedBeyond(AngTobeCheck,SearchInLeftOrRightPlaneFlag);
	return false;
	
}
int ApfPathPlan::SearchNearestDis()
{
	//考虑更改搜索方式，不再取巧，挨个角度搜索，放到扫描类实现
		int NearestObstacleIndex=0;//搜到的最近障碍物标号，从1开始，为m_nFreRoadMap[i]
		m_dMinObstalceDis=1000000;//最小距离初始值取1000;bushi目标距离
		int i=m_iSelfOrientation,j=m_iSelfOrientation;
		int DisBetweenTarA2SelfA=int(m_iTargetAngle-m_iSelfOrientation);//当前朝向与目标角间的差值
	// 	robot_console_printf("DisBetweenTarA2SelfA=%d\n",DisBetweenTarA2SelfA);
		if ((DisBetweenTarA2SelfA>=0&&DisBetweenTarA2SelfA<180)||DisBetweenTarA2SelfA<-180)//目标处于自己当前朝向的角度的左边DisBetweenTarA2SelfA<-180是为当朝向反向准备的
		{
			//下面DisBetweenTarA2SelfA将暂存当前搜索角与目标角之差，与前面的内容无关
			while ((DisBetweenTarA2SelfA>=0&&DisBetweenTarA2SelfA<180)||DisBetweenTarA2SelfA<-180) //相对目标角，当前角在右平面，
				//这种循环方式的前提是，在目标角与当前朝向之间，无非常大的障碍物使得当前角直接跨越了右平面．
			{			
				if (CheckCurAngFindNexAng(i,NearestObstacleIndex,DisBetweenTarA2SelfA,1))
				{
					break;
				}				
			}
		}
		else//目标处于自己当前朝向的角度的右边
		{	
			while ((DisBetweenTarA2SelfA<0&&DisBetweenTarA2SelfA>-180)||DisBetweenTarA2SelfA>180) 
			{
				if (CheckCurAngFindNexAng(i,NearestObstacleIndex,DisBetweenTarA2SelfA,-1))
				{
					break;
				}
			}
		}
		return NearestObstacleIndex; 
}

/*********************************************************************
CheckObstacleIsBehind函数用来监测该障碍物是否位于身后而横跨0~360度分界线
输入为该障碍物的角度信息
************************************************************************/
// inline bool ApfPathPlan::CheckObstacleIsBehind(double dLBoundaryAngle,double dCenterAngle,double dRightBoundaryAngle)
// {
// //新添加的横跨360度分界线的判断标准，见GetObstacleList中的说明
// 	if ((dCenterAngle<dLBoundaryAngle||dCenterAngle>dRightBoundaryAngle)||
// 		(fabs(dRightBoundaryAngle-dLBoundaryAngle)>fabs(dRightBoundaryAngle))&&fabs(dCenterAngle-180)>110)
// 	{
// 		return true;
// 	}
// 	else
// 	{
// 		return false;
// 	}
// }
//inline int ApfPathPlan::GetShortDistanceBetween2Angle(int Angle1, int Angle2)
//{
//		int DisOfTarA2SelfA=abs(Angle1-Angle2);
//		if (DisOfTarA2SelfA>180)
//		{
//			DisOfTarA2SelfA=360-DisOfTarA2SelfA;
//		}
//		return DisOfTarA2SelfA;
//}
inline double ApfPathPlan::SelectTargetAngleFromAddAndDec(bool AngleInAdd,bool AngleInDec)
{
	int AngleDisAdd2OriTmp,AngleDisAdd2PreTar,AngleDisAdd2Tar;
	int AngleDisDec2OriTmp,AngleDisDec2PreTar,AngleDisDec2Tar;
	double CostFunAdd,CostFunDec;
	double CostFunMin=999999999,MinCostAngle;
	if (AngleInAdd)
	{
		//robot_console_printf("m_iNumOfTargetAngleAdd=%d",CostFunAdd);
		for (int i=0;i<m_iNumOfTargetAngleAdd;i++)
		{
			AngleDisAdd2OriTmp=GetShortDistanceBetween2Angle(TargetAngleAdd[i],m_iSelfOrientation);
			AngleDisAdd2PreTar=GetShortDistanceBetween2Angle(TargetAngleAdd[i],m_iPreTargetAngle);
			AngleDisAdd2Tar=GetShortDistanceBetween2Angle(TargetAngleAdd[i],m_aPathTarget.angle);
			CostFunAdd=AngleDisAdd2OriTmp*m_dParCostFun_Orient2Angle+m_dParCostFun_Tar2Angle*AngleDisAdd2Tar+
			m_dParCostFun_PreTarAngle2Angle*AngleDisAdd2PreTar;
		/*	robot_console_printf("\n1CostFunAdd=%f",CostFunAdd);
			robot_console_printf("\n1TargetAngleAdd[i]=%d",TargetAngleAdd[i]);*/
			if (CostFunMin>CostFunAdd)
			{
				
				CostFunMin=CostFunAdd;
				MinCostAngle=TargetAngleAdd[i];
			}
		}
	}
	if (AngleInDec)
	{
		for (int i=0;i<m_iNumOfTargetAngleDec;i++)
		{
			AngleDisDec2OriTmp=GetShortDistanceBetween2Angle(TargetAngleDec[i],m_iSelfOrientation);
			AngleDisDec2PreTar=GetShortDistanceBetween2Angle(TargetAngleDec[i],m_iPreTargetAngle);
			AngleDisDec2Tar=GetShortDistanceBetween2Angle(TargetAngleDec[i],m_aPathTarget.angle);
			CostFunDec=AngleDisDec2OriTmp*m_dParCostFun_Orient2Angle+m_dParCostFun_Tar2Angle*AngleDisDec2Tar+
			m_dParCostFun_PreTarAngle2Angle*AngleDisDec2PreTar;	
	/*		robot_console_printf("2CostFunDec=%f",CostFunDec);
			robot_console_printf("2TargetAngleDec[i]=%d\n",TargetAngleDec[i]);*/

			if (CostFunMin>CostFunDec)
			{
				CostFunMin=CostFunDec;
				MinCostAngle=TargetAngleDec[i];
			}
		}
	}
	return MinCostAngle;
	//robot_console_printf("CostFunAdd=%d",CostFunAdd);


}
//inline bool ApfPathPlan::ExpandObstacleRegion(ObjectPolePos *pObstacleList,int indexOfmaObstacleStruct)
//{
//	ObjectPolePos* pObstacleTmp;//暂存指针
//	pObstacleTmp=&m_aObstacleStruct[indexOfmaObstacleStruct];//为了不影响外面传进来的障碍物 内容，在类里另存一份于数组中
//	memcpy(pObstacleTmp,pObstacleList,m_iSizeOfObjectPolePos);
//	double AngleAdd;//膨胀时的角度增量暂存
//	if (pObstacleList->distance-m_dExpandRadius<0)//若将障碍物膨胀后已把自己包进障碍物，即已无法避开
//	{//m_dExpandRadius为物体膨胀半径
//		AngleAdd=90;//2*asin(0.5*m_dExpandRadius/(pObstacleList->distance))*180*ONE_DEVIDE_PI;
//		pObstacleTmp->distance=0.02;
//// 		robot_console_printf("Collising_pObstacleList->distance=%f\n",pObstacleTmp->distance);
//		//return false;
//	}
//	else//正常情况下膨胀
//	{
//		AngleAdd=asin(m_dExpandRadius/(pObstacleList->distance))*180*ONE_DEVIDE_PI;
//		pObstacleTmp->distance=pObstacleList->distance-m_dExpandRadius;
//// 		robot_console_printf("Normal_pObstacleList->distance=%f\n",pObstacleTmp->distance);
//	}
//	
// // robot_console_printf("AngleAdd=%f\n",AngleAdd);
//	if (CheckObstacleIsBehind(pObstacleList->LBoundaryAngle,pObstacleList->angle,pObstacleList->RBoundaryAngle))
//	{
//		pObstacleTmp->LBoundaryAngle+=AngleAdd;
//		pObstacleTmp->RBoundaryAngle-=AngleAdd;
//	}
//	else
//	{	
//		pObstacleTmp->LBoundaryAngle-=AngleAdd;
//		pObstacleTmp->RBoundaryAngle+=AngleAdd;
//	}
//		CheckAngleLimitedBeyond(pObstacleTmp->LBoundaryAngle,0);
//		CheckAngleLimitedBeyond(pObstacleTmp->RBoundaryAngle,0);
///*万一膨胀后出现左角度大于右角度的情况应该将他们交换*/
//	if (pObstacleTmp->LBoundaryAngle > pObstacleTmp->RBoundaryAngle)
//	{
//		AngleAdd=pObstacleTmp->LBoundaryAngle;
//		pObstacleTmp->LBoundaryAngle=pObstacleTmp->RBoundaryAngle;
//		pObstacleTmp->RBoundaryAngle=AngleAdd;
//	}
//	return true;
//}
//////////////////////////////////////////////////////////////////////////
123

⌨️ 快捷键说明

复制代码 Ctrl + C
搜索代码 Ctrl + F
全屏模式 F11
切换主题 Ctrl + Shift + D
显示快捷键 ?
增大字号 Ctrl + =
减小字号 Ctrl + -