Я пытаюсь заставить робота в симуляции перемещаться на точное расстояние, отправляя сообщения с линейной скоростью. Прямо сейчас моя реализация не заставляет робота двигаться на точное расстояние. Это пример кода:
void Robot::travel(double x, double y)
{
// px and py are the current positions (constantly gets updated as the robot moves in the simulation)
// x and y is the target position that I want to go to
double startx = px;
double starty = py;
double distanceTravelled = 0;
align(x, y);
// This gets the distance from the robot's current position to the target position
double distance = calculateDistance(px, py, x, y);
// Message with velocity
geometry_msgs::Twist msg;
msg.linear.x = 1;
msg.angular.z = 0;
ros::Rate loop_rate(10);
while (distanceTravelled < distance)
{
distanceTravelled = calculateDistance(startx, starty, px, py);
// Publishes message telling the robot to move with the linear.x velocity
RobotVelocity_pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
}
}
Я спросил, и кто-то предположил, что использование ПИД-регулятора для контура обратной связи решит эту проблему, но после прочтения страницы в Википедии я действительно не понимаю, как я буду использовать его в этом контексте. На странице Википедии есть псевдокод алгоритма PID, но я понятия не имею, что чему соответствует.
previous_error = setpoint - process_feedback
integral = 0
start:
wait(dt)
error = setpoint - process_feedback
integral = integral + (error*dt)
derivative = (error - previous_error)/dt
output = (Kp*error) + (Ki*integral) + (Kd*derivative)
previous_error = error
goto start
Как бы я реализовал это в контексте скоростей и расстояний? Ошибка расстояния? Или скорость? Кто-нибудь может помочь, пожалуйста? Что такое интеграл? Производная? Кр? Ки? Кд? так далее.
Спасибо.
