pathplanindynamic.h

路径规划源程序

#pragma once
#include "ApfPathPlan.h"
#include "ObjectTracker.h"
#include "UncentedKalmanFilt/UKF_COMMAN_HEAD.h"
#include "GuauessProbCalcu.h"
/********************************************************************
	created:	2008/03/14
	created:	14:3:2008   9:53
	filename: 	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005\PathPlanInDynamic.h
	file path:	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005
	file base:	PathPlanInDynamic
	file ext:	h
	author:		GaoYang
	
	purpose:	动态环境中的路径规划类，取代了ApfPathPlan类．其中继承了ApfPathPlan，以及目标跟踪类．
	　　　　　　其中所有的速度都是远离为正接近为负．同目标跟踪类中的定义
    todo:       目前在本类中由两套单位系统,因此带来很多麻烦以及不必要的运算,在ObjectPolePos的结构体中,即输入观测数据中
				距离单位是米,角度单位是度,而在目标跟踪器中所用的单位是:距离单位为毫米,角度单位为度或者弧度.需要统一!!
				另:线速度的定义为远离为正,接近位负
*********************************************************************/
//using namespace personal_ShareObject_pathPlanUsingSpace;
namespace PthPlSpa=personal_ShareObject_pathPlanUsingSpace;
//using namespace personal_ShareObject_pathPlanUsingSpace;

using personal_ShareObject_pathPlanUsingSpace::TargetStyle;
#define OBSTACLE_MAX_ALTER_ANGLE 50
//typedef double[2] OBSTCLE_CENTER;
#define SAME_OBSTACLE_MAX_DIS 1//单位米
#define SUDDEN_OBSTACLE_MIN_DIS 500
#define MIN_LEAVE_SPEED_FOR_IGNORE 200//一帧内不考虑机器人自身移动即滤波结果中的线速度，远离的距离大于这值则不用考虑其威胁，单位毫米/s
class CPathPlanInDynamic:virtual public CObjectTracker,virtual public ApfPathPlan,public BOOST_DEBUG_TOOL,public CGuauessProbCalcu
{
public:
	CPathPlanInDynamic(void);
public:
	~CPathPlanInDynamic(void);
public:
	/**
	*@brief 初始化滤波器,设定初值等,
	*/
	inline void InitTracker(void);
	/**
	*@brief 开始运行跟踪，观测，获得速度
	*@parm dl 观测者左轮位移
	*@parm dr 观测者右轮位移
	*@parm OBSERVEDATA 观测数据,boost中的vector格式,目前仅有2个内容,其中的角度,输入时必须为弧度制,内部会依据USE_DEGREE_ANGLE宏是否定义来确定是否需要再转化为角度
	*/
	void RunTrackInDyn(double dl,double dr,OBSERVE_STYLE& OBSERVEDATA);
	
public:
	/**
	*@brief开始局部路径规划,是主要接口函数,内部会调用跟踪目标函数进行目标跟踪
	*
	*1找出所有有效物体，并将其存入m_vpConfirmedObject，从上一次的m_vLastFiltedCenterPos或m_mFilterResult中找出每一个有效物体上一时刻的位置
	*找到的进行跟踪，没找到的视为突然物体，不滤。利用滤出来的速度及位置写入地图并搜索，计算危险系数，计算左右轮速。
	*事实上是在ApfPathPlan::FreeRoad基础上修改而来的,作为动态环境蔽障的接口
	*输入参数增加了左右轮子移动路径长度的输入
	*/
	 bool 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);
	
protected:
		std::vector<int>aa;
	// 保存传入的障碍物链表头
	PthPlSpa::ObjectPolePos* m_pObstacleListHead;
	OBSERVE_STYLE m_OBSERVEDATA;///<传入的观测数据
	//STATE_STYLE m_mFilterResult;///<跟踪结果
		//struct  ObjectCorelationMod:PthPlSpa::ObjectPolePos
	//{
	//	int CorrelateToNumOfLastTime;
	//};
	
	//std::vector<double> m_vObstCenterDis;///<物体的中心极坐标距离值
	//std::vector<double> m_vObstCenterAngle;///<物体的中心极坐标角度值
	bool SetRoadMap(PthPlSpa::ObjectPolePos *pObstacleList,int nObstacleMaxNum);
protected:
	/**
	*@brief 搜索出所有的有效障碍物,并将其指针按顺序保存,也编了号,若一个有效的都没有则返回假
	*/
	bool ConfirmAllObstacle(PthPlSpa::ObjectPolePos *pObstacleList,int nObstacleMaxNum);
	/**
	*@brief依据本次观测和上一次滤波的结果确认本次的所有障碍物分别对应上一次的哪些?
	*
	*目前简单的假设自己不动,一一计算本次观测物体位置与上一次的物体位置的几何距离,距离最近的而且距离小于阈值SAME_OBSTACLE_MAX_DIS
	*即认为两者是同一物体,若距离最近的都大于SUDDEN_OBSTACLE_MIN_DIS则认为该物体上一次没有观测到,指该位m_viRelateParm为0
	*若没有找到一个相关的物体,则返回假,否则返回真
	*/
	bool FindRelatedPosInLastStep(PthPlSpa::ObjectPolePos *pObstacleList,int nObstacleMaxNum);
	/**
	*@brief 计算两个点的欧氏距离
	*/
	double CalCulateDisIn2Pos(PthPlSpa::ObjectPolePos *pObstacle1,PthPlSpa::ObjectPolePos *pObstacle2);
	///**
	//*@brief 为sort函数准备的比较两个距离的大小
	//*/
	//inline bool DisCompare(int DisIndex1,int DisIndex2);
