predictmodel.cpp

InnovLabSimu在vc++下实现

//#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])//左轮的移动距离>右轮的移动距离朝右拐即角度的变化为负角
	{
		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;//机器人中心的旋转半径,有正负的,沿正方向旋转时角速度为正,此时半径为正,反之为负

	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];//旋转矩阵
	/////////////////////////旋转矩阵赋值/////////////////////////////////////////////////
	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;///<依次为：将前一时刻坐标系中的物体1,0平移旋转得到在当前坐标系中的直角坐标/
	/////////////////将前一时刻的物体极坐标平移旋转得到在当前坐标系中的极坐标/////////////////////////////////////////////////////////
	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);
	///////////////经过坐标旋转得到预测点在当前坐标系中的极坐标///////////////////////////////////////////////////////////
	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;//机器人中心的旋转半径,有正负的,沿正方向旋转时角速度为正,此时半径为正,反之为负

	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*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];//旋转矩阵
	/////////////////////////旋转矩阵赋值/////////////////////////////////////////////////
	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;///<依次为：将前一时刻坐标系中的物体1,0平移旋转得到在当前坐标系中的直角坐标/
	/////////////////将前一时刻的物体极坐标平移旋转得到在当前坐标系中的极坐标/////////////////////////////////////////////////////////
	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);
	///////////////经过坐标旋转得到预测点在当前坐标系中的极坐标///////////////////////////////////////////////////////////
	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));//暂存下一预测点距离
//	AngularV=atan2(NextPredictY,NextPredictX);//暂存下一预测点角度，-Pi~Pi
//	LimitAngleIn0_2Pi(AngularV);//暂存下一预测点角度，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)//未扩展的状态变量有4个值
	{
		//TRACE("Only 4 state input with out augment");
	}
	else if (numOfInput==6)//扩展后的状态变量有6个值
	{
		
		InputState_Degree(x);
		//自身移动的距离输入并开始预测
		//double dl=x(4)+;
		//double dr=x(5);


		VelocityPredict(x(4)+dl, x(5)+dr);

		
		//状态转移完毕，将结果输出

		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)//未扩展的状态变量有4个值
	{
		//TRACE("Only 4 state input with out augment");
	}
	else if (numOfInput==6)//扩展后的状态变量有6个值
	{

		InputState_Degree(x);
		//自身移动的距离输入并开始预测
		//double dl=x(4)+;
		//double dr=x(5);


		VelocityPredict2WithTimeConsider(x(4)+dl, x(5)+dr,TimeStep);


		//状态转移完毕，将结果输出

		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)//未扩展的状态变量有4个值
	{
		//TRACE("Only 4 state input with out augment");
	}
	else if (numOfInput==6)//扩展后的状态变量有6个值
	{
		InputState(x);
		//自身移动的距离输入并开始预测
		//double dl=x(4)+;
		//double dr=x(5);
		VelocityPredict(x(4)+dl, x(5)+dr);
		//状态转移完毕，将结果输出
		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;
}