?? predictmodel.cpp
字號:
//#include "StdAfx.h"
#include "PredictModel.h"
CPredictModel::CPredictModel(void):ObjectAngleCheck(),m_vStateAfPredit(4)
{
}
CPredictModel::~CPredictModel(void)
{
}
inline void CPredictModel::GetSelfOriention()
{
if (m_ddeltaLeftAndRight[0]>m_ddeltaLeftAndRight[1])//左輪的移動距離>右輪的移動距離朝右拐即角度的變化為負(fù)角
{
m_ddeltaOrientation=-(double)(m_ddeltaLeftAndRight[0]-m_ddeltaLeftAndRight[1])/(ROBOT_WHEEL_DIS);//目前單位為弧度
m_iRightNeg_LeftPosit=-1;
}
else if (fabs(m_ddeltaLeftAndRight[0]-m_ddeltaLeftAndRight[1])<0.001)
{
m_ddeltaOrientation=0;
m_iRightNeg_LeftPosit=0;
}
else//左輪的移動距離<右輪的移動距離,朝左拐即角度的變化為正角
{
m_ddeltaOrientation=(double)(m_ddeltaLeftAndRight[1]-m_ddeltaLeftAndRight[0])/(ROBOT_WHEEL_DIS);//目前單位為弧度
m_iRightNeg_LeftPosit=1;
}
/*robot_console_printf("m_iRightNeg_LeftPosit=%d\n",m_iRightNeg_LeftPosit);
robot_console_printf("m_ddeltaOrientation=%f\n",m_ddeltaOrientation);*/
}
void CPredictModel::VelocityPredict(double dl, double dr)
{
m_ddeltaLeftAndRight[0]=dl;
m_ddeltaLeftAndRight[1]=dr;
GetSelfOriention();
double CosDeltaTheta=cos(m_ddeltaOrientation);
double SinDeltaTheta=sin(m_ddeltaOrientation);
float CenterTrackLength;///<兩幀之間路徑的長度
double PathLength;//已走過的路徑總長單位毫米
double CenterRadiu;//機(jī)器人中心的旋轉(zhuǎn)半徑,有正負(fù)的,沿正方向旋轉(zhuǎn)時角速度為正,此時半徑為正,反之為負(fù)
if (m_iRightNeg_LeftPosit!=0)
{
CenterRadiu=0.5*ROBOT_WHEEL_DIS*(m_ddeltaLeftAndRight[0]+m_ddeltaLeftAndRight[1])/(m_ddeltaLeftAndRight[1]-m_ddeltaLeftAndRight[0]);
m_sdeltaCoorinate.delta_x=CenterRadiu*(sin(HALF_PI+m_ddeltaOrientation)-1);
m_sdeltaCoorinate.delta_y=CenterRadiu*(-cos(HALF_PI+m_ddeltaOrientation));
}
else
{
PathLength=0.5*(m_ddeltaLeftAndRight[0]+m_ddeltaLeftAndRight[1]);
m_sdeltaCoorinate.delta_x=0;
m_sdeltaCoorinate.delta_y=PathLength;
}
/*robot_console_printf("m_sdeltaCoorinate.delta_x=%f",m_sdeltaCoorinate.delta_x);
robot_console_printf("m_sdeltaCoorinate.delta_y=%f\n",m_sdeltaCoorinate.delta_y);*/
m_sObjectPoleCoordP1_in_Pre.distance=m_sObjectPoleCoordP0_in_Pre.distance+m_sObject_Velocity_in_pre.linearV;
m_sObjectPoleCoordP1_in_Pre.angle=m_sObjectPoleCoordP0_in_Pre.angle+m_sObject_Velocity_in_pre.AngularV;
double RotateMatrix[2][2];//旋轉(zhuǎn)矩陣
/////////////////////////旋轉(zhuǎn)矩陣賦值/////////////////////////////////////////////////
RotateMatrix[0][0]=CosDeltaTheta;
RotateMatrix[1][1]=RotateMatrix[0][0];
RotateMatrix[0][1]=-SinDeltaTheta;
RotateMatrix[1][0]=-RotateMatrix[0][1];
double X_t0,Y_t0,X_t00,Y_t00;///<依次為：將前一時刻坐標(biāo)系中的物體1,0平移旋轉(zhuǎn)得到在當(dāng)前坐標(biāo)系中的直角坐標(biāo)/
/////////////////將前一時刻的物體極坐標(biāo)平移旋轉(zhuǎn)得到在當(dāng)前坐標(biāo)系中的極坐標(biāo)/////////////////////////////////////////////////////////
X_t00=m_sObjectPoleCoordP0_in_Pre.distance*cos(m_sObjectPoleCoordP0_in_Pre.angle)-m_sdeltaCoorinate.delta_x;
Y_t00=m_sObjectPoleCoordP0_in_Pre.distance*sin(m_sObjectPoleCoordP0_in_Pre.angle)-m_sdeltaCoorinate.delta_y;
m_sObjectPoleCoordP0_in_Current.distance=sqrt(pow(X_t00,2)+pow(Y_t00,2));
m_sObjectPoleCoordP0_in_Current.angle=atan2(Y_t00*CosDeltaTheta-X_t00*SinDeltaTheta,X_t00*CosDeltaTheta+Y_t00*SinDeltaTheta);
//if m_sObjectPoleCoordP0_in_Current_angle==NaN||m_sObjectPoleCoordP0_in_Current_angle>7||m_sObjectPoleCoordP0_in_Current_angle<-7;
//m_sObjectPoleCoordP0_in_Current_angle=NaN;
LimitAngleIn0_2Pi(m_sObjectPoleCoordP0_in_Current.angle);
///////////////經(jīng)過坐標(biāo)旋轉(zhuǎn)得到預(yù)測點(diǎn)在當(dāng)前坐標(biāo)系中的極坐標(biāo)///////////////////////////////////////////////////////////
X_t0=m_sObjectPoleCoordP1_in_Pre.distance*cos(m_sObjectPoleCoordP1_in_Pre.angle)-m_sdeltaCoorinate.delta_x;
Y_t0=m_sObjectPoleCoordP1_in_Pre.distance*sin(m_sObjectPoleCoordP1_in_Pre.angle)-m_sdeltaCoorinate.delta_y;
m_sObjectPoleCoordP1_in_Current.distance=sqrt(pow(X_t0,2)+pow(Y_t0,2));
m_sObjectPoleCoordP1_in_Current.angle=atan2(Y_t0*CosDeltaTheta-X_t0*SinDeltaTheta,X_t0*CosDeltaTheta+Y_t0*SinDeltaTheta);
LimitAngleIn0_2Pi(m_sObjectPoleCoordP1_in_Current.angle);
m_sObject_Velocity_in_Current.linearV=m_sObjectPoleCoordP1_in_Current.distance-m_sObjectPoleCoordP0_in_Current.distance;
m_sObject_Velocity_in_Current.AngularV=GetAngulardisplacementIn2Angle(m_sObjectPoleCoordP0_in_Current.angle,m_sObjectPoleCoordP1_in_Current.angle);
}
void CPredictModel::VelocityPredict2WithTimeConsider(double dl, double dr,double TimeStep)
{
m_ddeltaLeftAndRight[0]=dl;
m_ddeltaLeftAndRight[1]=dr;
GetSelfOriention();
double CosDeltaTheta=cos(m_ddeltaOrientation);
double SinDeltaTheta=sin(m_ddeltaOrientation);
float CenterTrackLength;///<兩幀之間路徑的長度
double PathLength;//已走過的路徑總長單位毫米
double CenterRadiu;//機(jī)器人中心的旋轉(zhuǎn)半徑,有正負(fù)的,沿正方向旋轉(zhuǎn)時角速度為正,此時半徑為正,反之為負(fù)
if (m_iRightNeg_LeftPosit!=0)//坐標(biāo)平移旋轉(zhuǎn)量
{
CenterRadiu=0.5*ROBOT_WHEEL_DIS*(m_ddeltaLeftAndRight[0]+m_ddeltaLeftAndRight[1])/(m_ddeltaLeftAndRight[1]-m_ddeltaLeftAndRight[0]);
m_sdeltaCoorinate.delta_x=CenterRadiu*(sin(HALF_PI+m_ddeltaOrientation)-1);
m_sdeltaCoorinate.delta_y=CenterRadiu*(-cos(HALF_PI+m_ddeltaOrientation));
}
else
{
PathLength=0.5*(m_ddeltaLeftAndRight[0]+m_ddeltaLeftAndRight[1]);
m_sdeltaCoorinate.delta_x=0;
m_sdeltaCoorinate.delta_y=PathLength;
}
/*robot_console_printf("m_sdeltaCoorinate.delta_x=%f",m_sdeltaCoorinate.delta_x);
robot_console_printf("m_sdeltaCoorinate.delta_y=%f\n",m_sdeltaCoorinate.delta_y);*/
//上一時刻值預(yù)測當(dāng)前位置
m_sObjectPoleCoordP1_in_Pre.distance=m_sObjectPoleCoordP0_in_Pre.distance+m_sObject_Velocity_in_pre.linearV*TimeStep*0.001;//速度單位為秒
m_sObjectPoleCoordP1_in_Pre.angle=m_sObjectPoleCoordP0_in_Pre.angle+m_sObject_Velocity_in_pre.AngularV*TimeStep*0.001;//速度單位為秒
double RotateMatrix[2][2];//旋轉(zhuǎn)矩陣
/////////////////////////旋轉(zhuǎn)矩陣賦值/////////////////////////////////////////////////
RotateMatrix[0][0]=CosDeltaTheta;
RotateMatrix[1][1]=RotateMatrix[0][0];
RotateMatrix[0][1]=-SinDeltaTheta;
RotateMatrix[1][0]=-RotateMatrix[0][1];
double X_t0,Y_t0,X_t00,Y_t00;///<依次為：將前一時刻坐標(biāo)系中的物體1,0平移旋轉(zhuǎn)得到在當(dāng)前坐標(biāo)系中的直角坐標(biāo)/
/////////////////將前一時刻的物體極坐標(biāo)平移旋轉(zhuǎn)得到在當(dāng)前坐標(biāo)系中的極坐標(biāo)/////////////////////////////////////////////////////////
X_t00=m_sObjectPoleCoordP0_in_Pre.distance*cos(m_sObjectPoleCoordP0_in_Pre.angle)-m_sdeltaCoorinate.delta_x;
Y_t00=m_sObjectPoleCoordP0_in_Pre.distance*sin(m_sObjectPoleCoordP0_in_Pre.angle)-m_sdeltaCoorinate.delta_y;
m_sObjectPoleCoordP0_in_Current.distance=sqrt(pow(X_t00,2)+pow(Y_t00,2));
m_sObjectPoleCoordP0_in_Current.angle=atan2(Y_t00*CosDeltaTheta-X_t00*SinDeltaTheta,X_t00*CosDeltaTheta+Y_t00*SinDeltaTheta);
//if m_sObjectPoleCoordP0_in_Current_angle==NaN||m_sObjectPoleCoordP0_in_Current_angle>7||m_sObjectPoleCoordP0_in_Current_angle<-7;
//m_sObjectPoleCoordP0_in_Current_angle=NaN;
LimitAngleIn0_2Pi(m_sObjectPoleCoordP0_in_Current.angle);
///////////////經(jīng)過坐標(biāo)旋轉(zhuǎn)得到預(yù)測點(diǎn)在當(dāng)前坐標(biāo)系中的極坐標(biāo)///////////////////////////////////////////////////////////
X_t0=m_sObjectPoleCoordP1_in_Pre.distance*cos(m_sObjectPoleCoordP1_in_Pre.angle)-m_sdeltaCoorinate.delta_x;
Y_t0=m_sObjectPoleCoordP1_in_Pre.distance*sin(m_sObjectPoleCoordP1_in_Pre.angle)-m_sdeltaCoorinate.delta_y;
m_sObjectPoleCoordP1_in_Current.distance=sqrt(pow(X_t0,2)+pow(Y_t0,2));
m_sObjectPoleCoordP1_in_Current.angle=atan2(Y_t0*CosDeltaTheta-X_t0*SinDeltaTheta,X_t0*CosDeltaTheta+Y_t0*SinDeltaTheta);
LimitAngleIn0_2Pi(m_sObjectPoleCoordP1_in_Current.angle);
X_t0=m_sObjectPoleCoordP1_in_Current.distance*cos(m_sObjectPoleCoordP1_in_Current.angle);
Y_t0=m_sObjectPoleCoordP1_in_Current.distance*sin(m_sObjectPoleCoordP1_in_Current.angle);
X_t00=m_sObjectPoleCoordP0_in_Current.distance*cos(m_sObjectPoleCoordP0_in_Current.angle);
Y_t00=m_sObjectPoleCoordP0_in_Current.distance*sin(m_sObjectPoleCoordP0_in_Current.angle);
OrthogonalCoordVel2PolarCoordVel(X_t0,Y_t0,X_t0-X_t00,Y_t0-Y_t00,
m_sObject_Velocity_in_Current.linearV,m_sObject_Velocity_in_Current.AngularV,
TimeStep,m_sObjectPoleCoordP1_in_Current);
//m_sObject_Velocity_in_Current.linearV=m_sObjectPoleCoordP1_in_Current.distance-m_sObjectPoleCoordP0_in_Current.distance;
//m_sObject_Velocity_in_Current.AngularV=GetAngulardisplacementIn2Angle(m_sObjectPoleCoordP0_in_Current.angle,m_sObjectPoleCoordP1_in_Current.angle);
}
//void CPredictModel::OrthogonalCoordVel2PolarCoordVel(double& CurrentPredictX,double& CurrentPredictY,
// double& DeltX,double& DeltY,double& LinearV,double& AngularV,double TimeStep,ObjectPolePos&CurrenPredictPolePos)
//{
// double NextPredictX=CurrentPredictX+DeltX;
// double NextPredictY=CurrentPredictY+DeltY;
// LinearV=sqrt(pow(NextPredictX,2)+pow(NextPredictY,2));//暫存下一預(yù)測點(diǎn)距離
// AngularV=atan2(NextPredictY,NextPredictX);//暫存下一預(yù)測點(diǎn)角度，-Pi~Pi
// LimitAngleIn0_2Pi(AngularV);//暫存下一預(yù)測點(diǎn)角度，0~2Pi
// LinearV=LinearV-CurrenPredictPolePos.distance;
// AngularV=GetAngulardisplacementIn2Angle(CurrenPredictPolePos.angle,AngularV);
//
//}
STATE_STYLE& CPredictModel::fx_Degree( STATE_STYLE& x,double dl,double dr)
{
int numOfInput=x.size();
if (numOfInput==4)//未擴(kuò)展的狀態(tài)變量有4個值
{
//TRACE("Only 4 state input with out augment");
}
else if (numOfInput==6)//擴(kuò)展后的狀態(tài)變量有6個值
{
InputState_Degree(x);
//自身移動的距離輸入并開始預(yù)測
//double dl=x(4)+;
//double dr=x(5);
VelocityPredict(x(4)+dl, x(5)+dr);
//狀態(tài)轉(zhuǎn)移完畢，將結(jié)果輸出
m_vStateAfPredit(0)=m_sObjectPoleCoordP1_in_Current.distance;
m_vStateAfPredit(1)=TransAngleToDegree(m_sObjectPoleCoordP1_in_Current.angle);
m_vStateAfPredit(2)=m_sObject_Velocity_in_Current.linearV;
m_vStateAfPredit(3)=TransAngleToDegree(m_sObject_Velocity_in_Current.AngularV);
}
return m_vStateAfPredit;
}
STATE_STYLE& CPredictModel::fx_Degree2ConsiderTime( STATE_STYLE& x,double dl,double dr,double TimeStep)
{
int numOfInput=x.size();
if (numOfInput==4)//未擴(kuò)展的狀態(tài)變量有4個值
{
//TRACE("Only 4 state input with out augment");
}
else if (numOfInput==6)//擴(kuò)展后的狀態(tài)變量有6個值
{
InputState_Degree(x);
//自身移動的距離輸入并開始預(yù)測
//double dl=x(4)+;
//double dr=x(5);
VelocityPredict2WithTimeConsider(x(4)+dl, x(5)+dr,TimeStep);
//狀態(tài)轉(zhuǎn)移完畢，將結(jié)果輸出
m_vStateAfPredit(0)=m_sObjectPoleCoordP1_in_Current.distance;
m_vStateAfPredit(1)=TransAngleToDegree(m_sObjectPoleCoordP1_in_Current.angle);
m_vStateAfPredit(2)=m_sObject_Velocity_in_Current.linearV;
m_vStateAfPredit(3)=TransAngleToDegree(m_sObject_Velocity_in_Current.AngularV);
}
return m_vStateAfPredit;
}
STATE_STYLE& CPredictModel::fx( STATE_STYLE& x,double dl,double dr)
{
int numOfInput=x.size();
if (numOfInput==4)//未擴(kuò)展的狀態(tài)變量有4個值
{
//TRACE("Only 4 state input with out augment");
}
else if (numOfInput==6)//擴(kuò)展后的狀態(tài)變量有6個值
{
InputState(x);
//自身移動的距離輸入并開始預(yù)測
//double dl=x(4)+;
//double dr=x(5);
VelocityPredict(x(4)+dl, x(5)+dr);
//狀態(tài)轉(zhuǎn)移完畢，將結(jié)果輸出
m_vStateAfPredit(0)=m_sObjectPoleCoordP1_in_Current.distance;
m_vStateAfPredit(1)=m_sObjectPoleCoordP1_in_Current.angle;
m_vStateAfPredit(2)=m_sObject_Velocity_in_Current.linearV;
m_vStateAfPredit(3)=m_sObject_Velocity_in_Current.AngularV;
}
return m_vStateAfPredit;
}
bool CPredictModel::InputState(const STATE_STYLE& x)
{
m_sObjectPoleCoordP0_in_Pre.distance=x(0);
m_sObjectPoleCoordP0_in_Pre.angle=x(1);
m_sObject_Velocity_in_pre.linearV=x(2);
m_sObject_Velocity_in_pre.AngularV=x(3);
return true;
}
bool CPredictModel::InputState_Degree(const STATE_STYLE& x)
{
m_sObjectPoleCoordP0_in_Pre.distance=x(0);
m_sObjectPoleCoordP0_in_Pre.angle=TransAngleToArc((double)x(1));
m_sObject_Velocity_in_pre.linearV=x(2);
m_sObject_Velocity_in_pre.AngularV=TransAngleToArc((double)x(3));
return true;
}?? 快捷鍵說明
復(fù)制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -