apfpathplan.cpp

路径规划源程序

// ApfPathPlan.cpp: implementation of the ApfPathPlan class.
//
//////////////////////////////////////////////////////////////////////
//#include "stdafx.h"
#include "ApfPathPlan.h"

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
//extern double NewCoordCenterX;
//extern double NewCoordCenterZ;
//extern double m_dAngleChange;///<上一次自己的朝向与这一次自己的朝向之差
//using namespace personal_ShareObject_pathPlanUsingSpace;
template <class type>
bool CheckAngleLimitedBeyond(type & Angle,int Add_Pos_Dec_Neg)
{
	//=180或-180度均不超界
	if (Add_Pos_Dec_Neg==1) //->当Add_Pos_Dec_Neg==1时限幅同时任一方向过界均报错
	{
		if (Angle>360) //->i，j越界处理
		{
			Angle=Angle-360;
			return true;//->返回过界标志
			//	AfxMessageBox("角度>180范围");
		}
		else if (Angle<0) 
		{
			Angle=360+Angle;
			return true;//->返回过界标志
			//		AfxMessageBox("角度<-180范围");
		}
		else
		{return false;}

	}
	else if(Add_Pos_Dec_Neg==0)//->当Add_Pos_Dec_Neg==0时，表示不再区分从哪个方向过界，仅将输入的角度限制在-180~180
	{
		if (Angle>360)//->i，j越界处理
		{
			Angle=Angle-360;
			return true;//->返回过界标志
		}//->错误报警			
		else if (Angle<0) 
		{
			Angle=360+Angle;
			return true;//->返回过界标志
		}
		else
		{
			return false;//->未过界
		}
	}		
	else if (Add_Pos_Dec_Neg>0)//->假如是检测正方向是否过界
	{
		if (Angle>360) //->i，j越界处理
		{
			Angle=Angle-360;
			return true;//->返回过界标志
		}
		else if (Angle<0) 
		{

			// 				Angle=360+Angle;//AfxMessageBox("+方向搜索的角度<-180范围");
			// 				return true
		}
		else
		{
			return false;//->未过界
		}
	}
	else//->Add_Pos_Dec_Neg<0假如是检测-方向是否过界
	{
		if (Angle>360)//->i，j越界处理
		{//AfxMessageBox("-方向搜索的角度>180范围");
		}//->错误报警			
		else if (Angle<0) 
		{
			Angle=360+Angle;
			return true;//->返回过界标志
		}
		else
		{
			return false;//->未过界
		}
	}
}
ApfPathPlan::ApfPathPlan():m_const_Pi(3.1415926),m_const_dTransDegree2arc(3.1415926/180),
							m_const_dTransarc2Degree(180/3.1415926),m_const_dRobotWidth2(0.5)
{
	m_dMaxAngleSpeedWithBall=MAX_ANGLE_SPEED_WITH_BALL;
	m_dMaxAngleSpeedNoBall=MAX_ANGLE_SPEED_NO_BALL;
	m_dMidLineSpeed=HIGH_CENTER_LINE_SPEED;
	m_dMidLineSpeed=MID__CENTER_LINE_SPEED;
	m_dSlowLineSpeed=SLOW__CENTER_LINE_SPEED;
//	m_nSharpTurnThresAngle=SHARP_TURN_ANGLEY;//急转的判断角
	//m_dSafeWidth=SAFE_WIDTH;
	//m_dSafeWidthRich=m_dSafeWidth*0.01;
	//m_dExpandRadius=0.5*(m_dSafeWidth+m_dSafeWidthRich);
// 	parB=PAR_ANGLE_SPEED;//0.07
//	memset(m_apObstaclePointerContainer,0,sizeof(m_apObstaclePointerContainer));
//	m_dAngleSpeedPar=m_const_dRobotWidth2;
	m_iSizeOfObjectPolePos=sizeof(ObjectPolePos);
	m_dLineVPre=0;///<上一帧的线速度
	m_dDangerDegree=0;///<风险值
	m_iPreTargetAngle=0;
	m_dParCostFun_Orient2Angle=2;
	m_dParCostFun_PreTarAngle2Angle=2;
	m_dParCostFun_Tar2Angle=6;
	m_dSpeedCalcuPar=1*m_const_dRobotWidth2;//0.85是在外部调试时试凑出来的系数，为避免重复计算，纳入这里
	//memset(m_nPrem_fFinalFreRoad,0,sizeof(m_nPrem_fFinalFreRoad));
	m_iPreObstacleNumAfterCheck=0;
}

