?? 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_)
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -