СИСТЕМА УПРАВЛЕНИЯ ПОДВОДНЫМ АППАРАТОМ
UNDERWATER VEHICLE CONTROL SYSTEM

Изобретение относится к системам управления и может быть использовано при разработке систем управления подводными аппаратами, обеспечивающими их ориентацию и перемещение по заданной траектории с заданной траекторией скоростью, или в заданную точку по требуемой траектории без предъявления требований к траекторией скорости, или в заданную точку с нулевой конечной скоростью.
Известна структура устройства управления роботом [С.Ф. Бурдаков, Р.Э. Стельмаков, С.В. Штайнле "Синтез траекторий и управление мобильными роботами в условиях неопределенности". Материалы VIII научно-технической конференции "Экстремальная робототехника". Под научной редакцией проф. Е.И. Юревича. - СПб.: из-во СПбГТУ, 1997, 439 с. - с.198-209], которая предполагает наличие планировщика траекторий, блока датчиков информации, блока сенсорной поддержки, регулятора положения, регулятора скорости, блока адаптации, блока ограничений, блока кинематических преобразований блоков сравнения, расчетных блоков. Эта структура позволяет организовать движение по заданной траектории с заданной скоростью.
Признаками данного аналога, общими с заявляемой системой, являются планировщик траекторий и блок датчиков информации.
Причиной, препятствующей достижению в этом аналоге технического результата, обеспечиваемого изобретением, является низкая точность планирования и отработки траектории, что обусловлено процедурой локальной аппроксимации сложных траекторий, вносящей погрешность в планирование, а следовательно, и в отработку траектории. Наличие блока кинематических преобразований для трансформации заданной траектории из пространства внешних координат в пространство непосредственно управляемых внутренних координат при сложной организации механической системы объекта управления вносит в эту трансформацию, а следовательно, и в отработку траектории дополнительную погрешность.
Известно также устройство траекторного управления [П.Д. Крутько. Управление исполнительными системами роботов. - М.: Наука, главная ред. физ.мат. литературы, 1997, 336 с. - с.308-313], содержащее планировщик траекторий, блок решения обратной задачи кинематики, блок аппроксимации, блок интерполяции, блок регуляторов, блок датчиков информации, вычислительные блоки, перемножители и сумматоры. Это устройство позволяет обеспечить асимптотическую устойчивость объекта управления при отработке планируемых траекторий.
Признаками этого аналога, общими с заявляемой системой, являются планировщик траекторий, блок датчиков информации, вычислительные блоки, сумматоры и перемножители.
Причинами, препятствующими достижению в этом аналоге технического результата, обеспечиваемого изобретением, являются низкая точность отработки заданной траектории и узкие функциональные возможности. Дело в том, что аппроксимация и интерполяция вносят существенную погрешность в планирование и отработку траектории. Кроме того, аппроксимация траектории осуществляется только отрезками и дугами окружностей, что существенно ограничивает функциональные возможности устройства. Следует отметить, что имеющая место в этом аналоге необходимость решения обратной задачи кинематики является дополнительным ограничением возможностей устройства, так как это решение связано со значительными трудностями и не всегда возможно в аналитической форме.
Наиболее близким по технической сущности к заявляемой системе (прототипом) является устройство управления подвижным объектом, защищенное патентом РФ №2393522, кл. G05D 1/00, G05B 19/19, 2009 г. Это устройство содержит планировщик траекторий, первый, второй и третий вычислители матричных коэффициентов, вычислитель сигнала управления, первый и второй блоки транспонирования матриц, блок датчиков информации, блок формирования матрицы коэффициентов управления, блок формирования вектора нелинейных элементов, два блока формирования матрицы-производной вектор-столбца, блок формирования вектора внешних скоростей, исполнительное устройство, механический блок и установленный на управляемом объекте приемник сигналов спутниковых навигационных систем (СНС).
Признаками прототипа, совпадающими с существенными признаками заявляемой системы, являются все перечисленные элементы.
Причиной, препятствующей достижению в устройстве-прототипе технического результата, обеспечиваемого заявляемой системой, является ограниченность области применения устройства-прототипа. Дело в том, что определение внешних координат управляемого объекта в устройстве-прототипе осуществляется с помощью приемника сигналов СНС, установленного на управляемом объекте, и в случае, если таким объектом является подводный аппарат, находящийся в подводном положении, то прием таковых сигналов невозможен.
Технической задачей, на решение которой направлено создание изобретения, является обеспечение возможности управления движением подводного аппарата.
Технический результат достигается тем, что в известное устройство управления подвижным объектом дополнительно введены судовой пункт управления, два приемопередатчика с антеннами, гидролокатор с антенной и блок пересчета координат, при этом объектом управления является подводный аппарат, а большая часть оборудования установлена на судовом пункте управления.
Для достижения технического результата в известное устройство управления подвижным объектом, содержащее планировщик траекторий, первый, второй и третий вычислители матричных коэффициентов, вычислитель сигнала управления, первый и второй блоки транспонирования матриц, исполнительное устройство, механический блок, блок датчиков информации, блок формирования вектора нелинейных элементов, блок формирования матрицы коэффициентов управления, блок формирования матрицы-производной вектор-столбца внешних скоростей по вектор-строке внутренних координат, блок формирования матрицы-производной вектор-столбца внешних скоростей по вектор-строке внешних координат, блок формирования вектора внешних скоростей и приемник сигналов спутниковых навигационных систем с приемной антенной, в котором первый, второй, третий и четвертый выходы планировщика траекторий соединены соответственно с первым, вторым, третьим и четвертым входами третьего вычислителя матричных коэффициентов, пятый и шестой выходы планировщика траекторий соединены соответственно с третьим и четвертым входами первого и третьего вычислителей матричных коэффициентов, первый и второй входы второго вычислителя матричного коэффициента соединены соответственно со вторым и третьим входами планировщика траекторий, первый вход планировщика траекторий соединен с первыми входами блока формирования вектора нелинейных элементов блока формирования матрицы коэффициентов управления, блоков формирования матриц-производных и блока формирования вектора внешних скоростей, а второй - со входом первого блока транспонирования матриц, вторыми входами блоков формирования матриц-производных, вторым входом блока формирования вектора внешних скоростей и пятым входом третьего вычислителя матричного коэффициента, выход первого блока транспонирования матриц соединен с шестым входом третьего вычислителя матричного коэффициента и пятым входом второго вычислителя матричного коэффициента, выход второго блока транспонирования матриц соединен с шестым входом третьего вычислителя матричного коэффициента и вторым входом первого вычислителя матричного коэффициента, первый и пятый входы которого соединены соответственно со вторым и третьим выходами второго вычислителя матричного коэффициента, выход первого вычислителя матричного коэффициента, первый выход второго вычислителя матричного коэффициента и выход третьего вычислителя матричного коэффициента соединены соответственно с первым, вторым и третьим входами вычислителя сигнала управления, четвертый, пятый, шестой, седьмой и восьмой входы которого соединены с выходами блоков формирования вектора нелинейных элементов, матрицы коэффициентов управления, матриц-производных и вектора внешних скоростей соответственно, дополнительно введены судовой пункт управления, гидролокатор с приемно-передающей антенной, блок пересчета координат и два приемопередатчика с антеннами для подводной радиосвязи, при этом объектом управления является подводный аппарат, непосредственно на подводном аппарате установлены только исполнительное устройство, механический блок и один приемопередатчик с антенной, вход-выход приемопередатчика подводного аппарата соединен со входом-выходом приемно-передающей антенны, выход - со входом исполнительного устройства, выход которого соединен со входами механического блока и блока датчиков информации, соединенного своим выходом со входом приемопередатчика, остальная аппаратура установлена на судовом пункте управления, при этом последний удерживается на водной поверхности непосредственно над управляемым подводным аппаратом, приемно-передающие антенны гидролокатора и приемопередатчика устанавливаются с направлением максимумов характеристик направленности антенн на подводный аппарат, вход-выход приемопередатчика судового пункта управления соединен со входом-выходом приемно-передающей антенны, вход - с выходом вычислителя сигнала управления, а выход - с первым входом блока планировщика траекторий, вход-выход гидролокатора соединен со входом-выходом приемно-передающей антенны, а выход - с первым входом блока пересчета координат, вход приемника сигналов спутниковых навигационных систем соединен с выходом приемной антенны, выход - со вторым входом блока пересчета координат, первый выход которого соединен со вторым входом планировщика траекторий, а второй выход - со входом второго блока транспонирования матриц.
Совокупность вновь введенных судового пункта управления, нового оборудования, новых особенностей его расположения и новых связей не известна из имеющихся в распоряжении заявителя источников информации. Поэтому заявляемую систему управления следует считать новой и соответствующей изобретательскому уровню.
Сущность изобретения поясняется фиг.1, где приведена структурная схема заявляемой системы.
Система управления подводным аппаратом содержит планировщик 1 траекторий, первый 2, второй 3 и третий 4 вычислители матричных коэффициентов, вычислитель 5 сигнала управления, первый 6 и второй 7 блоки транспонирования матриц, блок 8 датчиков информации, блок 9 пересчета координат, блок 10 формирования вектора нелинейных элементов, блок 11 формирования матрицы коэффициентов управления, блок 12 формирования матрицы-производной вектор-столбца внешних скоростей по вектор-строке внутренних координат, блок 13 формирования матрицы производной вектор-столбца внешних скоростей по вектор-строке внешних координат, блок 14 формирования вектора внешних скоростей, исполнительное устройство 15, механический блок 16, приемопередатчики 17.1 и 17.2 с приемно-передающими антеннами 18.1 и 18.2, приемник 19 сигналов спутниковых навигационных систем с приемной антенной 20 и гидролокатор 21 с приемно-передающей антенной 22.
Оборудование системы установлено на двух объектах - судовом пункте управления и собственно объекте управления - подводном аппарате.
На подводном аппарате установлены приемопередатчик 17.1 с антенной 18.1, исполнительное устройство 15, механический блок 16 и блок 8 датчиков информации. Вход-выход приемопередатчика 17.1 соединен со входом-выходом антенны 18.1, выход - со входом устройства 15, а вход - с выходом блока 8, соединенного своим входом с выходом устройства 15 и входом блока 16.
Остальное оборудование установлено на судовом пункте управления. Первый, второй, третий и четвертый выходы планировщика 1 соединены соответственно с первым, вторым, третьим и четвертым входами вычислителя 4 матричных коэффициентов. Пятый и шестой выходы планировщика 1 соединены соответственно с третьим и четвертым входами вычислителей 2 и 3. Первый и второй входы вычислителя 3 соединены соответственно со вторым и третьим входами планировщика 1, первый вход которого соединен с выходом приемопередатчика 17.2 и первыми входами блоков 11, 12, 13 и 14, а второй - с первым входом блока 9, входом блока 6, вторыми входами блоков 12, 13 и 14 и пятым входом вычислителя 4. Выход блока 6 соединен с шестым входом вычислителя 4 и пятым входом вычислителя 3. Вход блока 7 соединен со вторым выходом блока 9, а выход - с шестым входом вычислителя 3 и вторым входом вычислителя 2, первый и пятый входы которого соединены соответственно со вторым и третьим выходами вычислителя 3. Выход вычислителя 2, первый выход вычислителя 3 и выход вычислителя 4 соединены соответственно с первым, вторым и третьим входами вычислителя 5, четвертый, пятый, шестой, седьмой и восьмой входы которого соединены с выходами блоков 10, 11, 12 13 и 14 соответственно. Выход вычислителя 5 соединен со входом приемопередатчика 17.2, вход-выход которого соединен со входом-выходом антенны 18.2.
Работа системы заключается в следующем.
Между судовым пунктом управления и подводным аппаратом с помощью приемопередатчиков 17.1 и 17.2 и антенн 18.1 и 18.2 устанавливается подводная радиосвязь. Благодаря этой радиосвязи формируемый на выходе вычислителя 5 сигнал U управления подводным аппаратом (процесс формирования сигнала U изложен ниже) передается с помощью приемопередатчика 17.2 и антенны 18.2 из судового пункта управления и принимается с помощью антенны 18.1 и приемопередатчика 17.1 на подводном аппарате и далее поступает на вход устройства 15. Устройство 15 и блоки 16 и 8, подключенные к его выходу, отрабатывают этот сигнал. Блок 8 осуществляет измерение внутренних координат подводного аппарата. На его выходах формируется вектор Z внутренних координат размерностью n, который поступает на вход приемопередатчика 17.1. Далее с помощью приемопередатчика 17.1. и антенны 18.1 эта информация передается на судовой пункт управления, где принимается с помощью антенны 18.2 и приемопередатчика 17.2 и с его выхода поступает на первый вход планировщика 1 и на первые входы блоков 10, 11, 12, 13 и 14.
<mover accent="true"><mi>Y</mi><mo>˙</mo></mover>
Блок 9, приемник 19 с антенной 20 и гидролокатор 21 с антенной 22 предназначены для формирования вектора Y внешних координат подводного аппарата и вектора
производной вектора Y.
Это осуществляется следующим образом.
Г
В
Г
В
СПУ
СПУ
Г
В
<mover accent="true"><mi>Y</mi><mo>˙</mo></mover>
<mover accent="true"><mi>Y</mi><mo>˙</mo></mover>
<mover accent="true"><mi>Y</mi><mo>˙</mo></mover>
Гидролокатор 21 с антенной 22 осуществляют обнаружение и захват на сопровождение подводного аппарата. Его приблизительные координаты всегда известны. В дальнейшем осуществляется сопровождение подводного аппарата гидролокатором 21 и поддержание максимумов характеристик направленности антенн 18.2 и 22 в направлении на подводный аппарат. При этом судно - носитель судового пункта управления - поддерживается на водной поверхности как можно ближе к подводному аппарату. После захвата подводного аппарата на сопровождение гидролокатор 21 уточняет дальность D до него от судового пункта управления и углы его визирования с судового пункта управления в горизонтальной φ и вертикальной φ плоскостях. Измеренные дальность D и углы φ и φ однозначно определяют координаты подводного аппарата относительно судового пункта управления. Далее они с выхода гидролокатора 21 поступают на первый вход блока 9. Собственные внешние координаты судового пункта управления определяются с помощью приемника 19 с антенной 20. Вектор Y внешних координат судового пункта управления формируется на выходе приемника 19 и поступает оттуда на второй вход блока 9. Блок 9 осуществляет преобразование вектора Y внешних координат судового пункта управления в вектор Y внешних координат подводного аппарата и его производную
с учетом измеренных дальности D и углов визирования φ и φ. На первом выходе блока 9 формируется вектор Y, а на втором - его производная - вектор
. Координаты вектора Y с первого выхода блока 9 поступают на второй вход планировщика 1, на вход блока 6, на пятый вход вычислителя 4 и на вторые входы блоков 12, 13 и 14, а координаты вектора
со второго выхода блока 9 поступают на первый вход блока 7.
Планировщик 1 под действием управляющих сигналов Z и Y формирует на своих выходах следующие управляющие сигналы:
K
- траекторная (контурная) скорость V подводного аппарата;
1j
2j
3j
<mrow><mi>j</mi><mo>=</mo><mover accent="true"><mrow><mn>1</mn><mo>,</mo><mi>n</mi></mrow><mo stretchy="true">¯</mo></mover></mrow>
- матрицы N, N и N квадратичных форм от внешних координат, где
, а n - число измеряемых внутренних координат;
- диагональные матрицы С и А постоянных коэффициентов размерностью n×n.
K
1j
2j
3j
Сигнал V формируется на первом выходе планировщика 1 и поступает оттуда на первый вход вычислителя 4. Сигнал N формируется на втором выходе планировщика 1 и поступает оттуда на первый вход вычислителя 3 и на второй вход вычислителя 4. Сигнал N формируется на третьем выходе планировщика 1 и поступает оттуда на второй вход вычислителя 3 и третий вход вычислителя 4. Сигнал N формируется на четвертом выходе планировщика 1 и поступает оттуда на четвертый вход вычислителя 4. Матрица C формируется на пятом выходе планировщика 1 и поступает оттуда на третьи входы вычислителей 2 и 3. Матрица А формируется на шестом выходе планировщика 1 и поступает оттуда на четвертые входы вычислителей 2 и 3 и седьмой вход вычислителя 4.
В блоке 10 формируется вектор F нелинейного преобразования внутренних координат Z. Этот вектор - нелинейная функция, своя для каждого объекта управления. С выхода блока 10 вектор F поступает на четвертый вход вычислителя 5.
Аналогично в блоке 11 формируется матрица В нелинейного преобразования внутренних координат Z. Она представляет собой матрицу коэффициентов управления и так же, как и вектор F, является специфичной для каждого объекта управления. С выхода блока 11 матрица В поступает на пятый вход вычислителя 5.
В блоке 12 формируется матрица R - производная вектор-столбца внешних скоростей по вектор-строке внутренних координат. Она формируется как нелинейная функция не только внутренних Z, а и внешних Y координат и также является специфичной для каждого объекта управления. С выхода блока 12 матрица R поступает на шестой вход вычислителя 5.
В блоке 13 аналогично формируется матрица L - производная вектор-столбца внешних скоростей по вектор-строке внешних координат. Она так же, как и матрица R, формируется как нелинейная функция координат Z и Y и является специфичной для каждого конкретного объекта управления. С выхода блока 13 матрица L поступает на седьмой вход вычислителя 5.
В блоке 14 формируется вектор М внешних скоростей, также являющийся нелинейной функцией координат Z и У, специфичной для каждого объекта управления. С выхода блока 14 вектор М поступает на восьмой вход вычислителя 5.
T
Блок 6 осуществляет транспонирование поступившего на его вход вектора У внешних координат. Результат Y транспонирования с выхода блока 6 поступает на пятый вход вычислителя 3 и на шестой вход вычислителя 4.
<mover accent="true"><mi>Y</mi><mo>˙</mo></mover>
<mrow><msup><mover accent="true"><mi>Y</mi><mo>˙</mo></mover><mi>T</mi></msup></mrow>
Аналогично блок 7 осуществляет транспонирование поступившего на его вход вектора
- производной вектора Y внешних координат. Результат
транспонирования с выхода блока 7 поступает на второй вход вычислителя 2 и шестой вход вычислителя 3.
В вычислителе 3 формируются следующие сигналы:
2n
- матрица N квадратичной формы от внешних координат;
j
i
- вспомогательные матрицы D и D, рассчитываемые по формулам:
j
1j
2j
T
D=2Y·N+N,
i
1i
2i
T
D=2Y·N+N,
где i=n-1.
2
- второй матричный коэффициент K. в соответствии с уравнением:
1
где 0 - вектор нулевых элементов размерностью n.
2
i
2n
Коэффициент K с первого выхода вычислителя 3 поступает на второй вход вычислителя 5, матрица D со второго выхода вычислителя 3 поступает на первый вход вычислителя 2, а матрица N с третьего выхода вычислителя 3 поступает на пятый вход вычислителя 2.
1
В вычислителе 2 формируется первый матричный коэффициент K в соответствии с уравнением:
где E - единичная матрица размерностью m×m,
m - число измеряемых внешних координат, удовлетворяющее условию n≤m≤6.
1
Коэффициент K с выхода вычислителя 2 поступает на первый вход вычислителя 5.
3
В вычислителе 4 формируется третий матричный коэффициент K в соответствии с уравнением:
3
С выхода вычислителя 4 матричный коэффициент K поступает на третий вход вычислителя 5.
В вычислителе 5 формируется матрица U - сигнал управления подводным аппаратом в соответствии с уравнением:
Сформированный сигнал U с выхода вычислителя 5 поступает на вход приемопередатчика 17.2
Далее описанный процесс повторяется.
Таким образом, реализуемый заявляемым устройством процесс формирования управляющего сигнала U состоит в следующем.
1. С помощью устройства 15, блоков 16 и 8 измеряются внутренние координаты Z подводного аппарата и с помощью аппаратуры подводной радиосвязи (приемопередатчики 17.1, 17.2 с антеннами 18.1 и 18.2) передаются с подводного аппарата на судовой пункт управления в планировщик 1 и в блоки 10,11, 12, 13 и 14.
<mover accent="true"><mi>Y</mi><mo>˙</mo></mover>
2. Измеряются внешние координаты Y подводного аппарата и их производные
. Эта операция осуществляется в три этапа:
СПУ
- с помощью приемника 19 с антенной 20 определяются внешние координаты Y судового пункта управления;
Г
В
- с помощью гидролокатора 21 с антенной 22 захватывается на сопровождение подводный аппарат и измеряются дальность D до него от судового пункта управления и углы его визирования с судового пункта управления в горизонтальной φ и вертикальной φ плоскостях;
СПУ
<mover accent="true"><mi>Y</mi><mo>˙</mo></mover>
- с помощью блока 9 осуществляется пересчет внешних координат судового пункта управления Y во внешние координаты подводного аппарата Y и их производные
.
1j
2j
3j
3. С помощью планировщика 1 формируются требуемая траектория, матрицы N, ·N, ·N, квадратичных форм и диагональные матрицы А и В.
4. С помощью блоков 10, 11, 12, 13 и 14 формируются вектор М внешних скоростей, вектор F и матрица В нелинейного преобразования внутренних координат и матрицы - производные R и L вектор-столбца внешних скоростей.
i
j
1·
2·
3·
5. С помощью вычислителей 2, 3, 4 и блоков 6 и 7 формируются вспомогательные матрицы D и D и матричные коэффициенты K, K и K
6. С помощью вычислителя 5 формируется матрица сигнала управления U движением подводного аппарата.
Описанные алгоритм и приведенные выше уравнения полностью соответствуют алгоритму управления, приведенному в работе [В.Х. Пшихопов "Аналитический синтез синергетических регуляторов для позиционно-траекторных систем управления мобильными роботами". Материалы XI научно-технической конференции "Экстремальная робототехника". Под научной редакцией проф. Е.И. Юревича. - СПб.: СПбГТУ, 2000]. Этот алгоритм обеспечивает управление движением объекта по заданной траектории.
Таким образом, предлагаемая система, как и устройство-прототип, обеспечивает движение объекта управления по заданной траектории. Однако нетрудно видеть, что заявляемая система в отличие от прототипа обеспечивает и движение по заданной траектории подводного аппарата. В ней координаты объекта управления, находящегося под водой, измеряются относительно судового пункта управления с помощью гидролокатора, при этом точные внешние координаты самого судового пункта управления определяются с помощью установленного на судовом пункте управления приемника спутниковых навигационных систем и пересчитываются во внешние координаты объекта управления с учетом координат, измеренных гидролокатором
Система довольно легко реализуема.
Судовой пункт управления размещен на сопровождающем судне-носителе, снабженным соответствующим спусковым оборудованием, необходимым для обслуживания АНПА.
В качестве гидролокатора может быть использован модуль «Сарган-ЭМ», работающий на частотах 19.7 кГц и 135 кГц, имеющий дальность обнаружения одиночной цели не менее 1200 м.
В качестве приемопередатчиков с антеннами могут быть использованы модули «GIRAO», работающие в диапазоне частот 31…45 кГц на расстоянии до 5 км.
В качестве приемника сигналов спутниковых навигационных систем может быть использован модуль СН-4715. Количество каналов приема - 24. Точность определение координат - 5 м. Обеспечивает прием и обработку сигналов спутниковых навигационных систем ГЛОНАСС, GPS, SBAS (EGNOS/WAAS/MSAS). Частота обновления координат - до 5 Гц..
Блок пересчета координат может быть реализован на базе одноплатного компьютера СРС308.
Остальные элементы системы могут быть выполнены на основе соответствующих элементов того же назначения устройства-прототипа.