apfpathplan.h

路径规划源程序

// ApfPathPlan.h: interface for the ApfPathPlan class.
//
//////////////////////////////////////////////////////////////////////
/********************************************************************
	created:	2008/06/12
	created:	12:6:2008   9:27
	filename: 	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2\PathPlan\ApfPathPlan.h
	file path:	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2\PathPlan
	file base:	ApfPathPlan
	file ext:	h
	author:		GaoYang
	InputFunc:  GetList(通信内容)
	OutputVar:  m_pObstacleList障碍物链表的头指针；
	m_daBallPoleCord[2]球或目标的极坐标
	Version:    2.0

	purpose:	单机器人避障程序，对外接口函数为FreeRoad。本版本要做的是按面向对象编成指导原则规范化
	            并且使其适用于激光雷达蔽障。
*********************************************************************/
#if !defined(AFX_APFPATHPLAN_H__B66BB6C3_2F1D_48FF_87F3_07A2D4BBF7AA__INCLUDED_)
#define AFX_APFPATHPLAN_H__B66BB6C3_2F1D_48FF_87F3_07A2D4BBF7AA__INCLUDED_

#if _MSC_VER > 1000
#pragma once
#endif // _MSC_VER > 1000

#include "ShareObject.h"
#include "Fuzzy.h"

#include "ObjectAngleCheck.h"
#include "LocalMap.h"
#include "..\RobotMathMod\RobotMathmaticalModel.h"
//////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////
/********************************************************************
	created:	2007/02/08
	改进:	2007/8/05 21:02
	Classname: 	ApfPathPlan
	InputFunc:  GetList(通信内容)
	OutputVar:  m_pObstacleList障碍物链表的头指针；
				m_daBallPoleCord[2]球或目标的极坐标
	author:		高扬
	
	purpose:	避障
*********************************************************************/

using namespace personal_ShareObject_pathPlanUsingSpace;
namespace PthPlSpa=personal_ShareObject_pathPlanUsingSpace;
class ApfPathPlan:virtual public ObjectAngleCheck,virtual public CLocalVisionMap,public CRobotMathmaticalModel   
{
public:
	

	ApfPathPlan();
	virtual ~ApfPathPlan();
	TargetStyle m_aPathTarget;	
	//double m_dLeftSpeed,m_dRightSpeed;
	/**
	* 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 uSpeedLevel123_High 参数uSpeedLevel123_High为无符整型,取值为:1,2,3.分别表示机器人质心移动线速度的档位:
	*       1,为低速档,2为中速,3为高速.
	* @param LeftForbidden 参数LeftForbidden表示是否禁止许机器人向左走，true为是
	* @param RightForbidden 参数RightForbidden表示是否禁止许机器人向右走，true为是
	* @parm ReverseMoveDirect 参数ReverseMoveDirect表示是否机器人倒着走，true为是
	* @return 传回BOOL型返回值表示自己是否成功找到合适的方向供前进,无则返回false,此时结果成员变量m_dLeftSpeed,m_dRightSpeed
	*中放着上一次的规划结果。
	*/ 
	bool FreeRoad(PthPlSpa::ObjectPolePos *ObstacleList,
					int ObstacleMaxNum,
					PthPlSpa::ObjectPolePos* Target,
					bool bControlBall,unsigned uSpeedLevel123_High,
					bool LeftForbidden,bool RightForbidden,bool  ReverseMoveDirect);
	/**
	*@brief 计算碰撞危险
	*/
	float CalCulateDanger(int IndexOfObstacle, float LineVMax);
	///**
	//*@brief 获取两个角度间的夹角
	//*/
	//inline int GetShortDistanceBetween2Angle(int Angle1,int Angle2);

protected://部分参数
	/** 
	*@brief CheckObstacleIsBehind函数用于检测某障碍物是否横跨0、360度分界线，
	*在-180~180坐标系中则检测是否横跨在+-180度分界线上
	*
	*输入依次为该物体的左边界角、中心角、右边界角，当是横跨时为真，否则为假。
	*@todo 在本方法中横跨的情况所出问题最多，检测的依据也至今仍非完善。
	*/
// 	inline bool CheckObstacleIsBehind(double dLBoundaryAngle,double dCenterAngle,double dRightBoundaryAngle);

