Śledziennie trajektorii zadanej wzorem

[ps19]

Witam

Próbuję zaimplementować algorytm podążania robota mobilnego 2-kołowego po trajektorii zadanej z góry wzorem na podstawie znalezionych materiałów.

materiały:

https://www.researchgate.net/publication/225543929_Control_of_Wheeled_Mobile_Robots_An_Experimental_Overview 

(strona 16 - Nonlinear Control Design)

https://kcir.pwr.edu.pl/~mucha/Pracki/Rafal_Cyminski_praca_magisterska.pdf 

(strona 38)

Problem polega na tym, że robot albo jedzie tylko część trajektorii albo ruch wynikowy nie przypomina ani trochę trajektorii wynikowej.

 

void lissajousInit(lissajousTypedef *liss,trajectoryTypedef *traj, sampsonTypedef *samp)
{
	traj->ts = 0.01F;
	robotMove1.ts = traj->ts;
	liss->amplitudeX_f32 = 0.5F;
	liss->amplitudeY_f32 = 0.6F;

	liss->paramA_u8 = 1.0F;
	liss->paramB_u8 = 2.0F;

	liss->paramDelta_f32 = M_PI/2;

	samp->Kp_f32 = 0.526;
	samp->Kd_f32 = 0.00263;

	traj->translationDesigned_f32 = integrateTrapz(0, M_TWOPI, 1000.0F, lissajousTrajectoryVelocitySpecial);
}
void lissajousTrajectoryGenarate(lissajousTypedef *liss,trajectoryTypedef *traj, sampsonTypedef *samp,  robotMoveTypedef *robMov)
{
	if(robMov->translationMeters <= traj->translationDesigned_f32)
	{
		traj->time_f32 += traj->ts;//M_PI_DIV;
		traj->trajRunning_u8 = 1;

		lissajousTrajectoryPath(traj, liss);
		calcAngleRadians(traj);
		trajectoryVelocity(traj);
		trajectoryAcceleration(traj);

		samp->angleNow_f32 = robMov->robotAngleDiff;
		samp->angleErr_f32 = (traj->angleRadiansDiff_f32- robMov->robotAngleDiff);

		samp->xErr_f32 = (traj->px_f32 - robMov->x);
		samp->yErr_f32 = (traj->py_f32 - robMov->y);

		samp->progrVelDesigned_f32 = traj->vxy_f32;
		samp->angVelDesigned_f32 = ((traj->ay_f32*traj->vx_f32) - (traj->ax_f32*traj->vy_f32))/traj->vxy_f32;
		sampsonAlgorithm(samp);
	}
	else
	{
		traj->trajRunning_u8 = 0;
		//robMov->translationMeters = traj->translationDesigned_f32;
	}
}
void lissajousTrajectoryPath(trajectoryTypedef *traj, lissajousTypedef *liss)
{
	traj->pxPrev_f32 = traj->px_f32;
	traj->pyPrev_f32 = traj->py_f32;

	traj->px_f32 = liss->amplitudeX_f32*arm_sin_f32(liss->paramA_u8*traj->time_f32 + liss->paramDelta_f32);
	traj->py_f32 = liss->amplitudeY_f32*arm_sin_f32(liss->paramB_u8*traj->time_f32);
}
void trajectoryVelocity(trajectoryTypedef *traj)
{
	if(lisTraj.amplitudeX_f32 > 0)
	{
		traj->vx_f32 = lisTraj.amplitudeX_f32*lisTraj.paramA_u8*cosf(lisTraj.paramA_u8*traj->ts + lisTraj.paramDelta_f32);
		traj->vy_f32 = lisTraj.amplitudeY_f32*lisTraj.paramB_u8*cosf(lisTraj.paramB_u8*traj->ts);
	}
	else
	{
		//for anothor purpose
		traj->vxPrev_f32 = traj->vx_f32;
		traj->vyPrev_f32 = traj->vy_f32;

		traj->vx_f32 = (traj->pxPrev_f32 - traj->px_f32)/traj->ts;
		traj->vy_f32 = (traj->pyPrev_f32 - traj->py_f32)/traj->ts;

		if(traj->velocityDesigned_f32)
		{
			if(traj->vx_f32 != 0) traj->vx_f32 = (traj->velocityDesigned_f32/traj->vx_f32)*traj->vx_f32;
			if(traj->vy_f32 != 0) traj->vy_f32 = (traj->velocityDesigned_f32/traj->vy_f32)*traj->vy_f32;
		}
	}

	float32_t sumOfSqares = (traj->vx_f32*traj->vx_f32) + (traj->vy_f32*traj->vy_f32);
	traj->vxy_f32 = sqrtf(sumOfSqares);
}

void trajectoryAcceleration(trajectoryTypedef *traj)
{
	if(lisTraj.amplitudeX_f32 > 0)
	{
		traj->ax_f32 = -lisTraj.amplitudeX_f32*lisTraj.paramA_u8*lisTraj.paramA_u8*sinf(lisTraj.paramA_u8*traj->ts + lisTraj.paramDelta_f32);
		traj->ay_f32 = -lisTraj.amplitudeY_f32*lisTraj.paramB_u8*lisTraj.paramB_u8*sinf(lisTraj.paramB_u8*traj->ts);
	}
	else
	{
		//for anothor purpose
		traj->ax_f32 = (traj->vxPrev_f32 - traj->vx_f32)/traj->ts;
		traj->ay_f32 = (traj->vyPrev_f32 - traj->vy_f32)/traj->ts;
	}

	float32_t sumOfSqares = (traj->ax_f32*traj->ax_f32) + (traj->ay_f32*traj->ay_f32);
	traj->axy_f32 = sqrtf(sumOfSqares);
}
void calcAngleRadians(trajectoryTypedef *traj)
{
	traj->angleRadiansPrev_f32 = traj->angleRadians_f32;
	traj->angleRadians_f32 = atan2f(traj->py_f32, traj->px_f32);
	traj->angleRadiansDiff_f32 = traj->angleRadians_f32 - traj->angleRadiansPrev_f32;
}
float deg2rad(float num)
{
	return num * (M_PI / M_PI_DEG);
}
float rad2deg(float num)
{
	return num * (M_PI_DEG / M_PI);
}