ApfPathPlan::~ApfPathPlan()
{

}

/**
* FreeRoad函数用来寻找当前机器人面前可以通过的安全方向
*
* FreeRoad函数包括如下两部分:
* 将传入的障碍物信息经过有效性判断后写入矩阵m_nFreRoadMap中（成员变量），该矩阵361行2列：
*      第180+i行存的是机器人坐标系中第i度方向的障碍物信息。
*      0列存的是0或1，表示该方向是否有障碍物：0为无，1为有。
*      1列存的是距离值，表示该方向的障碍物离自己的距离值。
*  
* @param pObstacleList   参数pObstacleList是一个障碍物链表的指针(ObjectPolePos型指针)，链表中的每一个障碍物信息包括：
*      该障碍物的极坐标，左右边界角度。
* @param nObstacleMaxNum 参数nObstacleMaxNum为障碍物个数。
* @param Target         参数Target为一个指向目标信息的指针(ObjectPolePos型指针)必须包括目标点的极坐标
* @param bControlBall    参数bControlBall表示自己是否控到球,true为控到.
* @param BoundaryClose  参数BoundaryClose表示自己是否接近场地边界,true为接近边界.
* @param uSpeedLevel123_High 参数uSpeedLevel123_High为无符整型,取值为:1,2,3.分别表示机器人质心移动线速度的档位:
*       1,为低速档,2为中速,3为高速.
*
* @return 传回BOOL型返回值表示自己是否成功找到合适的方向供前进,无则返回false,此时结果成员变量m_dLeftSpeed,m_dRightSpeed
*中放着上一次的规划结果。
*/ 