	double m_dHighLineSpeed,m_dMidLineSpeed,m_dSlowLineSpeed;//高、中、低速档的速度值
	int m_iSelfOrientation;///<在机器人相对坐标系中机器人当前朝向所在角度
	const double m_const_Pi;//->3.1415926
	const double m_const_dTransDegree2arc;//->由角度转化到弧度所乘的系数：180/3.1415926
	const double m_const_dTransarc2Degree;//->由弧度转化到角度所乘的系数：180/3.1415926
	const double m_const_dRobotWidth2;//->机器人自身宽度
	int m_dParCostFun_Orient2Angle;///<代价函数中用的，当前朝向与该角度之差部分的权重
	int m_dParCostFun_PreTarAngle2Angle;///<代价函数中用的，上次所选目标角与该角度之差部分的权重
	int m_dParCostFun_Tar2Angle;///<代价函数中用的，目标的角度与该角度之差部分的权重

protected://建立地图相关	

	
protected://搜索转向中的最近距离
	/**
	* @ brief  SearchNearestDis()函数用来查找自己当前角到目标角之间的最近障碍物距离
	*
	*@output 返回的是当前角到目标角间最近障碍物在地图中的标号，从1开始，若返回0 则表示根本没有搜到最近障碍物
	*/	
	int SearchNearestDis();
	/**
	*@ brief SearchNearestDis()调用的，用来检测当前角度是否最近距离，是的话记录，并且指针指向到下一轮要检测的角度,若中途发现不用继续搜索了则返回true否则返回false
	*@parm AngTobeCheck　当前检测角度
	*@parm NearestObstacleIndex　至今最近障碍物编号
	*@parm DisBetweenTarA2SelfA　目标角度与当前角度之差
	*@parm SearchInLeftOrRightPlaneFlag 加方向搜索还是减方向搜索的标志位，取值１或－１.１为加方向
	*/
	bool CheckCurAngFindNexAng(int& AngTobeCheck,int& NearestObstacleIndex,
		int& DisBetweenTarA2SelfA,int SearchInLeftOrRightPlaneFlag);
	/**
	*@ brief GetShortDistanceBetween2Angle函数用于计算输入其中的两个角度间的最小角度差
	*不管正负之分，仅在同一个极坐标系中计算两个角度所在射线所成的最小角（无正负）。
	*/

	double m_dMinObstalceDis;///<自己从当前位置切入目标角度前可能碰到的障碍物最近距离
protected://计算左右轮速
	/**
	* @ brief 确认当前线速度档位
	*/
	float DecideCurrentLinearVParm(unsigned uSpeedLevel123_High)
	{
		if (uSpeedLevel123_High==3)//根据输入确定速度的档位，高中低三档。
		{
			return m_dMidLineSpeed;
		} 
		else if (uSpeedLevel123_High==2)
		{
			return m_dMidLineSpeed;
		}
		else
		{
			return m_dSlowLineSpeed;
		}
	}
	float DecideRealAngularV(float nCornerSpeedCoefficient,bool bControlBall,int DisBetweenTarA2SelfA);
	/**
	* @ brief SetLRSpeedFreeRoad函数用来计算左右轮速建议速度
	* @ parm  bControlBall 用来表示是否控球的bool型成员变量
	* @ parm  nlineSpeedCoefficient  线速度设置参数 用来控制线速度的大小
	* @ parm  nCornerSpeedCoefficient角速度设置参数 用来控制线速度的大小
	*/
	void SetLRSpeedFreeRoad(bool bControlBall,double nlineSpeedCoefficient,
							double nCornerSpeedCoefficient,
							unsigned uSpeedLevel123_High);
	double m_dMaxAngleSpeedWithBall,m_dMaxAngleSpeedNoBall;
	FuzzyControl m_cFuzzyControlMod;
	double m_dSpeedCalcuPar;//计算左右轮速时用的参数；构造函数中初始化0.85*RobotWidth,其中0.85可调
	///////////风险函数用的变量///////////////////////////////////////////////////////////////
	double m_dLineVPre;///<上一帧的线速度
	double m_dDangerDegree;///<风险值,其值越大，风险越小
	//////////////////////////////////////////////////////////////////////////

protected://搜索目标角	
	/**
	*@brief OutputCandidateAngleFinded函数调用的保存当前找到的侯选目标路径角，SaveCandidateArray，FoundCandAngleNum都是输出
	*
	*/
	void SaveFoundCandidateAngle(int* pSaveCandidateArray,int& FoundCandAngleNum,int freeNum,int GapBeginAngle,int CurrentAngle,int AddPos_DecNeg);
	
	/**
	* 本函数用于发现+-目标角，将他输出
	* 返回为该函数发现的目标角个数0~2
	*/
	inline void OutputCandidateAngleFinded(int freeNumAdd,int GapBeginAngle,int CurrentAngle,int AddPos_DecNeg);
	///**
	//* 本函数用于求出给定角在坐标系移动后在当前新坐标系中的角度值
	//* @ parm index index为给定角度，单位度
	//*/	
	//double GetOldAngleInNewCoord(int index);
	///**
	//*@brief 搜索动态障碍物，是一个简单估计物体角速度的主要函数，是CheckAllDynamicObstacle的前一版本
	//*/
	//void AddFreeRoadForDynamic();

