Skocz do zawartości

Śledziennie trajektorii zadanej wzorem


ps19

Pomocna odpowiedź

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 przez ps19
Link do komentarza
Share on other sites

Dołącz do dyskusji, napisz odpowiedź!

Jeśli masz już konto to zaloguj się teraz, aby opublikować wiadomość jako Ty. Możesz też napisać teraz i zarejestrować się później.
Uwaga: wgrywanie zdjęć i załączników dostępne jest po zalogowaniu!

Anonim
Dołącz do dyskusji! Kliknij i zacznij pisać...

×   Wklejony jako tekst z formatowaniem.   Przywróć formatowanie

  Dozwolonych jest tylko 75 emoji.

×   Twój link będzie automatycznie osadzony.   Wyświetlać jako link

×   Twoja poprzednia zawartość została przywrócona.   Wyczyść edytor

×   Nie możesz wkleić zdjęć bezpośrednio. Prześlij lub wstaw obrazy z adresu URL.

×
×
  • Utwórz nowe...

Ważne informacje

Ta strona używa ciasteczek (cookies), dzięki którym może działać lepiej. Więcej na ten temat znajdziesz w Polityce Prywatności.