Алгоритм стабилизации при наведении на определённую цель

Пусть есть установка, показанная на рисунке. На платформе установлен двигатель с осью вращения в точке О1. Сама платформа может колебаться – при этом по оси О она неподвижна, то есть колебания происходят относительно оси О.

На платформе установлен измеритель угловой скорости. Положение платформы будем определять углом отклонения от горизонтали в положительном или отрицательном направлении.

На оси, на которой установлен двигатель, имеется также датчик угла, определяющий угловое положение. Угол определяется отклонением луча О₁В от вертикали.

У нас заданы величины ОА – расстояние от точки О до цели, на которую будет наведение. И расстояние ОО₁ - расстояние от точки О до оси вращения двигателя О₁.

Нам необходимо компенсировать колебания платформы вращением двигателя так, чтобы луч О₁В проходил через точку А. Это моделирует ситуацию с наведением на цель.

Рис 4.1.1 Схема установки

Будем считывать с интервалом ∆t показания с датчика угла и с измерителя угловой скорости, а также посылать сигнал на двигатель: угол поворота и угловая скорость.

Пусть вначале платформа находится в горизонтальном положении, а затем через время ∆t она повернулась на угол φ (рис. Н). Тогда чтобы луч О₁В проходил через точку А нужно повернуть двигатель на угол φ против часовой стрелки до вертикального положения. А затем на угол α также против часовой стрелки.

Найдём угол α с помощью заданных величин ОА = h, ОО₁ = r. А также .

Из треугольника ОО₁С находим: ,

Отсюда .

Теперь можно найти tgα: ;

Отсюда находим .

Получили, что двигатель должен совершить поворот на угол (α+φ) против часовой стрелки.

Рис 4.1.2 Нахождение угла поворота ротора на первом шаге

Далее пусть прошло ещё ∆t времени. Пусть двигатель был повёрнут на угол α1 = α от вертикали. А за время ∆t платформа повернулась на угол φ1 и отклонение платформы равно углу φ – сумме двух поворотов.

В положении 2 необходимый угол отклонения от вертикали луча О₁В обозначим как α2. Угол α2 находится аналогично из величин h, r и угла φ:

.

Чтобы перейти из положения 1 в положение 2 с наведением на цель, необходимы следующим образом поворачивать двигатель: сначала по часовой стрелке на угол α1, затем против часовой стрелки на угол (α2 + φ). То есть повернуть против часовой стрелки на угол (φ + (α2 – α1)) = (φ + Δ α), где Δ α = α2 – α1.

Рис 4.1.3 Нахождение угла поворота ротора на произвольном шаге

Мы знаем, как рассчитать угол поворота двигателя. Нам нужно, чтобы прогнозировался угол поворота платформы через Δt.

Так как Δt у нас мало – порядка десятков миллисекунд, то можно вначале считать угловую скорость ω1 с измерителя угловой скорости, установленного на платформе. А затем считать, что угол Δφ, на который повернётся платформа в течение Δt равным ω1Δt.

Также мы будем считывать и показания датчика угла – φ1 – угол определяющий положение платформы.

Тогда мы считаем, что через Δt положение платформы будет определяться углом φ = φ1 + Δφ. И поэтому можно найти углы α1 и α2 по формулам:

;

.

И тогда нужно задать, что двигатель должен повернуться на угол α = α2 – – α1 + Δφ с угловой скоростью ω = α / Δt.

Сформулируем алгоритм полностью:

1. Пусть начальное положение платформы горизонтальное и луч наведён на цель. Тогда φ = 0, α = 0, также заданы r и h;

Дальнейшие пункты повторяем при t = nΔt, где n = 0, 1, 2, …

2. Считываем угловую скорость платформы ω1;

3. Считываем положение платформы φ1;

4. Прогнозируем положение платформы через Δt: φ = φ1 + ω1Δt;

5. По формулам находим α1 и α2:

; .

6. Подаем на двигатель значения угла, на который нужно совершить поворот и угловой скорости этого поворота: α = α2 – α1 + Δφ, ω = α / Δt.