Система управления движением судна по заданной траектории

Система управления движением судна по заданной траектории, содержащая обеспечивающие формирование двух законов:
1) типовой закон автоматического управления движением судна по линиям отрезков заданной траектории, который формируется в блоках СУД:
зд.1
воз.
зд.1
зд1.
задатчик курса судна ϕ. 9., приемник СНС 10., рулевой привод 11., датчик угловой скорости судна ω. 12. и блок 13 - момент внешнего возмущения М, выход которого подключен к входу регулятора 8, к входу которого также подключены: сигнал руля δ с выхода рулевого привода 11., задатчик курса судна ϕ. 9. (через переключатель 7., установленный в 1-е положение), приемник СНС 10., выход регулятора 8. подключен к входу рулевого привода 11. (на выходе регулятора 8. формируется закон управления (1а) движения судна по заданному курсу ϕ- типовое автоматическое управление движением судна по отрезкам заданной траектории, который имеет вид:
,
где ϕ - текущий курс судна, используется сигнал из приемника СНС 10,
зд1.
ϕ - заданный курс судна при установке переключателя 7. в 1-е положение, - типовое автоматическое управление движением судна по отрезкам заданной траектории,
ω- сигнал угловой скорости судна- с датчика угловой скорости судна 12,
δ - сигнал руля с выхода рулевого привода.11,
возм.
М - момент от внешнего возмущения, с блока 13.,
i
K - коэффициенты регулирования,
2) закон автоматического управления движением судна при переходе на другую заданную траекторию использует блоки:
зд.2.
зд.2.
ое
ого
задания траектории 1., вычисления ближайшей точки траектории 2., вычисления направления избранного отрезка линии заданной траектории 3., вычисления направления на ближайшую точку заданной траектории 4., вычисления параметра «γ» 5., вычисления заданного курса судна ϕ 6, который через переключатель 7. (установленный во 2 положение), подключен к входу регулятора 8., выход которого в виде 2 закона управления движением судна по заданному курсу ϕ (1 б):
,
где ϕ - текущий курс судна, используется сигнал от приемника СНС 10,
зд2
ϕ - заданный курс судна, используется сигнал от задатчика курса судна 9.
ое
(при установке переключателя 7. во 2 положение (режим) - переход судна на другую заданную линию отрезка заданной траектории,
ω - сигнал угловой скорости судна - с датчика угловой скорости судна 12,
- угол руля, с выхода рулевого привода 11.,
возм.
М - блок 13. момент внешнего возмущения,
i
K - коэффициенты регулирования,
зд2
подключен к входу рулевого привода 11. (обеспечивая 2-й режим качественного переход судна по заданному курсу ϕ на линию отрезка другой заданной траектории),
му
особенности формирования перехода судна на другую заданную траекторию по 2 закону(1б).,
зд2
2-й закон перехода судна по курсу ϕ формируется с использованием блоков: переключателя 7., регулятора 8., приемника СНС 10., рулевого привода 11., датчика угловой скорости 12., блока 13 отличающийся тем, что включает дополнительно шесть блоков 1-6, необходимые для формирования закона (1б).,
C
C
на вход блока задания точек траектории 1. из приемника СНС 10. поступают координаты судна (X, Z)., которые хранит в запоминающем устройстве пары последовательных координат точек траектории, описывающие эту траекторию как ломаную линию выход блока 1. соединен с блоками 2, 3 и 4.;
1
1
2
2
N
N
C
C
блок вычисления ближайшей точки траектории 2. представляет собой программируемый микроконтроллер, на входы которого поступают: набор точек заданной траектории (X, Z), …, (Х, Z), …, (X, Z) - с блока 1; текущие координаты судна (X, Z) - с приемника СНС 10., номер n отрезка ломаной линии заданной траектории, на которой находится точка, ближайшая к судну (подается на вход блока 3):
T
T
координаты (X, Z) найденной ближайшей на вход блока 4),
расстояние  от судна до ближайшей к нему точки заданной траектории (подается на вход блока 5.),
программа микроконтроллера устроена следующим образом:
в цикле от 1 до N-1 (N - число точек заданной траектории, поступившей из блока 1. перебираются все отрезки траектории,
i
i
для отрезка траектории с номером i (i ∈ [1, N-1]), вычисляется ближайшая к судну точка (X, Z),
на этом отрезке расстояние  от судна до этой точки вычисления производятся следующим образом:
i
i
i+1
i+1
вычисляются коэффициенты параметрического уравнения прямой текущего отрезка траектории (X, Z)-(X, Z):
вычисляется значение параметра уравнения прямой, соответствующее точке основания перпендикуляра, опущенного на эту прямую из точки текущего положения судна:
min
i
i
если t≤0, координаты () считаются равными (Z, Z):
min
если 0<l<1, координаты () вычисляются следующим образом:
,
min
если t≥1 и i≠N-1, координаты () не вычисляются (цикл пропускает этот отрезок заданной траектории),
min
если t≥1 и i=N-1, координаты () вычисляются следующим образом:
,
,
если координаты точки () вычислены, вычисляется расстояние от  судна до этой точки:
i
l= корень квадратный из
среди всех вычисленных точек () выбирается такой номер точки n, что расстояние  минимально:
T
T
в качестве координат (X, Z) берутся ():
n
T
T
вычисленные значения n, l, (X, Z) выдаются на выход блока 2.,
1
блок вычисления направления отрезка траектории 3. вычисляет курс ϕ, которым должно идти судно, чтобы следовать вдоль (параллельно) отрезку заданной траектории с номером n. на входы блока поступают:
1
1
2
2
N
N
набор точек заданной траектории (X, Z), (Х, Z), …, (X, Z) - с блока 1;
номер отрезка траектории n, на котором находится ближайшая в данный момент к судну точка этой траектории - с блока 2.,
n
n
n+1
n+1
блок вычисляет и выдает на выход (соединен с блоком 6) угол отклонения направления отрезка (X, Z)-(X, Z) от направления на север:
где atan 2 - стандартная функция восстановления значения угла по его синусу и косинусу:
2
блок вычисления направления на ближайшую точку 4. вычисляет заданный курс ϕ, которым должно идти судно, чтобы следовать к ближайшей точке траектории, вычисленной блоком 2. на входы блока поступают:
C
C
текущие координаты судна (X, Z) - с приемника СНС 10;
T
T
координаты ближайшей к судну точке траектории (X, Z) - с блока 2.,
C
C
T
T
блок вычисляет и выдает на выход (соединен с блоком 6.) угол отклонения направления отрезка (X, Z)-(X, Z) от направления на север:
n
n
блок вычисления  5.. вычисляет весовой коэффициент γ, зависящий от расстояния между судном и заданной траекторией l. на вход блока поступает расстояние до ближайшей точки траектории l с блока 2, выход блока γ. 5. подается на вход блока 6.,
весовой коэффициент γ вычисляется нелинейной функцией  такой, что:
при =0: γ=0;
при  γ=1 ( - пороговое значение расстояния, зависящее от динамики конкретного судна и определяемое при оптимизации системы управления),
зд.2.
зд.2.
зд.1
блок вычисления задания курса ϕ 6. вычисляет задание курса ϕ, поступающее на типовую судовую систему управления движением по курсу вместо задания курса ϕ
через переключатель 7. на входы блока поступают:
1
направление вдоль заданной траектории ϕ - с блока 3;
зд2
направление на ближайшую точку траектории ϕ - с блока 4;
весовой коэффициент γ - с блока 5.
зд.2
зд.1
зд.1
зд.1
блок вычисляет и выдает через переключатель 7. на вход рулевого привода задание курса судна 14. ϕ, (1б.); при вхождении судна в область допустимого отклонения судна от траектории заданного курса ϕ, переключатель 7. отключает выход блока 6. с законом (1б.), от регулятора 8, а к входу регулятора 8. подключает задатчик курса 9 через переключатель 7., на выходе регулятора 8., при этом сформируется типовой режим управления движением судна по отрезкам заданной траектории с заданным курсом ϕ. по зависимости (1а), при этом установится типовое автоматическое управление движением судна по заданному курсу ϕ.