?? apfpathplan.cpp
字號:
// ApfPathPlan.cpp: implementation of the ApfPathPlan class.
//
//////////////////////////////////////////////////////////////////////
//#include "stdafx.h"
#include "ApfPathPlan.h"
//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
//extern double NewCoordCenterX;
//extern double NewCoordCenterZ;
//extern double m_dAngleChange;///<上一次自己的朝向與這一次自己的朝向之差
//using namespace personal_ShareObject_pathPlanUsingSpace;
template <class type>
bool CheckAngleLimitedBeyond(type & Angle,int Add_Pos_Dec_Neg)
{
//=180或-180度均不超界
if (Add_Pos_Dec_Neg==1) //->當(dāng)Add_Pos_Dec_Neg==1時限幅同時任一方向過界均報錯
{
if (Angle>360) //->i，j越界處理
{
Angle=Angle-360;
return true;//->返回過界標(biāo)志
// AfxMessageBox("角度>180范圍");
}
else if (Angle<0)
{
Angle=360+Angle;
return true;//->返回過界標(biāo)志
// AfxMessageBox("角度<-180范圍");
}
else
{return false;}
}
else if(Add_Pos_Dec_Neg==0)//->當(dāng)Add_Pos_Dec_Neg==0時，表示不再區(qū)分從哪個方向過界，僅將輸入的角度限制在-180~180
{
if (Angle>360)//->i，j越界處理
{
Angle=Angle-360;
return true;//->返回過界標(biāo)志
}//->錯誤報警
else if (Angle<0)
{
Angle=360+Angle;
return true;//->返回過界標(biāo)志
}
else
{
return false;//->未過界
}
}
else if (Add_Pos_Dec_Neg>0)//->假如是檢測正方向是否過界
{
if (Angle>360) //->i，j越界處理
{
Angle=Angle-360;
return true;//->返回過界標(biāo)志
}
else if (Angle<0)
{
// Angle=360+Angle;//AfxMessageBox("+方向搜索的角度<-180范圍");
// return true
}
else
{
return false;//->未過界
}
}
else//->Add_Pos_Dec_Neg<0假如是檢測-方向是否過界
{
if (Angle>360)//->i，j越界處理
{//AfxMessageBox("-方向搜索的角度>180范圍");
}//->錯誤報警
else if (Angle<0)
{
Angle=360+Angle;
return true;//->返回過界標(biāo)志
}
else
{
return false;//->未過界
}
}
}
ApfPathPlan::ApfPathPlan():m_const_Pi(3.1415926),m_const_dTransDegree2arc(3.1415926/180),
m_const_dTransarc2Degree(180/3.1415926),m_const_dRobotWidth2(0.5)
{
m_dMaxAngleSpeedWithBall=MAX_ANGLE_SPEED_WITH_BALL;
m_dMaxAngleSpeedNoBall=MAX_ANGLE_SPEED_NO_BALL;
m_dMidLineSpeed=HIGH_CENTER_LINE_SPEED;
m_dMidLineSpeed=MID__CENTER_LINE_SPEED;
m_dSlowLineSpeed=SLOW__CENTER_LINE_SPEED;
// m_nSharpTurnThresAngle=SHARP_TURN_ANGLEY;//急轉(zhuǎn)的判斷角
//m_dSafeWidth=SAFE_WIDTH;
//m_dSafeWidthRich=m_dSafeWidth*0.01;
//m_dExpandRadius=0.5*(m_dSafeWidth+m_dSafeWidthRich);
// parB=PAR_ANGLE_SPEED;//0.07
// memset(m_apObstaclePointerContainer,0,sizeof(m_apObstaclePointerContainer));
// m_dAngleSpeedPar=m_const_dRobotWidth2;
m_iSizeOfObjectPolePos=sizeof(ObjectPolePos);
m_dLineVPre=0;///<上一幀的線速度
m_dDangerDegree=0;///<風(fēng)險值
m_iPreTargetAngle=0;
m_dParCostFun_Orient2Angle=2;
m_dParCostFun_PreTarAngle2Angle=2;
m_dParCostFun_Tar2Angle=6;
m_dSpeedCalcuPar=1*m_const_dRobotWidth2;//0.85是在外部調(diào)試時試湊出來的系數(shù)，為避免重復(fù)計算，納入這里
//memset(m_nPrem_fFinalFreRoad,0,sizeof(m_nPrem_fFinalFreRoad));
m_iPreObstacleNumAfterCheck=0;
}
ApfPathPlan::~ApfPathPlan()
{
}
/**
* FreeRoad函數(shù)用來尋找當(dāng)前機器人面前可以通過的安全方向
*
* FreeRoad函數(shù)包括如下兩部分:
* 將傳入的障礙物信息經(jīng)過有效性判斷后寫入矩陣m_nFreRoadMap中（成員變量），該矩陣361行2列：
* 第180+i行存的是機器人坐標(biāo)系中第i度方向的障礙物信息。
* 0列存的是0或1，表示該方向是否有障礙物：0為無，1為有。
* 1列存的是距離值，表示該方向的障礙物離自己的距離值。
*
* @param pObstacleList 參數(shù)pObstacleList是一個障礙物鏈表的指針(ObjectPolePos型指針)，鏈表中的每一個障礙物信息包括：
* 該障礙物的極坐標(biāo)，左右邊界角度。
* @param nObstacleMaxNum 參數(shù)nObstacleMaxNum為障礙物個數(shù)。
* @param Target 參數(shù)Target為一個指向目標(biāo)信息的指針(ObjectPolePos型指針)必須包括目標(biāo)點的極坐標(biāo)
* @param bControlBall 參數(shù)bControlBall表示自己是否控到球,true為控到.
* @param BoundaryClose 參數(shù)BoundaryClose表示自己是否接近場地邊界,true為接近邊界.
* @param uSpeedLevel123_High 參數(shù)uSpeedLevel123_High為無符整型,取值為:1,2,3.分別表示機器人質(zhì)心移動線速度的檔位:
* 1,為低速檔,2為中速,3為高速.
*
* @return 傳回BOOL型返回值表示自己是否成功找到合適的方向供前進(jìn),無則返回false,此時結(jié)果成員變量m_dLeftSpeed,m_dRightSpeed
*中放著上一次的規(guī)劃結(jié)果。
*/
bool ApfPathPlan::FreeRoad(ObjectPolePos *pObstacleList,
int nObstacleMaxNum,
ObjectPolePos* Target,
bool bControlBall,
unsigned uSpeedLevel123_High,bool LeftForbidden,
bool RightForbidden, bool ReverseMoveDirect)
{
//__asm int 3;
// robot_console_printf("______TargetDis=%f\n",Target->distance);
if (!Target||Target->distance<0||Target->distance>18||fabs(Target->angle)>360)
{
// TRACE("目標(biāo)導(dǎo)入避撞模塊錯誤");
Target->angle=(int)Target->angle;
// robot_console_printf("\nTarget->distance=%f\n",Target->distance);
return false;
}///<導(dǎo)入目標(biāo)信息，檢查信息是否有效。
// robot_console_printf("\nAngle=%f\n",Target->angle);
m_aPathTarget.x=Target->centerX;//m_aPathTarget為存儲路徑規(guī)劃目標(biāo)信息
m_aPathTarget.y=Target->centerY;//m_aPathTarget為存儲路徑規(guī)劃目標(biāo)信息
m_aPathTarget.distance=Target->distance;//m_aPathTarget為存儲路徑規(guī)劃目標(biāo)信息
m_aPathTarget.angle=Target->angle;//m_aPathTarget為存儲路徑規(guī)劃目標(biāo)信息
if (!ReverseMoveDirect)
{
m_iSelfOrientation=90;
}
else
{
m_iSelfOrientation=270;
// robot_console_printf("ReverseMoveDirectm_iSelfOrientation=%f\n",m_iSelfOrientation);
}
SetRoadMap(pObstacleList,nObstacleMaxNum,m_aPathTarget.distance);
///獲得目標(biāo)角度與自身轉(zhuǎn)到該角度所能碰到的最近障礙物距離///
// AddFreeRoadForDynamic();
// CheckAllDynamicObstacle();
if (!SearchFreeTargetAngle(LeftForbidden,RightForbidden))
{
m_iTargetAngle=0;
return false;
}
//__asm int 3;
//robot_console_printf("m_iTargetAngle=%d\n",m_iTargetAngle);
int IndexOfNearestObstacle=SearchNearestDis();
// robot_console_printf(" SearchNearestDisOK\n");
//SetLRSpeedFreeRoad(bControlBall,BoundaryClose,uSpeedLevel123_High);
// robot_console_printf("AvoidanceAngle=%f\n",m_iTargetAngle);
// robot_console_printf("AvoidanceDis=%f\n",m_dMinObstalceDis);
//robot_console_printf("m_dDangerDegree=%f\n",m_dDangerDegree);
// robot_console_printf("m_dDangerDegree=%f\n",m_dDangerDegree);
float LineVMax=DecideCurrentLinearVParm(uSpeedLevel123_High);
CalCulateDanger(IndexOfNearestObstacle,LineVMax);
double DisOfTarA2SelfA=GetShortDistanceBetween2Angle(m_iTargetAngle,m_iSelfOrientation);
m_cFuzzyControlMod.ProcessFuzzyControl(DisOfTarA2SelfA,m_dDangerDegree);//->模糊控制依據(jù)前面的結(jié)果規(guī)劃出速度。
SetLRSpeedFreeRoad(bControlBall,m_cFuzzyControlMod.LineSpeedOutPut,m_cFuzzyControlMod.CornerSpeedOutPut,uSpeedLevel123_High);
// robot_console_printf("SetLRSpeedFreeRoadOK\n");
return true;
}
float ApfPathPlan::CalCulateDanger(int IndexOfObstacle, float LineVMax)
{
if (IndexOfObstacle==0)//沒有威脅
{
m_dDangerDegree=0;
return 0;
}
if (m_dMinObstalceDis<0.2)//計算風(fēng)險函數(shù)
{
m_dDangerDegree=1;
}
else
{
//m_dDangerDegree=0.25*(4-m_dMinObstalceDis-0.3+m_dLineVPre*0.2);
m_dDangerDegree=0.8*(4-m_dMinObstalceDis)/4+m_dLineVPre*0.2/LineVMax;
}
m_dDangerDegree=m_dDangerDegree>0?m_dDangerDegree:0;
m_dDangerDegree=m_dDangerDegree<1?m_dDangerDegree:1;
return m_dDangerDegree;
}
/**
* SetLRSpeedFreeRoad函數(shù)用來依據(jù)前面尋到的目標(biāo)方向等計算建議轉(zhuǎn)速
*
* SetLRSpeedFreeRoad函數(shù)的計算依據(jù)是：在給定質(zhì)心移動線速度的情況下，依據(jù)目標(biāo)角度、最近障礙物距離等
*信息計算合適的角速度使機器人的軌跡剛好是一條合適的弧線到達(dá)目標(biāo)點（無障礙情況）或者擦著最近的障礙物
*轉(zhuǎn)到目標(biāo)角度m_iTargetAngle。因此已知機器人運動模型的情況下關(guān)鍵是計算出合適的角速度值,從而確定合適的路徑.
* 該函數(shù)包括兩個角速度計算部分：
* 1 由自定義的系數(shù)PAR_ANGLE_SPEED乘以m_iTargetAngle得到一個角速度aaatemp。
* 2 第2個角速度bbbtemp分多種情況：
* 若m_iTargetAngle>m_nSharpTurnThresAngle（自定義的急轉(zhuǎn)角度閾值）則直接取m_dMaxAngleSpeedNoBall或m_dMaxAngleSpeedWithBall（看是否控球）
* 否則以路徑不碰到最近障礙物計算角速度
* 若m_iTargetAngle>70且目標(biāo)距離<0.7m則原地轉(zhuǎn)動。
* 計算出角速度后代入機器人運動模型計算出相應(yīng)左右輪速。
*
* @param bControlBall 參數(shù)bControlBall是一個表示是否控球的bool型參數(shù)，真為控球。
* 該障礙物的極坐標(biāo)，左右邊界角度。
* @param BoundaryClose 參數(shù)BoundaryClose是一個表示是否接近邊界的bool型參數(shù)，真為接近。（未用到）
* @param uSpeedLevel123_High 參數(shù)uSpeedLevel123_High為無符整型,取值為:1,2,3.分別表示機器人質(zhì)心移動線速度的檔位:
* 1,為低速檔,2為中速,3為高速.
*
*/
void ApfPathPlan::SetLRSpeedFreeRoad(bool bControlBall,
double nlineSpeedCoefficient,
double nCornerSpeedCoefficient,
unsigned uSpeedLevel123_High)
{
?? 快捷鍵說明
復(fù)制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -