Стабилизация — уменьшение погрешности положения при максимальном ускорении, максимальном замедлении и максимальной скорости

Есть проблема, с которой я борюсь, и мне нужна помощь. Я постараюсь объяснить это полностью.

Я работаю над симуляцией танка. Танк составлен так: у меня есть шасси. На шасси расположена турель, которая может вращаться только по азимуту. На башне есть несколько других вещей, таких как ствол (который может вращаться по высоте), азимутальная часть системы прицеливания стрелка (GSS) и азимутальная часть системы прицеливания командира (CSS). В азимутальной части GSS — это отметка GSS, а в азимутальной части CSS — это отметка CSS. Конечно, азимутальные части GSS и CSS могут вращаться только по азимуту, и то же самое верно для частей возвышения, которые могут вращаться только по высоте. Конечно, поскольку GSS находится на револьверной головке, если револьверная головка вращается, это также заставляет GSS вращаться.

Итак, давайте вспомним немного:

Chassis <- Turret (AZ) <- Barrel (EL).
Chassis <- Turret (AZ) <- GSS (AZ) <- GSS (EL).
Chassis <- Turret (AZ) <- CSS (AZ) <- CSS (EL).

Каждая часть может рассматриваться как DOF (область свободы), заблокированная только на одной оси (азимут или угол места).

Теперь каждая часть имеет некоторые ограничения движения:

Турель может вращаться в диапазоне [-360 °, + 360 °] (поэтому на этом нет ограничений).

Ствол может вращаться в диапазоне [-10 °, + 50 °] относительно его родителя.
турель.

GSS AZ может вращаться в диапазоне [-5 °, + 5 °].

GSS EL может вращаться в диапазоне [-10 °, + 50 °].

CSS AZ может вращаться в диапазоне [-360 °, + 360 °].

CSS EL может вращаться в диапазоне [-10 °, + 50 °].

Каждая часть также имеет некоторые физические ограничения:

GSS AZ, GSS EL, CSS AZ и CSS EL имеют максимальное ускорение 570 ° / с², максимальное замедление также 570 ° / с² и максимальную скорость 57 ° / с.

У турели максимальное ускорение 32 ° / с², максимальное замедление 160 ° / с² и максимальная скорость 40 ° / с.

Баррель имеет максимальное ускорение 57 ° / с², максимальное замедление 114 ° с² и максимальную скорость 34 ° / с.

Теперь проблема заключается в следующем. Для GSS у меня есть два фиксированных значения, которые представляют мировое направление. Один для азимута, другой для возвышения. Давайте представим, что это 25 ° (AZ) и 3 ° (EL). Это означает, что часть СОБ должна быть в этом направлении в мировой референции. Эти значения могут быть обновлены в режиме реального времени (например, мы можем управлять этими значениями с помощью джойстика, чтобы перемещать камеру). Я хочу, чтобы GSS всегда выровнялась с этими значениями (я могу допустить небольшую ошибку только в 5 * 10 ^ (- 5) радиан), даже когда мы едем и шасси движется (как в положении, так и в ориентации) , Конечно, поскольку GSS имеет некоторые ограничения, он не всегда может быть выровнен с направлением. Чтобы решить эту проблему, револьверная головка также должна быть выровнена с этими значениями (и поэтому, когда револьверная головка вращается, чтобы быть выровненной по азимуту, она приближает GSS к требуемому направлению).

В любой момент у меня есть доступ к положению (азимуту или углу места) объекта относительно его родителя (и, таким образом, я могу иметь его направление в мировой референции) и скорости вращения относительно его родителя.

Единственный параметр, по которому я могу играть для управления каждой частью, — это скорость части относительно ее родителя.

Что я сделал, так это конвертировал все мировые направления в референтную ссылку своего родителя. Отсюда я просто вычисляю желаемую скорость, соблюдая при этом различные ограничения (в отношении ускорения, замедления и скорости). Вот код, который я использую для вычисления скорости:

double desiredVelocity = (wrappedDesiredPosition - wrappedCurrentPosition) / deltaTime;
if (std::abs(desiredVelocity) > object.maximumVelocity)
desiredVelocity *= object.maximumVelocity / std::abs(desiredVelocity);
const double maximumForce = (std::abs(desiredVelocity) < std::abs(object.velocity) || Sign(desiredVelocity) == -Sign(object.velocity)) ? object.maximumDeceleration : object.maximumAcceleration;
double requiredForce = (desiredVelocity - object.velocity) / deltaTime;
if (std::abs(requiredForce) > maximumForce)
{
requiredForce *= maximumForce / std::abs(requiredForce);
desiredVelocity = object.velocity + requiredForce * deltaTime;
if (std::abs(desiredVelocity) > object.maximumVelocity)
desiredVelocity *= object.maximumVelocity / std::abs(desiredVelocity);
}

Здесь уже учтены ограничения в положении (обернутаяДополненная позиция была зафиксирована).

Это работает, однако, я точно не достаточно точен. Камера, как правило, немного колеблется (хотя и немного), прежде чем она стабилизируется правильно. Я сделал несколько измерений, и я стабилизировался с точностью около 3.5*10^(-3) радианы, когда шасси движется (со скоростью 16 км / ч с некоторыми неровностями). Это определенно недостаточно хорошо, так как я хочу достичь 5.0*10^(-5) радианы в точности.

Что я могу сделать, чтобы улучшить мою стабилизацию? Мне нужно улучшить его на величину ~ 70, что кажется огромным. Я знаю о ПИД-контроллерах, которые могут помочь, но я не уверен, как правильно применять их здесь, и я не уверен, что это может помочь мне достичь моей цели.

Если у кого-то есть советы, как это улучшить, я был бы рад услышать об этом.