	//double GetTargetAngleInDynamicEnvir(double TargetAngle,int Ball0Add1Dec2);
	double m_daUpdatedAngle[3];///<存放位置更新后的三个角度，分别为直奔球去角、+方向搜索角、-方向搜索角
/**
* @ brief  SearchFreeTargetAngle()函数用来查找自己最近安全路径的角度方向
*
* @ parm LeftForbidden LeftForbidden是控制是否不向左走
*/
	bool SearchFreeTargetAngle(bool LeftForbidden,bool RightForbidden);
	/**
	*@ brief 从备选路径角中搜索出最终目标路径角
	*
	*/
	bool FindTargetAngleInCandidate(int NumCountAdd,int NumCountDec,bool LeftForbidden,bool RightForbidden);
	/**
	* @ brief  SelectTargetAngleFromAddAndDec()函数用来从加方向、减方向搜索的结果中选取一个作为最终目标角。
	*
	* @ parm TargetAngleAdd是加方向结果
	* @ parm TargetAngleDec是减方向结果
	*/
	inline double SelectTargetAngleFromAddAndDec(bool AngleInAdd,bool AngleInDec);
	int m_iPreTargetAngle;///<上次搜索出来的目标角度结果
public:
	int m_iTargetAngle;///<本次搜索出来的目标角度结果
	//float m_fFinaFreRoad[361];
	int TargetAngleAdd[2];//->加方向搜索出来的推荐目标角,每个方向最多两个
	int TargetAngleDec[2];//->减方向搜索出来的推荐目标角每个方向最多两个
	int m_iNumOfTargetAngleAdd;///<本次搜索出几个
	int m_iNumOfTargetAngleDec;///<
	bool m_bNeedToSpeedUpInAdd;///<加方向搜到的目标角含动态的成分,需加速
	bool m_bNeedToSpeedUpInDec;///<减方向搜到的目标角含动态的成分,需加速
	//int m_nPrem_fFinalFreRoad[360];
	int m_iPreObstacleNumAfterCheck;

private:
/*
*@brief 沿+或-方向搜索自由路径,搜到合适路径或完成搜索
*@parm freeNum该方向截至目前已经找到的空白角度数
*@parm NumCount该方向截至目前已经经过的角度数
*@parm FindGap截至目前是否发现了空隙，或者说此前角度是否在空隙中
*@parm AngIndOfCurr当前角度
*@parm GapBeginAngle空隙开始的角度
*@parm PassBound 越界标志
*@parm AddDecFlag 加方向搜索还是减方向搜索的标志位，取值１或－１.１为加方向
*/
	void SearchFreeAngleInHalfPlane(int& freeNum,int& NumCount,
		bool& FindGap,int& AngIndOfCurr,int& GapBeginAngle,bool& PassBound,int AddDecFlag);
	/*
	*@brief 检查当前角度，看到现在是否搜到合适路径或完成搜索，需要跳出则返回真，否则返回假
	*@parm freeNum该方向截至目前已经找到的空白角度数
	*@parm NumCount该方向截至目前已经经过的角度数
	*@parm FindGap截至目前是否发现了空隙，或者说此前角度是否在空隙中
	*@parm AngIndOfCurr当前角度
	*@parm GapBeginAngle空隙开始的角度
	*@parm AddDecFlag 加方向搜索还是减方向搜索的标志位，取值１或－１.１为加方向
	*/
	bool CheckCurrentAngleInHalfPlane(int& freeNum,int& NumCount,
		bool& FindGap,int& AngIndOfCurr,int& GapBeginAngle,int AddDecFlag);
	/**
	*@brief 发现一个空白角度后的动作函数
	*/
	void FindAFreeAngle(int&freeNum,int&NumCount,
		bool&FindGap,int& AngIndOfCurr,int&GapBeginAngle,int AddDecFlag)
	{
		if (FindGap==false)//发现的是间隙的头
		{
			GapBeginAngle=AngIndOfCurr;
			FindGap=true;
			freeNum=0;
		}
		freeNum++;
		NumCount++;
		AngIndOfCurr=AngIndOfCurr+AddDecFlag;						
	}
	
	/**
	*@brief 发现一个存在障碍物的角度后的动作函数
	*/
	bool FindABlockedAng(int&freeNum,int&NumCount,bool&FindGap,int& AngIndOfCurr,int&GapBeginAngle,int AddDecFlag);
	/**
	*@brief 当前检测到一个跨越了０，３６０分界线的障碍物时，此函数计算各种情况下的累计检查角度NumCountAdd，由FindABlockedAng调用
	*/
	int CountingABehindObject(int NumCount,int& AngIndOfCurr,int AddDecFlag);

};


#endif // !defined(AFX_APFPATHPLAN_H__B66BB6C3_2F1D_48FF_87F3_07A2D4BBF7AA__INCLUDED_)