static float32_t lissajousTrajectoryVelocitySpecial(float32_t mytime)
{
	static float32_t vxySpecial;
	trajectory1.vx_f32 = lisTraj.amplitudeX_f32*lisTraj.paramA_u8*cosf((lisTraj.paramA_u8*mytime) + lisTraj.paramDelta_f32);
	trajectory1.vy_f32 = lisTraj.amplitudeY_f32*lisTraj.paramB_u8*cosf(lisTraj.paramB_u8*mytime);

	float32_t sumOfSqares = (trajectory1.vx_f32*trajectory1.vx_f32) + (trajectory1.vy_f32*trajectory1.vy_f32);
	vxySpecial = sqrtf(sumOfSqares);

	return vxySpecial;
}

float32_t integrateTrapz(float32_t xp, float32_t xk, uint16_t N, float32_t(*f)(float32_t))
{
	float32_t dx = 0, s = 0;
	s = 0;
	dx = (xk - xp) / N;
	for (int i = 1; i < N; i++)
	{
		s += (*f)(xp + i * dx);
	}
	s = (s + ((*f)(xp) + (*f)(xk)) / 2) * dx;

	return s;
}

Algorytm omawiany w wyżej wymienionych publikacjach:

void sampsonAlgorithm(sampsonTypedef *alg)
{
	alg->xTrackErr_f32 =  cosf(alg->angleNow_f32)*alg->xErr_f32 + sinf(alg->angleNow_f32)*alg->yErr_f32;
	alg->yTrackErr_f32 = -sinf(alg->angleNow_f32)*alg->xErr_f32 + cosf(alg->angleNow_f32)*alg->yErr_f32;

	if(alg->angleErr_f32 == 0) alg->angleErr_f32 = FLT_MIN; //very small number - 0 casued NaN (Not a Number)
	alg->progrVelRef_f32 = alg->Kp_f32*alg->xTrackErr_f32 + alg->progrVelDesigned_f32*cosf(alg->angleErr_f32);
	alg->angVelRef_f32 = alg->angVelDesigned_f32 + (alg->Kd_f32*alg->angleErr_f32) + (alg->progrVelDesigned_f32*alg->yTrackErr_f32*(sinf(alg->angleErr_f32)/alg->angleErr_f32));

	alg->velLeftWheel_f32 =  alg->progrVelRef_f32 - (ROBOT_LENGHT/2)*alg->angVelRef_f32;
	alg->velRightWheel_f32 = alg->progrVelRef_f32 + (ROBOT_LENGHT/2)*alg->angVelRef_f32;

	alg->velLeftWheelTicks_f32 = (convertMetersToTicks(alg->velLeftWheel_f32)/100.0F);
	alg->velRightWheelTicks_f32 = (convertMetersToTicks(alg->velRightWheel_f32)/100.0F);
}

Obliczanie pozycji robota:

void calculateRobotKinematics(encoderTypedef *encLeft, encoderTypedef *encRight, robotMoveTypedef *robMov)
{
	robMov->translationMeters = (encLeft->distanceMeters_f32 + encRight->distanceMeters_f32)/2;
	robMov->translationMetersDiff = (encLeft->distanceMetersDiff_f32 + encRight->distanceMetersDiff_f32)/2;
	MPU6050_Read_Gyro(&hi2c3, &MPU6050);
    if(fabsf(MPU6050.Gz) < 0.1F) //high-pass filter [deg]
    {
    	MPU6050.Gz = 0.0F;
    }
	robMov->rotationRadians += deg2rad(MPU6050.Gz)*robMov->ts;
			// (encRight->distanceMeters - encLeft->distanceMeters)/ROBOT_LENGHT;

	robMov->rotationRadiansDiff = robMov->rotationRadians - robMov->rotationRadiansPrev; //może *ts ???
	robMov->robotAngleDiff = robMov->robotAngle - robMov->robotAnglePrev;  //może *ts ???

	robMov->x = robMov->translationMeters*cosf(robMov->rotationRadians);
	robMov->y = robMov->translationMeters*sinf(robMov->rotationRadians);

	robMov->robotAngle = atan2f(robMov->y, robMov->x);

	robMov->rotationRadiansPrev = robMov->rotationRadians;
	robMov->robotAnglePrev = robMov->robotAngle;
}

Wywołanie całości (co 10ms --> f = 100Hz):

			if(START_PILOT_VIENNA || START_PILOT_JSUMO)
			{
				lissajousTrajectoryGenarate(&lisTraj,&trajectory1, &sampsonData,  &robotMove1);
				PIDwheelVelocity(&pidLeftWheel, &encoderLeft, sampsonData.velLeftWheelTicks_f32);
				PIDwheelVelocity(&pidRightWheel, &encoderRight, sampsonData.velRightWheelTicks_f32);
				if(trajectory1.trajRunning_u8)
				{
					MotorSpeed(pidLeftWheel.PWMout, pidRightWheel.PWMout);
				}
				else
				{
					MotorSpeed(0,0);
				}
			}
			else
			{
				MotorSpeed(0,0);
			}

 

Edytowano Styczeń 8, 2021 przez ps19