public:
	std::vector<STATE_STYLE> m_mFilterResult;///<对所有物体的跟踪结果挨个存入,本轮没有进行跟踪的静止物体也要存入
	std::vector<VARIANCE> m_mFilterResultVar;///<对所有物体的跟踪结果的方差挨个存入
	std::vector<VARIANCE> m_mModelNoiseVar;///<对所有物体的自适应跟踪后,模型噪声挨个存入
	std::vector<CAdaptiveOptimaser>m_vAdaptiveOptimizerVec;///<自适应调节器队列，每个跟踪的障碍物一个
	//std::vector<LARGE_INTEGER>m_vPreSystemCountNumVec;///<每一个障碍物上一次进行跟踪时的系统时钟点数
protected:
	/*******************************注意，容易混淆处说明*********************************************************
	*注意m_mFilterResult与m_vpConfirmedObject是一致的内容，顺序也一致，其实两者有重复，但因用到地方不同，现不考虑删除
	*另外它们与ApfPathPlan::m_apObstaclePointerContainer中的内容也是一致的，但m_apObstaclePointerContainer中的内容以
	*下标1开始存储。
	*
	*/
	std::vector<PthPlSpa::ObjectPolePos> m_vLastFiltedCenterPos;///<上一次滤波后的物体中心位置
	std::vector<PthPlSpa::ObjectPolePos*> m_vpConfirmedObject;///<经确认有效的所有物体指针
	std::vector<int> m_viRelateParm;///<本次观测数据对应上一次滤波后的物体中心位置中的哪一个.若为0则表示该项观测上一次没有对应项.从1开始
	//比如下标1单元存放数3,则表示当前经确认有效的障碍物列表m_vpConfirmedObject中第1个物体对应上一次的保存的位置结果中的下标为3-1的物体
	
	/**
	*@brief 从ApfPathPlan类SetRoadMap函数中分离出来的部分功能:将指定障碍物及其编号写入地图,并记录该障碍物指针
	*@parm pObstacleList 指定障碍物指针
	*@parm nObstacleMaxNum 障碍物编号
	*/
	/*bool AddObject2RoadMap(PthPlSpa::ObjectPolePos *pObstacleList,int numOb);*/
	/**
	*考虑获得的速度将指定的物体设定到环境地图
	*
	*@parm ObjectDisplace为物体距离变化率，考虑了机器人自身移动的影响
	*/
	bool SetLocalDynamicMap(int numOb,PthPlSpa::ObjectPolePos* pObstacle,STATE_STYLE FilterResult,double &TimeStep);//,float ObjectDisplace
	/**
	*@brief在已经确认本次观测各个物体在前一时刻位置的基础上，进行跟踪并依据估计速度，构造地图。
	**/
	void ConstructRoadMap(double dl,double dr,double& TimeStep);
	public:
		unsigned m_uTrackedNum;///<经过跟踪的物体数量

	/**
	*@brief 计算危险系数
	*
	*@parm IndexOfObstacle为最近的障碍物的地图内编号，因m_mFilterResult与m_vpConfirmedObject是一致的，所以该编号也同样
	*可以在m_mFilterResult中使用，用来寻找物体的速度等参数
	*
	*/
	double CalCulateDanger(int IndexOfObstacle);
	double m_dLengthPerTimeStep;///<一个时间步长内，机器人移动的距离,
	//std::vector<double> m_mFilterResult;
public:
	/**
	*考虑速度,估计自己到障碍物附近时其预计位置
	*以障碍物除以当前一个时间步内的移动位移来计算所需的到达时间
	*当物体运动线速度为负，且幅度大于MIN_LEAVE_SPEED_FOR_IGNORE，返回false，则后面忽略该物体,不写入地图。
	*
	*/
	bool AlterObstaclePosOnVel(PthPlSpa::ObjectPolePos* pObstacle,STATE_STYLE& FilterResult,double &TimeStep);
	/**
	*考虑速度以及速度估计的概率分布,估计自己到障碍物附近时其预计位置
	*由于假设了速度分布为高斯分布，因此以其80%可能性的速度值分布扩充物体。
	*
	*若由方差判断未收敛则返回false,否则进行扩充返回true;
	*/
	bool EnlargeObstaclePosOnVel_Probaility(PthPlSpa::ObjectPolePos* pObstacle,CObjectTracker* pMyTracker,float ArriveTime);
public:
	// 是否值得进行跟踪的判断函数，本函数在FindRelatedPosInLastStep中调用，若输入的为值得跟踪则返回true
	bool ConfirmTrackableObject(PthPlSpa::ObjectPolePos *pObstacleList);
protected:
	// 物体相对机器人自身的距离变化率，既考虑了机器人自身移动因素，又考虑了物体移动的因素，注意需与m_vpConfirmedObject的内容一致
	//std::vector<float> m_vDisplaceOfObject2Self;
};