?? 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) //->當Add_Pos_Dec_Neg==1時限幅同時任一方向過界均報錯
{
if (Angle>360) //->i，j越界處理
{
Angle=Angle-360;
return true;//->返回過界標志
// AfxMessageBox("角度>180范圍");
}
else if (Angle<0)
{
Angle=360+Angle;
return true;//->返回過界標志
// AfxMessageBox("角度<-180范圍");
}
else
{return false;}
}
else if(Add_Pos_Dec_Neg==0)//->當Add_Pos_Dec_Neg==0時，表示不再區分從哪個方向過界，僅將輸入的角度限制在-180~180
{
if (Angle>360)//->i，j越界處理
{
Angle=Angle-360;
return true;//->返回過界標志
}//->錯誤報警
else if (Angle<0)
{
Angle=360+Angle;
return true;//->返回過界標志
}
else
{
return false;//->未過界
}
}
else if (Add_Pos_Dec_Neg>0)//->假如是檢測正方向是否過界
{
if (Angle>360) //->i，j越界處理
{
Angle=Angle-360;
return true;//->返回過界標志
}
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;//->返回過界標志
}
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;//急轉的判斷角
//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;///<風險值
m_iPreTargetAngle=0;
m_dParCostFun_Orient2Angle=2;
m_dParCostFun_PreTarAngle2Angle=2;
m_dParCostFun_Tar2Angle=6;
m_dSpeedCalcuPar=1*m_const_dRobotWidth2;//0.85是在外部調試時試湊出來的系數，為避免重復計算，納入這里
//memset(m_nPrem_fFinalFreRoad,0,sizeof(m_nPrem_fFinalFreRoad));
m_iPreObstacleNumAfterCheck=0;
}
ApfPathPlan::~ApfPathPlan()
{
}
/**
* 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 BoundaryClose 參數BoundaryClose表示自己是否接近場地邊界,true為接近邊界.
* @param uSpeedLevel123_High 參數uSpeedLevel123_High為無符整型,取值為:1,2,3.分別表示機器人質心移動線速度的檔位:
* 1,為低速檔,2為中速,3為高速.
*
* @return 傳回BOOL型返回值表示自己是否成功找到合適的方向供前進,無則返回false,此時結果成員變量m_dLeftSpeed,m_dRightSpeed
*中放著上一次的規劃結果。
*/
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("目標導入避撞模塊錯誤");
Target->angle=(int)Target->angle;
// robot_console_printf("\nTarget->distance=%f\n",Target->distance);
return false;
}///<導入目標信息，檢查信息是否有效。
// robot_console_printf("\nAngle=%f\n",Target->angle);
m_aPathTarget.x=Target->centerX;//m_aPathTarget為存儲路徑規劃目標信息
m_aPathTarget.y=Target->centerY;//m_aPathTarget為存儲路徑規劃目標信息
m_aPathTarget.distance=Target->distance;//m_aPathTarget為存儲路徑規劃目標信息
m_aPathTarget.angle=Target->angle;//m_aPathTarget為存儲路徑規劃目標信息
if (!ReverseMoveDirect)
{
m_iSelfOrientation=90;
}
else
{
m_iSelfOrientation=270;
// robot_console_printf("ReverseMoveDirectm_iSelfOrientation=%f\n",m_iSelfOrientation);
}
SetRoadMap(pObstacleList,nObstacleMaxNum,m_aPathTarget.distance);
///獲得目標角度與自身轉到該角度所能碰到的最近障礙物距離///
// 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);//->模糊控制依據前面的結果規劃出速度。
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)//計算風險函數
{
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函數用來依據前面尋到的目標方向等計算建議轉速
*
* SetLRSpeedFreeRoad函數的計算依據是：在給定質心移動線速度的情況下，依據目標角度、最近障礙物距離等
*信息計算合適的角速度使機器人的軌跡剛好是一條合適的弧線到達目標點（無障礙情況）或者擦著最近的障礙物
*轉到目標角度m_iTargetAngle。因此已知機器人運動模型的情況下關鍵是計算出合適的角速度值,從而確定合適的路徑.
* 該函數包括兩個角速度計算部分：
* 1 由自定義的系數PAR_ANGLE_SPEED乘以m_iTargetAngle得到一個角速度aaatemp。
* 2 第2個角速度bbbtemp分多種情況：
* 若m_iTargetAngle>m_nSharpTurnThresAngle（自定義的急轉角度閾值）則直接取m_dMaxAngleSpeedNoBall或m_dMaxAngleSpeedWithBall（看是否控球）
* 否則以路徑不碰到最近障礙物計算角速度
* 若m_iTargetAngle>70且目標距離<0.7m則原地轉動。
* 計算出角速度后代入機器人運動模型計算出相應左右輪速。
*
* @param bControlBall 參數bControlBall是一個表示是否控球的bool型參數，真為控球。
* 該障礙物的極坐標，左右邊界角度。
* @param BoundaryClose 參數BoundaryClose是一個表示是否接近邊界的bool型參數，真為接近。（未用到）
* @param uSpeedLevel123_High 參數uSpeedLevel123_High為無符整型,取值為:1,2,3.分別表示機器人質心移動線速度的檔位:
* 1,為低速檔,2為中速,3為高速.
*
*/
void ApfPathPlan::SetLRSpeedFreeRoad(bool bControlBall,
double nlineSpeedCoefficient,
double nCornerSpeedCoefficient,
unsigned uSpeedLevel123_High)
{
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -