pathplanindynamic.cpp

InnovLabSimu在vc++下实现

//#include "stdafx.h"
#include "PathPlanInDynamic.h"

CPathPlanInDynamic::CPathPlanInDynamic(void)
: m_pObstacleListHead(NULL),m_OBSERVEDATA(2)//,m_mFilterResult(4)
{
	InitFlt();//初始化滤波器,
	m_uTrackedNum=0;
}

CPathPlanInDynamic::~CPathPlanInDynamic(void)
{
}
inline void CPathPlanInDynamic::InitTracker(void)
{
	CObjectTracker::InitFlt();
}
bool CPathPlanInDynamic::SetLocalDynamicMap(int numOb,PthPlSpa::ObjectPolePos* pObstacle,STATE_STYLE FilterResult,double &TimeStep)//,float ObjectDisplace
{
	if (AlterObstaclePosOnVel(pObstacle, FilterResult,TimeStep))//依据速度移动物体的位置,ObjectDisplace
	{
		AddObject2RoadMap(pObstacle,numOb,m_aPathTarget.distance);//将考虑速度后的物体加入到地图中
	}	
	else//若物体远离,无需写入地图,需注意,此时还需要将该障碍物保存到m_aObstacleStruct中去,因为这毕竟是一个有效物体,
	//而且要求m_aObstacleStruct中的内容与m_vpConfirmedObject一至，否则会出错
	{
		m_aObstacleStruct.push_back(*pObstacle);
	}
	return true;
}
//本函数中，外部输入的观测数据距离单位为m角度单位为度，由于跟踪器中所用单位分别为毫米、角度（也可能是弧度由USE_DEGREE_ANGLE决定）
//注意: 以ObjecPolePos结构体形式存的所有数据都采用观测数据的单位,而m_mFilterResult的矩阵形式则采用跟踪器中的格式，所以本函数中进行了转换，
void CPathPlanInDynamic::ConstructRoadMap(double dl,double dr,double& TimeStep)
{
	std::vector<int>::iterator pRelateParm=m_viRelateParm.begin();
	int iIndexOfAllConfirmedObstacle=0;//已确认的障碍物，从0开始
	std::vector<STATE_STYLE> SavedFilterResult(m_mFilterResult);
	std::vector<VARIANCE>SavedFilterResultVar(m_mFilterResultVar);
	std::vector<VARIANCE>SavedModelNoiseVar(m_mModelNoiseVar);
	std::vector<CAdaptiveOptimaser>SavedAdaptiveOptimizer(m_vAdaptiveOptimizerVec);
	//std::vector<LARGE_INTEGER>SavedPreSystemCountNum(m_vPreSystemCountNumVec);
//清空上一轮保存的跟踪状态，并重新保存，以确保本轮跟踪中没有用到的物体的状态被去除掉
	m_vLastFiltedCenterPos.clear();
	m_mFilterResult.clear();
	m_mModelNoiseVar.clear();
	m_mFilterResultVar.clear();
	m_vAdaptiveOptimizerVec.clear();
	//m_vPreSystemCountNumVec.clear();
	m_aObstacleStruct.clear();

	memset(m_nFreRoadMap,0,sizeof(m_nFreRoadMap));
//	memset(m_apObstaclePointerContainer,0,sizeof(m_apObstaclePointerContainer));
	m_uTrackedNum=0;
#ifdef DEBUG_ON_VC_OUT_PUT_MORE
		robot_console_printf("Confirmed Num=%d",m_vpConfirmedObject.size());
#endif
//robot_console_printf("m_vpConfirmedObject.size()=%d\n",m_vpConfirmedObject.size());


	while (iIndexOfAllConfirmedObstacle<m_vpConfirmedObject.size())//对所有确认有效的物体
	{
		//__asm int3;
	/*	robot_console_printf("iIndexOfAllConfirmedObstacle=%d\n",iIndexOfAllConfirmedObstacle);
		robot_console_printf("m_viRelateParm.size()=%d\n",m_viRelateParm.size());*/
//#ifdef OUT_PUT_OBSERVE_ONLY
//		m_OBSERVEDATA(0)=m_vpConfirmedObject[iIndexOfAllConfirmedObstacle]->distance*1000;//当前已找到上一时刻位置项的数据进入滤波跟踪
//		m_OBSERVEDATA(1)=TransAngleToArc(m_vpConfirmedObject[iIndexOfAllConfirmedObstacle]->angle);//滤波器的输入都是弧度
//		robot_console_printf("%f,%f,%f,%f,%f,\n",m_OBSERVEDATA(0),m_OBSERVEDATA(1),dl,dr,TimeStep);
//		//robot_console_printf("=%f\n",m_OBSERVEDATA(1));
//#endif
		
		if (m_viRelateParm.size()>0)//找到过相关项
		{
			if ((*pRelateParm)!=0)//确实在前一时刻的结果中找到了自己的位置则:滤波,保存对应状态,地图中置动态障碍物
			{
				//m_viRelateParm中的值从1开始,而下标从0开始故需要-1
				SetFltState(SavedFilterResult[*pRelateParm-1],SavedFilterResultVar[*pRelateParm-1],SavedModelNoiseVar[*pRelateParm-1]);//将上一个时刻该物体的滤波器状态恢复
				m_OBSERVEDATA(0)=m_vpConfirmedObject[iIndexOfAllConfirmedObstacle]->distance*1000;//当前已找到上一时刻位置项的数据进入滤波跟踪
				m_OBSERVEDATA(1)=TransAngleToArc(m_vpConfirmedObject[iIndexOfAllConfirmedObstacle]->angle);//滤波器的输入都是弧度

				//__asm int 3;
				//RunTrack(dl,dr,m_OBSERVEDATA);//跟踪
				RunTrack(dl,dr,m_OBSERVEDATA,TimeStep,&(SavedAdaptiveOptimizer[*pRelateParm-1]));//跟踪
				m_uTrackedNum++;
				/////保存滤波器状态滤出来的角直接是度了///
				//my_filter.m_mStateMeanResult(1)=TransAngleToDegree(my_filter.m_mStateMeanResult(1));//
				//my_filter.m_mStateMeanResult(3)=TransAngleToDegree(my_filter.m_mStateMeanResult(3));
				/*robot_console_printf("Dis=%f,Angle=%f\n",my_filter.m_mStateMeanResult(0),my_filter.m_mStateMeanResult(1));
				robot_console_printf("LV=%f,AV=%f\n",my_filter.m_mStateMeanResult(2),my_filter.m_mStateMeanResult(3));*/
				m_mFilterResult.push_back(my_filter.m_mStateMeanResult);//跟踪结果挨个存入注意其中的距离单位是毫米
				m_mFilterResultVar.push_back(my_filter.m_mStateCov);//跟踪结果的方差挨个存入
				m_mModelNoiseVar.push_back(m_cAdaptiveOptimiser.m_mNewModelNoise);//自适应跟踪后,模型噪声挨个存入
				m_vAdaptiveOptimizerVec.push_back(SavedAdaptiveOptimizer[*pRelateParm-1]);//将用到的自适应器保存起来，没用到的将去掉
				//m_vPreSystemCountNumVec.push_back(m_LICurrentTimeCount);//将对当前障碍物信息的保存
				PthPlSpa::ObjectPolePos DynamicObjPosTmp;
				DynamicObjPosTmp.distance=my_filter.m_mStateMeanResult(0)/1000;//要除1000
				DynamicObjPosTmp.angle=my_filter.m_mStateMeanResult(1);
				m_vLastFiltedCenterPos.push_back(DynamicObjPosTmp);//将结果另存一份格式,todo:考虑只存一分，
				//AlterObstaclePosOnVel(m_vpConfirmedObject[iIndexOfAllConfirmedObstacle],my_filter.m_mStateMeanResult);
				//float DisDisplace=my_filter.m_mStateMeanResult(0)-SavedFilterResult[*pRelateParm-1](0);//这里的单位是毫米,而下面用时单位是米所以需除1000
				SetLocalDynamicMap(iIndexOfAllConfirmedObstacle,m_vpConfirmedObject[iIndexOfAllConfirmedObstacle],my_filter.m_mStateMeanResult,TimeStep);//,DisDisplace/1000
			}
			else////在前一时刻的结果中找不到自己的位置则:地图中置静态障碍物,保存当前的位置,以及各方差为初始值
			{
				AddObject2RoadMap(m_vpConfirmedObject[iIndexOfAllConfirmedObstacle],iIndexOfAllConfirmedObstacle,m_aPathTarget.distance);
				STATE_STYLE SudenObjStateTmp(my_filter.m_mStateMeanResult.size());
				SudenObjStateTmp(0)=m_vpConfirmedObject[iIndexOfAllConfirmedObstacle]->distance*1000;
				SudenObjStateTmp(1)=m_vpConfirmedObject[iIndexOfAllConfirmedObstacle]->angle;//注意保存时不需要转化为角度
				SudenObjStateTmp(2)=0;
				SudenObjStateTmp(3)=0;//注意保存时不需要转化为角度
	
				m_mFilterResult.push_back(SudenObjStateTmp);//跟踪结果挨个存入
				m_mFilterResultVar.push_back(m_vInitStateCov);
				m_mModelNoiseVar.push_back(m_vInitModelNoise);
				m_vLastFiltedCenterPos.push_back(*m_vpConfirmedObject[iIndexOfAllConfirmedObstacle]);//将结果另存一份格式,todo:考虑只存一个
				CAdaptiveOptimaser NewAdaptiveOptimizer;
				NewAdaptiveOptimizer.initAdapter(m_cAdaptiveOptimiser.m_mNodelNoiseOriginalVal,m_cAdaptiveOptimiser.m_fStableDifAmplitudeParm);
				m_vAdaptiveOptimizerVec.push_back(NewAdaptiveOptimizer);//创建自适应调节器
				//LARGE_INTEGER LITmp;
				//QueryPerformanceCounter(&LITmp);
				//m_vPreSystemCountNumVec.push_back(LITmp);//每发现上一时刻位置则将当前时间点记下;
			}
			pRelateParm++;//注意必须与iIndexOfAllConfirmedObstacle一致且同步变化
			iIndexOfAllConfirmedObstacle++;
		}
		else//根本就没有搜到相关项,一开始会出现这种情况
		{
			CAdaptiveOptimaser NewAdaptiveOptimizer;
			NewAdaptiveOptimizer.initAdapter(m_cAdaptiveOptimiser.m_mNodelNoiseOriginalVal,m_cAdaptiveOptimiser.m_fStableDifAmplitudeParm);
			m_vAdaptiveOptimizerVec.push_back(NewAdaptiveOptimizer);//创建自适应调节器
			AddObject2RoadMap(m_vpConfirmedObject[iIndexOfAllConfirmedObstacle],iIndexOfAllConfirmedObstacle,m_aPathTarget.distance);
			STATE_STYLE SudenObjStateTmp(my_filter.m_mStateMeanResult.size());
			SudenObjStateTmp(0)=m_vpConfirmedObject[iIndexOfAllConfirmedObstacle]->distance*1000;
			SudenObjStateTmp(1)=m_vpConfirmedObject[iIndexOfAllConfirmedObstacle]->angle;//注意保存时不需要转化为角度
			SudenObjStateTmp(2)=0;
			SudenObjStateTmp(3)=0;//注意保存时不需要转化为角度
			m_mFilterResult.push_back(SudenObjStateTmp);//跟踪结果挨个存入
			m_mFilterResultVar.push_back(m_vInitStateCov);
			m_mModelNoiseVar.push_back(m_vInitModelNoise);
			m_vLastFiltedCenterPos.push_back(*m_vpConfirmedObject[iIndexOfAllConfirmedObstacle]);//将结果另存一份格式,todo:考虑只存一个
			//LARGE_INTEGER LITmp;
			//QueryPerformanceCounter(&LITmp);
			//m_vPreSystemCountNumVec.push_back(LITmp);//每发现上一时刻位置则将当前时间点记下;
			iIndexOfAllConfirmedObstacle++;
		}
	}
	//robot_console_printf("m_aObstacleStruct.size()=%d\n",m_aObstacleStruct.size());
}
#define LESS_DANGER_LEAVE_SPEED_MIN_TH 4//不可忽略但物体在远离危险较小的线速度阈值幅度
double CPathPlanInDynamic::CalCulateDanger(int IndexOfObstacle)
{
	if (IndexOfObstacle==0)//没有威胁
	{
		m_dDangerDegree=0;
		return 0;
	}
	double ObstacleLinVelocity=m_mFilterResult[IndexOfObstacle-1](2);//注意单位是毫米/时间段
	if (m_dMinObstalceDis<0.2&&ObstacleLinVelocity>-LESS_DANGER_LEAVE_SPEED_MIN_TH)//计算风险函数,不可忽略,但是物体在远离
	{
		m_dDangerDegree=1-ObstacleLinVelocity/MIN_LEAVE_SPEED_FOR_IGNORE;//注意有符号
		
	}
	else if (m_dMinObstalceDis<0.2)
	{
		m_dDangerDegree=1;
	}
	else
	{
		//robot_console_printf("NormalVelObstacle,Danger=%f\n",m_dDangerDegree);
		m_dDangerDegree=0.25*(4-m_dMinObstalceDis-ObstacleLinVelocity/MIN_LEAVE_SPEED_FOR_IGNORE);//后一项考虑到了线速度的影响
	//robot_console_printf("NormalVelObstacle,Danger=%f\n",m_dDangerDegree);
	}
	m_dDangerDegree=m_dDangerDegree>0?m_dDangerDegree:0;
	m_dDangerDegree=m_dDangerDegree<1?m_dDangerDegree:1;
	//if (m_dDangerDegree<0.001)
	//{
	//	__asm int 3;
	//}
	return m_dDangerDegree;
}
bool CPathPlanInDynamic::PathPlanMov(PthPlSpa::ObjectPolePos *pObstacleList,
				 int nObstacleMaxNum,
				 PthPlSpa::ObjectPolePos* Target,
				 bool bControlBall,
				 unsigned uSpeedLevel123_High,bool LeftForbidden,
				 bool RightForbidden, bool ReverseMoveDirect,
				 double dl,double dr,
				 double& TimeStep)
{

	if (!Target||Target->distance<0||Target->distance>18||fabs(Target->angle)>360)
	{
		Target->angle=(int)Target->angle;
			return false;
	}///<导入目标信息，检查信息是否有效。
	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为存储路径规划目标信息
	m_dLengthPerTimeStep=8;//(0.5*(dl+dr));//自身本段时间内移动的距离，代表了自身移动速度500mm*1000/32
	/*robot_console_printf("LengthPerTimeStep=%f",m_dLengthPerTimeStep);*/
	if (!ReverseMoveDirect)
	{
		m_iSelfOrientation=90;
	}
	else
	{
		m_iSelfOrientation=270;
		//	robot_console_printf("ReverseMoveDirectm_iSelfOrientation=%f\n",m_iSelfOrientation);
	}

	m_pObstacleListHead=pObstacleList;
//	SetRoadMap(pObstacleList,nObstacleMaxNum);
///**假设性暂时用第一个障碍物实验***/
//	m_OBSERVEDATA(0)=m_apObstaclePointerContainer[1]->distance;
//	m_OBSERVEDATA(1)=m_apObstaclePointerContainer[1]->angle;
//	RunTrackInDyn(dl,dr,m_OBSERVEDATA);//跟踪
//	m_mFilterResult=my_filter.m_mStateMeanResult;
	//if (nObstacleMaxNum>15)//障碍物最大15个
	//{
	//	return false;
	//}
	if(FindRelatedPosInLastStep(pObstacleList,nObstacleMaxNum))//寻找前一时刻自身的位置
	{
		//__asm int 3;
		ConstructRoadMap(dl,dr,TimeStep);		
	}
	else //一个都没有找到则是静态地图
	{
		//__asm int 3;
		ConstructRoadMap(dl,dr,TimeStep);
		//ApfPathPlan::SetRoadMap(pObstacleList,nObstacleMaxNum);
	}
	//__asm int 3;
	if (!SearchFreeTargetAngle(LeftForbidden,RightForbidden)) 
	{
		m_iTargetAngle=0;
		return false;
	}
	//robot_console_print("SearchTargeAngleComplete\n");
	int NearestObstacleIndex=SearchNearestDis();
	//robot_console_print("SearchNearDisComplete\n");
	//__asm int 3;
	CalCulateDanger(NearestObstacleIndex);
#ifdef DEBUG_ON_VC_OUT_PUT_MORE
	robot_console_printf("m_iTargetAngle=%d",m_iTargetAngle);
	robot_console_printf("m_dDangerDegree=%f",m_dDangerDegree);
#endif
robot_console_printf("Danger=%f\n",m_dDangerDegree);
	double DisOfTarA2SelfA=GetShortDistanceBetween2Angle(m_iTargetAngle,m_iSelfOrientation);
	m_cFuzzyControlMod.ProcessFuzzyControl(DisOfTarA2SelfA,m_dDangerDegree);//->模糊控制依据前面的结果规划出速度。
	SetLRSpeedFreeRoad(bControlBall,m_cFuzzyControlMod.LineSpeedOutPut,m_cFuzzyControlMod.CornerSpeedOutPut,uSpeedLevel123_High);
	return true;
}
void CPathPlanInDynamic::RunTrackInDyn(double dl,double dr,OBSERVE_STYLE& OBSERVEDATA)
{
//	RunTrack(dl,dr,OBSERVEDATA);
}
//bool CPathPlanInDynamic::AddObject2RoadMap(PthPlSpa::ObjectPolePos *pObstacleList,int numOb)
//{
//	//int numOb;//障碍物的编号
//	//numOb=0;//0表示没障碍物
//	//int ObsObsoleteNum=0;
//	PthPlSpa::ObjectPolePos * pObstacleAferExpand;
//	if (ExpandObstacleRegion(pObstacleList))
//	{
//		//	robot_console_printf("m_aObstacleStruct[indexOfmaObstacleStruct].angle=%f\n",m_aObstacleStruct[m_iObstacleNumAfterCheck].angle);
//
//		pObstacleAferExpand=&m_aObstacleStruct[m_iObstacleNumAfterCheck];
//		m_apObstaclePointerContainer[numOb+1]=pObstacleAferExpand;//>将有效障碍物的指针保存，同时该障碍物在m_apObstaclePointerContainer中的编号即numOb+1
//		m_iObstacleNumAfterCheck++;
//	}
//	else//若发现膨胀失败后障碍物把自己包进去了则仍然用未经膨胀的指针而不再用膨胀后存放的数组m_aObstacleStruct
//	{
//		pObstacleAferExpand=pObstacleList;
//		m_apObstaclePointerContainer[numOb+1]=pObstacleList;
//	}
//
//	//按障碍物面积从小到大的顺序排列它与目标的夹角
//	if (CheckObstacleIsBehind(pObstacleAferExpand->LBoundaryAngle,pObstacleAferExpand->angle,pObstacleAferExpand->RBoundaryAngle))
//		//>障碍物在身后横跨360度分界线
//	{
//		/*对横跨在背后的障碍物，其信息写入时
//		*要分两批写入,第一批:j从RBoundaryAngle加到360度
//		*第二批:j从LBoundaryAngle减到0度*/
//		//	robot_console_printf("BehindObstacleNum=%d\n",numOb);
//		int j=0;
//		for (j=(int)pObstacleAferExpand->RBoundaryAngle;j<=360;j++)
//			//第一批写入信息pObstacleAferExpand->RBoundaryAngle为大的角度也是0~180,
//		{
//			//9.16日添加的修改，考虑障碍物的区域有相重的话取近的一个
//			if ((int)m_nFreRoadMap[j]!=0)//已经有过障碍物在该角度赋过值
//			{
//				if (pObstacleAferExpand->distance<m_apObstaclePointerContainer[m_nFreRoadMap[j]]->distance)//新的障碍物更近，需要更新的话
//				{
//					m_nFreRoadMap[j]=numOb+1;//>在该角度方向存下障碍物的编号
//				}//新的更远的话则不更新m_nFreRoadMap;
//			}
//			else//第一次赋
//			{		
//				m_nFreRoadMap[j]=numOb+1;//>在该角度方向存下障碍物的编号，-180对应m_nFreRoadMap[361]中的第0号数据（第1个），180度对应第360号（第361个）
//			}
//			//				robot_console_printf("AngleOfObstacleNumIsBehind=%d",j);
//			//				robot_console_printf("   NumberOfObstacleNumIsBehind=%d\n",numOb);
//
//		}
//		for (j=(int)pObstacleAferExpand->LBoundaryAngle;j>=0;j--)//>障碍物在后面时左半边的写入。pObstacleAferExpand->LBoundaryAngle也是-180~180
//			//第二批写入信息pObstacleAferExpand->LBoundaryAngle为大的角度也是-180~0,
//		{
//			if ((int)m_nFreRoadMap[j]!=0)//已经有过障碍物在该角度赋过值
//			{
//				if (pObstacleAferExpand->distance<m_apObstaclePointerContainer[m_nFreRoadMap[j]]->distance)//新的障碍物更近，需要更新的话
//				{
//					m_nFreRoadMap[j]=numOb+1;//0对应m_nFreRoadMap[361]中的第0号数据（第1个），180度对应第360号（第361个）
//				}//新的更远的话则不更新m_nFreRoadMap
//			}