bool ApfPathPlan::FreeRoad(ObjectPolePos *pObstacleList,
						   int nObstacleMaxNum,
						   ObjectPolePos* Target,
						   bool bControlBall,
						   unsigned uSpeedLevel123_High,bool LeftForbidden,
						   bool RightForbidden, bool ReverseMoveDirect)
{
	//__asm int 3;
	// robot_console_printf("______TargetDis=%f\n",Target->distance);
	if (!Target||Target->distance<0||Target->distance>18||fabs(Target->angle)>360)
	{
//		TRACE("目标导入避撞模块错误");
		Target->angle=(int)Target->angle;
//		robot_console_printf("\nTarget->distance=%f\n",Target->distance);
		return false;
	}///<导入目标信息，检查信息是否有效。
	
//	robot_console_printf("\nAngle=%f\n",Target->angle);
	m_aPathTarget.x=Target->centerX;//m_aPathTarget为存储路径规划目标信息
	m_aPathTarget.y=Target->centerY;//m_aPathTarget为存储路径规划目标信息
	m_aPathTarget.distance=Target->distance;//m_aPathTarget为存储路径规划目标信息
	m_aPathTarget.angle=Target->angle;//m_aPathTarget为存储路径规划目标信息
	if (!ReverseMoveDirect)
	{
		m_iSelfOrientation=90;
	}
	else
	{
		m_iSelfOrientation=270;
	//	robot_console_printf("ReverseMoveDirectm_iSelfOrientation=%f\n",m_iSelfOrientation);
	}
	SetRoadMap(pObstacleList,nObstacleMaxNum,m_aPathTarget.distance);
	///获得目标角度与自身转到该角度所能碰到的最近障碍物距离///

//	AddFreeRoadForDynamic();
//	CheckAllDynamicObstacle();
 	
	if (!SearchFreeTargetAngle(LeftForbidden,RightForbidden)) 
	{
	
		m_iTargetAngle=0;
		return false;
 	} 
	//__asm int 3;
  	//robot_console_printf("m_iTargetAngle=%d\n",m_iTargetAngle);
	int IndexOfNearestObstacle=SearchNearestDis();
//  robot_console_printf("	SearchNearestDisOK\n");
	//SetLRSpeedFreeRoad(bControlBall,BoundaryClose,uSpeedLevel123_High);
// 	robot_console_printf("AvoidanceAngle=%f\n",m_iTargetAngle);
 // 	robot_console_printf("AvoidanceDis=%f\n",m_dMinObstalceDis);
	//robot_console_printf("m_dDangerDegree=%f\n",m_dDangerDegree);
// robot_console_printf("m_dDangerDegree=%f\n",m_dDangerDegree);

	float LineVMax=DecideCurrentLinearVParm(uSpeedLevel123_High);
	CalCulateDanger(IndexOfNearestObstacle,LineVMax);
	double DisOfTarA2SelfA=GetShortDistanceBetween2Angle(m_iTargetAngle,m_iSelfOrientation);
	
	m_cFuzzyControlMod.ProcessFuzzyControl(DisOfTarA2SelfA,m_dDangerDegree);//->模糊控制依据前面的结果规划出速度。
	SetLRSpeedFreeRoad(bControlBall,m_cFuzzyControlMod.LineSpeedOutPut,m_cFuzzyControlMod.CornerSpeedOutPut,uSpeedLevel123_High);
// 	robot_console_printf("SetLRSpeedFreeRoadOK\n");
	return true;
}
float ApfPathPlan::CalCulateDanger(int IndexOfObstacle, float LineVMax)
{

	if (IndexOfObstacle==0)//没有威胁
	{
		m_dDangerDegree=0;
		return 0;
	}
	if (m_dMinObstalceDis<0.2)//计算风险函数
	{
		m_dDangerDegree=1;
	}
	else
	{
		//m_dDangerDegree=0.25*(4-m_dMinObstalceDis-0.3+m_dLineVPre*0.2);
		m_dDangerDegree=0.8*(4-m_dMinObstalceDis)/4+m_dLineVPre*0.2/LineVMax;
	}
	m_dDangerDegree=m_dDangerDegree>0?m_dDangerDegree:0;
	m_dDangerDegree=m_dDangerDegree<1?m_dDangerDegree:1;
	return m_dDangerDegree;
}
/**
* SetLRSpeedFreeRoad函数用来依据前面寻到的目标方向等计算建议转速
*
* SetLRSpeedFreeRoad函数的计算依据是：在给定质心移动线速度的情况下，依据目标角度、最近障碍物距离等
*信息计算合适的角速度使机器人的轨迹刚好是一条合适的弧线到达目标点（无障碍情况）或者擦着最近的障碍物
*转到目标角度m_iTargetAngle。因此已知机器人运动模型的情况下关键是计算出合适的角速度值,从而确定合适的路径.
* 该函数包括两个角速度计算部分：
*      1 由自定义的系数PAR_ANGLE_SPEED乘以m_iTargetAngle得到一个角速度aaatemp。
*      2 第2个角速度bbbtemp分多种情况：
*         若m_iTargetAngle>m_nSharpTurnThresAngle（自定义的急转角度阈值）则直接取m_dMaxAngleSpeedNoBall或m_dMaxAngleSpeedWithBall（看是否控球）
*         否则以路径不碰到最近障碍物计算角速度
*         若m_iTargetAngle>70且目标距离<0.7m则原地转动。
* 计算出角速度后代入机器人运动模型计算出相应左右轮速。     
*  
* @param bControlBall         参数bControlBall是一个表示是否控球的bool型参数，真为控球。
*                        该障碍物的极坐标，左右边界角度。
* @param BoundaryClose       参数BoundaryClose是一个表示是否接近边界的bool型参数，真为接近。（未用到）
* @param uSpeedLevel123_High 参数uSpeedLevel123_High为无符整型,取值为:1,2,3.分别表示机器人质心移动线速度的档位:
*                        1,为低速档,2为中速,3为高速.
*
*/ 
void ApfPathPlan::SetLRSpeedFreeRoad(bool bControlBall,
									 double nlineSpeedCoefficient,
									 double nCornerSpeedCoefficient,
									 unsigned uSpeedLevel123_High)
{