?? pathplanindynamic.cpp
字號:
//#include "stdafx.h"
#include "PathPlanInDynamic.h"
CPathPlanInDynamic::CPathPlanInDynamic(void)
: m_pObstacleListHead(NULL),m_OBSERVEDATA(2)//,m_mFilterResult(4)
{
InitFlt();//初始化濾波器,
m_uTrackedNum=0;
}
CPathPlanInDynamic::~CPathPlanInDynamic(void)
{
}
inline void CPathPlanInDynamic::InitTracker(void)
{
CObjectTracker::InitFlt();
}
bool CPathPlanInDynamic::SetLocalDynamicMap(int numOb,PthPlSpa::ObjectPolePos* pObstacle,STATE_STYLE FilterResult,double &TimeStep)//,float ObjectDisplace
{
if (AlterObstaclePosOnVel(pObstacle, FilterResult,TimeStep))//依據速度移動物體的位置,ObjectDisplace
{
AddObject2RoadMap(pObstacle,numOb,m_aPathTarget.distance);//將考慮速度后的物體加入到地圖中
}
else//若物體遠離,無需寫入地圖,需注意,此時還需要將該障礙物保存到m_aObstacleStruct中去,因為這畢竟是一個有效物體,
//而且要求m_aObstacleStruct中的內容與m_vpConfirmedObject一至，否則會出錯
{
m_aObstacleStruct.push_back(*pObstacle);
}
return true;
}
//本函數中，外部輸入的觀測數據距離單位為m角度單位為度，由于跟蹤器中所用單位分別為毫米、角度（也可能是弧度由USE_DEGREE_ANGLE決定）
//注意: 以ObjecPolePos結構體形式存的所有數據都采用觀測數據的單位,而m_mFilterResult的矩陣形式則采用跟蹤器中的格式，所以本函數中進行了轉換，
void CPathPlanInDynamic::ConstructRoadMap(double dl,double dr,double& TimeStep)
{
std::vector<int>::iterator pRelateParm=m_viRelateParm.begin();
int iIndexOfAllConfirmedObstacle=0;//已確認的障礙物，從0開始
std::vector<STATE_STYLE> SavedFilterResult(m_mFilterResult);
std::vector<VARIANCE>SavedFilterResultVar(m_mFilterResultVar);
std::vector<VARIANCE>SavedModelNoiseVar(m_mModelNoiseVar);
std::vector<CAdaptiveOptimaser>SavedAdaptiveOptimizer(m_vAdaptiveOptimizerVec);
//std::vector<LARGE_INTEGER>SavedPreSystemCountNum(m_vPreSystemCountNumVec);
//清空上一輪保存的跟蹤狀態，并重新保存，以確保本輪跟蹤中沒有用到的物體的狀態被去除掉
m_vLastFiltedCenterPos.clear();
m_mFilterResult.clear();
m_mModelNoiseVar.clear();
m_mFilterResultVar.clear();
m_vAdaptiveOptimizerVec.clear();
//m_vPreSystemCountNumVec.clear();
m_aObstacleStruct.clear();
memset(m_nFreRoadMap,0,sizeof(m_nFreRoadMap));
// memset(m_apObstaclePointerContainer,0,sizeof(m_apObstaclePointerContainer));
m_uTrackedNum=0;
#ifdef DEBUG_ON_VC_OUT_PUT_MORE
robot_console_printf("Confirmed Num=%d",m_vpConfirmedObject.size());
#endif
//robot_console_printf("m_vpConfirmedObject.size()=%d\n",m_vpConfirmedObject.size());
while (iIndexOfAllConfirmedObstacle<m_vpConfirmedObject.size())//對所有確認有效的物體
{
//__asm int3;
/* robot_console_printf("iIndexOfAllConfirmedObstacle=%d\n",iIndexOfAllConfirmedObstacle);
robot_console_printf("m_viRelateParm.size()=%d\n",m_viRelateParm.size());*/
//#ifdef OUT_PUT_OBSERVE_ONLY
// m_OBSERVEDATA(0)=m_vpConfirmedObject[iIndexOfAllConfirmedObstacle]->distance*1000;//當前已找到上一時刻位置項的數據進入濾波跟蹤
// m_OBSERVEDATA(1)=TransAngleToArc(m_vpConfirmedObject[iIndexOfAllConfirmedObstacle]->angle);//濾波器的輸入都是弧度
// robot_console_printf("%f,%f,%f,%f,%f,\n",m_OBSERVEDATA(0),m_OBSERVEDATA(1),dl,dr,TimeStep);
// //robot_console_printf("=%f\n",m_OBSERVEDATA(1));
//#endif
if (m_viRelateParm.size()>0)//找到過相關項
{
if ((*pRelateParm)!=0)//確實在前一時刻的結果中找到了自己的位置則:濾波,保存對應狀態,地圖中置動態障礙物
{
//m_viRelateParm中的值從1開始,而下標從0開始故需要-1
SetFltState(SavedFilterResult[*pRelateParm-1],SavedFilterResultVar[*pRelateParm-1],SavedModelNoiseVar[*pRelateParm-1]);//將上一個時刻該物體的濾波器狀態恢復
m_OBSERVEDATA(0)=m_vpConfirmedObject[iIndexOfAllConfirmedObstacle]->distance*1000;//當前已找到上一時刻位置項的數據進入濾波跟蹤
m_OBSERVEDATA(1)=TransAngleToArc(m_vpConfirmedObject[iIndexOfAllConfirmedObstacle]->angle);//濾波器的輸入都是弧度
//__asm int 3;
//RunTrack(dl,dr,m_OBSERVEDATA);//跟蹤
RunTrack(dl,dr,m_OBSERVEDATA,TimeStep,&(SavedAdaptiveOptimizer[*pRelateParm-1]));//跟蹤
m_uTrackedNum++;
/////保存濾波器狀態濾出來的角直接是度了///
//my_filter.m_mStateMeanResult(1)=TransAngleToDegree(my_filter.m_mStateMeanResult(1));//
//my_filter.m_mStateMeanResult(3)=TransAngleToDegree(my_filter.m_mStateMeanResult(3));
/*robot_console_printf("Dis=%f,Angle=%f\n",my_filter.m_mStateMeanResult(0),my_filter.m_mStateMeanResult(1));
robot_console_printf("LV=%f,AV=%f\n",my_filter.m_mStateMeanResult(2),my_filter.m_mStateMeanResult(3));*/
m_mFilterResult.push_back(my_filter.m_mStateMeanResult);//跟蹤結果挨個存入注意其中的距離單位是毫米
m_mFilterResultVar.push_back(my_filter.m_mStateCov);//跟蹤結果的方差挨個存入
m_mModelNoiseVar.push_back(m_cAdaptiveOptimiser.m_mNewModelNoise);//自適應跟蹤后,模型噪聲挨個存入
m_vAdaptiveOptimizerVec.push_back(SavedAdaptiveOptimizer[*pRelateParm-1]);//將用到的自適應器保存起來，沒用到的將去掉
//m_vPreSystemCountNumVec.push_back(m_LICurrentTimeCount);//將對當前障礙物信息的保存
PthPlSpa::ObjectPolePos DynamicObjPosTmp;
DynamicObjPosTmp.distance=my_filter.m_mStateMeanResult(0)/1000;//要除1000
DynamicObjPosTmp.angle=my_filter.m_mStateMeanResult(1);
m_vLastFiltedCenterPos.push_back(DynamicObjPosTmp);//將結果另存一份格式,todo:考慮只存一分，
//AlterObstaclePosOnVel(m_vpConfirmedObject[iIndexOfAllConfirmedObstacle],my_filter.m_mStateMeanResult);
//float DisDisplace=my_filter.m_mStateMeanResult(0)-SavedFilterResult[*pRelateParm-1](0);//這里的單位是毫米,而下面用時單位是米所以需除1000
SetLocalDynamicMap(iIndexOfAllConfirmedObstacle,m_vpConfirmedObject[iIndexOfAllConfirmedObstacle],my_filter.m_mStateMeanResult,TimeStep);//,DisDisplace/1000
}
else////在前一時刻的結果中找不到自己的位置則:地圖中置靜態障礙物,保存當前的位置,以及各方差為初始值
{
AddObject2RoadMap(m_vpConfirmedObject[iIndexOfAllConfirmedObstacle],iIndexOfAllConfirmedObstacle,m_aPathTarget.distance);
STATE_STYLE SudenObjStateTmp(my_filter.m_mStateMeanResult.size());
SudenObjStateTmp(0)=m_vpConfirmedObject[iIndexOfAllConfirmedObstacle]->distance*1000;
SudenObjStateTmp(1)=m_vpConfirmedObject[iIndexOfAllConfirmedObstacle]->angle;//注意保存時不需要轉化為角度
SudenObjStateTmp(2)=0;
SudenObjStateTmp(3)=0;//注意保存時不需要轉化為角度
m_mFilterResult.push_back(SudenObjStateTmp);//跟蹤結果挨個存入
m_mFilterResultVar.push_back(m_vInitStateCov);
m_mModelNoiseVar.push_back(m_vInitModelNoise);
m_vLastFiltedCenterPos.push_back(*m_vpConfirmedObject[iIndexOfAllConfirmedObstacle]);//將結果另存一份格式,todo:考慮只存一個
CAdaptiveOptimaser NewAdaptiveOptimizer;
NewAdaptiveOptimizer.initAdapter(m_cAdaptiveOptimiser.m_mNodelNoiseOriginalVal,m_cAdaptiveOptimiser.m_fStableDifAmplitudeParm);
m_vAdaptiveOptimizerVec.push_back(NewAdaptiveOptimizer);//創建自適應調節器
//LARGE_INTEGER LITmp;
//QueryPerformanceCounter(&LITmp);
//m_vPreSystemCountNumVec.push_back(LITmp);//每發現上一時刻位置則將當前時間點記下;
}
pRelateParm++;//注意必須與iIndexOfAllConfirmedObstacle一致且同步變化
iIndexOfAllConfirmedObstacle++;
}
else//根本就沒有搜到相關項,一開始會出現這種情況
{
CAdaptiveOptimaser NewAdaptiveOptimizer;
NewAdaptiveOptimizer.initAdapter(m_cAdaptiveOptimiser.m_mNodelNoiseOriginalVal,m_cAdaptiveOptimiser.m_fStableDifAmplitudeParm);
m_vAdaptiveOptimizerVec.push_back(NewAdaptiveOptimizer);//創建自適應調節器
AddObject2RoadMap(m_vpConfirmedObject[iIndexOfAllConfirmedObstacle],iIndexOfAllConfirmedObstacle,m_aPathTarget.distance);
STATE_STYLE SudenObjStateTmp(my_filter.m_mStateMeanResult.size());
SudenObjStateTmp(0)=m_vpConfirmedObject[iIndexOfAllConfirmedObstacle]->distance*1000;
SudenObjStateTmp(1)=m_vpConfirmedObject[iIndexOfAllConfirmedObstacle]->angle;//注意保存時不需要轉化為角度
SudenObjStateTmp(2)=0;
SudenObjStateTmp(3)=0;//注意保存時不需要轉化為角度
m_mFilterResult.push_back(SudenObjStateTmp);//跟蹤結果挨個存入
m_mFilterResultVar.push_back(m_vInitStateCov);
m_mModelNoiseVar.push_back(m_vInitModelNoise);
m_vLastFiltedCenterPos.push_back(*m_vpConfirmedObject[iIndexOfAllConfirmedObstacle]);//將結果另存一份格式,todo:考慮只存一個
//LARGE_INTEGER LITmp;
//QueryPerformanceCounter(&LITmp);
//m_vPreSystemCountNumVec.push_back(LITmp);//每發現上一時刻位置則將當前時間點記下;
iIndexOfAllConfirmedObstacle++;
}
}
//robot_console_printf("m_aObstacleStruct.size()=%d\n",m_aObstacleStruct.size());
}
#define LESS_DANGER_LEAVE_SPEED_MIN_TH 4//不可忽略但物體在遠離危險較小的線速度閾值幅度
double CPathPlanInDynamic::CalCulateDanger(int IndexOfObstacle)
{
if (IndexOfObstacle==0)//沒有威脅
{
m_dDangerDegree=0;
return 0;
}
double ObstacleLinVelocity=m_mFilterResult[IndexOfObstacle-1](2);//注意單位是毫米/時間段
if (m_dMinObstalceDis<0.2&&ObstacleLinVelocity>-LESS_DANGER_LEAVE_SPEED_MIN_TH)//計算風險函數,不可忽略,但是物體在遠離
{
m_dDangerDegree=1-ObstacleLinVelocity/MIN_LEAVE_SPEED_FOR_IGNORE;//注意有符號
}
else if (m_dMinObstalceDis<0.2)
{
m_dDangerDegree=1;
}
else
{
//robot_console_printf("NormalVelObstacle,Danger=%f\n",m_dDangerDegree);
m_dDangerDegree=0.25*(4-m_dMinObstalceDis-ObstacleLinVelocity/MIN_LEAVE_SPEED_FOR_IGNORE);//后一項考慮到了線速度的影響
//robot_console_printf("NormalVelObstacle,Danger=%f\n",m_dDangerDegree);
}
m_dDangerDegree=m_dDangerDegree>0?m_dDangerDegree:0;
m_dDangerDegree=m_dDangerDegree<1?m_dDangerDegree:1;
//if (m_dDangerDegree<0.001)
//{
// __asm int 3;
//}
return m_dDangerDegree;
}
bool CPathPlanInDynamic::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)
{
if (!Target||Target->distance<0||Target->distance>18||fabs(Target->angle)>360)
{
Target->angle=(int)Target->angle;
return false;
}///<導入目標信息，檢查信息是否有效。
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為存儲路徑規劃目標信息
m_dLengthPerTimeStep=8;//(0.5*(dl+dr));//自身本段時間內移動的距離，代表了自身移動速度500mm*1000/32
/*robot_console_printf("LengthPerTimeStep=%f",m_dLengthPerTimeStep);*/
if (!ReverseMoveDirect)
{
m_iSelfOrientation=90;
}
else
{
m_iSelfOrientation=270;
// robot_console_printf("ReverseMoveDirectm_iSelfOrientation=%f\n",m_iSelfOrientation);
}
m_pObstacleListHead=pObstacleList;
// SetRoadMap(pObstacleList,nObstacleMaxNum);
///**假設性暫時用第一個障礙物實驗***/
// m_OBSERVEDATA(0)=m_apObstaclePointerContainer[1]->distance;
// m_OBSERVEDATA(1)=m_apObstaclePointerContainer[1]->angle;
// RunTrackInDyn(dl,dr,m_OBSERVEDATA);//跟蹤
// m_mFilterResult=my_filter.m_mStateMeanResult;
//if (nObstacleMaxNum>15)//障礙物最大15個
//{
// return false;
//}
if(FindRelatedPosInLastStep(pObstacleList,nObstacleMaxNum))//尋找前一時刻自身的位置
{
//__asm int 3;
ConstructRoadMap(dl,dr,TimeStep);
}
else //一個都沒有找到則是靜態地圖
{
//__asm int 3;
ConstructRoadMap(dl,dr,TimeStep);
//ApfPathPlan::SetRoadMap(pObstacleList,nObstacleMaxNum);
}
//__asm int 3;
if (!SearchFreeTargetAngle(LeftForbidden,RightForbidden))
{
m_iTargetAngle=0;
return false;
}
//robot_console_print("SearchTargeAngleComplete\n");
int NearestObstacleIndex=SearchNearestDis();
//robot_console_print("SearchNearDisComplete\n");
//__asm int 3;
CalCulateDanger(NearestObstacleIndex);
#ifdef DEBUG_ON_VC_OUT_PUT_MORE
robot_console_printf("m_iTargetAngle=%d",m_iTargetAngle);
robot_console_printf("m_dDangerDegree=%f",m_dDangerDegree);
#endif
robot_console_printf("Danger=%f\n",m_dDangerDegree);
double DisOfTarA2SelfA=GetShortDistanceBetween2Angle(m_iTargetAngle,m_iSelfOrientation);
m_cFuzzyControlMod.ProcessFuzzyControl(DisOfTarA2SelfA,m_dDangerDegree);//->模糊控制依據前面的結果規劃出速度。
SetLRSpeedFreeRoad(bControlBall,m_cFuzzyControlMod.LineSpeedOutPut,m_cFuzzyControlMod.CornerSpeedOutPut,uSpeedLevel123_High);
return true;
}
void CPathPlanInDynamic::RunTrackInDyn(double dl,double dr,OBSERVE_STYLE& OBSERVEDATA)
{
// RunTrack(dl,dr,OBSERVEDATA);
}
//bool CPathPlanInDynamic::AddObject2RoadMap(PthPlSpa::ObjectPolePos *pObstacleList,int numOb)
//{
// //int numOb;//障礙物的編號
// //numOb=0;//0表示沒障礙物
// //int ObsObsoleteNum=0;
// PthPlSpa::ObjectPolePos * pObstacleAferExpand;
// if (ExpandObstacleRegion(pObstacleList))
// {
// // robot_console_printf("m_aObstacleStruct[indexOfmaObstacleStruct].angle=%f\n",m_aObstacleStruct[m_iObstacleNumAfterCheck].angle);
//
// pObstacleAferExpand=&m_aObstacleStruct[m_iObstacleNumAfterCheck];
// m_apObstaclePointerContainer[numOb+1]=pObstacleAferExpand;//>將有效障礙物的指針保存，同時該障礙物在m_apObstaclePointerContainer中的編號即numOb+1
// m_iObstacleNumAfterCheck++;
// }
// else//若發現膨脹失敗后障礙物把自己包進去了則仍然用未經膨脹的指針而不再用膨脹后存放的數組m_aObstacleStruct
// {
// pObstacleAferExpand=pObstacleList;
// m_apObstaclePointerContainer[numOb+1]=pObstacleList;
// }
//
// //按障礙物面積從小到大的順序排列它與目標的夾角
// if (CheckObstacleIsBehind(pObstacleAferExpand->LBoundaryAngle,pObstacleAferExpand->angle,pObstacleAferExpand->RBoundaryAngle))
// //>障礙物在身后橫跨360度分界線
// {
// /*對橫跨在背后的障礙物，其信息寫入時
// *要分兩批寫入,第一批:j從RBoundaryAngle加到360度
// *第二批:j從LBoundaryAngle減到0度*/
// // robot_console_printf("BehindObstacleNum=%d\n",numOb);
// int j=0;
// for (j=(int)pObstacleAferExpand->RBoundaryAngle;j<=360;j++)
// //第一批寫入信息pObstacleAferExpand->RBoundaryAngle為大的角度也是0~180,
// {
// //9.16日添加的修改，考慮障礙物的區域有相重的話取近的一個
// if ((int)m_nFreRoadMap[j]!=0)//已經有過障礙物在該角度賦過值
// {
// if (pObstacleAferExpand->distance<m_apObstaclePointerContainer[m_nFreRoadMap[j]]->distance)//新的障礙物更近，需要更新的話
// {
// m_nFreRoadMap[j]=numOb+1;//>在該角度方向存下障礙物的編號
// }//新的更遠的話則不更新m_nFreRoadMap;
// }
// else//第一次賦
// {
// m_nFreRoadMap[j]=numOb+1;//>在該角度方向存下障礙物的編號，-180對應m_nFreRoadMap[361]中的第0號數據（第1個），180度對應第360號（第361個）
// }
// // robot_console_printf("AngleOfObstacleNumIsBehind=%d",j);
// // robot_console_printf(" NumberOfObstacleNumIsBehind=%d\n",numOb);
//
// }
// for (j=(int)pObstacleAferExpand->LBoundaryAngle;j>=0;j--)//>障礙物在后面時左半邊的寫入。pObstacleAferExpand->LBoundaryAngle也是-180~180
// //第二批寫入信息pObstacleAferExpand->LBoundaryAngle為大的角度也是-180~0,
// {
// if ((int)m_nFreRoadMap[j]!=0)//已經有過障礙物在該角度賦過值
// {
// if (pObstacleAferExpand->distance<m_apObstaclePointerContainer[m_nFreRoadMap[j]]->distance)//新的障礙物更近，需要更新的話
// {
// m_nFreRoadMap[j]=numOb+1;//0對應m_nFreRoadMap[361]中的第0號數據（第1個），180度對應第360號（第361個）
// }//新的更遠的話則不更新m_nFreRoadMap
// }
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -