pathplanindynamic.cpp

InnovLabSimu在vc++下实现

//			else//第一次赋
//			{		
//				m_nFreRoadMap[j]=numOb+1;//该角度方向的障碍物为numOb+1号				
//			}
//			//				robot_console_printf("AngleOfObstacleNumIsBehind=%d",j);
//			//				robot_console_printf("   NumberOfObstacleNumIsBehind=%d\n",numOb);
//
//		}			
//	}
//	else//>障碍物并非横跨后方左右时
//	{
//		for (int j=(int)pObstacleAferExpand->LBoundaryAngle;j<=pObstacleAferExpand->RBoundaryAngle;j++)
//		{
//			if ((int)m_nFreRoadMap[j]!=0)//已经有过障碍物在该角度赋过值
//			{
//				//		robot_console_printf("!!!!!!m_nFreRoadMap[180+j]=%d,j=%d\n",m_nFreRoadMap[180+j],j);
//
//				if (pObstacleAferExpand->distance<m_apObstaclePointerContainer[m_nFreRoadMap[j]]->distance)//新的障碍物更近，需要更新的话
//				{
//
//					m_nFreRoadMap[j]=numOb+1;//-180对应m_nFreRoadMap[361]中的第0号数据（第1个），180度对应第360号（第361个）
//				}//新的更远的话则不更新m_nFreRoadMap
//			}
//			else//第一次赋
//			{		
//				m_nFreRoadMap[j]=numOb+1;
//			}
//		}
//	}
//	return true;
//}
bool CPathPlanInDynamic::SetRoadMap(PthPlSpa::ObjectPolePos *pObstacleList,int nObstacleMaxNum)
{
	memset(m_nFreRoadMap,0,sizeof(m_nFreRoadMap));
	int numOb;//障碍物的编号
	numOb=0;//0表示没障碍物

	
	return false;
}
bool CPathPlanInDynamic::AlterObstaclePosOnVel(PthPlSpa::ObjectPolePos* pObstacle,STATE_STYLE& FilterResult,double &TimeStep)//,float ObjectDisplace
{
#ifdef  DEBUG_USE_FLAG
	ViewVector(FilterResult);
	m_mSigma_Viewer;
#endif
#ifdef DEBUG_ON_VC_OUT_PUT_MORE
	robot_console_printf("FilterResult(3)=%f",FilterResult(3));
	robot_console_printf("FilterResult(2)=%f",FilterResult(2));
	robot_console_printf("FilterResult(1)=%f",FilterResult(1));
	robot_console_printf("FilterResult(0)=%f",FilterResult(0));
	robot_console_printf("pObstacle->angle=%f",pObstacle->angle);
#endif

	if (FilterResult(2)>MIN_LEAVE_SPEED_FOR_IGNORE)//物体高速远离可以忽略
	{
		return false;
	}
	else
	{
	//	double DisInFiltedAngleAndRawAngle=GetAngulardisplacementIn2Angle(pObstacle->angle,FilterResult(1));//滤波后的中心与原始数据中心的角度差
		double Displace=0;
		float ArriveTime=0;
		if (fabs(m_dLengthPerTimeStep)>0.0001)//机器人移动过位置ObjectDisplace为负表示接近
		{
#ifdef CONSIDER_TIME_IN_VEL
	ArriveTime=FilterResult(0)/m_dLengthPerTimeStep*TimeStep*0.001;//预计到达时间单位为秒
	//robot_console_printf("TimeStep=%f\n",TimeStep);
#else
	ArriveTime=FilterResult(0)/m_dLengthPerTimeStep;//预计到达时间单位为当前时间步长
#endif
			
			Displace=ArriveTime*FilterResult(3);//暂用自身移动速度来代表接近速度，有正负，当自身倒退时
			//	double arriveTime=FilterResult(0)/FilterResult(3);
			//	double Displace=arriveTime*FilterResult(4);
			//	double DistanceDisplace=arriveTime*FilterResult(3);
			if (Displace<-OBSTACLE_MAX_ALTER_ANGLE)//限幅
			{
				Displace=-OBSTACLE_MAX_ALTER_ANGLE;
			}
			else if (Displace>OBSTACLE_MAX_ALTER_ANGLE)
			{
				Displace=OBSTACLE_MAX_ALTER_ANGLE;
			}	
			//__asm int 3;
			EnlargeObstaclePosOnVel_Probaility(pObstacle,this,ArriveTime);

		}
#ifdef DEBUG_ON_VC_OUT_PUT_MORE
	robot_console_printf("m_dLengthPerTimeStep=%f\n",m_dLengthPerTimeStep);
	robot_console_printf("Displace=%f\n",Displace);
#endif
		//if (Displace>0)//增加方向扩充
		//{
		//	pObstacle->RBoundaryAngle=pObstacle->RBoundaryAngle+Displace;
		//	LimitAngleIn0_360Degree(pObstacle->LBoundaryAngle);
		//}
		//else
		//{
		//	pObstacle->LBoundaryAngle=pObstacle->LBoundaryAngle+Displace;
		//	LimitAngleIn0_360Degree(pObstacle->LBoundaryAngle);
		//}
		return true;
	}
}
#define VAR_CONVERGENT_THRES_PARM 0.1///<用于判断是否收敛的方差系数,若位置方差值小于此系数乘初始值即收敛
bool CPathPlanInDynamic::EnlargeObstaclePosOnVel_Probaility(PthPlSpa::ObjectPolePos* pObstacle,CObjectTracker* pMyTracker,float ArriveTime)
{
#ifdef  DEBUG_USE_FLAG
	ViewMatrix(pMyTracker->my_filter.m_mStateCov);
	m_mSigma_Viewer;
#endif
	const float AngularVInitVar=TransAngleToDegree(pMyTracker->m_vInitStateCov(3,3));
	if (pMyTracker->my_filter.m_mStateCov(3,3)>VAR_CONVERGENT_THRES_PARM*AngularVInitVar)
	{
		//robot_console_printf("AngleVVar=%f\n",pMyTracker->my_filter.m_mStateCov(3,3));
		return false;
	}
	else
	{
		
		GausVaribleAdd(pMyTracker->my_filter.m_mStateMeanResult(1),pMyTracker->my_filter.m_mStateMeanResult(3),
			pMyTracker->my_filter.m_mStateCov(1,1),pMyTracker->my_filter.m_mStateCov(3,3),pMyTracker->my_filter.m_mStateCov(1,3),
			ArriveTime);//获得位置角度与角速度乘时间值相加后的变量分布
		float StdVar=sqrt(m_sGausVaribleAddResult.Variance);
		float Displace=m_sGausVaribleAddResult.Expect-pMyTracker->my_filter.m_mStateMeanResult(1);
		//robot_console_printf("Displace=%f\n",Displace);
		if (StdVar>20)//方差太大，不应按照方差来移动扩充物体
		{
				if (Displace<-OBSTACLE_MAX_ALTER_ANGLE)//限幅
				{
					Displace=-OBSTACLE_MAX_ALTER_ANGLE;
				}
				else if (Displace>OBSTACLE_MAX_ALTER_ANGLE)
				{
					Displace=OBSTACLE_MAX_ALTER_ANGLE;
				}	
				if (Displace>0)//增加方向扩充
				{
					pObstacle->RBoundaryAngle=pObstacle->RBoundaryAngle+Displace;
					LimitAngleIn0_360Degree(pObstacle->LBoundaryAngle);
				}
				else
				{
					pObstacle->LBoundaryAngle=pObstacle->LBoundaryAngle+Displace;
					LimitAngleIn0_360Degree(pObstacle->LBoundaryAngle);
				}
		}
		else
		{
			//__asm int 3;
			GetDisbuteRangeGivenProb(m_sGausVaribleAddResult.Expect,StdVar);
			pObstacle->distance=pMyTracker->my_filter.m_mStateMeanResult(0)*0.001;//位置暂时不动
			pObstacle->angle=m_sGausVaribleAddResult.Expect;//角度用滤波结果+潜在移动代替
			pObstacle->RBoundaryAngle=pObstacle->RBoundaryAngle+Displace+m_fsDisbuteRange[1];//右边界角度为大角加上速度潜在移动,以及总体波动范围的上界影响
			pObstacle->LBoundaryAngle=pObstacle->LBoundaryAngle+Displace+m_fsDisbuteRange[0];//左边界角度为大角加上速度潜在移动,以及总体波动范围的下界影响
			LimitAngleIn0_360Degree(pObstacle->angle);
			LimitAngleIn0_360Degree(pObstacle->RBoundaryAngle);
			LimitAngleIn0_360Degree(pObstacle->LBoundaryAngle);
			if (pObstacle->LBoundaryAngle>pObstacle->RBoundaryAngle)
			{
				float tmp=pObstacle->LBoundaryAngle;
				pObstacle->LBoundaryAngle=pObstacle->RBoundaryAngle;
				pObstacle->RBoundaryAngle=tmp;
			}
		}	
		if (fabs(pObstacle->LBoundaryAngle)>400)
		{
			__asm int 3
		}

		return true;
	}
	
}
std::vector<double> DisVec;///<距离的向量,存上一次物体与当前看到物体的距离
///**
//*@brief 为sort函数准备的比较两个距离的大小
//*/
bool DisCompare(int DisIndex1,int DisIndex2)
{
	return DisVec[DisIndex1-1]<DisVec[DisIndex2-1];
}
bool CPathPlanInDynamic::FindRelatedPosInLastStep(PthPlSpa::ObjectPolePos *pObstacleList,int nObstacleMaxNum)
{
	//robot_console_printf("FindRelationNow\n");
//	__asm int 3;
	if (m_viRelateParm.size()>0)
	{
		m_viRelateParm.clear();//清除所有元素
	}	
	if(!ConfirmAllObstacle(pObstacleList,nObstacleMaxNum))
	{
		//robot_console_printf("ConfirmedNumOfObj=%d",m_vpConfirmedObject.size());
		return false;
	}
	else if (m_vLastFiltedCenterPos.size()<1)
	{
		return false;
	}
	std::vector<PthPlSpa::ObjectPolePos> ::iterator p_LastPos= m_vLastFiltedCenterPos.begin();
	std::vector<PthPlSpa::ObjectPolePos*> ::iterator p_CurrentPos= m_vpConfirmedObject.begin();
	
	std::vector<int> DisIndex;
	std::vector<int>::iterator itDisIndex;
	
	//robot_console_printf("Confirmed\n");
	for (int i=1;i<=m_vLastFiltedCenterPos.size();i++)
	{
		DisIndex.push_back(i);//DisIndex中存的对应的物体号码1开始,1代表的物体是m_vLastFiltedCenterPos中的0下标物体
	}
	while (p_CurrentPos!=m_vpConfirmedObject.end())
	{
		//robot_console_printf("Confirmed,and there is object");
		if (ConfirmTrackableObject(*p_CurrentPos))//确认当前输入的物体是否值得跟踪，若否则当作突然出现的物体不跟踪
		{
			DisVec.clear();
			//DisIndex.clear();
			p_LastPos= m_vLastFiltedCenterPos.begin();
			//robot_console_printf("ConfirmedObjectAngle=%f",(*p_CurrentPos)->angle);
			int i=0;
			while(p_LastPos!=m_vLastFiltedCenterPos.end())//遍历上一次存下来的点
			{
				DisVec.push_back(CalCulateDisIn2Pos(*p_CurrentPos,&(*p_LastPos)));
				p_LastPos++;
			}//DisVec中现在存了当前该物体与前一是刻存的所有物体点的几何距离
			itDisIndex=min_element(DisIndex.begin ( ),DisIndex.end ( ),DisCompare);//上一时刻所有点中找到当前点最近的点的编号，编号1开始
#ifdef DEBUG_ON_VC_OUT_PUT_MORE
			robot_console_printf("RelatedObIndex=%d\n",*itDisIndex);
			robot_console_printf("NearestDis=%f\n",DisVec[(*itDisIndex)-1]);
#endif

			if (DisVec[(*itDisIndex)-1]>SAME_OBSTACLE_MAX_DIS)
			{
				m_viRelateParm.push_back(0);//突然出现的物体,上一次找不到相应项则该项置0值
//				m_vDisplaceOfObject2Self.push_back(0);//将该物体的接近速度存起,此时认为是０
			}
			else
			{
				m_viRelateParm.push_back(*itDisIndex);//找到相应项,存对应的物体号码1开始
				//m_vDisplaceOfObject2Self.push_back((*p_CurrentPos).distance-m_mFilterResult[*itDisIndex-1](0));//将该物体的接近速度存起接近为正

			}
		}
		else
		{
			m_viRelateParm.push_back(0);//不应跟踪，当作突然出现的物体,相应项则该项置0值
			//m_vDisplaceOfObject2Self.push_back(0);//将该物体的接近速度存起,此时认为是０
		}
		
		p_CurrentPos++;
	}	
	int maxIndex=(*max_element(m_viRelateParm.begin(),m_viRelateParm.end()));
	if (maxIndex=0)//一个都没有找到
	{
		return false;
	}
	return true;
}
bool CPathPlanInDynamic::ConfirmAllObstacle(PthPlSpa::ObjectPolePos *pObstacleList,int nObstacleMaxNum)
{
	m_vpConfirmedObject.clear();//删除所有元素
	for (int i=0;i<nObstacleMaxNum;i++) 
	{
		if (!pObstacleList)
		{
			break;
		}
		if (!ConfirmObstacleList(pObstacleList,m_aPathTarget.distance))//>若无效
		{	
			pObstacleList=pObstacleList->next;//去掉无效的
			continue;
		}
		m_vpConfirmedObject.push_back(pObstacleList);
		pObstacleList=pObstacleList->next;
		//numOb++;//>经确认的障碍物个数为numOb+1个
	}
	if (m_vpConfirmedObject.size()<1)
	{
		return false;
	}
	
}
double  CPathPlanInDynamic::CalCulateDisIn2Pos(PthPlSpa::ObjectPolePos *pObstacle1,PthPlSpa::ObjectPolePos *pObstacle2)
{
	double Angle1=TransAngleToArc(pObstacle1->angle);
	double Angle2=TransAngleToArc(pObstacle2->angle);

	double x1=pObstacle1->distance*cos(Angle1);
	double y1=pObstacle1->distance*sin(Angle1);

	double x2=pObstacle2->distance*cos(Angle2);
	double y2=pObstacle2->distance*sin(Angle2);
	return sqrt(pow(x1-x2,2)+pow(y1-y2,2));
}


// 是否值得进行跟踪的判断函数，本函数在FindRelatedPosInLastStep中调用，若输入的为值得跟踪则返回true
bool CPathPlanInDynamic::ConfirmTrackableObject(PthPlSpa::ObjectPolePos *pObstacle)
{
	
	if (pObstacle->angle<0||pObstacle->angle>180) 
	{
		//return false;
	}
	else if(pObstacle->distance>3&&pObstacle->distance<1)
	{		
		return false;//该障碍物无效	
	}
	else
	{
		return true;
	}	
	return true;
}