?? 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))//依據(jù)速度移動物體的位置,ObjectDisplace
{
AddObject2RoadMap(pObstacle,numOb,m_aPathTarget.distance);//將考慮速度后的物體加入到地圖中
}
//而且要求m_aObstacleStruct中的內(nèi)容與m_vpConfirmedObject一至，否則會出錯
{
m_aObstacleStruct.push_back(*pObstacle);
}
return true;
}
//本函數(shù)中，外部輸入的觀測數(shù)據(jù)距離單位為m角度單位為度，由于跟蹤器中所用單位分別為毫米、角度（也可能是弧度由USE_DEGREE_ANGLE決定）
//注意: 以ObjecPolePos結構體形式存的所有數(shù)據(jù)都采用觀測數(shù)據(jù)的單位,而m_mFilterResult的矩陣形式則采用跟蹤器中的格式，所以本函數(shù)中進行了轉換，
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);
//清空上一輪保存的跟蹤狀態(tài)，并重新保存，以確保本輪跟蹤中沒有用到的物體的狀態(tài)被去除掉
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;//當前已找到上一時刻位置項的數(shù)據(jù)進入濾波跟蹤
// 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)//確實在前一時刻的結果中找到了自己的位置則:濾波,保存對應狀態(tài),地圖中置動態(tài)障礙物
{
//m_viRelateParm中的值從1開始,而下標從0開始故需要-1
SetFltState(SavedFilterResult[*pRelateParm-1],SavedFilterResultVar[*pRelateParm-1],SavedModelNoiseVar[*pRelateParm-1]);//將上一個時刻該物體的濾波器狀態(tài)恢復
m_OBSERVEDATA(0)=m_vpConfirmedObject[iIndexOfAllConfirmedObstacle]->distance*1000;//當前已找到上一時刻位置項的數(shù)據(jù)進入濾波跟蹤
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++;
/////保存濾波器狀態(tài)濾出來的角直接是度了///
//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////在前一時刻的結果中找不到自己的位置則:地圖中置靜態(tài)障礙物,保存當前的位置,以及各方差為初始值
{
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);
//LARGE_INTEGER LITmp;
//QueryPerformanceCounter(&LITmp);
//m_vPreSystemCountNumVec.push_back(LITmp);//每發(fā)現(xiàn)上一時刻位置則將當前時間點記下;
}
pRelateParm++;//注意必須與iIndexOfAllConfirmedObstacle一致且同步變化
iIndexOfAllConfirmedObstacle++;
}
else//根本就沒有搜到相關項,一開始會出現(xiàn)這種情況
{
CAdaptiveOptimaser NewAdaptiveOptimizer;
NewAdaptiveOptimizer.initAdapter(m_cAdaptiveOptimiser.m_mNodelNoiseOriginalVal,m_cAdaptiveOptimiser.m_fStableDifAmplitudeParm);
m_vAdaptiveOptimizerVec.push_back(NewAdaptiveOptimizer);//創(chuàng)建自適應調(diào)節(jié)器
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);//每發(fā)現(xiàn)上一時刻位置則將當前時間點記下;
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)//計算風險函數(shù),不可忽略,但是物體在遠離
{
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為存儲路徑規(guī)劃目標信息
m_aPathTarget.y=Target->centerY;//m_aPathTarget為存儲路徑規(guī)劃目標信息
m_aPathTarget.distance=Target->distance;//m_aPathTarget為存儲路徑規(guī)劃目標信息
m_aPathTarget.angle=Target->angle;//m_aPathTarget為存儲路徑規(guī)劃目標信息
m_dLengthPerTimeStep=8;//(0.5*(dl+dr));//自身本段時間內(nèi)移動的距離，代表了自身移動速度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 //一個都沒有找到則是靜態(tài)地圖
{
//__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);//->模糊控制依據(jù)前面的結果規(guī)劃出速度。
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//若發(fā)現(xiàn)膨脹失敗后障礙物把自己包進去了則仍然用未經(jīng)膨脹的指針而不再用膨脹后存放的數(shù)組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日添加的修改，考慮障礙物的區(qū)域有相重的話取近的一個
// if ((int)m_nFreRoadMap[j]!=0)//已經(jīng)有過障礙物在該角度賦過值
// {
// 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號數(shù)據(jù)（第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)//已經(jīng)有過障礙物在該角度賦過值
// {
// if (pObstacleAferExpand->distance<m_apObstaclePointerContainer[m_nFreRoadMap[j]]->distance)//新的障礙物更近，需要更新的話
// {
// m_nFreRoadMap[j]=numOb+1;//0對應m_nFreRoadMap[361]中的第0號數(shù)據(jù)（第1個），180度對應第360號（第361個）
// }//新的更遠的話則不更新m_nFreRoadMap
// }
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -